From 2072a78dafb88427080adb6da31f7fd0416d1d25 Mon Sep 17 00:00:00 2001 From: tfield Date: Tue, 23 Mar 2010 18:34:54 +0000 Subject: [PATCH] collada_urdf: fixing bindings --- collada_urdf/src/urdf_to_collada.cpp | 111 +++++++++++++++++---------- 1 file changed, 72 insertions(+), 39 deletions(-) diff --git a/collada_urdf/src/urdf_to_collada.cpp b/collada_urdf/src/urdf_to_collada.cpp index b824f5d..1232a3c 100644 --- a/collada_urdf/src/urdf_to_collada.cpp +++ b/collada_urdf/src/urdf_to_collada.cpp @@ -93,8 +93,11 @@ public: domLibrary_effectsRef effectsLib_; //domLibrary_articulated_systemsRef articulatedSystemsLib_; + domKinematics_modelRef kmodel_; + map geometry_ids_; // link.name -> geometry.id map joint_sids_; // joint.name -> joint.sid + map node_ids_; // joint.name -> node.id public: ColladaWriter(urdf::Model* robot) : robot_(robot) { @@ -114,7 +117,7 @@ public: } dom_ = daeSafeCast(doc->getDomRoot()); - dom_->setAttribute("xmlns:math","http://www.w3.org/1998/Math/MathML"); + dom_->setAttribute("xmlns:math", "http://www.w3.org/1998/Math/MathML"); // Create the required asset tag domAssetRef asset = daeSafeCast(dom_->createAndPlace(COLLADA_ELEMENT_ASSET)); @@ -216,6 +219,7 @@ public: addKinematics(scene); addVisuals(scene); addMaterials(); + addBindings(scene); collada_->writeAll(); @@ -508,6 +512,16 @@ public: joint->setName(urdf_joint->name.c_str()); joint->setSid(joint_sid.c_str()); joint_sids_[urdf_joint->name] = joint_sid; + + double axis_x = urdf_joint->axis.x; + double axis_y = urdf_joint->axis.y; + double axis_z = urdf_joint->axis.z; + if (axis_x == 0.0 && axis_y == 0.0 && axis_z == 0.0) { + axis_x = 1.0; + axis_y = 0.0; + axis_z = 0.0; + } + switch (urdf_joint->type) { case urdf::Joint::REVOLUTE: { @@ -519,9 +533,9 @@ public: domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); { axis->getValue().setCount(3); - axis->getValue()[0] = urdf_joint->axis.x; - axis->getValue()[1] = urdf_joint->axis.y; - axis->getValue()[2] = urdf_joint->axis.z; + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; } // @@ -547,9 +561,9 @@ public: domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); { axis->getValue().setCount(3); - axis->getValue()[0] = urdf_joint->axis.x; - axis->getValue()[1] = urdf_joint->axis.y; - axis->getValue()[2] = urdf_joint->axis.z; + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; } // } @@ -565,9 +579,9 @@ public: domAxisRef axis = daeSafeCast(prismatic->createAndPlace(COLLADA_ELEMENT_AXIS)); { axis->getValue().setCount(3); - axis->getValue()[0] = urdf_joint->axis.x; - axis->getValue()[1] = urdf_joint->axis.y; - axis->getValue()[2] = urdf_joint->axis.z; + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; } // @@ -592,9 +606,9 @@ public: domAxisRef axis = daeSafeCast(revolute->createAndPlace(COLLADA_ELEMENT_AXIS)); { axis->getValue().setCount(3); - axis->getValue()[0] = urdf_joint->axis.x; - axis->getValue()[1] = urdf_joint->axis.y; - axis->getValue()[2] = urdf_joint->axis.z; + axis->getValue()[0] = axis_x; + axis->getValue()[1] = axis_y; + axis->getValue()[2] = axis_z; } // @@ -629,8 +643,47 @@ public: } } - void addKinematics(SCENE scene) - { + void addBindings(SCENE scene) { + string model_id = kmodel_->getID(); + string inst_model_sid = string("inst_") + model_id; + + // + // + domBind_kinematics_modelRef kmodel_bind = daeSafeCast(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); + kmodel_bind->setNode("v1.node0"); // @todo + daeSafeCast(kmodel_bind->createAndPlace(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str()); + + for (map >::const_iterator i = robot_->joints_.begin(); i != robot_->joints_.end(); i++) { + boost::shared_ptr urdf_joint = i->second; + + int idof = 0; // @todo assuming 1 dof joints + string joint_sid = joint_sids_[urdf_joint->name]; + string axis_name = string("axis") + boost::lexical_cast(idof); + string joint_axis_sid = string("kscene.inst_") + model_id + string(".") + joint_sid + string(".") + axis_name; + string joint_axis_value_sid = joint_axis_sid + string("_value"); + + // + domBind_joint_axisRef joint_bind = daeSafeCast(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_JOINT_AXIS)); + string node_name = node_ids_[urdf_joint->name]; + joint_bind->setTarget((node_name + string("/node_") + joint_sid + string("_") + axis_name).c_str()); + { + // + domCommon_sidref_or_paramRef axis_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_AXIS)); + { + daeSafeCast(axis_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_sid.c_str()); + } + // + // + domCommon_float_or_paramRef value_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_VALUE)); + { + daeSafeCast(value_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_axis_value_sid.c_str()); + } + } + // + } + } + + void addKinematics(SCENE scene) { // domKinematics_modelRef kmodel = daeSafeCast(kinematicsModelsLib_->createAndPlace(COLLADA_ELEMENT_KINEMATICS_MODEL)); kmodel->setId("k1"); @@ -646,17 +699,12 @@ public: addKinematicLink(robot_->getRoot(), technique, link_num); // } + kmodel_ = kmodel; // string model_id = kmodel->getID(); string inst_model_sid = string("inst_") + model_id; - // - // - domBind_kinematics_modelRef kmodel_bind = daeSafeCast(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_KINEMATICS_MODEL)); - kmodel_bind->setNode(robot_->getName().c_str()); - daeSafeCast(kmodel_bind->createAndPlace(COLLADA_ELEMENT_PARAM))->setValue(string(string(scene.kscene->getID()) + string(".") + inst_model_sid).c_str()); - // domInstance_kinematics_modelRef ikm = daeSafeCast(scene.kscene->createAndPlace(COLLADA_ELEMENT_INSTANCE_KINEMATICS_MODEL)); ikm->setUrl((string("#") + model_id).c_str()); @@ -702,24 +750,6 @@ public: daeSafeCast(newparam_value->createAndPlace(COLLADA_ELEMENT_FLOAT))->setValue(0.0f); } // - - // - domBind_joint_axisRef joint_bind = daeSafeCast(scene.kiscene->createAndPlace(COLLADA_ELEMENT_BIND_JOINT_AXIS)); - joint_bind->setTarget((string("node_") + joint_sid + string("_") + axis_name).c_str()); - { - // - domCommon_sidref_or_paramRef axis_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_AXIS)); - { - daeSafeCast(axis_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(newparam_sid.c_str()); - } - // - // - domCommon_float_or_paramRef value_bind = daeSafeCast(joint_bind->createAndPlace(COLLADA_ELEMENT_VALUE)); - { - daeSafeCast(value_bind->createAndPlace(COLLADA_TYPE_PARAM))->setValue(newparam_value_sid.c_str()); - } - } - // } } // @@ -865,6 +895,9 @@ public: 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()); + + cout << "setting " << urdf_link->parent_joint->name << " to " << node_id << endl; + node_ids_[urdf_link->parent_joint->name] = node_id; } //