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 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 robot::setLocalRespondableMaskCummulative_alternate(int objHandle,bool bitSet)
{
if (simGetObjectType(objHandle)==sim_object_shape_type)
{
int p;
simGetObjectIntParameter(objHandle,3004,&p);
if (p!=0)
{
if (bitSet)
simSetObjectIntParameter(objHandle,3019,0xff01);
else
simSetObjectIntParameter(objHandle,3019,0xff02);
bitSet=!bitSet;
}
}
int index=0;
while (true)
{
int childHandle=simGetObjectChild(objHandle,index++);
if (childHandle==-1)
break;
setLocalRespondableMaskCummulative_alternate(childHandle,bitSet);
}
}
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;
}
