kinematics_constraint_aware.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 *
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2012, Willow Garage, Inc.
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of Willow Garage, Inc. nor the names of its
19 * contributors may be used to endorse or promote products derived
20 * from this software without specific prior written permission.
21 *
22 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33 * POSSIBILITY OF SUCH DAMAGE.
34 *
35 * Author: Sachin Chitta
36 *********************************************************************/
37 
38 // // ROS msgs
40 #include <moveit/robot_model/robot_model.h>
41 #include <moveit/planning_scene/planning_scene.h>
42 #include <Eigen/Geometry.h>
43 #include <eigen_conversions/eigen_msg.h>
44 #include <boost/bind.hpp>
45 
47 {
48 KinematicsConstraintAware::KinematicsConstraintAware(const robot_model::RobotModelConstPtr& kinematic_model,
49  const std::string& group_name)
50 {
51  if (!kinematic_model->hasJointModelGroup(group_name))
52  {
53  ROS_ERROR_NAMED("kinematics_constraint_aware", "The group %s does not exist", group_name.c_str());
54  joint_model_group_ = NULL;
55  return;
56  }
57  kinematic_model_ = kinematic_model;
58  group_name_ = group_name;
59  joint_model_group_ = kinematic_model_->getJointModelGroup(group_name);
60  if (joint_model_group_->getSolverInstance())
61  {
62  has_sub_groups_ = false;
63  sub_groups_names_.push_back(group_name_);
64  }
65  else
66  {
67  ROS_DEBUG_NAMED("kinematics_constraint_aware", "No kinematics solver instance defined for group %s",
68  group_name.c_str());
69  bool is_solvable_group = true;
70  if (!(joint_model_group_->getSubgroupNames().empty()))
71  {
72  const std::vector<std::string> sub_groups_names = joint_model_group_->getSubgroupNames();
73  for (std::size_t i = 0; i < sub_groups_names.size(); ++i)
74  {
75  if (!kinematic_model_->getJointModelGroup(sub_groups_names[i])->getSolverInstance())
76  {
77  is_solvable_group = false;
78  break;
79  }
80  }
81  if (is_solvable_group)
82  {
83  ROS_DEBUG_NAMED("kinematics_constraint_aware", "Group %s is a group for which we can solve IK",
84  joint_model_group_->getName().c_str());
85  sub_groups_names_ = sub_groups_names;
86  }
87  else
88  {
89  joint_model_group_ = NULL;
90  return;
91  }
92  }
93  else
94  {
95  joint_model_group_ = NULL;
96  ROS_INFO_NAMED("kinematics_constraint_aware", "No solver allocated for group %s", group_name.c_str());
97  }
98  has_sub_groups_ = true;
99  }
100  ik_attempts_ = 10;
101 }
102 
103 bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPtr& planning_scene,
106 {
107  if (!joint_model_group_)
108  {
109  ROS_ERROR_NAMED("kinematics_constraint_aware", "This solver has not been constructed properly");
110  return false;
111  }
112 
113  if (!planning_scene)
114  {
115  ROS_ERROR_NAMED("kinematics_constraint_aware", "Planning scene must be allocated");
116  return false;
117  }
118 
119  if (!response.solution_)
120  {
121  response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState()));
122  }
123 
124  ros::WallTime start_time = ros::WallTime::now();
125  if (request.group_name_ != group_name_)
126  {
127  response.error_code_.val = response.error_code_.INVALID_GROUP_NAME;
128  return false;
129  }
130 
131  // Setup the seed and the values for all other joints in the robot
132  robot_state::RobotState kinematic_state = *request.robot_state_;
133  std::vector<std::string> ik_link_names = request.ik_link_names_;
134 
135  // Transform request to tip frame if necessary
136  if (!request.ik_link_names_.empty())
137  {
138  for (std::size_t i = 0; i < request.pose_stamped_vector_.size(); ++i)
139  {
140  geometry_msgs::PoseStamped tmp_pose = request.pose_stamped_vector_[i];
141  // The assumption is that this new link is rigidly attached to the tip link for the group
142  if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i])->hasLinkModel(request.ik_link_names_[i]) &&
143  kinematic_model_->getJointModelGroup(sub_groups_names_[i])->isLinkUpdated(request.ik_link_names_[i]))
144  {
145  tmp_pose.pose = getTipFramePose(planning_scene, kinematic_state, request.pose_stamped_vector_[i].pose,
146  request.ik_link_names_[i], i);
147  ik_link_names[i] =
148  kinematic_model_->getJointModelGroup(sub_groups_names_[i])->getSolverInstance()->getTipFrame();
149  }
150  else if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i])
151  ->canSetStateFromIK(request.ik_link_names_[i]))
152  {
153  ROS_ERROR_NAMED("kinematics_constraint_aware", "Could not find IK solver for link %s for group %s",
154  request.ik_link_names_[i].c_str(), sub_groups_names_[i].c_str());
155  return false;
156  }
157  }
158  }
159 
160  // Transform the requests to the base frame of the kinematic model
161  EigenSTL::vector_Affine3d goals =
162  transformPoses(planning_scene, kinematic_state, request.pose_stamped_vector_, kinematic_model_->getModelFrame());
163 
164  robot_state::StateValidityCallbackFn constraint_callback_fn =
165  boost::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, _1, _2);
166 
167  bool result = false;
168  if (has_sub_groups_)
169  {
170  result = kinematic_state.getJointStateGroup(group_name_)
171  ->setFromIK(goals, ik_link_names, ik_attempts_, request.timeout_.toSec(), constraint_callback_fn);
172  }
173  else
174  {
175  result =
176  ik_link_names.empty() ?
177  kinematic_state.getJointStateGroup(group_name_)
178  ->setFromIK(goals[0], ik_attempts_, request.timeout_.toSec(), constraint_callback_fn) :
179  kinematic_state.getJointStateGroup(group_name_)
180  ->setFromIK(goals[0], ik_link_names[0], ik_attempts_, request.timeout_.toSec(), constraint_callback_fn);
181  }
182 
183  if (result)
184  {
185  std::vector<double> solution_values;
186  kinematic_state.getJointStateGroup(group_name_)->getVariableValues(solution_values);
187  response.solution_->getJointStateGroup(group_name_)->setVariableValues(solution_values);
188  response.error_code_.val = response.error_code_.SUCCESS;
189  }
190 
191  if (response.error_code_.val == 0)
192  {
193  response.error_code_.val = response.error_code_.NO_IK_SOLUTION;
194  }
195  return result;
196 }
197 
198 bool KinematicsConstraintAware::validityCallbackFn(const planning_scene::PlanningSceneConstPtr& planning_scene,
201  robot_state::JointStateGroup* joint_state_group,
202  const std::vector<double>& joint_group_variable_values) const
203 {
204  joint_state_group->setVariableValues(joint_group_variable_values);
205 
206  // Now check for collisions
207  if (request.check_for_collisions_)
208  {
209  collision_detection::CollisionRequest collision_request;
210  collision_detection::CollisionResult collision_result;
211  collision_request.group_name = request.group_name_;
212  planning_scene->checkCollision(collision_request, collision_result, *joint_state_group->getRobotState());
213  if (collision_result.collision)
214  {
215  ROS_DEBUG_NAMED("kinematics_constraint_aware", "IK solution is in collision");
216  response.error_code_.val = response.error_code_.GOAL_IN_COLLISION;
217  return false;
218  }
219  }
220 
221  // Now check for constraints
222  if (request.constraints_)
223  {
224  kinematic_constraints::ConstraintEvaluationResult constraint_result;
225  constraint_result =
226  request.constraints_->decide(*joint_state_group->getRobotState(), response.constraint_eval_results_);
227  if (!constraint_result.satisfied)
228  {
229  ROS_DEBUG_NAMED("kinematics_constraint_aware", "IK solution violates constraints");
230  response.error_code_.val = response.error_code_.GOAL_VIOLATES_PATH_CONSTRAINTS;
231  return false;
232  }
233  }
234 
235  // Now check for user specified constraints
236  if (request.constraint_callback_)
237  {
238  if (!request.constraint_callback_(joint_state_group, joint_group_variable_values))
239  {
240  ROS_DEBUG_NAMED("kinematics_constraint_aware", "IK solution violates user specified constraints");
241  response.error_code_.val = response.error_code_.GOAL_VIOLATES_PATH_CONSTRAINTS;
242  return false;
243  }
244  }
245 
246  return true;
247 }
248 
249 bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPtr& planning_scene,
250  const moveit_msgs::GetConstraintAwarePositionIK::Request& request,
251  moveit_msgs::GetConstraintAwarePositionIK::Response& response) const
252 {
253  if (!joint_model_group_)
254  {
255  ROS_ERROR_NAMED("kinematics_constraint_aware", "This solver has not been constructed properly");
256  return false;
257  }
258 
259  if (!planning_scene)
260  {
261  ROS_ERROR_NAMED("kinematics_constraint_aware", "Planning scene must be allocated");
262  return false;
263  }
264 
267 
268  if (!convertServiceRequest(planning_scene, request, kinematics_request, kinematics_response))
269  {
270  response.error_code = kinematics_response.error_code_;
271  return false;
272  }
273 
274  bool result = getIK(planning_scene, kinematics_request, kinematics_response);
275  response.error_code = kinematics_response.error_code_;
276  kinematics_response.solution_->getStateValues(response.solution.joint_state);
277  return result;
278 }
279 
281  const planning_scene::PlanningSceneConstPtr& planning_scene,
282  const moveit_msgs::GetConstraintAwarePositionIK::Request& request,
284  kinematics_constraint_aware::KinematicsResponse& kinematics_response) const
285 {
286  if (request.ik_request.group_name != group_name_)
287  {
288  ROS_ERROR_NAMED("kinematics_constraint_aware", "This kinematics solver does not support requests for group: %s",
289  request.ik_request.group_name.c_str());
290  kinematics_response.error_code_.val = kinematics_response.error_code_.INVALID_GROUP_NAME;
291  return false;
292  }
293 
294  if (!request.ik_request.pose_stamped_vector.empty() &&
295  request.ik_request.pose_stamped_vector.size() != sub_groups_names_.size())
296  {
297  ROS_ERROR_NAMED("kinematics_constraint_aware",
298  "Number of poses in request: %d must match number of sub groups %d in this group",
299  request.ik_request.pose_stamped_vector.size(), sub_groups_names_.size());
300  kinematics_response.error_code_.val = kinematics_response.error_code_.INVALID_GROUP_NAME;
301  return false;
302  }
303 
304  if (!request.ik_request.ik_link_names.empty() && request.ik_request.ik_link_names.size() != sub_groups_names_.size())
305  {
306  ROS_ERROR_NAMED("kinematics_constraint_aware",
307  "Number of ik_link_names in request: "
308  "%d must match number of sub groups %d in this group or must be zero",
309  request.ik_request.ik_link_names.size(), sub_groups_names_.size());
310  kinematics_response.error_code_.val = kinematics_response.error_code_.INVALID_GROUP_NAME;
311  return false;
312  }
313 
314  if (request.ik_request.ik_link_names.empty() && request.ik_request.ik_link_name != "")
315  kinematics_request.ik_link_names_.push_back(request.ik_request.ik_link_name);
316  else
317  kinematics_request.ik_link_names_ = request.ik_request.ik_link_names;
318 
319  if (request.ik_request.pose_stamped_vector.empty())
320  kinematics_request.pose_stamped_vector_.push_back(request.ik_request.pose_stamped);
321  else
322  kinematics_request.pose_stamped_vector_ = request.ik_request.pose_stamped_vector;
323 
324  kinematics_request.robot_state_.reset(new robot_state::RobotState(planning_scene->getCurrentState()));
325  kinematics_request.robot_state_->setStateValues(request.ik_request.robot_state.joint_state);
326  kinematics_request.constraints_.reset(
327  new kinematic_constraints::KinematicConstraintSet(kinematic_model_, planning_scene->getTransforms()));
328  kinematics_request.constraints_->add(request.constraints);
329  kinematics_request.timeout_ = request.ik_request.timeout;
330  kinematics_request.group_name_ = request.ik_request.group_name;
331  kinematics_request.check_for_collisions_ = true;
332 
333  kinematics_response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState()));
334 
335  return true;
336 }
337 
339  const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state,
340  const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& target_frame) const
341 {
342  Eigen::Affine3d eigen_pose, eigen_pose_2;
343  EigenSTL::vector_Affine3d result(poses.size());
344  bool target_frame_is_root_frame = (target_frame == kinematic_state.getRobotModel()->getModelFrame());
345  for (std::size_t i = 0; i < poses.size(); ++i)
346  {
347  geometry_msgs::Pose pose = poses[i].pose;
348  tf::poseMsgToEigen(pose, eigen_pose_2);
349  planning_scene->getTransforms()->transformPose(kinematic_state, poses[i].header.frame_id, eigen_pose_2, eigen_pose);
350  if (!target_frame_is_root_frame)
351  {
352  eigen_pose_2 = planning_scene->getTransforms()->getTransform(kinematic_state, target_frame);
353  eigen_pose = eigen_pose_2.inverse(Eigen::Isometry) * eigen_pose;
354  }
355  result[i] = eigen_pose;
356  }
357  return result;
358 }
359 
361  const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state,
362  const geometry_msgs::Pose& pose, const std::string& link_name, unsigned int sub_group_index) const
363 {
364  geometry_msgs::Pose result;
365  Eigen::Affine3d eigen_pose_in, eigen_pose_link, eigen_pose_tip;
366  std::string tip_name =
367  kinematic_model_->getJointModelGroup(sub_groups_names_[sub_group_index])->getSolverInstance()->getTipFrame();
368  tf::poseMsgToEigen(pose, eigen_pose_in);
369  eigen_pose_link = planning_scene->getTransforms()->getTransform(kinematic_state, link_name);
370  eigen_pose_tip = planning_scene->getTransforms()->getTransform(kinematic_state, tip_name);
371  eigen_pose_in = eigen_pose_in * (eigen_pose_link.inverse(Eigen::Isometry) * eigen_pose_tip);
372  tf::poseEigenToMsg(eigen_pose_in, result);
373  return result;
374 }
375 }
EigenSTL::vector_Affine3d transformPoses(const planning_scene::PlanningSceneConstPtr &planning_scene, const robot_state::RobotState &kinematic_state, const std::vector< geometry_msgs::PoseStamped > &poses, const std::string &target_frame) const
std::vector< geometry_msgs::PoseStamped > pose_stamped_vector_
bool convertServiceRequest(const planning_scene::PlanningSceneConstPtr &planning_scene, const moveit_msgs::GetConstraintAwarePositionIK::Request &request, kinematics_constraint_aware::KinematicsRequest &kinematics_request, kinematics_constraint_aware::KinematicsResponse &kinematics_response) const
bool getIK(const planning_scene::PlanningSceneConstPtr &planning_scene, const kinematics_constraint_aware::KinematicsRequest &request, kinematics_constraint_aware::KinematicsResponse &response) const
Solve the planning problem.
bool validityCallbackFn(const planning_scene::PlanningSceneConstPtr &planning_scene, const kinematics_constraint_aware::KinematicsRequest &request, kinematics_constraint_aware::KinematicsResponse &response, robot_state::JointStateGroup *joint_state_group, const std::vector< double > &joint_group_variable_values) const
std::vector< kinematic_constraints::ConstraintEvaluationResult > constraint_eval_results_
kinematic_constraints::KinematicConstraintSetPtr constraints_
geometry_msgs::Pose getTipFramePose(const planning_scene::PlanningSceneConstPtr &planning_scene, const robot_state::RobotState &kinematic_state, const geometry_msgs::Pose &pose, const std::string &link_name, unsigned int sub_group_index) const
robot_state::StateValidityCallbackFn constraint_callback_
KinematicsConstraintAware(const robot_model::RobotModelConstPtr &kinematic_model, const std::string &group_name)
Default constructor.


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Wed Jul 10 2019 04:03:02