moveit2
The MoveIt Motion Planning Framework for ROS 2.
srv_kinematics_plugin.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2014, JSK, The University of Tokyo.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of JSK, The University of Tokyo nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman, Masaki Murooka */
36 
37 #include <moveit_msgs/srv/get_position_ik.hpp>
39 #include <class_loader/class_loader.hpp>
41 #include <iterator>
42 
43 // Eigen
44 #include <Eigen/Core>
45 #include <Eigen/Geometry>
46 
47 // register SRVKinematics as a KinematicsBase implementation
49 
50 namespace srv_kinematics_plugin
51 {
52 static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_srv_kinematics_plugin.srv_kinematics_plugin");
53 
55 {
56 }
57 
58 bool SrvKinematicsPlugin::initialize(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModel& robot_model,
59  const std::string& group_name, const std::string& base_frame,
60  const std::vector<std::string>& tip_frames, double search_discretization)
61 {
62  node_ = node;
63  bool debug = false;
64 
65  RCLCPP_INFO(LOGGER, "SrvKinematicsPlugin initializing");
66 
67  // Get Solver Parameters
68  std::string kinematics_param_prefix = "robot_description_kinematics." + group_name;
69  param_listener_ = std::make_shared<srv_kinematics::ParamListener>(node, kinematics_param_prefix);
70  params_ = param_listener_->get_params();
71 
72  storeValues(robot_model, group_name, base_frame, tip_frames, search_discretization);
73  joint_model_group_ = robot_model_->getJointModelGroup(group_name);
74  if (!joint_model_group_)
75  return false;
76 
77  if (debug)
78  {
79  std::cout << "Joint Model Variable Names: ------------------------------------------- \n ";
80  const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
81  std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
82  std::cout << '\n';
83  }
84 
85  // Get the dimension of the planning group
86  dimension_ = joint_model_group_->getVariableCount();
87  RCLCPP_INFO_STREAM(LOGGER, "Dimension planning group '"
88  << group_name << "': " << dimension_
89  << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
90  << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
91 
92  // Copy joint names
93  for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i)
94  {
95  ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
96  }
97 
98  if (debug)
99  {
100  RCLCPP_ERROR(LOGGER, "tip links available:");
101  std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
102  }
103 
104  // Make sure all the tip links are in the link_names vector
105  for (const std::string& tip_frame : tip_frames_)
106  {
107  if (!joint_model_group_->hasLinkModel(tip_frame))
108  {
109  RCLCPP_ERROR(LOGGER, "Could not find tip name '%s' in joint group '%s'", tip_frame.c_str(), group_name.c_str());
110  return false;
111  }
112  ik_group_info_.link_names.push_back(tip_frame);
113  }
114 
115  // Setup the joint state groups that we need
116  robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
117  robot_state_->setToDefaultValues();
118 
119  // Create the ROS2 service client
120  RCLCPP_DEBUG(LOGGER, "IK Service client topic : %s", params_.kinematics_solver_service_name.c_str());
121  ik_service_client_ = node_->create_client<moveit_msgs::srv::GetPositionIK>(params_.kinematics_solver_service_name);
122 
123  if (!ik_service_client_->wait_for_service(std::chrono::seconds(1)))
124  { // wait 0.1 seconds, blocking
125  RCLCPP_WARN_STREAM(LOGGER,
126  "Unable to connect to ROS service client with name: " << ik_service_client_->get_service_name());
127  }
128  else
129  {
130  RCLCPP_INFO_STREAM(LOGGER,
131  "Service client started with ROS service name: " << ik_service_client_->get_service_name());
132  }
133 
134  active_ = true;
135  RCLCPP_DEBUG(LOGGER, "ROS service-based kinematics solver initialized");
136  return true;
137 }
138 
139 bool SrvKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
140 {
141  if (num_possible_redundant_joints_ < 0)
142  {
143  RCLCPP_ERROR(LOGGER, "This group cannot have redundant joints");
144  return false;
145  }
146  if (int(redundant_joints.size()) > num_possible_redundant_joints_)
147  {
148  RCLCPP_ERROR(LOGGER, "This group can only have %d redundant joints", num_possible_redundant_joints_);
149  return false;
150  }
151 
152  return true;
153 }
154 
155 bool SrvKinematicsPlugin::isRedundantJoint(unsigned int index) const
156 {
157  for (unsigned int redundant_joint_indice : redundant_joint_indices_)
158  {
159  if (redundant_joint_indice == index)
160  return true;
161  }
162  return false;
163 }
164 
165 int SrvKinematicsPlugin::getJointIndex(const std::string& name) const
166 {
167  for (unsigned int i = 0; i < ik_group_info_.joint_names.size(); ++i)
168  {
169  if (ik_group_info_.joint_names[i] == name)
170  return i;
171  }
172  return -1;
173 }
174 
175 bool SrvKinematicsPlugin::timedOut(const rclcpp::Time& start_time, double duration) const
176 {
177  return ((node_->now() - start_time).seconds() >= duration);
178 }
179 
180 bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::msg::Pose& ik_pose,
181  const std::vector<double>& ik_seed_state, std::vector<double>& solution,
182  moveit_msgs::msg::MoveItErrorCodes& error_code,
184 {
185  std::vector<double> consistency_limits;
186 
187  return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, consistency_limits, solution, IKCallbackFn(),
188  error_code, options);
189 }
190 
191 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
192  const std::vector<double>& ik_seed_state, double timeout,
193  std::vector<double>& solution,
194  moveit_msgs::msg::MoveItErrorCodes& error_code,
196 {
197  std::vector<double> consistency_limits;
198 
199  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
200  options);
201 }
202 
203 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
204  const std::vector<double>& ik_seed_state, double timeout,
205  const std::vector<double>& consistency_limits, std::vector<double>& solution,
206  moveit_msgs::msg::MoveItErrorCodes& error_code,
208 {
209  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, IKCallbackFn(), error_code,
210  options);
211 }
212 
213 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
214  const std::vector<double>& ik_seed_state, double timeout,
215  std::vector<double>& solution, const IKCallbackFn& solution_callback,
216  moveit_msgs::msg::MoveItErrorCodes& error_code,
218 {
219  std::vector<double> consistency_limits;
220  return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
221  options);
222 }
223 
224 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::msg::Pose& ik_pose,
225  const std::vector<double>& ik_seed_state, double timeout,
226  const std::vector<double>& consistency_limits, std::vector<double>& solution,
227  const IKCallbackFn& solution_callback,
228  moveit_msgs::msg::MoveItErrorCodes& error_code,
230 {
231  // Convert single pose into a vector of one pose
232  std::vector<geometry_msgs::msg::Pose> ik_poses;
233  ik_poses.push_back(ik_pose);
234 
235  return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
236  options);
237 }
238 
239 bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses,
240  const std::vector<double>& ik_seed_state, double /*timeout*/,
241  const std::vector<double>& /*consistency_limits*/,
242  std::vector<double>& solution, const IKCallbackFn& solution_callback,
243  moveit_msgs::msg::MoveItErrorCodes& error_code,
244  const kinematics::KinematicsQueryOptions& /*options*/,
245  const moveit::core::RobotState* /*context_state*/) const
246 {
247  // Check if active
248  if (!active_)
249  {
250  RCLCPP_ERROR(LOGGER, "kinematics not active");
251  error_code.val = error_code.NO_IK_SOLUTION;
252  return false;
253  }
254 
255  // Check if seed state correct
256  if (ik_seed_state.size() != dimension_)
257  {
258  RCLCPP_ERROR_STREAM(LOGGER,
259  "Seed state must have size " << dimension_ << " instead of size " << ik_seed_state.size());
260  error_code.val = error_code.NO_IK_SOLUTION;
261  return false;
262  }
263 
264  // Check that we have the same number of poses as tips
265  if (tip_frames_.size() != ik_poses.size())
266  {
267  RCLCPP_ERROR_STREAM(LOGGER, "Mismatched number of pose requests (" << ik_poses.size() << ") to tip frames ("
268  << tip_frames_.size()
269  << ") in searchPositionIK");
270  error_code.val = error_code.NO_IK_SOLUTION;
271  return false;
272  }
273 
274  // Create the service message
275  auto ik_srv = std::make_shared<moveit_msgs::srv::GetPositionIK::Request>();
276  ik_srv->ik_request.avoid_collisions = true;
277  ik_srv->ik_request.group_name = getGroupName();
278 
279  // Copy seed state into virtual robot state and convert into moveit_msg
280  robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state);
281  moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv->ik_request.robot_state);
282 
283  // Load the poses into the request in difference places depending if there is more than one or not
284  geometry_msgs::msg::PoseStamped ik_pose_st;
285  ik_pose_st.header.frame_id = base_frame_;
286  if (tip_frames_.size() > 1)
287  {
288  // Load into vector of poses
289  for (std::size_t i = 0; i < tip_frames_.size(); ++i)
290  {
291  ik_pose_st.pose = ik_poses[i];
292  ik_srv->ik_request.pose_stamped_vector.push_back(ik_pose_st);
293  ik_srv->ik_request.ik_link_names.push_back(tip_frames_[i]);
294  }
295  }
296  else
297  {
298  ik_pose_st.pose = ik_poses[0];
299 
300  // Load into single pose value
301  ik_srv->ik_request.pose_stamped = ik_pose_st;
302  ik_srv->ik_request.ik_link_name = getTipFrames()[0];
303  }
304 
305  RCLCPP_DEBUG(LOGGER, "Calling service: %s", ik_service_client_->get_service_name());
306  auto result_future = ik_service_client_->async_send_request(ik_srv);
307  const auto& response = result_future.get();
308  if (rclcpp::spin_until_future_complete(node_, result_future) == rclcpp::FutureReturnCode::SUCCESS)
309  {
310  // Check error code
311  error_code.val = response->error_code.val;
312  if (error_code.val != error_code.SUCCESS)
313  {
314  // TODO (JafarAbdi) Print the entire message for ROS2?
315  // RCLCPP_DEBUG("srv", "An IK that satisfies the constraints and is collision free could not be found."
316  // << "\nRequest was: \n"
317  // << ik_srv.request.ik_request << "\nResponse was: \n"
318  // << ik_srv.response.solution);
319  switch (error_code.val)
320  {
321  case moveit_msgs::msg::MoveItErrorCodes::FAILURE:
322  RCLCPP_ERROR(LOGGER, "Service failed with with error code: FAILURE");
323  break;
324  case moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION:
325  RCLCPP_DEBUG(LOGGER, "Service failed with with error code: NO IK SOLUTION");
326  break;
327  default:
328  RCLCPP_DEBUG_STREAM(LOGGER, "Service failed with with error code: " << error_code.val);
329  }
330  return false;
331  }
332  }
333  else
334  {
335  RCLCPP_DEBUG_STREAM(LOGGER, "Service call failed to connect to service: " << ik_service_client_->get_service_name());
336  error_code.val = error_code.FAILURE;
337  return false;
338  }
339  // Convert the robot state message to our robot_state representation
340  if (!moveit::core::robotStateMsgToRobotState(response->solution, *robot_state_))
341  {
342  RCLCPP_ERROR(LOGGER, "An error occurred converting received robot state message into internal robot state.");
343  error_code.val = error_code.FAILURE;
344  return false;
345  }
346 
347  // Get just the joints we are concerned about in our planning group
348  robot_state_->copyJointGroupPositions(joint_model_group_, solution);
349 
350  // Run the solution callback (i.e. collision checker) if available
351  if (solution_callback)
352  {
353  RCLCPP_DEBUG(LOGGER, "Calling solution callback on IK solution");
354 
355  // hack: should use all poses, not just the 0th
356  solution_callback(ik_poses[0], solution, error_code);
357 
358  if (error_code.val != error_code.SUCCESS)
359  {
360  switch (error_code.val)
361  {
362  case moveit_msgs::msg::MoveItErrorCodes::FAILURE:
363  RCLCPP_ERROR(LOGGER, "IK solution callback failed with with error code: FAILURE");
364  break;
365  case moveit_msgs::msg::MoveItErrorCodes::NO_IK_SOLUTION:
366  RCLCPP_ERROR(LOGGER, "IK solution callback failed with with error code: "
367  "NO IK SOLUTION");
368  break;
369  default:
370  RCLCPP_ERROR_STREAM(LOGGER, "IK solution callback failed with with error code: " << error_code.val);
371  }
372  return false;
373  }
374  }
375 
376  RCLCPP_INFO(LOGGER, "IK Solver Succeeded!");
377  return true;
378 }
379 
380 bool SrvKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
381  const std::vector<double>& joint_angles,
382  std::vector<geometry_msgs::msg::Pose>& poses) const
383 {
384  if (!active_)
385  {
386  RCLCPP_ERROR(LOGGER, "kinematics not active");
387  return false;
388  }
389  poses.resize(link_names.size());
390  if (joint_angles.size() != dimension_)
391  {
392  RCLCPP_ERROR(LOGGER, "Joint angles vector must have size: %d", dimension_);
393  return false;
394  }
395 
396  RCLCPP_ERROR(LOGGER, "Forward kinematics not implemented");
397 
398  return false;
399 }
400 
401 const std::vector<std::string>& SrvKinematicsPlugin::getJointNames() const
402 {
403  return ik_group_info_.joint_names;
404 }
405 
406 const std::vector<std::string>& SrvKinematicsPlugin::getLinkNames() const
407 {
408  return ik_group_info_.link_names;
409 }
410 
411 const std::vector<std::string>& SrvKinematicsPlugin::getVariableNames() const
412 {
413  return joint_model_group_->getVariableNames();
414 }
415 
416 } // namespace srv_kinematics_plugin
Provides an interface for kinematics solvers.
void storeValues(const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_frame, const std::vector< std::string > &tip_frames, double search_discretization)
std::vector< unsigned int > redundant_joint_indices_
static const rclcpp::Logger LOGGER
virtual const std::vector< std::string > & getTipFrames() const
Return the names of the tip frames of the kinematic tree on which the solver is operating....
std::vector< std::string > tip_frames_
std::function< void(const geometry_msgs::msg::Pose &, const std::vector< double > &, moveit_msgs::msg::MoveItErrorCodes &)> IKCallbackFn
Signature for a callback to validate an IK solution. Typically used for collision checking.
moveit::core::RobotModelConstPtr robot_model_
virtual const std::string & getGroupName() const
Return the name of the group that the solver is operating on.
const std::vector< const JointModel * > & getActiveJointModels() const
Get the active joints in this group (that have controllable DOF). This does not include mimic joints.
const std::vector< std::string > & getJointModelNames() const
Get the names of the joints in this group. These are the names of the joints returned by getJointMode...
const std::vector< const JointModel * > & getJointModels() const
Get all the joints in this group (including fixed and mimic joints).
bool hasLinkModel(const std::string &link) const
Check if a link is part of this group.
unsigned int getVariableCount() const
Get the number of variables that describe this joint group. This includes variables necessary for mim...
const std::vector< std::string > & getVariableNames() const
Get the names of the variables that make up the joints included in this group. The number of returned...
const std::vector< const JointModel * > & getMimicJointModels() const
Get the mimic joints that are part of this group.
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
Specific implementation of kinematics using ROS service calls to communicate with external IK solvers...
bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices) override
Set a set of redundant joints for the kinematics solver to use. This can fail, depending on the IK so...
const std::vector< std::string > & getJointNames() const override
Return all the joint names in the order they are used internally.
bool searchPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
Given a desired pose of the end-effector, search for the joint angles required to reach it....
bool getPositionIK(const geometry_msgs::msg::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::msg::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const override
Given a desired pose of the end-effector, compute the joint angles to reach it.
const std::vector< std::string > & getVariableNames() const
Return all the variable names in the order they are represented internally.
const std::vector< std::string > & getLinkNames() const override
Return all the link names in the order they are represented internally.
bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::msg::Pose > &poses) const override
Given a set of joint angles and a set of links, compute their pose.
bool initialize(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModel &robot_model, const std::string &group_name, const std::string &base_name, const std::vector< std::string > &tip_frames, double search_discretization) override
Initialization function for the kinematics, for use with kinematic chain IK solvers.
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::msg::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
Convert a robot state msg (with accompanying extra transforms) to a MoveIt robot state.
name
Definition: setup.py:7
CLASS_LOADER_REGISTER_CLASS(stomp_moveit::StompSmoothingAdapter, planning_request_adapter::PlanningRequestAdapter)
A set of options for the kinematics solver.