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

fixed visual model transforms in urdf_to_collada exporter

This commit is contained in:
rdiankov 2010-07-23 04:35:48 +00:00
parent 2f7c19c8a2
commit 2cac8c6027
1 changed files with 39 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
collada_urdf/src/collada_writer.cpp
View File

@ -198,13 +198,19 @@ void ColladaWriter::addGeometries() {
for (map<string, shared_ptr<urdf::Link> >::const_iterator i = robot_.links_.begin(); i != robot_.links_.end(); i++) { for (map<string, shared_ptr<urdf::Link> >::const_iterator i = robot_.links_.begin(); i != robot_.links_.end(); i++) {
shared_ptr<urdf::Link> urdf_link = i->second; shared_ptr<urdf::Link> urdf_link = i->second;
boost::shared_ptr< urdf::Geometry > geometry;
if (urdf_link->visual == NULL || urdf_link->visual->geometry == NULL) if( !!urdf_link->visual ) {
geometry = urdf_link->visual->geometry;
}
else if (!!urdf_link->collision ) {
geometry = urdf_link->collision->geometry;
}
if( !geometry ) {
continue; continue;
}
switch (urdf_link->visual->geometry->type) { switch (geometry->type) {
case urdf::Geometry::MESH: { case urdf::Geometry::MESH: {
urdf::Mesh* urdf_mesh = (urdf::Mesh*) urdf_link->visual->geometry.get(); urdf::Mesh* urdf_mesh = (urdf::Mesh*) geometry.get();
string filename = urdf_mesh->filename; string filename = urdf_mesh->filename;
urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale urdf::Vector3 scale = urdf_mesh->scale; // @todo use scale
@ -235,7 +241,7 @@ void ColladaWriter::addGeometries() {
break; break;
} }
default: { default: {
std::cerr << "Warning: geometry type " << urdf_link->visual->geometry->type << " of link " << urdf_link->name << " not exported" << std::endl; std::cerr << "Warning: geometry type " << geometry->type << " of link " << urdf_link->name << " not exported" << std::endl;
break; break;
} }
} }
@ -777,31 +783,23 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
domNodeRef parent_node = node; domNodeRef parent_node = node;
{ {
if (urdf_link->parent_joint != NULL) { if (!!urdf_link->parent_joint) {
// <translate>x y z</translate> // <translate>x y z</translate>
addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position, NULL, true); addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position, NULL, true);
// <rotate>x y z w</rotate> // <rotate>x y z w</rotate>
addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation, NULL, true); addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation, NULL, true);
if (urdf_link->visual != NULL) { domRotateRef joint_rotate;
// <node id="v1.node0.visual" name="visual" sid="visual">
domNodeRef visual_node = daeSafeCast<domNode>(node->add(COLLADA_ELEMENT_NODE));
string visual_sid("visual");
string visual_id = node_id + "." + visual_sid;
visual_node->setName("visual");
visual_node->setSid(visual_sid.c_str());
visual_node->setId(visual_id.c_str());
parent_node = visual_node;
// <translate>x y z</translate>
addTranslate(parent_node, urdf_link->visual->origin.position, NULL, true);
// <rotate>x y z w</rotate>
addRotate(parent_node, urdf_link->visual->origin.rotation, NULL, true);
}
// <rotate sid="node_joint0_axis0">x y z angle</rotate> // <rotate sid="node_joint0_axis0">x y z angle</rotate>
domRotateRef joint_rotate = addRotate(parent_node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation); //joint_rotate = addRotate(parent_node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation);
joint_rotate = daeSafeCast<domRotate>(parent_node->add(COLLADA_ELEMENT_ROTATE));
joint_rotate->getValue().setCount(4);
joint_rotate->getValue()[0] = urdf_link->parent_joint->axis.x;
joint_rotate->getValue()[1] = urdf_link->parent_joint->axis.y;
joint_rotate->getValue()[2] = urdf_link->parent_joint->axis.z;
joint_rotate->getValue()[3] = 0;
string joint_sid = joint_sids_[urdf_link->parent_joint->name]; string joint_sid = joint_sids_[urdf_link->parent_joint->name];
string joint_rotate_sid = string("node_") + joint_sid + string("_axis0"); string joint_rotate_sid = string("node_") + joint_sid + string("_axis0");
joint_rotate->setSid(joint_rotate_sid.c_str()); joint_rotate->setSid(joint_rotate_sid.c_str());
@ -809,6 +807,22 @@ void ColladaWriter::addVisualLink(shared_ptr<urdf::Link const> urdf_link, daeEle
node_ids_[urdf_link->parent_joint->name] = node_id; node_ids_[urdf_link->parent_joint->name] = node_id;
} }
if (!!urdf_link->visual) {
// <node id="v1.node0.visual" name="visual" sid="visual">
domNodeRef visual_node = daeSafeCast<domNode>(node->add(COLLADA_ELEMENT_NODE));
string visual_sid("visual");
string visual_id = node_id + "." + visual_sid;
visual_node->setName("visual");
visual_node->setSid(visual_sid.c_str());
visual_node->setId(visual_id.c_str());
parent_node = visual_node;
// <translate>x y z</translate>
addTranslate(parent_node, urdf_link->visual->origin.position, NULL, true);
// <rotate>x y z w</rotate>
addRotate(parent_node, urdf_link->visual->origin.rotation, NULL, true);
}
// <instance_geometry url="#g1.link0.geom"> // <instance_geometry url="#g1.link0.geom">
map<string, string>::const_iterator i = geometry_ids_.find(urdf_link->name); map<string, string>::const_iterator i = geometry_ids_.find(urdf_link->name);
if (i != geometry_ids_.end()) { if (i != geometry_ids_.end()) {