From 791f756659a43d235f2ee1da485d17f9a4f62bd7 Mon Sep 17 00:00:00 2001 From: Ioan Sucan Date: Wed, 16 Nov 2011 19:03:57 +0000 Subject: [PATCH] undoing changes to mimic joints (simple version is sufficient) --- collada_parser/src/collada_parser.cpp | 4 +- collada_urdf/src/collada_urdf.cpp | 16 +++---- urdf_interface/include/urdf_interface/joint.h | 10 ++-- urdf_parser/src/joint.cpp | 47 +++---------------- 4 files changed, 22 insertions(+), 55 deletions(-) diff --git a/collada_parser/src/collada_parser.cpp b/collada_parser/src/collada_parser.cpp index 76faeab..fe10afb 100644 --- a/collada_parser/src/collada_parser.cpp +++ b/collada_parser/src/collada_parser.cpp @@ -736,8 +736,8 @@ namespace urdf{ if( !!pbasejoint ) { // set the mimic properties pjoint->mimic.reset(new JointMimic()); - pjoint->mimic->joint_names.push_back(pbasejoint->name); - pjoint->mimic->multipliers.push_back(a); + pjoint->mimic->joint_name = pbasejoint->name; + pjoint->mimic->multiplier = a; pjoint->mimic->offset = b; ROS_DEBUG_STREAM(str(boost::format("assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b)); } diff --git a/collada_urdf/src/collada_urdf.cpp b/collada_urdf/src/collada_urdf.cpp index 26fdf82..d5ef6a1 100644 --- a/collada_urdf/src/collada_urdf.cpp +++ b/collada_urdf/src/collada_urdf.cpp @@ -998,7 +998,7 @@ protected: FOREACHC(itjoint, _robot.joints_) { string jointsid = _ComputeId(itjoint->second->name); boost::shared_ptr pjoint = itjoint->second; - if( !pjoint->mimic || pjoint->mimic->joint_names.empty() || pjoint->mimic->multipliers.empty()) { + if( !pjoint->mimic ) { continue; } @@ -1025,10 +1025,10 @@ protected: { daeElementRef pmath_times = pmath_apply1->add("times"); daeElementRef pmath_const0 = pmath_apply1->add("cn"); - pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multipliers[0])); + pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); daeElementRef pmath_symb = pmath_apply1->add("csymbol"); pmath_symb->setAttribute("encoding","COLLADA"); - pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0]))); + pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name))); } daeElementRef pmath_const1 = pmath_apply->add("cn"); pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); @@ -1038,15 +1038,15 @@ protected: { daeElementRef derivelt = pftec->add("equation"); derivelt->setAttribute("type","first_partial"); - derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0])).c_str()); + derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str()); daeElementRef pmath_const0 = derivelt->add("cn"); - pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multipliers[0])); + pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); } // save second partial derivative { daeElementRef derivelt = pftec->add("equation"); derivelt->setAttribute("type","second_partial"); - derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0])).c_str()); + derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name)).c_str()); daeElementRef pmath_const0 = derivelt->add("cn"); pmath_const0->setCharData("0"); } @@ -1063,10 +1063,10 @@ protected: { daeElementRef pmath_times = pmath_apply1->add("times"); daeElementRef pmath_const0 = pmath_apply1->add("cn"); - pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multipliers[0])); + pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier)); daeElementRef pmath_symb = pmath_apply1->add("csymbol"); pmath_symb->setAttribute("encoding","COLLADA"); - pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0]))); + pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_name))); } daeElementRef pmath_const1 = pmath_apply->add("cn"); pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); diff --git a/urdf_interface/include/urdf_interface/joint.h b/urdf_interface/include/urdf_interface/joint.h index 57ea339..445c646 100644 --- a/urdf_interface/include/urdf_interface/joint.h +++ b/urdf_interface/include/urdf_interface/joint.h @@ -155,14 +155,14 @@ class JointMimic public: JointMimic() { this->clear(); }; double offset; - std::vector multipliers; - std::vector joint_names; + double multiplier; + std::string joint_name; void clear() { - offset = 0; - multipliers.clear(); - joint_names.clear(); + offset = 0.0; + multiplier = 0.0; + joint_name.clear(); }; bool initXml(TiXmlElement* config); }; diff --git a/urdf_parser/src/joint.cpp b/urdf_parser/src/joint.cpp index eb0bc80..200649e 100644 --- a/urdf_parser/src/joint.cpp +++ b/urdf_parser/src/joint.cpp @@ -37,7 +37,6 @@ #include #include #include -#include namespace urdf{ @@ -315,11 +314,7 @@ bool JointMimic::initXml(TiXmlElement* config) this->clear(); // Get name of joint to mimic - const char* joint_name_str = config->Attribute("joints"); - - // if there is only one joint, the attribute can optionally be called 'joint' - if (joint_name_str == NULL) - joint_name_str = config->Attribute("joint"); + const char* joint_name_str = config->Attribute("joint"); if (joint_name_str == NULL) { @@ -327,57 +322,29 @@ bool JointMimic::initXml(TiXmlElement* config) //return false; } else - { // parse the joint names from a white-space separated list - std::string jnames = joint_name_str; - std::stringstream ss(jnames); - while (ss.good() && !ss.eof()) - { - std::string jn; ss >> jn >> std::ws; - this->joint_names.push_back(jn); - } - } + this->joint_name = joint_name_str; // Get mimic multiplier - const char* multiplier_str = config->Attribute("multipliers"); - - // if there is only one multiplier, the attribute can optionally be called 'multiplier' - if (multiplier_str == NULL) - multiplier_str = config->Attribute("multiplier"); + const char* multiplier_str = config->Attribute("multiplier"); if (multiplier_str == NULL) { - if (!this->joint_names.empty()) - { - ROS_DEBUG("joint mimic: no multiplier, using default value of 1"); - this->multipliers.resize(this->joint_names.size(), 1); - } + ROS_DEBUG("joint mimic: no multiplier, using default value of 1"); + this->multiplier = 1; } else { try { - std::string jmuls = multiplier_str; - std::stringstream ss(jmuls); - while (ss.good() && !ss.eof()) - { - std::string jm; ss >> jm >> std::ws; - this->multipliers.push_back(boost::lexical_cast(jm)); - } + this->multiplier = boost::lexical_cast(multiplier_str); } catch (boost::bad_lexical_cast &e) { - ROS_ERROR("multiplier value (%s) is not an array of floats",multiplier_str); + ROS_ERROR("multiplier value (%s) is not a float",multiplier_str); return false; } } - // make sure the number of joint names and the number of multipliers is the same - if (this->joint_names.size() != this->multipliers.size()) - { - ROS_ERROR("%lu joint names specified for mimic, but %lu multipliers specified instead", - this->joint_names.size(), this->multipliers.size()); - return false; - } // Get mimic offset const char* offset_str = config->Attribute("offset");