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

collada writer now exports mimic joints through formulas

This commit is contained in:
rdiankov 2010-05-20 04:08:54 +00:00
parent 2ac03e3094
commit 1afc96d0c5
2 changed files with 42 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
collada_urdf/include/collada_urdf/collada_writer.h
View File

@ -112,7 +112,7 @@ private:
domTranslateRef addTranslate(daeElementRef parent, urdf::Vector3 const& position, daeElementRef before = NULL, bool ignore_zero_translations = false);
domRotateRef addRotate(daeElementRef parent, urdf::Rotation const& r, daeElementRef before = NULL, bool ignore_zero_rotations = false);
void addMimicJoint(domFormulaRef formula, const std::string& joint_sid,const std::string& joint_mimic_sid, double multiplier, double offset);
std::string getTimeStampString() const;
private:

41
collada_urdf/src/collada_writer.cpp
View File

@ -574,6 +574,18 @@ void ColladaWriter::addKinematics(SCENE const& scene) {
int link_num = 0;
addKinematicLink(robot_.getRoot(), technique, link_num);
// </link>
// add a formula for each mimic joint
for (map<string, shared_ptr<urdf::Joint> >::const_iterator i = robot_.joints_.begin(); i != robot_.joints_.end(); i++) {
shared_ptr<urdf::Joint> urdf_joint = i->second;
if( !!urdf_joint->mimic ) {
string joint_sid = string("k1/")+joint_sids_[urdf_joint->name];
string joint_mimic_sid = string("k1/")+joint_sids_[urdf_joint->mimic->joint_name];
// <formula>
addMimicJoint(daeSafeCast<domFormula>(technique->add(COLLADA_ELEMENT_FORMULA)), joint_sid,joint_mimic_sid,urdf_joint->mimic->multiplier,urdf_joint->mimic->offset);
// </formula>
}
}
}
kmodel_ = kmodel;
// </kinematics_model>
@ -892,6 +904,35 @@ domRotateRef ColladaWriter::addRotate(daeElementRef parent, urdf::Rotation const
return rot;
}
void ColladaWriter::addMimicJoint(domFormulaRef formula, const std::string& joint_sid,const std::string& joint_mimic_sid, double multiplier, double offset)
{
string sid = joint_sid+string(".formula");
formula->setSid(sid.c_str());
domCommon_float_or_paramRef ptarget = daeSafeCast<domCommon_float_or_param>(formula->createAndPlace(COLLADA_ELEMENT_TARGET));
daeSafeCast<domCommon_param>(ptarget->createAndPlace(COLLADA_TYPE_PARAM))->setValue(joint_sid.c_str());
domFormula_techniqueRef pftec = daeSafeCast<domFormula_technique>(formula->createAndPlace(COLLADA_ELEMENT_TECHNIQUE_COMMON));
// <apply> <plus/> <apply> <times/> <cn>a</cn> x </apply> <cn>b</cn> </apply>
daeElementRef pmath_math = pftec->createAndPlace("math");
daeElementRef pmath_apply = pmath_math->createAndPlace("apply");
{
daeElementRef pmath_plus = pmath_apply->createAndPlace("plus");
daeElementRef pmath_apply1 = pmath_apply->createAndPlace("apply");
{
daeElementRef pmath_times = pmath_apply1->createAndPlace("times");
daeElementRef pmath_const0 = pmath_apply1->createAndPlace("cn");
pmath_const0->setCharData(boost::lexical_cast<string>(multiplier));
daeElementRef pmath_symb = pmath_apply1->createAndPlace("csymbol");
pmath_symb->setAttribute("encoding","COLLADA");
pmath_symb->setCharData(joint_mimic_sid);
}
daeElementRef pmath_const1 = pmath_apply->createAndPlace("cn");
pmath_const1->setCharData(boost::lexical_cast<string>(offset));
}
}
string ColladaWriter::getTimeStampString() const {
// facet becomes owned by locale, so no need to explicitly delete
boost::posix_time::time_facet* facet = new boost::posix_time::time_facet("%Y-%m-%dT%H:%M:%s");