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; }