38 #include <geometry_msgs/PoseStamped.h>
58 PR2ArmKinematics::PR2ArmKinematics(
bool create_tf_listener): node_handle_(
"~"),dimension_(7)
65 ROS_ERROR(
"Could not load robot model. Are you sure the robot model is on the parameter server?");
70 ROS_FATAL(
"PR2IK: No root name found on parameter server");
74 ROS_FATAL(
"PR2IK: No tip name found on parameter server");
84 if(create_tf_listener) {
143 moveit_msgs::GetPositionIK::Response &response)
154 geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
155 geometry_msgs::PoseStamped pose_msg_out;
167 request.ik_request.pose_stamped = pose_msg_out;
173 moveit_msgs::GetPositionIK::Response &response)
175 KDL::Frame pose_desired;
179 KDL::JntArray jnt_pos_in;
180 KDL::JntArray jnt_pos_out;
187 jnt_pos_in(tmp_index) = request.ik_request.robot_state.joint_state.position[i];
191 ROS_ERROR(
"i: %d, No joint index for %s",i,request.ik_request.robot_state.joint_state.name[i].c_str());
195 std::vector<KDL::JntArray> jnt_array;
196 jnt_array.push_back(jnt_pos_out);
200 (
const double)(request.ik_request.timeout.toSec()));
206 response.solution.joint_state.header = request.ik_request.pose_stamped.header;
214 response.solution.joint_state.position[i] = jnt_array[0](i);
215 ROS_DEBUG(
"IK Solution: %s %d: %f",
response.solution.joint_state.name[i].c_str(),i,jnt_array[0](i));
222 ROS_DEBUG(
"An IK solution could not be found");
228 moveit_msgs::GetPositionFK::Response &response)
240 KDL::JntArray jnt_pos_in;
241 geometry_msgs::PoseStamped pose;
245 for(
int i=0; i < (int) request.robot_state.joint_state.position.size(); i++)
249 jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
252 response.pose_stamped.resize(request.fk_link_names.size());
253 response.fk_link_names.resize(request.fk_link_names.size());
255 for(
unsigned int i=0; i < request.fk_link_names.size(); i++)
269 response.fk_link_names[i] = request.fk_link_names[i];
274 ROS_ERROR(
"Could not compute FK for %s",request.fk_link_names[i].c_str());
282 const geometry_msgs::PoseStamped& pose_in,
283 geometry_msgs::PoseStamped& pose_out)
290 ROS_ERROR(
"Could not transform FK pose to frame: %s",des_frame.c_str());
294 ROS_WARN_STREAM(
"No tf listener, can't transform to frame " << des_frame);