bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

collada_urdf: finished kinematics models

This commit is contained in:
tfield 2010-03-09 04:17:14 +00:00
parent 5e656bbd7f
commit 9e684833d6
1 changed files with 97 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

143
collada_urdf/src/urdf_to_collada.cpp
View File

@ -104,7 +104,7 @@ public:
//domLibrary_articulated_systemsRef articulatedSystemsLib_; //domLibrary_articulated_systemsRef articulatedSystemsLib_;
map<string, string> geometry_ids_; // link.name -> geometry.id map<string, string> geometry_ids_; // link.name -> geometry.id
map<string, string> joint_ids_; // joint.name -> joint.sid map<string, string> joint_sids_; // joint.name -> joint.sid
public: public:
ColladaWriter(urdf::Model* robot) : robot_(robot) { ColladaWriter(urdf::Model* robot) : robot_(robot) {
@ -157,7 +157,7 @@ public:
kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES)); kinematicsScenesLib_ = daeSafeCast<domLibrary_kinematics_scenes>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_SCENES));
kinematicsScenesLib_->setId("kscenes"); kinematicsScenesLib_->setId("kscenes");
kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS)); kinematicsModelsLib_ = daeSafeCast<domLibrary_kinematics_models>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_KINEMATICS_MODELS));
kinematicsModelsLib_->setId("kinematics_models"); kinematicsModelsLib_->setId("kmodels");
jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS)); jointsLib_ = daeSafeCast<domLibrary_joints>(dom_->createAndPlace(COLLADA_ELEMENT_LIBRARY_JOINTS));
jointsLib_->setId("joints"); jointsLib_->setId("joints");
@ -222,11 +222,10 @@ public:
SCENE scene = createScene(); SCENE scene = createScene();
setupPhysics(scene); setupPhysics(scene);
addScene(scene);
addGeometries(); addGeometries();
addJoints(); addJoints();
addKinematics(); addKinematics(scene);
addVisuals(scene); addVisuals(scene);
collada_->writeAll(); collada_->writeAll();
@ -245,32 +244,6 @@ public:
// </technique_common> // </technique_common>
} }
void addScene(SCENE scene) {
boost::shared_ptr<const urdf::Link> urdf_root = robot_->getRoot();
// <kinematics_model id="k1" name="pr2">
domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL));
kmodel->setId("k1");
kmodel->setName(urdf_root->name.c_str());
// <instance_kinematics_model url="#k1" sid="inst_k1">
domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
string model_id = kmodel->getID();
string inst_model_sid = string("inst_") + model_id;
ikm->setUrl((string("#") + model_id).c_str());
ikm->setSid(inst_model_sid.c_str());
{
// <newparam>
domKinematics_newparamRef newparam = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM));
newparam->setSid("kscene.inst_k1");
{
// <SIDREF>kscene/inst_k1</SIDREF>
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue("kscene/inst_k1");
}
// </newparam>
}
}
void addGeometries() { void addGeometries() {
int link_num = 0; int link_num = 0;
@ -529,13 +502,17 @@ public:
} }
void addJoints() { void addJoints() {
int joint_num = 0;
for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) {
boost::shared_ptr<urdf::Joint> urdf_joint = i->second; boost::shared_ptr<urdf::Joint> urdf_joint = i->second;
// <joint name="base_laser_joint" sid="joint0"> // <joint name="base_laser_joint" sid="joint0">
domJointRef joint = daeSafeCast<domJoint>(jointsLib_->createAndPlace(COLLADA_ELEMENT_JOINT)); domJointRef joint = daeSafeCast<domJoint>(jointsLib_->createAndPlace(COLLADA_ELEMENT_JOINT));
joint->setId(urdf_joint->name.c_str()); string joint_sid = string("joint") + boost::lexical_cast<string>(joint_num);
joint_num++;
joint->setName(urdf_joint->name.c_str()); joint->setName(urdf_joint->name.c_str());
joint->setSid(joint_sid.c_str());
joint_sids_[urdf_joint->name] = joint_sid;
switch (urdf_joint->type) switch (urdf_joint->type)
{ {
case urdf::Joint::REVOLUTE: { case urdf::Joint::REVOLUTE: {
@ -657,26 +634,80 @@ public:
} }
} }
void addKinematics() void addKinematics(SCENE scene)
{ {
// <kinematics_model id="k1" name="pr2"> // <kinematics_model id="k1" name="pr2">
domKinematics_modelRef model = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL)); domKinematics_modelRef kmodel = daeSafeCast<domKinematics_model>(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL));
model->setId("k1"); kmodel->setId("k1");
kmodel->setName(robot_->getName().c_str());
{ {
// <technique_common> // <technique_common>
domKinematics_model_techniqueRef technique = daeSafeCast<domKinematics_model_technique>(model->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON)); domKinematics_model_techniqueRef technique = daeSafeCast<domKinematics_model_technique>(kmodel->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
for (map<string, boost::shared_ptr<urdf::Link> >::const_iterator i = robot_->links_.begin(); i != robot_->links_.end(); i++) { for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) {
boost::shared_ptr<urdf::Link> urdf_link = i->second; boost::shared_ptr<urdf::Joint> urdf_joint = i->second;
// <instance_joint> // <instance_joint url="#joint1">
domInstance_jointRef instance_joint = daeSafeCast<domInstance_joint>(technique->createAndPlace(COLLADA_ELEMENT_INSTANCE_JOINT)); domInstance_jointRef instance_joint = daeSafeCast<domInstance_joint>(technique->createAndPlace(COLLADA_ELEMENT_INSTANCE_JOINT));
instance_joint->setUrl("#joint_1"); string joint_id = joint_sids_[urdf_joint->name];
string instance_joint_url = string("#") + joint_id;
instance_joint->setUrl(instance_joint_url.c_str());
// </instance_joint>
} }
// </technique_common> // </technique_common>
// <link ...>
int link_num = 0; int link_num = 0;
addKinematicLink(robot_->getRoot(), technique, link_num); addKinematicLink(robot_->getRoot(), technique, link_num);
// </link>
} }
// </kinematics_model>
// <instance_kinematics_model url="#k1" sid="inst_k1">
domInstance_kinematics_modelRef ikm = daeSafeCast<domInstance_kinematics_model>(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL));
string model_id = kmodel->getID();
string inst_model_sid = string("inst_") + model_id;
ikm->setUrl((string("#") + model_id).c_str());
ikm->setSid(inst_model_sid.c_str());
{
// <newparam sid="kscene.inst_k1">
domKinematics_newparamRef newparam_model = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM));
string newparam_model_sid = string("kscene.inst_") + model_id;
newparam_model->setSid(newparam_model_sid.c_str());
{
// <SIDREF>kscene/inst_k1</SIDREF>
string model_sidref = string("kscene/inst_") + model_id;
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam_model->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(model_sidref.c_str());
}
// </newparam>
for (map<string, boost::shared_ptr<urdf::Joint> >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) {
boost::shared_ptr<urdf::Joint> urdf_joint = i->second;
string joint_sid = joint_sids_[urdf_joint->name];
// <newparam sid="kscene.inst_k1.joint0.axis0">
domKinematics_newparamRef newparam = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM));
string newparam_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".axis0");
newparam->setSid(newparam_sid.c_str());
{
// <SIDREF>kscene/inst_k1/joint0/axis0</SIDREF>
string sidref = string("kscene/inst_") + model_id + string("/") + joint_sid + string("/axis0");
daeSafeCast<domKinematics_newparam::domSIDREF>(newparam->createAndPlace(COLLADA_ELEMENT_SIDREF))->setValue(sidref.c_str());
}
// </newparam>
// <newparam sid="kscene.inst_k1.joint0.axis0_value">
domKinematics_newparamRef newparam_value = daeSafeCast<domKinematics_newparam>(ikm->createAndPlace(COLLADA_ELEMENT_NEWPARAM));
string newparam_value_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".axis0_value");
newparam_value->setSid(newparam_value_sid.c_str());
{
// <float>0</float>
daeSafeCast<domKinematics_newparam::domFloat>(newparam_value->createAndPlace(COLLADA_ELEMENT_FLOAT))->setValue(0.0f);
}
// </newparam>
}
}
// </instance_kinematics_model>
} }
void addKinematicLink(boost::shared_ptr<const urdf::Link> urdf_link, daeElementRef parent, int& link_num) { void addKinematicLink(boost::shared_ptr<const urdf::Link> urdf_link, daeElementRef parent, int& link_num) {
@ -733,17 +764,37 @@ public:
// <rotate sid="node_joint0_axis0">x y z angle</rotate> // <rotate sid="node_joint0_axis0">x y z angle</rotate>
domRotateRef joint_rotate = addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); domRotateRef joint_rotate = addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation);
joint_rotate->setSid("node_joint0_axis0"); // @todo use joint_num string joint_sid = joint_sids_[urdf_link->parent_joint->name];
string joint_rotate_sid = string("node_") + joint_sid + string("_axis0");
joint_rotate->setSid(joint_rotate_sid.c_str());
} }
// <instance_geometry url="#g1.link0.geom"> // <instance_geometry url="#g1.link0.geom">
domInstance_geometryRef instance_geometry = daeSafeCast<domInstance_geometry>(node->createAndPlace(COLLADA_ELEMENT_INSTANCE_GEOMETRY)); map<string, string>::const_iterator i = geometry_ids_.find(urdf_link->name);
instance_geometry->setUrl("#g1.link0.geom"); // @todo use link_num if (i != geometry_ids_.end()) {
{ domInstance_geometryRef instance_geometry = daeSafeCast<domInstance_geometry>(node->createAndPlace(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
// @todo <bind_material> string geometry_id = geometry_ids_[urdf_link->name];
// @todo <technique_common> string instance_geometry_url = string("#") + geometry_id;
// @todo <instance_material> instance_geometry->setUrl(instance_geometry_url.c_str());
{
// <bind_material>
domBind_materialRef bind_material = daeSafeCast<domBind_material>(instance_geometry->createAndPlace(COLLADA_ELEMENT_BIND_MATERIAL));
{
// <technique_common>
domBind_material::domTechnique_commonRef technique_common = daeSafeCast<domBind_material::domTechnique_common>(bind_material->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
{
// <instance_material>
domInstance_materialRef instance_material = daeSafeCast<domInstance_material>(technique_common->createAndPlace(COLLADA_ELEMENT_INSTANCE_MATERIAL));
instance_material->setTarget((instance_geometry_url + string(".mat")).c_str());
instance_material->setSymbol("mat0");
// </instance_material>
}
// </technique_common>
}
// </bind_material>
}
} }
// </instance_geometry>
// <node ...> // <node ...>
for (vector<boost::shared_ptr<urdf::Link> >::const_iterator i = urdf_link->child_links.begin(); i != urdf_link->child_links.end(); i++) for (vector<boost::shared_ptr<urdf::Link> >::const_iterator i = urdf_link->child_links.begin(); i != urdf_link->child_links.end(); i++)