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);
}
}
bool IKController::compute_ik(
const KDL::JntArrayVel &positions,
const KDL::Frame &tip_frame_des,
KDL::JntArray &ik_hint,
KDL::JntArrayVel &positions_des,
KDL::JntArrayVel &positions_des_last,
const bool debug,
const ros::Duration dt)
{
// Select ik hint by joint
for(unsigned int i=0; i<n_dof_; i++) {
switch(hint_modes_[i]) {
case 0:
ik_hint(i) = positions_.q(i); break;
case 1:
ik_hint(i) = (joint_limits_min_(i) + joint_limits_max_(i))/2.0; break;
case 2:
ik_hint(i) = hint_positions_(i); break;
};
}
// Compute joint coordinates of the target tip frame
int ik_ret = kdl_ik_solver_pos_->CartToJnt(ik_hint, tip_frame_des, positions_des.q);
if(ik_ret < 0) {
return false;
}
if(debug) {
RTT::log(RTT::Debug)<<"Unwrapped angles: "<<(positions_des.q.data.transpose)()<<RTT::endlog();
}
// Unwrap angles
for(unsigned int i=0; i<n_dof_; i++) {
if(positions_des.q(i) > 0) {
positions_des.q(i) = fmod(positions_des.q(i)+M_PI,2.0*M_PI)-M_PI;
} else {
positions_des.q(i) = fmod(positions_des.q(i)-M_PI,2.0*M_PI)+M_PI;
}
}
// Compute joint velocities
//if(dt.toSec() > 1E-5) {
//positions_des_.qdot.data = (positions_des_.q.data - positions_des_last_.q.data)/dt.toSec();
//} else {
positions_des.qdot.data.setZero();
//}
if(debug) {
RTT::log(RTT::Debug)<<"Wrapped angles: "<<(positions_des.q.data.transpose())<<RTT::endlog();
RTT::log(RTT::Debug)<<"Current position: "<<positions.q.data.transpose()<<RTT::endlog();
RTT::log(RTT::Debug)<<"Current velocity: "<<positions.qdot.data.transpose()<<RTT::endlog();
RTT::log(RTT::Debug)<<"IK result: "<<positions_des.q.data.transpose()<<RTT::endlog();
RTT::log(RTT::Debug)<<"Displacement: "<<(positions_des.q.data-positions_.q.data).transpose()<<RTT::endlog();
}
return true;
}
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);
}
