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

forgot to commit this as part of the previous change

This commit is contained in:
Ioan Sucan 2011-11-10 00:31:28 +00:00
parent f1fa2bf898
commit 08a6ecc59f
2 changed files with 10 additions and 10 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_name = pbasejoint->name;
pjoint->mimic->multiplier = a;
pjoint->mimic->joint_names.push_back(pbasejoint->name);
pjoint->mimic->multipliers.push_back(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 ) {
if( !pjoint->mimic || pjoint->mimic->joint_names.empty() || pjoint->mimic->multipliers.empty()) {
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->multiplier));
pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multipliers[0]));
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_name)));
pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0])));
}
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_name)).c_str());
derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0])).c_str());
daeElementRef pmath_const0 = derivelt->add("cn");
pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multiplier));
pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multipliers[0]));
}
// 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_name)).c_str());
derivelt->setAttribute("target",str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0])).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->multiplier));
pmath_const0->setCharData(str(boost::format("%f")%pjoint->mimic->multipliers[0]));
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_name)));
pmath_symb->setCharData(str(boost::format("%s/%s")%kmodel->getID()%_ComputeId(pjoint->mimic->joint_names[0])));
}
daeElementRef pmath_const1 = pmath_apply->add("cn");
pmath_const1->setCharData(str(boost::format("%f")%pjoint->mimic->offset));