robot::robot(std::string filename,bool hideCollisionLinks,bool hideJoints,bool convexDecomposeNonConvexCollidables,bool createVisualIfNone,bool showConvexDecompositionDlg,bool centerAboveGround,bool makeModel,bool noSelfCollision,bool positionCtrl): filenameAndPath(filename) { printToConsole("URDF import operation started."); openFile(); readJoints(); readLinks(); readSensors(); createJoints(hideJoints,positionCtrl); createLinks(hideCollisionLinks,convexDecomposeNonConvexCollidables,createVisualIfNone,showConvexDecompositionDlg); createSensors(); std::vector<int> parentlessObjects; std::vector<int> allShapes; std::vector<int> allObjects; std::vector<int> allSensors; for (int i=0;i<int(vLinks.size());i++) { if (simGetObjectParent(vLinks[i]->nLinkVisual)==-1) parentlessObjects.push_back(vLinks[i]->nLinkVisual); allObjects.push_back(vLinks[i]->nLinkVisual); allShapes.push_back(vLinks[i]->nLinkVisual); if (vLinks[i]->nLinkCollision!=-1) { if (simGetObjectParent(vLinks[i]->nLinkCollision)==-1) parentlessObjects.push_back(vLinks[i]->nLinkCollision); allObjects.push_back(vLinks[i]->nLinkCollision); allShapes.push_back(vLinks[i]->nLinkCollision); } } for (int i=0;i<int(vJoints.size());i++) { if (vJoints[i]->nJoint!=-1) { if (simGetObjectParent(vJoints[i]->nJoint)==-1) parentlessObjects.push_back(vJoints[i]->nJoint); allObjects.push_back(vJoints[i]->nJoint); } } for (int i=0;i<int(vSensors.size());i++) { if (vSensors[i]->nSensor!=-1) { if (simGetObjectParent(vSensors[i]->nSensor)==-1) parentlessObjects.push_back(vSensors[i]->nSensor); allObjects.push_back(vSensors[i]->nSensor); allSensors.push_back(vSensors[i]->nSensor); } if (vSensors[i]->nSensorAux!=-1) { allObjects.push_back(vSensors[i]->nSensorAux); allSensors.push_back(vSensors[i]->nSensorAux); } } // If we want to alternate respondable mask: if (!noSelfCollision) { for (int i=0;i<int(parentlessObjects.size());i++) setLocalRespondableMaskCummulative_alternate(parentlessObjects[i],true); } // Now center the model: if (centerAboveGround) { bool firstValSet=false; C3Vector minV,maxV; for (int shNb=0;shNb<int(allShapes.size());shNb++) { float* vertices; int verticesSize; int* indices; int indicesSize; if (simGetShapeMesh(allShapes[shNb],&vertices,&verticesSize,&indices,&indicesSize,NULL)!=-1) { C7Vector tr; simGetObjectPosition(allShapes[shNb],-1,tr.X.data); C3Vector euler; simGetObjectOrientation(allShapes[shNb],-1,euler.data); tr.Q.setEulerAngles(euler); for (int i=0;i<verticesSize/3;i++) { C3Vector v(vertices+3*i); v*=tr; if (!firstValSet) { minV=v; maxV=v; firstValSet=true; } else { minV.keepMin(v); maxV.keepMax(v); } } simReleaseBuffer((char*)vertices); simReleaseBuffer((char*)indices); } } C3Vector shiftAmount((minV+maxV)*-0.5f); shiftAmount(2)+=(maxV(2)-minV(2))*0.5f; for (int i=0;i<int(parentlessObjects.size());i++) { C3Vector p; simGetObjectPosition(parentlessObjects[i],-1,p.data); p+=shiftAmount; simSetObjectPosition(parentlessObjects[i],-1,p.data); } } // Now create a model bounding box if that makes sense: if ((makeModel)&&(parentlessObjects.size()==1)) { int p=simGetModelProperty(parentlessObjects[0]); p|=sim_modelproperty_not_model; simSetModelProperty(parentlessObjects[0],p-sim_modelproperty_not_model); for (int i=0;i<int(allObjects.size());i++) { if (allObjects[i]!=parentlessObjects[0]) { int p=simGetObjectProperty(allObjects[i]); simSetObjectProperty(allObjects[i],p|sim_objectproperty_selectmodelbaseinstead); } } for (int i=0;i<int(allSensors.size());i++) { if (allSensors[i]!=parentlessObjects[0]) { int p=simGetObjectProperty(allSensors[i]); simSetObjectProperty(allSensors[i],p|sim_objectproperty_dontshowasinsidemodel); // sensors are usually large and it is ok if they do not appear as inside of the model bounding box! } } } // Now select all new objects: simRemoveObjectFromSelection(sim_handle_all,-1); for (int i=0;i<int(allObjects.size());i++) simAddObjectToSelection(sim_handle_single,allObjects[i]); printToConsole("URDF import operation finished.\n\n"); }
inline bool nonzeroHeightEqualsOne() const { assert_gt(height(), 0); return !(data >> (1 + shiftAmount())); }
inline void incrementHeight() { data += (1 << shiftAmount()); }
static inline uint64_t _dataValue(level_index_type h, int idx) { return (uint64_t(h) << shiftAmount()) + idx; }
inline int index() const { return int(data & ( (1 << shiftAmount()) - 1) ); }
inline bool heightIsGreaterThan(level_index_type h) const { // Easier for compiler to move computations out of inner loops return (data >= ( (h + 1) << shiftAmount())); }