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 ) {
// 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));
}

16
collada_urdf/src/collada_urdf.cpp
View File

@ -998,7 +998,7 @@ protected:
FOREACHC(itjoint, _robot.joints_) {
string jointsid = _ComputeId(itjoint->second->name);
boost::shared_ptr<urdf::Joint> 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));

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

@ -155,14 +155,14 @@ class JointMimic
public:
JointMimic() { this->clear(); };
double offset;
std::vector<double> multipliers;
std::vector<std::string> 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);
};

45
urdf_parser/src/joint.cpp
View File

@ -37,7 +37,6 @@
#include <urdf_interface/joint.h>
#include <ros/console.h>
#include <boost/lexical_cast.hpp>
#include <sstream>
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);
}
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<double>(jm));
}
this->multiplier = boost::lexical_cast<double>(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");