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 (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_NAMED("srv", "An IK that satisifes the constraints and is collision free could not be found.");
331  switch (error_code.val)
332  {
333  // Debug mode for failure:
334  ROS_DEBUG_STREAM("Request was: \n" << ik_srv.request.ik_request);
335  ROS_DEBUG_STREAM("Response was: \n" << ik_srv.response.solution);
336 
337  case moveit_msgs::MoveItErrorCodes::FAILURE:
338  ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: FAILURE");
339  break;
340  case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
341  ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: NO IK SOLUTION");
342  break;
343  default:
344  ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: " << error_code.val);
345  }
346  return false;
347  }
348  }
349  else
350  {
351  ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService());
352  error_code.val = error_code.FAILURE;
353  return false;
354  }
355 
356  // Convert the robot state message to our robot_state representation
357  if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_))
358  {
359  ROS_ERROR_STREAM_NAMED("srv", "An error occured converting recieved robot state message into internal robot "
360  "state.");
361  error_code.val = error_code.FAILURE;
362  return false;
363  }
364 
365  // Get just the joints we are concerned about in our planning group
366  robot_state_->copyJointGroupPositions(joint_model_group_, solution);
367 
368  // Run the solution callback (i.e. collision checker) if available
369  if (!solution_callback.empty())
370  {
371  ROS_DEBUG_STREAM_NAMED("srv", "Calling solution callback on IK solution");
372 
373  // hack: should use all poses, not just the 0th
374  solution_callback(ik_poses[0], solution, error_code);
375 
376  if (error_code.val != error_code.SUCCESS)
377  {
378  switch (error_code.val)
379  {
380  case moveit_msgs::MoveItErrorCodes::FAILURE:
381  ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: FAILURE");
382  break;
383  case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
384  ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: "
385  "NO IK SOLUTION");
386  break;
387  default:
388  ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: " << error_code.val);
389  }
390  return false;
391  }
392  }
393 
394  ROS_INFO_STREAM_NAMED("srv", "IK Solver Succeeded!");
395  return true;
396 }
397 
398 bool SrvKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
399  const std::vector<double>& joint_angles,
400  std::vector<geometry_msgs::Pose>& poses) const
401 {
403  if (!active_)
404  {
405  ROS_ERROR_NAMED("srv", "kinematics not active");
406  return false;
407  }
408  poses.resize(link_names.size());
409  if (joint_angles.size() != dimension_)
410  {
411  ROS_ERROR_NAMED("srv", "Joint angles vector must have size: %d", dimension_);
412  return false;
413  }
414 
415  ROS_ERROR_STREAM_NAMED("srv", "Forward kinematics not implemented");
416 
417  return false;
418 }
419 
420 const std::vector<std::string>& SrvKinematicsPlugin::getJointNames() const
421 {
422  return ik_group_info_.joint_names;
423 }
424 
425 const std::vector<std::string>& SrvKinematicsPlugin::getLinkNames() const
426 {
427  return ik_group_info_.link_names;
428 }
429 
430 const std::vector<std::string>& SrvKinematicsPlugin::getVariableNames() const
431 {
432  return joint_model_group_->getVariableNames();
433 }
434 
435 } // 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)
#define ROS_DEBUG_STREAM(args)
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 Jul 12 2018 02:54:22