bool TreeKinematics::loadModel(const std::string xml, KDL::Tree &kdl_tree, std::string &tree_root_name, unsigned int &nr_of_jnts, KDL::JntArray &joint_min, KDL::JntArray &joint_max, KDL::JntArray &joint_vel_max) { urdf::Model robot_model; if (!robot_model.initString(xml)) { ROS_FATAL("Could not initialize robot model!"); return false; } if (!kdl_parser::treeFromUrdfModel(robot_model, kdl_tree)) { ROS_FATAL("Could not initialize tree object!"); return false; } if (!readJoints(robot_model, kdl_tree, tree_root_name, nr_of_jnts, joint_min, joint_max, joint_vel_max)) { ROS_FATAL("Could not read information about the joints!"); return false; } return true; }
/* Method to load all the values from the parameter server * @returns true is successful */ bool Kinematics::loadModel(const std::string xml) { urdf::Model robot_model; KDL::Tree tree; if (!robot_model.initString(xml)) { ROS_FATAL("Could not initialize robot model"); return -1; } if (!kdl_parser::treeFromString(xml, tree)) { ROS_ERROR("Could not initialize tree object"); return false; } if (!tree.getChain(root_name, tip_name, chain)) { ROS_ERROR("Could not initialize chain object for root_name %s and tip_name %s",root_name.c_str(), tip_name.c_str()); return false; } if (!readJoints(robot_model)) { ROS_FATAL("Could not read information about the joints"); return false; } return true; }
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"); }