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: cleaning up joint tags

This commit is contained in:
tfield 2010-03-08 23:54:19 +00:00
parent 58c5b78357
commit 5e656bbd7f
1 changed files with 100 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

110
collada_urdf/src/urdf_to_collada.cpp
View File

@ -103,6 +103,9 @@ public:
domLibrary_effectsRef effectsLib_; domLibrary_effectsRef effectsLib_;
//domLibrary_articulated_systemsRef articulatedSystemsLib_; //domLibrary_articulated_systemsRef articulatedSystemsLib_;
map<string, string> geometry_ids_; // link.name -> geometry.id
map<string, string> joint_ids_; // joint.name -> joint.sid
public: public:
ColladaWriter(urdf::Model* robot) : robot_(robot) { ColladaWriter(urdf::Model* robot) : robot_(robot) {
daeErrorHandler::setErrorHandler(this); daeErrorHandler::setErrorHandler(this);
@ -291,6 +294,7 @@ public:
{ {
loadMesh(filename, geometry, geometry_id); loadMesh(filename, geometry, geometry_id);
} }
geometry_ids_[urdf_link->name] = geometry_id;
// </geometry> // </geometry>
link_num++; link_num++;
@ -528,79 +532,109 @@ public:
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;
// Create COLLADA joint // <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()); joint->setId(urdf_joint->name.c_str());
joint->setName(urdf_joint->name.c_str()); joint->setName(urdf_joint->name.c_str());
switch (urdf_joint->type) switch (urdf_joint->type)
{ {
case urdf::Joint::REVOLUTE: { case urdf::Joint::REVOLUTE: {
// joint.axis // <revolute sid="axis0">
vector<domAxis_constraintRef> axes(1); domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE));
axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); revolute->setSid("axis0");
axes[0]->setSid("axis0"); {
domAxisRef axis = daeSafeCast<domAxis>(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS)); // <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->createAndPlace(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = urdf_joint->axis.x; axis->getValue()[0] = urdf_joint->axis.x;
axis->getValue()[1] = urdf_joint->axis.y; axis->getValue()[1] = urdf_joint->axis.y;
axis->getValue()[2] = urdf_joint->axis.z; axis->getValue()[2] = urdf_joint->axis.z;
}
// </axis>
// joint.limits // <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(axes[0]->createAndPlace(COLLADA_TYPE_LIMITS)); domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->createAndPlace(COLLADA_TYPE_LIMITS));
{
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower; daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower;
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper; daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper;
}
// </limits>
}
// </revolute>
break; break;
} }
case urdf::Joint::CONTINUOUS: { case urdf::Joint::CONTINUOUS: {
// Model as a REVOLUTE joint without limits // Model as a REVOLUTE joint without limits
// joint.axis // <revolute sid="axis0">
vector<domAxis_constraintRef> axes(1); domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE));
axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); revolute->setSid("axis0");
axes[0]->setSid("axis0"); {
domAxisRef axis = daeSafeCast<domAxis>(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS)); // <axis>
domAxisRef axis = daeSafeCast<domAxis>(revolute->createAndPlace(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = urdf_joint->axis.x; axis->getValue()[0] = urdf_joint->axis.x;
axis->getValue()[1] = urdf_joint->axis.y; axis->getValue()[1] = urdf_joint->axis.y;
axis->getValue()[2] = urdf_joint->axis.z; axis->getValue()[2] = urdf_joint->axis.z;
}
// </axis>
}
// </revolute>
break; break;
} }
case urdf::Joint::PRISMATIC: { case urdf::Joint::PRISMATIC: {
// joint.axis // <prismatic sid="axis0">
vector<domAxis_constraintRef> axes(1); domAxis_constraintRef prismatic = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC));
axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_PRISMATIC)); prismatic->setSid("axis0");
axes[0]->setSid("axis0"); {
domAxisRef axis = daeSafeCast<domAxis>(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS)); // <axis>
domAxisRef axis = daeSafeCast<domAxis>(prismatic->createAndPlace(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = urdf_joint->axis.x; axis->getValue()[0] = urdf_joint->axis.x;
axis->getValue()[1] = urdf_joint->axis.y; axis->getValue()[1] = urdf_joint->axis.y;
axis->getValue()[2] = urdf_joint->axis.z; axis->getValue()[2] = urdf_joint->axis.z;
}
// </axis>
// joint.limits // <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(axes[0]->createAndPlace(COLLADA_TYPE_LIMITS)); domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(prismatic->createAndPlace(COLLADA_TYPE_LIMITS));
{
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower; daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = urdf_joint->limits->lower;
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper; daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = urdf_joint->limits->upper;
}
// </limits>
}
// </prismatic>
break; break;
} }
case urdf::Joint::FIXED: { case urdf::Joint::FIXED: {
// Model as a REVOLUTE joint with no leeway // Model as a REVOLUTE joint with no leeway
// joint.axis domAxis_constraintRef revolute = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE));
vector<domAxis_constraintRef> axes(1); revolute->setSid("axis0");
axes[0] = daeSafeCast<domAxis_constraint>(joint->createAndPlace(COLLADA_ELEMENT_REVOLUTE)); {
axes[0]->setSid("axis0"); // <axis>
domAxisRef axis = daeSafeCast<domAxis>(axes[0]->createAndPlace(COLLADA_ELEMENT_AXIS)); domAxisRef axis = daeSafeCast<domAxis>(revolute->createAndPlace(COLLADA_ELEMENT_AXIS));
{
axis->getValue().setCount(3); axis->getValue().setCount(3);
axis->getValue()[0] = urdf_joint->axis.x; axis->getValue()[0] = urdf_joint->axis.x;
axis->getValue()[1] = urdf_joint->axis.y; axis->getValue()[1] = urdf_joint->axis.y;
axis->getValue()[2] = urdf_joint->axis.z; axis->getValue()[2] = urdf_joint->axis.z;
}
// </axis>
// joint.limits // <limits>
domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(axes[0]->createAndPlace(COLLADA_TYPE_LIMITS)); domJoint_limitsRef limits = daeSafeCast<domJoint_limits>(revolute->createAndPlace(COLLADA_TYPE_LIMITS));
{
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = 0.0; daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MIN))->getValue() = 0.0;
daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = 0.0; daeSafeCast<domMinmax>(limits->createAndPlace(COLLADA_ELEMENT_MAX))->getValue() = 0.0;
}
// </limits>
}
// </revolute>
break; break;
} }
case urdf::Joint::UNKNOWN: { case urdf::Joint::UNKNOWN: {
@ -699,14 +733,22 @@ 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"); joint_rotate->setSid("node_joint0_axis0"); // @todo use joint_num
} }
for (vector<boost::shared_ptr<urdf::Link> >::const_iterator i = urdf_link->child_links.begin(); i != urdf_link->child_links.end(); i++) { // <instance_geometry url="#g1.link0.geom">
boost::shared_ptr<urdf::Link> urdf_child_link = *i; domInstance_geometryRef instance_geometry = daeSafeCast<domInstance_geometry>(node->createAndPlace(COLLADA_ELEMENT_INSTANCE_GEOMETRY));
instance_geometry->setUrl("#g1.link0.geom"); // @todo use link_num
addVisualLink(urdf_child_link, node, link_num); {
// @todo <bind_material>
// @todo <technique_common>
// @todo <instance_material>
} }
// <node ...>
for (vector<boost::shared_ptr<urdf::Link> >::const_iterator i = urdf_link->child_links.begin(); i != urdf_link->child_links.end(); i++)
addVisualLink(*i, node, link_num);
// </node>
} }
// </node> // </node>
} }