38 #include <geometry_msgs/PoseStamped.h>
57 PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin():active_(false){}
67 const std::string& group_name,
68 const std::string& base_frame,
69 const std::string& tip_frame,
70 double search_discretization)
72 setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
77 while(!
loadRobotModel(private_handle,robot_model,xml_string) && private_handle.ok())
79 ROS_ERROR(
"Could not load robot model. Are you sure the robot model is on the parameter server?");
91 private_handle.param<
int>(
"free_angle",
free_angle_,2);
118 ROS_DEBUG(
"PR2KinematicsPlugin::active for %s",group_name.c_str());
126 const std::vector<double> &ik_seed_state,
127 std::vector<double> &solution,
128 moveit_msgs::MoveItErrorCodes &error_code,
134 error_code.val = error_code.NO_IK_SOLUTION;
138 KDL::Frame pose_desired;
142 KDL::JntArray jnt_pos_in;
143 KDL::JntArray jnt_pos_out;
147 jnt_pos_in(i) = ik_seed_state[i];
155 error_code.val = error_code.NO_IK_SOLUTION;
164 solution[i] = jnt_pos_out(i);
166 error_code.val = error_code.SUCCESS;
171 ROS_DEBUG(
"An IK solution could not be found");
172 error_code.val = error_code.NO_IK_SOLUTION;
178 const std::vector<double> &ik_seed_state,
180 std::vector<double> &solution,
181 moveit_msgs::MoveItErrorCodes &error_code,
185 static std::vector<double> consistency_limits;
186 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
190 const std::vector<double> &ik_seed_state,
192 const std::vector<double> &consistency_limits,
193 std::vector<double> &solution,
194 moveit_msgs::MoveItErrorCodes &error_code,
198 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
202 const std::vector<double> &ik_seed_state,
204 std::vector<double> &solution,
205 const IKCallbackFn &solution_callback,
206 moveit_msgs::MoveItErrorCodes &error_code,
209 static std::vector<double> consistency_limits;
210 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
214 const std::vector<double> &ik_seed_state,
216 const std::vector<double> &consistency_limits,
217 std::vector<double> &solution,
218 const IKCallbackFn &solution_callback,
219 moveit_msgs::MoveItErrorCodes &error_code,
225 error_code.val = error_code.FAILURE;
228 if(!consistency_limits.empty() && consistency_limits.size() !=
dimension_)
231 error_code.val = error_code.FAILURE;
235 KDL::Frame pose_desired;
239 KDL::JntArray jnt_pos_in;
240 KDL::JntArray jnt_pos_out;
244 jnt_pos_in(i) = ik_seed_state[i];
248 if(consistency_limits.empty())
256 boost::bind(solution_callback, _1, _2, _3):
268 boost::bind(solution_callback, _1, _2, _3):
280 solution[i] = jnt_pos_out(i);
286 ROS_DEBUG(
"An IK solution could not be found");
292 const std::vector<double> &joint_angles,
293 std::vector<geometry_msgs::Pose> &poses)
const
302 KDL::JntArray jnt_pos_in;
303 geometry_msgs::PoseStamped pose;
309 jnt_pos_in(i) = joint_angles[i];
313 poses.resize(link_names.size());
316 for(
unsigned int i=0; i < poses.size(); i++)
325 ROS_ERROR(
"Could not compute FK for %s",link_names[i].c_str());