Ejemplo n.º 1
0
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);
}