diff --git a/collada_urdf/src/collada_writer.cpp b/collada_urdf/src/collada_writer.cpp index b734a24..f34b9ea 100644 --- a/collada_urdf/src/collada_writer.cpp +++ b/collada_urdf/src/collada_writer.cpp @@ -656,10 +656,10 @@ void ColladaWriter::addKinematicLink(shared_ptr urdf_link, dae string attachment_joint = string("k1/") + joint_sids_[urdf_joint->name]; attachment_full->setJoint(attachment_joint.c_str()); { - // x y z w - addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation, NULL, true); // x y z addTranslate(attachment_full, urdf_joint->parent_to_joint_origin_transform.position, NULL, true); + // x y z w + addRotate(attachment_full, urdf_joint->parent_to_joint_origin_transform.rotation, NULL, true); addKinematicLink(robot_.getLink(urdf_joint->child_link_name), attachment_full, link_num); } @@ -778,10 +778,10 @@ void ColladaWriter::addVisualLink(shared_ptr urdf_link, daeEle { if (urdf_link->parent_joint != NULL) { - // x y z w - addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation, NULL, true); // x y z addTranslate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.position, NULL, true); + // x y z w + addRotate(node, urdf_link->parent_joint->parent_to_joint_origin_transform.rotation, NULL, true); if (urdf_link->visual != NULL) { // @@ -794,10 +794,10 @@ void ColladaWriter::addVisualLink(shared_ptr urdf_link, daeEle parent_node = visual_node; - // x y z w - addRotate(parent_node, urdf_link->visual->origin.rotation, NULL, true); // x y z addTranslate(parent_node, urdf_link->visual->origin.position, NULL, true); + // x y z w + addRotate(parent_node, urdf_link->visual->origin.rotation, NULL, true); } // x y z angle