void DrawMeshHandler::synchronize() {
// We update DrawMeshHandler's data from its associated scene object custom data:
// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
int buffSize = simGetObjectCustomDataLength(_associatedObjectID,
CustomDataHeaders::DEVELOPER_DATA_HEADER);
char* datBuff = new char[buffSize];
simGetObjectCustomData(_associatedObjectID,
CustomDataHeaders::DEVELOPER_DATA_HEADER, datBuff);
std::vector<unsigned char> developerCustomData(datBuff, datBuff + buffSize);
delete[] datBuff;
// 2. From that retrieved data, try to extract sub-data with the DATA_MAIN tag:
std::vector<unsigned char> tempMainData;
if (CAccess::extractSerializationData(developerCustomData, DATA_MAIN,
tempMainData)) { // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
// Remove # chars for compatibility with ROS
_associatedObjectName = simGetObjectName(_associatedObjectID);
std::stringstream streamtemp;
streamtemp << "- [Draw Mesh] in '" << _associatedObjectName << "'."
<< std::endl;
}
}
void SetTwistHandler::synchronize(){
// We update SetTwistHandler's data from its associated scene object custom data:
// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// 2. From that retrieved data, try to extract sub-data with the SET_OBJ_TWIST_DATA_MAIN tag:
std::vector<unsigned char> tempMainData;
if (CAccess::extractSerializationData(developerCustomData,CustomDataHeaders::SET_OBJ_TWIST_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
// Remove # chars for compatibility with ROS
_associatedObjectName = simGetObjectName(_associatedObjectID);
std::string objectName(_associatedObjectName);
std::replace( objectName.begin(), objectName.end(), '#', '_');
//_pub = _nh.advertise<geometry_msgs::TwistStamped>(objectName+"/twist", 1);
std::stringstream streamtemp;
streamtemp << "- [Set Twist Target] in '" << _associatedObjectName << "'. Subscribed on the topic "<< objectName<<"/SetTwist"<< std::endl;
ConsoleHandler::printInConsole(streamtemp);
}
}
void CBubbleRob::_initialize()
{
if (_initialized)
return;
// We need to find the handle of BubbleRob's left/right motors, and proximity sensor. We know that
// the left motor has a BUBBLEROB_DATA_LEFTMOTOR-tag, the right motor has a BUBBLEROB_DATA_RIGHTMOTOR-tag,
// the proximity sensor has a BUBBLEROB_DATA_SENSOR-tag. Since there might be several BubbleRobs in the
// scene, we should only explore this BubbleRob's tree hierarchy to find those tags:
_handleOfLeftJoint=-1;
_handleOfRightJoint=-1;
_handleOfProximitySensor=-1;
std::vector<int> toExplore;
toExplore.push_back(_associatedObjectID); // We start exploration with the base of the BubbleRob-tree
while (toExplore.size()!=0)
{
int objHandle=toExplore[toExplore.size()-1];
toExplore.pop_back();
// 1. Add this object's children to the list to explore:
int index=0;
int childHandle=simGetObjectChild(objHandle,index++);
while (childHandle!=-1)
{
toExplore.push_back(childHandle);
childHandle=simGetObjectChild(objHandle,index++);
}
// 2. Now check if this object has one of the tags we are looking for:
// a. Get all the developer data attached to this scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objHandle,DEVELOPER_DATA_HEADER);
if (buffSize!=0)
{ // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
char* datBuff=new char[buffSize];
simGetObjectCustomData(objHandle,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// b. From that retrieved data, try to extract sub-data with the searched tags:
std::vector<unsigned char> bubbleRobTagData;
if (CAccess::extractSerializationData(developerCustomData,BUBBLEROB_DATA_LEFTMOTOR,bubbleRobTagData))
_handleOfLeftJoint=objHandle; // We found the BUBBLEROB_DATA_LEFTMOTOR tag. This is the left motor!
if (CAccess::extractSerializationData(developerCustomData,BUBBLEROB_DATA_RIGHTMOTOR,bubbleRobTagData))
_handleOfRightJoint=objHandle; // We found the BUBBLEROB_DATA_RIGHTMOTOR tag. This is the right motor!
if (CAccess::extractSerializationData(developerCustomData,BUBBLEROB_DATA_SENSOR,bubbleRobTagData))
_handleOfProximitySensor=objHandle; // We found the BUBBLEROB_DATA_SENSOR tag. This is the proximity sensor!
}
}
_movementDuration=0.0f; // Start in a stopped state
_backMovementDuration=0.0f; // Forward
_backRelativeVelocities[0]=0.5f; // default value
_backRelativeVelocities[1]=0.25f; // default value
_setToZeroAtMovementEnd=NULL;
_initialized=true;
}
void CBubbleRob::synchronize()
{ // We update CBubbleRob's data from its associated scene object custom data:
// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(_associatedObjectID,DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(_associatedObjectID,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// 2. From that retrieved data, try to extract sub-data with the BUBBLEROB_DATA_MAXVELOCITY tag:
std::vector<unsigned char> bubbleRobVelocityData;
if (CAccess::extractSerializationData(developerCustomData,BUBBLEROB_DATA_MAXVELOCITY,bubbleRobVelocityData))
{ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
_maxVelocity=CAccess::pop_float(bubbleRobVelocityData);
}
}
void GetTwistHandler::_initialize(){
if (_initialized)
return;
std::vector<int> toExplore;
toExplore.push_back(_associatedObjectID); // We start exploration with the base of the quadrotor-tree
while (toExplore.size()!=0)
{
int objHandle=toExplore[toExplore.size()-1];
toExplore.pop_back();
// 1. Add this object's children to the list to explore:
int index=0;
int childHandle=simGetObjectChild(objHandle,index++);
while (childHandle!=-1) {
toExplore.push_back(childHandle);
// std::cout << "Adding " << simGetObjectName(childHandle) << " to exploration list." << std::endl;
childHandle=simGetObjectChild(objHandle,index++);
}
// 2. Now check if this object has one of the tags we are looking for:
// a. Get all the developer data attached to this scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER);
if (buffSize!=0) { // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
char* datBuff=new char[buffSize];
simGetObjectCustomData(objHandle,CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// b. From that retrieved data, try to extract sub-data with the searched tags:
std::vector<unsigned char> quadrotorTagData;
}
}
_lastPublishedObjTwistTime = -1e5;
//Check if the object is static
simGetObjectIntParameter(_associatedObjectID, 3003, &_isStatic);
if(_isStatic){
std::stringstream ss;
ss << "- [" << _associatedObjectName << "] WARNING: getting velocity of a static object might give inaccurate results." << std::endl;;
ConsoleHandler::printInConsole(ss);
}
_initialized=true;
}
void CBubbleRob::putBubbleRobTagToSceneObject(int objectHandle,float maxVelocity)
{ // This creates/updates a BUBBLEROB_DATA_MAXVELOCITY tag
// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objectHandle,DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(objectHandle,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// 2. From that retrieved data, extract sub-data with the BUBBLEROB_DATA_MAXVELOCITY tag, update it, and add it back to the retrieved data:
std::vector<unsigned char> bubbleRobVelocityData;
CAccess::extractSerializationData(developerCustomData,BUBBLEROB_DATA_MAXVELOCITY,bubbleRobVelocityData);
bubbleRobVelocityData.clear(); // we discard the old value (if present)
CAccess::push_float(bubbleRobVelocityData,maxVelocity); // we replace it with the new value
CAccess::insertSerializationData(developerCustomData,BUBBLEROB_DATA_MAXVELOCITY,bubbleRobVelocityData);
// 3. We add/update the scene object with the updated custom data:
simAddObjectCustomData(objectHandle,DEVELOPER_DATA_HEADER,(simChar*)&developerCustomData[0],int(developerCustomData.size()));
}
void CK3::synchronize()
{ // We update CK3's data from its associated scene object custom data:
// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(_associatedObjectID,DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(_associatedObjectID,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// 2. From that retrieved data, try to extract sub-data with the K3_MAXVELOCITY,K3_MODELVERSION, etc. tags:
std::vector<unsigned char> k3Data;
if (CAccess::extractSerializationData(developerCustomData,K3_MAXVELOCITY,k3Data))
_maxVelocity=CAccess::pop_float(k3Data);
if (CAccess::extractSerializationData(developerCustomData,K3_MODELVERSION,k3Data))
_modelVersion=CAccess::pop_int(k3Data);
if (CAccess::extractSerializationData(developerCustomData,K3_MAXACCELERATION,k3Data))
_maxAcceleration=CAccess::pop_float(k3Data);
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMAXACCELERATION,k3Data))
_maxArmAcceleration=CAccess::pop_float(k3Data);
}
void CK3::putK3TagToSceneObject(int objectHandle,int modelVersion,float maxVelocity,float maxAccel,float maxArmAccel)
{ // This creates/updates a K3_BASE tag
// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objectHandle,DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(objectHandle,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// 2. From that retrieved data, extract sub-data with the K3_BASE,K3_MAXVELOCITY, etc tags, update them, and add them back to the retrieved data:
std::vector<unsigned char> k3Data;
CAccess::extractSerializationData(developerCustomData,K3_BASE,k3Data);
k3Data.clear(); // we discard the old value (if present)
CAccess::push_float(k3Data,0); // we replace it with the new value
CAccess::insertSerializationData(developerCustomData,K3_BASE,k3Data);
CAccess::extractSerializationData(developerCustomData,K3_MAXVELOCITY,k3Data);
k3Data.clear();
CAccess::push_float(k3Data,maxVelocity);
CAccess::insertSerializationData(developerCustomData,K3_MAXVELOCITY,k3Data);
CAccess::extractSerializationData(developerCustomData,K3_MODELVERSION,k3Data);
k3Data.clear();
CAccess::push_int(k3Data,modelVersion);
CAccess::insertSerializationData(developerCustomData,K3_MODELVERSION,k3Data);
CAccess::extractSerializationData(developerCustomData,K3_MAXACCELERATION,k3Data);
k3Data.clear();
CAccess::push_float(k3Data,maxAccel);
CAccess::insertSerializationData(developerCustomData,K3_MAXACCELERATION,k3Data);
CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMAXACCELERATION,k3Data);
k3Data.clear();
CAccess::push_float(k3Data,maxArmAccel);
CAccess::insertSerializationData(developerCustomData,K3_GRIPPER_ARMMAXACCELERATION,k3Data);
// 3. We add/update the scene object with the updated custom data:
simAddObjectCustomData(objectHandle,DEVELOPER_DATA_HEADER,(simChar*)&developerCustomData[0],int(developerCustomData.size()));
}
void GetTwistHandler::synchronize(){
// We update GetTwistHandler's data from its associated scene object custom data:
// 1. Get all the developer data attached to the associated scene object (this is custom data added by the developer):
int buffSize = simGetObjectCustomDataLength(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(_associatedObjectID, CustomDataHeaders::DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// 2. From that retrieved data, try to extract sub-data with the OBJ_TWIST_DATA_MAIN tag:
std::vector<unsigned char> tempMainData;
if (CAccess::extractSerializationData(developerCustomData,CustomDataHeaders::OBJ_TWIST_DATA_MAIN,tempMainData)){ // Yes, the tag is there. For now we only have to synchronize _maxVelocity:
// Remove # chars for compatibility with ROS
_associatedObjectName = simGetObjectName(_associatedObjectID);
std::string objectName(_associatedObjectName);
std::replace( objectName.begin(), objectName.end(), '#', '_');
_pub = _nh.advertise<geometry_msgs::TwistStamped>(objectName+"/twist", 1);
std::stringstream streamtemp;
int temp_freq = CAccess::pop_int(tempMainData);
if (temp_freq > 0){
_ObjTwistFrequency = temp_freq;
//std::cout << "Found Twist target in " << _associatedObjectName << ". Frequency publisher: " << _ObjTwistFrequency << std::endl;
streamtemp << "- [Twist Target] in '" << _associatedObjectName << "'. Frequency publisher: " << _ObjTwistFrequency << "." << std::endl;
ConsoleHandler::printInConsole(streamtemp);
} else {
//std::cout << "Found Twist target in " << _associatedObjectName << " at the simulation frequency. " << std::endl;
streamtemp << "- [Twist Target] in '" << _associatedObjectName << "'. Frequency publisher: Simulation frequency. " << std::endl;
ConsoleHandler::printInConsole(streamtemp);
}
}
}
void CBubbleRobContainer::actualizeForSceneContent()
{ // This is called everytime the scene content in V-REP might have changed (e.g. object added/removed/etc.)
// We need to make sure every BubbleRob in V-REP's scene has exactly one CBubbleRob in this container.
// We now synchronize this container with current V-REP's scene content:
// 1. We first check which CBubbleRob is not valid anymore, and remove it
int i=0;
while (i<int(_allBubbleRobs.size()))
{
CBubbleRob* bubbleRob=_allBubbleRobs[i];
int associatedObject=bubbleRob->getAssociatedObject();
int uniqueID;
if (simGetObjectUniqueIdentifier(associatedObject,&uniqueID)==-1)
{ // the object doesn't exist anymore!
removeFromID(bubbleRob->getID());
i=-1; // ordering in _allBubbleRobs might have changed, we start from the beginning
}
else
{ // An object with that handle exists, is its unique ID same as getAssociatedObjectUniqueId()?
if (uniqueID!=bubbleRob->getAssociatedObjectUniqueId())
{ // the object doesn't exist anymore! (the object was destroyed and then another object was assigned the same handle (e.g. new scene was loaded))
removeFromID(bubbleRob->getID());
i=-1; // ordering in _allBubbleRobs might have changed, we start from the beginning
}
else
{ // the object still exists. Does it still have its BUBBLEROB_DATA_MAXVELOCITY tag?
// a. Get all the developer data attached to this object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(associatedObject,DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(associatedObject,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// b. From that retrieved data, try to extract sub-data with the BUBBLEROB_DATA_MAXVELOCITY tag:
std::vector<unsigned char> bubbleRobVelocityData;
if (!CAccess::extractSerializationData(developerCustomData,BUBBLEROB_DATA_MAXVELOCITY,bubbleRobVelocityData)) // the tag is composed by a string and an integer
{ // No, there is no data with the BUBBLEROB_DATA_MAXVELOCITY tag present! We remove this object:
removeFromID(bubbleRob->getID());
i=-1; // ordering in _allBubbleRobs might have changed, we start from the beginning
}
}
}
i++;
}
// 2. Now all CBubbleRob objects in this container are up-to-date. We now go through all objects in the scene
// that have the BUBBLEROB_DATA_MAXVELOCITY tag, and check if they have a related CBubbleRob object in this container.
// If not, we have to add a related CBubbleRob object:
int objHandle=0;
int ind=0;
while (objHandle!=-1)
{
objHandle=simGetObjects(ind++,sim_handle_all);
if (objHandle!=-1)
{ // Scene object present at index ind
// Is that scene object already associated with a CBubbleRob object?
if (getFromAssociatedObject(objHandle)==NULL)
{ // No, not yet
// Does the object have a BUBBLEROB_DATA_MAXVELOCITY tag?
// a. Get all the developer data attached to this object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objHandle,DEVELOPER_DATA_HEADER);
char* datBuff=new char[buffSize];
simGetObjectCustomData(objHandle,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// b. From that retrieved data, try to extract sub-data with the BUBBLEROB_DATA_MAXVELOCITY tag:
std::vector<unsigned char> bubbleRobVelocityData;
if (CAccess::extractSerializationData(developerCustomData,BUBBLEROB_DATA_MAXVELOCITY,bubbleRobVelocityData))
{ // Yes, the tag is there. We have to add a new CBubbleRob object associated with this scene object:
CBubbleRob* bubbleRob=new CBubbleRob();
insert(bubbleRob);
int uniqueID;
simGetObjectUniqueIdentifier(objHandle,&uniqueID);
bubbleRob->setAssociatedObject(objHandle,uniqueID);
bubbleRob->synchronize(); // Make sure the CBubbleRob object has the same values
}
}
}
}
}
void CK3::_initialize()
{
if (_initialized)
return;
checkScale();
// We need to find the handle of K3's motors and sensors according to data tags attached to objects.
// Since there might be several K3s in the scene, we should only explore this K3's tree hierarchy
// to find those tags:
for (int i=0;i<2;i++)
_k3MotorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3ColorSensorHandles[i]=-1;
for (int i=0;i<9;i++)
_k3IrSensorHandles[i]=-1;
for (int i=0;i<5;i++)
_k3UsSensorHandles[i]=-1;
for (int i=0;i<6;i++)
_k3GripperArmMotorHandles[i]=-1;
for (int i=0;i<3;i++)
_k3GripperFingerMotorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3GripperDistanceSensorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3GripperColorSensorHandles[i]=-1;
std::vector<int> toExplore;
toExplore.push_back(_associatedObjectID); // We start exploration with the base of the K3-tree
while (toExplore.size()!=0)
{
int objHandle=toExplore[toExplore.size()-1];
toExplore.pop_back();
// 1. Add this object's children to the list to explore:
int index=0;
int childHandle=simGetObjectChild(objHandle,index++);
while (childHandle!=-1)
{
toExplore.push_back(childHandle);
childHandle=simGetObjectChild(objHandle,index++);
}
// 2. Now check if this object has one of the tags we are looking for:
// a. Get all the developer data attached to this scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objHandle,DEVELOPER_DATA_HEADER);
if (buffSize!=0)
{ // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
char* datBuff=new char[buffSize];
simGetObjectCustomData(objHandle,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// b. From that retrieved data, try to extract sub-data with the searched tags:
std::vector<unsigned char> k3TagData;
if (CAccess::extractSerializationData(developerCustomData,K3_LEFTMOTOR,k3TagData))
_k3MotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_RIGHTMOTOR,k3TagData))
_k3MotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORLEFT,k3TagData))
_k3ColorSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORRIGHT,k3TagData))
_k3ColorSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR1,k3TagData))
_k3IrSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR2,k3TagData))
_k3IrSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR3,k3TagData))
_k3IrSensorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR4,k3TagData))
_k3IrSensorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR5,k3TagData))
_k3IrSensorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR6,k3TagData))
_k3IrSensorHandles[5]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR7,k3TagData))
_k3IrSensorHandles[6]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR8,k3TagData))
_k3IrSensorHandles[7]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR9,k3TagData))
_k3IrSensorHandles[8]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR1,k3TagData))
_k3UsSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR2,k3TagData))
_k3UsSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR3,k3TagData))
_k3UsSensorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR4,k3TagData))
_k3UsSensorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR5,k3TagData))
_k3UsSensorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR1,k3TagData))
_k3GripperArmMotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR2,k3TagData))
_k3GripperArmMotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR3,k3TagData))
_k3GripperArmMotorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR4,k3TagData))
_k3GripperArmMotorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR5,k3TagData))
_k3GripperArmMotorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR6,k3TagData))
_k3GripperArmMotorHandles[5]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR1,k3TagData))
_k3GripperFingerMotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR2,k3TagData))
_k3GripperFingerMotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR3,k3TagData))
_k3GripperFingerMotorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTDISTSENSOR,k3TagData))
_k3GripperDistanceSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTDISTSENSOR,k3TagData))
_k3GripperDistanceSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTCOLSENSOR,k3TagData))
_k3GripperColorSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTCOLSENSOR,k3TagData))
_k3GripperColorSensorHandles[1]=objHandle;
}
}
// Now get the display handle (Custom UI):
char* baseName=simGetObjectName(_associatedObjectID);
int suffixNumber=simGetNameSuffix(baseName);
simReleaseBuffer(baseName);
int globalSuffixSave=simGetNameSuffix(NULL);
simSetNameSuffix(suffixNumber);
_k3DisplayHandle=simGetUIHandle("K3_stateVisualization");
simSetNameSuffix(globalSuffixSave); // reset to previous value
_targetVelocities[0]=0.0f;
_targetVelocities[1]=0.0f;
_currentVelocities[0]=0.0f;
_currentVelocities[1]=0.0f;
_cumulativeMotorAngles[0]=0.0f;
_cumulativeMotorAngles[1]=0.0f;
_previousMotorAngles[0]=0.0f;
_previousMotorAngles[1]=0.0f;
_currentArmPosition=0.0f;
_targetArmPosition=0.0f;
_currentArmVelocity=0.0f;
_targetGripperGap_unscaled=0.055f;
_initialized=true;
}
