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

Merge pull request #81 from YoheiKakiuchi/indigo-devel-load-actuators 

collada_parser: add extract actuators, fix for using nominal torque
This commit is contained in:
Ioan A Sucan 2014-07-07 08:39:10 -07:00
parent 59b0b043a7 7c50adc588
commit f9daf957c4
1 changed files with 45 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
collada_parser/src/collada_parser.cpp
View File

@ -599,6 +599,7 @@ protected:
} }
} }
_ExtractRobotAttachedActuators(articulated_system);
_ExtractRobotManipulators(articulated_system); _ExtractRobotManipulators(articulated_system);
_ExtractRobotAttachedSensors(articulated_system); _ExtractRobotAttachedSensors(articulated_system);
return true; return true;
@ -1997,6 +1998,43 @@ protected:
return false; return false;
} }
/// \brief extract the robot actuators
void _ExtractRobotAttachedActuators(const domArticulated_systemRef as)
{
// over write joint parameters by elements in instance_actuator
for(size_t ie = 0; ie < as->getExtra_array().getCount(); ++ie) {
domExtraRef pextra = as->getExtra_array()[ie];
if( strcmp(pextra->getType(), "attach_actuator") == 0 ) {
//std::string aname = pextra->getAttribute("name");
domTechniqueRef tec = _ExtractOpenRAVEProfile(pextra->getTechnique_array());
if( !!tec ) {
boost::shared_ptr<Joint> pjoint;
daeElementRef domactuator;
{
daeElementRef bact = tec->getChild("bind_actuator");
pjoint = _getJointFromRef(bact->getAttribute("joint").c_str(), as);
if (!pjoint) continue;
}
{
daeElementRef iact = tec->getChild("instance_actuator");
if(!iact) continue;
std::string instance_url = iact->getAttribute("url");
domactuator = daeURI(*iact, instance_url).getElement();
if(!domactuator) continue;
}
daeElement *nom_torque = domactuator->getChild("nominal_torque");
if ( !! nom_torque ) {
if( !! pjoint->limits ) {
pjoint->limits->effort = boost::lexical_cast<double>(nom_torque->getCharData());
ROS_DEBUG("effort limit at joint (%s) is over written by %f",
pjoint->name.c_str(), pjoint->limits->effort);
}
}
}
}
}
}
/// \brief extract the robot manipulators /// \brief extract the robot manipulators
void _ExtractRobotManipulators(const domArticulated_systemRef as) void _ExtractRobotManipulators(const domArticulated_systemRef as)
{ {
@ -2379,7 +2417,13 @@ protected:
return boost::shared_ptr<Joint>(); return boost::shared_ptr<Joint>();
} }
boost::shared_ptr<Joint> pjoint = _model->joints_[std::string(pdomjoint->getName())]; boost::shared_ptr<Joint> pjoint;
std::string name(pdomjoint->getName());
if (_model->joints_.find(name) == _model->joints_.end()) {
pjoint.reset();
} else {
pjoint = _model->joints_.find(name)->second;
}
if(!pjoint) { if(!pjoint) {
ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName())); ROS_WARN_STREAM(str(boost::format("could not find openrave joint %s!\n")%pdomjoint->getName()));
} }