void KdlTreeFk::recursiveGetVels(bool getAll, const KDL::FrameVel &baseFrame,
const KDL::SegmentMap::const_iterator &it, const KDL::JntArrayVel &joints, std::map<std::string, KDL::FrameVel> &frames)
{
/// get FrameVel for this segment and store as needed
const std::string& segmentName = it->first;
const KDL::TreeElement& element = it->second;
const KDL::Segment& segment = element.segment;
unsigned int q_nr = element.q_nr;
KDL::FrameVel currentFrame = baseFrame * KDL::FrameVel(segment.pose(joints.q(q_nr)),
segment.twist(joints.q(q_nr), joints.qdot(q_nr)));
if (getAll)
{
frames[segmentName] = currentFrame;
}
else
{
std::map<std::string, KDL::FrameVel>::iterator frameIt = frames.find(segmentName);
if (frameIt != frames.end())
{
frameIt->second = currentFrame;
}
}
/// recurse
const std::vector<KDL::SegmentMap::const_iterator>& children = element.children;
for (std::vector<KDL::SegmentMap::const_iterator>::const_iterator childIt = children.begin();
childIt != children.end(); ++childIt)
{
recursiveGetVels(getAll, currentFrame, *childIt, joints, frames);
}
}
void RosArmCartesianControl::publishJointVelocities(KDL::JntArrayVel& joint_velocities) {
for (unsigned int i=0; i<joint_velocities.qdot.rows(); i++) {
jointMsg.velocities[i].value = joint_velocities.qdot(i);
ROS_DEBUG("%s: %.5f %s", jointMsg.velocities[i].joint_uri.c_str(),
jointMsg.velocities[i].value, jointMsg.velocities[i].unit.c_str());
if (isnan(jointMsg.velocities[i].value)) {
ROS_ERROR("invalid joint velocity: nan");
return;
}
if (fabs(jointMsg.velocities[i].value) > 1.0) {
ROS_ERROR("invalid joint velocity: too fast");
return;
}
}
cmd_vel_publisher.publish(jointMsg);
}