bool KDLKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses) const
{
ros::WallTime n1 = ros::WallTime::now();
if(!active_)
{
ROS_ERROR("kinematics not active");
return false;
}
if(poses.size() != link_names.size())
{
ROS_ERROR("Poses vector must have size: %zu",link_names.size());
return false;
}
if(joint_angles.size() != dimension_)
{
ROS_ERROR("Joint angles vector must have size: %d",dimension_);
return false;
}
KDL::Frame p_out;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
for(unsigned int i=0; i < dimension_; i++)
{
jnt_pos_in_(i) = joint_angles[i];
}
bool valid = true;
for(unsigned int i=0; i < poses.size(); i++)
{
ROS_DEBUG("End effector index: %d",getKDLSegmentIndex(link_names[i]));
if(fk_solver_->JntToCart(jnt_pos_in_,p_out,getKDLSegmentIndex(link_names[i])) >=0)
{
tf::poseKDLToMsg(p_out,poses[i]);
}
else
{
ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
valid = false;
}
}
return valid;
}
bool KDLArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
const std::vector<double> &joint_angles,
std::vector<geometry_msgs::Pose> &poses)
{
if(!active_)
{
ROS_ERROR("kinematics not active");
return false;
}
KDL::Frame p_out;
KDL::JntArray jnt_pos_in;
geometry_msgs::PoseStamped pose;
tf::Stamped<tf::Pose> tf_pose;
jnt_pos_in.resize(dimension_);
for(unsigned int i=0; i < dimension_; i++)
{
jnt_pos_in(i) = joint_angles[i];
}
poses.resize(link_names.size());
bool valid = true;
for(unsigned int i=0; i < poses.size(); i++)
{
ROS_DEBUG("End effector index: %d",getKDLSegmentIndex(link_names[i]));
if(fk_solver_->JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
{
tf::PoseKDLToMsg(p_out,poses[i]);
}
else
{
ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
valid = false;
}
}
return valid;
}
