37 #include <moveit_msgs/GetPositionIK.h>
44 #include <Eigen/Geometry>
56 const std::string& base_frame,
const std::vector<std::string>& tip_frames,
57 double search_discretization)
63 storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
64 joint_model_group_ = robot_model_->getJointModelGroup(group_name);
65 if (!joint_model_group_)
70 std::cout << std::endl <<
"Joint Model Variable Names: ------------------------------------------- " << std::endl;
71 const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
72 std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout,
"\n"));
73 std::cout << std::endl;
77 dimension_ = joint_model_group_->getVariableCount();
79 << group_name <<
"': " << dimension_
80 <<
". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
81 <<
". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
84 for (std::size_t i = 0; i < joint_model_group_->getJointModels().
size(); ++i)
86 ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
92 std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout,
"\n"));
96 for (
const std::string& tip_frame : tip_frames_)
98 if (!joint_model_group_->hasLinkModel(tip_frame))
100 ROS_ERROR_NAMED(
"srv",
"Could not find tip name '%s' in joint group '%s'", tip_frame.c_str(), group_name.c_str());
103 ik_group_info_.link_names.push_back(tip_frame);
108 <<
"/kinematics_solver_service_name");
109 std::string ik_service_name;
110 lookupParam(
"kinematics_solver_service_name", ik_service_name, std::string(
"solve_ik"));
113 robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
114 robot_state_->setToDefaultValues();
118 ik_service_client_ = std::make_shared<ros::ServiceClient>(
119 nonprivate_handle.serviceClient<moveit_msgs::GetPositionIK>(ik_service_name));
120 if (!ik_service_client_->waitForExistence(
ros::Duration(0.1)))
122 "Unable to connect to ROS service client with name: " << ik_service_client_->getService());
124 ROS_INFO_STREAM_NAMED(
"srv",
"Service client started with ROS service name: " << ik_service_client_->getService());
127 ROS_DEBUG_NAMED(
"srv",
"ROS service-based kinematics solver initialized");
131 bool SrvKinematicsPlugin::setRedundantJoints(
const std::vector<unsigned int>& redundant_joints)
133 if (num_possible_redundant_joints_ < 0)
138 if (
int(redundant_joints.size()) > num_possible_redundant_joints_)
140 ROS_ERROR_NAMED(
"srv",
"This group can only have %d redundant joints", num_possible_redundant_joints_);
147 bool SrvKinematicsPlugin::isRedundantJoint(
unsigned int index)
const
149 for (
const unsigned int& redundant_joint_indice : redundant_joint_indices_)
150 if (redundant_joint_indice == index)
155 int SrvKinematicsPlugin::getJointIndex(
const std::string& name)
const
157 for (
unsigned int i = 0; i < ik_group_info_.joint_names.size(); i++)
159 if (ik_group_info_.joint_names[i] == name)
170 bool SrvKinematicsPlugin::getPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
171 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
174 std::vector<double> consistency_limits;
176 return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, consistency_limits, solution, IKCallbackFn(),
177 error_code, options);
180 bool SrvKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
181 double timeout, std::vector<double>& solution,
182 moveit_msgs::MoveItErrorCodes& error_code,
185 std::vector<double> consistency_limits;
187 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution,
IKCallbackFn(), error_code,
191 bool SrvKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
192 double timeout,
const std::vector<double>& consistency_limits,
193 std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
196 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
200 bool SrvKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
201 double timeout, std::vector<double>& solution,
203 moveit_msgs::MoveItErrorCodes& error_code,
206 std::vector<double> consistency_limits;
207 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
211 bool SrvKinematicsPlugin::searchPositionIK(
const geometry_msgs::Pose& ik_pose,
const std::vector<double>& ik_seed_state,
212 double timeout,
const std::vector<double>& consistency_limits,
213 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
214 moveit_msgs::MoveItErrorCodes& error_code,
218 std::vector<geometry_msgs::Pose> ik_poses;
219 ik_poses.push_back(ik_pose);
221 return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
225 bool SrvKinematicsPlugin::searchPositionIK(
const std::vector<geometry_msgs::Pose>& ik_poses,
226 const std::vector<double>& ik_seed_state,
double ,
227 const std::vector<double>& ,
228 std::vector<double>& solution,
const IKCallbackFn& solution_callback,
229 moveit_msgs::MoveItErrorCodes& error_code,
237 error_code.val = error_code.NO_IK_SOLUTION;
242 if (ik_seed_state.size() != dimension_)
245 "Seed state must have size " << dimension_ <<
" instead of size " << ik_seed_state.size());
246 error_code.val = error_code.NO_IK_SOLUTION;
251 if (tip_frames_.size() != ik_poses.size())
253 ROS_ERROR_STREAM_NAMED(
"srv",
"Mismatched number of pose requests (" << ik_poses.size() <<
") to tip frames ("
254 << tip_frames_.size()
255 <<
") in searchPositionIK");
256 error_code.val = error_code.NO_IK_SOLUTION;
261 moveit_msgs::GetPositionIK ik_srv;
262 ik_srv.request.ik_request.avoid_collisions =
true;
263 ik_srv.request.ik_request.group_name = getGroupName();
266 robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state);
270 geometry_msgs::PoseStamped ik_pose_st;
271 ik_pose_st.header.frame_id = base_frame_;
272 if (tip_frames_.size() > 1)
275 for (std::size_t i = 0; i < tip_frames_.size(); ++i)
277 ik_pose_st.pose = ik_poses[i];
278 ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st);
279 ik_srv.request.ik_request.ik_link_names.push_back(tip_frames_[i]);
284 ik_pose_st.pose = ik_poses[0];
287 ik_srv.request.ik_request.pose_stamped = ik_pose_st;
288 ik_srv.request.ik_request.ik_link_name = getTipFrames()[0];
292 if (ik_service_client_->call(ik_srv))
295 error_code.val = ik_srv.response.error_code.val;
296 if (error_code.val != error_code.SUCCESS)
298 ROS_DEBUG_STREAM_NAMED(
"srv",
"An IK that satisifes the constraints and is collision free could not be found."
299 <<
"\nRequest was: \n"
300 << ik_srv.request.ik_request <<
"\nResponse was: \n"
301 << ik_srv.response.solution);
302 switch (error_code.val)
304 case moveit_msgs::MoveItErrorCodes::FAILURE:
307 case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
318 ROS_ERROR_STREAM(
"Service call failed to connect to service: " << ik_service_client_->getService());
319 error_code.val = error_code.FAILURE;
327 "An error occured converting received robot state message into internal robot state.");
328 error_code.val = error_code.FAILURE;
333 robot_state_->copyJointGroupPositions(joint_model_group_, solution);
336 if (!solution_callback.empty())
341 solution_callback(ik_poses[0], solution, error_code);
343 if (error_code.val != error_code.SUCCESS)
345 switch (error_code.val)
347 case moveit_msgs::MoveItErrorCodes::FAILURE:
350 case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
365 bool SrvKinematicsPlugin::getPositionFK(
const std::vector<std::string>& link_names,
366 const std::vector<double>& joint_angles,
367 std::vector<geometry_msgs::Pose>& poses)
const
374 poses.resize(link_names.size());
375 if (joint_angles.size() != dimension_)
377 ROS_ERROR_NAMED(
"srv",
"Joint angles vector must have size: %d", dimension_);
386 const std::vector<std::string>& SrvKinematicsPlugin::getJointNames()
const
388 return ik_group_info_.joint_names;
391 const std::vector<std::string>& SrvKinematicsPlugin::getLinkNames()
const
393 return ik_group_info_.link_names;
396 const std::vector<std::string>& SrvKinematicsPlugin::getVariableNames()
const
398 return joint_model_group_->getVariableNames();