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 
39 
40 // URDF, SRDF
41 #include <urdf_model/model.h>
42 #include <srdfdom/model.h>
43 
46 
47 // Eigen
48 #include <Eigen/Core>
49 #include <Eigen/Geometry>
50 
51 // register SRVKinematics as a KinematicsBase implementation
53 
55 {
56 SrvKinematicsPlugin::SrvKinematicsPlugin() : active_(false)
57 {
58 }
59 
60 bool SrvKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
61  const std::string& base_frame, const std::vector<std::string>& tip_frames,
62  double search_discretization)
63 {
64  bool debug = false;
65 
66  ROS_INFO_STREAM_NAMED("srv", "SrvKinematicsPlugin initializing");
67 
68  setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
69 
71  const srdf::ModelSharedPtr& srdf = rdf_loader.getSRDF();
72  const urdf::ModelInterfaceSharedPtr& urdf_model = rdf_loader.getURDF();
73 
74  if (!urdf_model || !srdf)
75  {
76  ROS_ERROR_NAMED("srv", "URDF and SRDF must be loaded for SRV kinematics solver to work."); // TODO: is this true?
77  return false;
78  }
79 
80  robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
81 
82  joint_model_group_ = robot_model_->getJointModelGroup(group_name);
83  if (!joint_model_group_)
84  return false;
85 
86  if (debug)
87  {
88  std::cout << std::endl << "Joint Model Variable Names: ------------------------------------------- " << std::endl;
89  const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
90  std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
91  std::cout << std::endl;
92  }
93 
94  // Get the dimension of the planning group
95  dimension_ = joint_model_group_->getVariableCount();
96  ROS_INFO_STREAM_NAMED("srv", "Dimension planning group '"
97  << group_name << "': " << dimension_
98  << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
99  << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
100 
101  // Copy joint names
102  for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i)
103  {
104  ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
105  }
106 
107  if (debug)
108  {
109  ROS_ERROR_STREAM_NAMED("srv", "tip links available:");
110  std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
111  }
112 
113  // Make sure all the tip links are in the link_names vector
114  for (std::size_t i = 0; i < tip_frames_.size(); ++i)
115  {
116  if (!joint_model_group_->hasLinkModel(tip_frames_[i]))
117  {
118  ROS_ERROR_NAMED("srv", "Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(),
119  group_name.c_str());
120  return false;
121  }
122  ik_group_info_.link_names.push_back(tip_frames_[i]);
123  }
124 
125  // Choose what ROS service to send IK requests to
126  ROS_DEBUG_STREAM_NAMED("srv", "Looking for ROS service name on rosparam server with param: "
127  << "/kinematics_solver_service_name");
128  std::string ik_service_name;
129  lookupParam("kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
130 
131  // Setup the joint state groups that we need
132  robot_state_.reset(new robot_state::RobotState(robot_model_));
133  robot_state_->setToDefaultValues();
134 
135  // Create the ROS service client
136  ros::NodeHandle nonprivate_handle("");
137  ik_service_client_ = std::make_shared<ros::ServiceClient>(
138  nonprivate_handle.serviceClient<moveit_msgs::GetPositionIK>(ik_service_name));
139  if (!ik_service_client_->waitForExistence(ros::Duration(0.1))) // wait 0.1 seconds, blocking
140  ROS_WARN_STREAM_NAMED("srv",
141  "Unable to connect to ROS service client with name: " << ik_service_client_->getService());
142  else
143  ROS_INFO_STREAM_NAMED("srv", "Service client started with ROS service name: " << ik_service_client_->getService());
144 
145  active_ = true;
146  ROS_DEBUG_NAMED("srv", "ROS service-based kinematics solver initialized");
147  return true;
148 }
149 
150 bool SrvKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
151 {
153  {
154  ROS_ERROR_NAMED("srv", "This group cannot have redundant joints");
155  return false;
156  }
157  if (int(redundant_joints.size()) > num_possible_redundant_joints_)
158  {
159  ROS_ERROR_NAMED("srv", "This group can only have %d redundant joints", num_possible_redundant_joints_);
160  return false;
161  }
162 
163  return true;
164 }
165 
166 bool SrvKinematicsPlugin::isRedundantJoint(unsigned int index) const
167 {
168  for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
169  if (redundant_joint_indices_[j] == index)
170  return true;
171  return false;
172 }
173 
174 int SrvKinematicsPlugin::getJointIndex(const std::string& name) const
175 {
176  for (unsigned int i = 0; i < ik_group_info_.joint_names.size(); i++)
177  {
178  if (ik_group_info_.joint_names[i] == name)
179  return i;
180  }
181  return -1;
182 }
183 
184 bool SrvKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
185 {
186  return ((ros::WallTime::now() - start_time).toSec() >= duration);
187 }
188 
189 bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
190  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
191  const kinematics::KinematicsQueryOptions& options) const
192 {
193  const IKCallbackFn solution_callback = 0;
194  std::vector<double> consistency_limits;
195 
196  return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, solution_callback, error_code,
197  consistency_limits, options);
198 }
199 
200 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
201  double timeout, std::vector<double>& solution,
202  moveit_msgs::MoveItErrorCodes& error_code,
203  const kinematics::KinematicsQueryOptions& options) const
204 {
205  const IKCallbackFn solution_callback = 0;
206  std::vector<double> consistency_limits;
207 
208  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
209  options);
210 }
211 
212 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
213  double timeout, const std::vector<double>& consistency_limits,
214  std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
215  const kinematics::KinematicsQueryOptions& options) const
216 {
217  const IKCallbackFn solution_callback = 0;
218  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
219  options);
220 }
221 
222 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
223  double timeout, std::vector<double>& solution,
224  const IKCallbackFn& solution_callback,
225  moveit_msgs::MoveItErrorCodes& error_code,
226  const kinematics::KinematicsQueryOptions& options) const
227 {
228  std::vector<double> consistency_limits;
229  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
230  options);
231 }
232 
233 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
234  double timeout, const std::vector<double>& consistency_limits,
235  std::vector<double>& solution, const IKCallbackFn& solution_callback,
236  moveit_msgs::MoveItErrorCodes& error_code,
237  const kinematics::KinematicsQueryOptions& options) const
238 {
239  return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
240  options);
241 }
242 
243 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
244  double timeout, std::vector<double>& solution,
245  const IKCallbackFn& solution_callback,
246  moveit_msgs::MoveItErrorCodes& error_code,
247  const std::vector<double>& consistency_limits,
248  const kinematics::KinematicsQueryOptions& options) const
249 {
250  // Convert single pose into a vector of one pose
251  std::vector<geometry_msgs::Pose> ik_poses;
252  ik_poses.push_back(ik_pose);
253 
254  return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
255  options);
256 }
257 
258 bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
259  const std::vector<double>& ik_seed_state, double timeout,
260  const std::vector<double>& consistency_limits, std::vector<double>& solution,
261  const IKCallbackFn& solution_callback,
262  moveit_msgs::MoveItErrorCodes& error_code,
263  const kinematics::KinematicsQueryOptions& options) const
264 {
265  // Check if active
266  if (!active_)
267  {
268  ROS_ERROR_NAMED("srv", "kinematics not active");
269  error_code.val = error_code.NO_IK_SOLUTION;
270  return false;
271  }
272 
273  // Check if seed state correct
274  if (ik_seed_state.size() != dimension_)
275  {
276  ROS_ERROR_STREAM_NAMED("srv", "Seed state must have size " << dimension_ << " instead of size "
277  << ik_seed_state.size());
278  error_code.val = error_code.NO_IK_SOLUTION;
279  return false;
280  }
281 
282  // Check that we have the same number of poses as tips
283  if (tip_frames_.size() != ik_poses.size())
284  {
285  ROS_ERROR_STREAM_NAMED("srv", "Mismatched number of pose requests (" << ik_poses.size() << ") to tip frames ("
286  << tip_frames_.size()
287  << ") in searchPositionIK");
288  error_code.val = error_code.NO_IK_SOLUTION;
289  return false;
290  }
291 
292  // Create the service message
293  moveit_msgs::GetPositionIK ik_srv;
294  ik_srv.request.ik_request.avoid_collisions = true;
295  ik_srv.request.ik_request.group_name = getGroupName();
296 
297  // Copy seed state into virtual robot state and convert into moveit_msg
298  robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state);
299  moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv.request.ik_request.robot_state);
300 
301  // Load the poses into the request in difference places depending if there is more than one or not
302  geometry_msgs::PoseStamped ik_pose_st;
303  ik_pose_st.header.frame_id = base_frame_;
304  if (tip_frames_.size() > 1)
305  {
306  // Load into vector of poses
307  for (std::size_t i = 0; i < tip_frames_.size(); ++i)
308  {
309  ik_pose_st.pose = ik_poses[i];
310  ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st);
311  ik_srv.request.ik_request.ik_link_names.push_back(tip_frames_[i]);
312  }
313  }
314  else
315  {
316  ik_pose_st.pose = ik_poses[0];
317 
318  // Load into single pose value
319  ik_srv.request.ik_request.pose_stamped = ik_pose_st;
320  ik_srv.request.ik_request.ik_link_name = getTipFrames()[0];
321  }
322 
323  ROS_DEBUG_STREAM_NAMED("srv", "Calling service: " << ik_service_client_->getService());
324  if (ik_service_client_->call(ik_srv))
325  {
326  // Check error code
327  error_code.val = ik_srv.response.error_code.val;
328  if (error_code.val != error_code.SUCCESS)
329  {
330  ROS_DEBUG_STREAM_NAMED("srv", "An IK that satisifes the constraints and is collision free could not be found."
331  << "\nRequest was: \n"
332  << ik_srv.request.ik_request << "\nResponse was: \n"
333  << ik_srv.response.solution);
334  switch (error_code.val)
335  {
336  case moveit_msgs::MoveItErrorCodes::FAILURE:
337  ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: FAILURE");
338  break;
339  case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
340  ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: NO IK SOLUTION");
341  break;
342  default:
343  ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: " << error_code.val);
344  }
345  return false;
346  }
347  }
348  else
349  {
350  ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService());
351  error_code.val = error_code.FAILURE;
352  return false;
353  }
354 
355  // Convert the robot state message to our robot_state representation
356  if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_))
357  {
358  ROS_ERROR_STREAM_NAMED("srv", "An error occured converting received robot state message into internal robot "
359  "state.");
360  error_code.val = error_code.FAILURE;
361  return false;
362  }
363 
364  // Get just the joints we are concerned about in our planning group
365  robot_state_->copyJointGroupPositions(joint_model_group_, solution);
366 
367  // Run the solution callback (i.e. collision checker) if available
368  if (!solution_callback.empty())
369  {
370  ROS_DEBUG_STREAM_NAMED("srv", "Calling solution callback on IK solution");
371 
372  // hack: should use all poses, not just the 0th
373  solution_callback(ik_poses[0], solution, error_code);
374 
375  if (error_code.val != error_code.SUCCESS)
376  {
377  switch (error_code.val)
378  {
379  case moveit_msgs::MoveItErrorCodes::FAILURE:
380  ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: FAILURE");
381  break;
382  case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
383  ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: "
384  "NO IK SOLUTION");
385  break;
386  default:
387  ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: " << error_code.val);
388  }
389  return false;
390  }
391  }
392 
393  ROS_INFO_STREAM_NAMED("srv", "IK Solver Succeeded!");
394  return true;
395 }
396 
397 bool SrvKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
398  const std::vector<double>& joint_angles,
399  std::vector<geometry_msgs::Pose>& poses) const
400 {
401  if (!active_)
402  {
403  ROS_ERROR_NAMED("srv", "kinematics not active");
404  return false;
405  }
406  poses.resize(link_names.size());
407  if (joint_angles.size() != dimension_)
408  {
409  ROS_ERROR_NAMED("srv", "Joint angles vector must have size: %d", dimension_);
410  return false;
411  }
412 
413  ROS_ERROR_STREAM_NAMED("srv", "Forward kinematics not implemented");
414 
415  return false;
416 }
417 
418 const std::vector<std::string>& SrvKinematicsPlugin::getJointNames() const
419 {
420  return ik_group_info_.joint_names;
421 }
422 
423 const std::vector<std::string>& SrvKinematicsPlugin::getLinkNames() const
424 {
425  return ik_group_info_.link_names;
426 }
427 
428 const std::vector<std::string>& SrvKinematicsPlugin::getVariableNames() const
429 {
430  return joint_model_group_->getVariableNames();
431 }
432 
433 } // namespace
std::vector< unsigned int > redundant_joint_indices_
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
virtual const std::string & getGroupName() const
moveit_msgs::KinematicSolverInfo ik_group_info_
const std::vector< std::string > & getJointNames() const
Return all the joint names in the order they are used internally.
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_ERROR_STREAM_NAMED(name, args)
bool isRedundantJoint(unsigned int index) const
const srdf::ModelSharedPtr & getSRDF() const
std::vector< std::string > tip_frames_
virtual bool setRedundantJoints(const std::vector< unsigned int > &redundant_joint_indices)
#define ROS_INFO_STREAM_NAMED(name, args)
virtual bool initialize(const std::string &robot_description, const std::string &group_name, const std::string &base_name, const std::string &tip_frame, double search_discretization)
virtual const std::vector< std::string > & getTipFrames() const
std::shared_ptr< ros::ServiceClient > ik_service_client_
const std::vector< std::string > & getVariableNames() const
Return all the variable names in the order they are represented internally.
#define ROS_DEBUG_NAMED(name,...)
const std::vector< std::string > & getLinkNames() const
Return all the link names in the order they are represented internally.
virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
robot_model::JointModelGroup * joint_model_group_
virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_seed_state, double timeout, std::vector< double > &solution, moveit_msgs::MoveItErrorCodes &error_code, const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions()) const
bool timedOut(const ros::WallTime &start_time, double duration) const
virtual bool getPositionFK(const std::vector< std::string > &link_names, const std::vector< double > &joint_angles, std::vector< geometry_msgs::Pose > &poses) const
bool robotStateMsgToRobotState(const Transforms &tf, const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
int getJointIndex(const std::string &name) const
virtual void setValues(const std::string &robot_description, const std::string &group_name, const std::string &base_frame, const std::string &tip_frame, double search_discretization)
bool lookupParam(const std::string &param, T &val, const T &default_val) const
boost::function< void(const geometry_msgs::Pose &ik_pose, const std::vector< double > &ik_solution, moveit_msgs::MoveItErrorCodes &error_code)> IKCallbackFn
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
static WallTime now()
Specific implementation of kinematics using ROS service calls to communicate with external IK solvers...
#define ROS_ERROR_NAMED(name,...)
#define ROS_ERROR_STREAM(args)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)
const urdf::ModelInterfaceSharedPtr & getURDF() const
#define ROS_WARN_STREAM_NAMED(name, args)


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Thu Oct 18 2018 02:47:50