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; }