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

undoing changes to mimic joints (simple version is sufficient)

This commit is contained in:
Ioan Sucan 2011-11-16 19:03:57 +00:00
parent 9565df3dd2
commit 791f756659
4 changed files with 22 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
collada_parser/src/collada_parser.cpp
View File

@ -736,8 +736,8 @@ namespace urdf{
if( !!pbasejoint ) { if( !!pbasejoint ) {
// set the mimic properties // set the mimic properties
pjoint->mimic.reset(new JointMimic()); pjoint->mimic.reset(new JointMimic());
pjoint->mimic->joint_names.push_back(pbasejoint->name); pjoint->mimic->joint_name = pbasejoint->name;
pjoint->mimic->multipliers.push_back(a); pjoint->mimic->multiplier = a;
pjoint->mimic->offset = b; 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)); ROS_DEBUG_STREAM(str(boost::format("assigning joint %s to mimic %s %f %f\n")%pjoint->name%pbasejoint->name%a%b));
} }

16
collada_urdf/src/collada_urdf.cpp
View File

@ -998,7 +998,7 @@ protected:
FOREACHC(itjoint, _robot.joints_) { FOREACHC(itjoint, _robot.joints_) {
string jointsid = _ComputeId(itjoint->second->name); string jointsid = _ComputeId(itjoint->second->name);
boost::shared_ptr<urdf::Joint> pjoint = itjoint->second; boost::shared_ptr<urdf::Joint> pjoint = itjoint->second;
if( !pjoint->mimic || pjoint->mimic->joint_names.empty() || pjoint->mimic->multipliers.empty()) { if( !pjoint->mimic ) {
continue; continue;
} }
@ -1025,10 +1025,10 @@ protected:
{ {
daeElementRef pmath_times = pmath_apply1->add("times"); daeElementRef pmath_times = pmath_apply1->add("times");
daeElementRef pmath_const0 = pmath_apply1->add("cn"); 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"); daeElementRef pmath_symb = pmath_apply1->add("csymbol");
pmath_symb->setAttribute("encoding","COLLADA"); 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"); daeElementRef pmath_const1 = pmath_apply->add("cn");
pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));
@ -1038,15 +1038,15 @@ protected:
{ {
daeElementRef derivelt = pftec->add("equation"); daeElementRef derivelt = pftec->add("equation");
derivelt->setAttribute("type","first_partial"); 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"); 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 // save second partial derivative
{ {
daeElementRef derivelt = pftec->add("equation"); daeElementRef derivelt = pftec->add("equation");
derivelt->setAttribute("type","second_partial"); 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"); daeElementRef pmath_const0 = derivelt->add("cn");
pmath_const0->setCharData("0"); pmath_const0->setCharData("0");
} }
@ -1063,10 +1063,10 @@ protected:
{ {
daeElementRef pmath_times = pmath_apply1->add("times"); daeElementRef pmath_times = pmath_apply1->add("times");
daeElementRef pmath_const0 = pmath_apply1->add("cn"); 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"); daeElementRef pmath_symb = pmath_apply1->add("csymbol");
pmath_symb->setAttribute("encoding","COLLADA"); 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"); daeElementRef pmath_const1 = pmath_apply->add("cn");
pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset)); pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));

10
urdf_interface/include/urdf_interface/joint.h
View File

@ -155,14 +155,14 @@ class JointMimic
public: public:
JointMimic() { this->clear(); }; JointMimic() { this->clear(); };
double offset; double offset;
std::vector<double> multipliers; double multiplier;
std::vector<std::string> joint_names; std::string joint_name;
void clear() void clear()
{ {
offset = 0; offset = 0.0;
multipliers.clear(); multiplier = 0.0;
joint_names.clear(); joint_name.clear();
}; };
bool initXml(TiXmlElement* config); bool initXml(TiXmlElement* config);
}; };

47
urdf_parser/src/joint.cpp
View File

@ -37,7 +37,6 @@
#include <urdf_interface/joint.h> #include <urdf_interface/joint.h>
#include <ros/console.h> #include <ros/console.h>
#include <boost/lexical_cast.hpp> #include <boost/lexical_cast.hpp>
#include <sstream>
namespace urdf{ namespace urdf{
@ -315,11 +314,7 @@ bool JointMimic::initXml(TiXmlElement* config)
this->clear(); this->clear();
// Get name of joint to mimic // Get name of joint to mimic
const char* joint_name_str = config->Attribute("joints"); const char* joint_name_str = config->Attribute("joint");
// if there is only one joint, the attribute can optionally be called 'joint'
if (joint_name_str == NULL)
joint_name_str = config->Attribute("joint");
if (joint_name_str == NULL) if (joint_name_str == NULL)
{ {
@ -327,57 +322,29 @@ bool JointMimic::initXml(TiXmlElement* config)
//return false; //return false;
} }
else else
{ // parse the joint names from a white-space separated list this->joint_name = joint_name_str;
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);
}
}
// Get mimic multiplier // Get mimic multiplier
const char* multiplier_str = config->Attribute("multipliers"); const char* multiplier_str = config->Attribute("multiplier");
// if there is only one multiplier, the attribute can optionally be called 'multiplier'
if (multiplier_str == NULL)
multiplier_str = config->Attribute("multiplier");
if (multiplier_str == NULL) if (multiplier_str == NULL)
{ {
if (!this->joint_names.empty()) ROS_DEBUG("joint mimic: no multiplier, using default value of 1");
{ this->multiplier = 1;
ROS_DEBUG("joint mimic: no multiplier, using default value of 1");
this->multipliers.resize(this->joint_names.size(), 1);
}
} }
else else
{ {
try try
{ {
std::string jmuls = multiplier_str; this->multiplier = boost::lexical_cast<double>(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<double>(jm));
}
} }
catch (boost::bad_lexical_cast &e) 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; 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 // Get mimic offset
const char* offset_str = config->Attribute("offset"); const char* offset_str = config->Attribute("offset");