kinematics_service_capability.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
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 Willow Garage 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: Ioan Sucan */
36 
42 
44 {
45 }
46 
48 {
49  fk_service_ =
51  ik_service_ =
53 }
54 
55 namespace
56 {
57 bool isIKSolutionValid(const planning_scene::PlanningScene* planning_scene,
59  robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
60  const double* ik_solution)
61 {
62  state->setJointGroupPositions(jmg, ik_solution);
63  state->update();
64  return (!planning_scene || !planning_scene->isStateColliding(*state, jmg->getName())) &&
65  (!constraint_set || constraint_set->decide(*state).satisfied);
66 }
67 }
68 
70  moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, moveit_msgs::MoveItErrorCodes& error_code,
71  robot_state::RobotState& rs, const robot_state::GroupStateValidityCallbackFn& constraint) const
72 {
73  const robot_state::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name);
74  if (jmg)
75  {
76  robot_state::robotStateMsgToRobotState(req.robot_state, rs);
77  const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
78 
79  if (req.pose_stamped_vector.empty() || req.pose_stamped_vector.size() == 1)
80  {
81  geometry_msgs::PoseStamped req_pose =
82  req.pose_stamped_vector.empty() ? req.pose_stamped : req.pose_stamped_vector[0];
83  std::string ik_link = (!req.pose_stamped_vector.empty()) ?
84  (req.ik_link_names.empty() ? "" : req.ik_link_names[0]) :
85  req.ik_link_name;
86 
87  if (performTransform(req_pose, default_frame))
88  {
89  bool result_ik = false;
90  if (ik_link.empty())
91  result_ik = rs.setFromIK(jmg, req_pose.pose, req.attempts, req.timeout.toSec(), constraint);
92  else
93  result_ik = rs.setFromIK(jmg, req_pose.pose, ik_link, req.attempts, req.timeout.toSec(), constraint);
94 
95  if (result_ik)
96  {
97  robot_state::robotStateToRobotStateMsg(rs, solution, false);
98  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
99  }
100  else
101  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
102  }
103  else
104  error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
105  }
106  else
107  {
108  if (req.pose_stamped_vector.size() != req.ik_link_names.size())
109  error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
110  else
111  {
112  bool ok = true;
113  EigenSTL::vector_Affine3d req_poses(req.pose_stamped_vector.size());
114  for (std::size_t k = 0; k < req.pose_stamped_vector.size(); ++k)
115  {
116  geometry_msgs::PoseStamped msg = req.pose_stamped_vector[k];
117  if (performTransform(msg, default_frame))
118  tf::poseMsgToEigen(msg.pose, req_poses[k]);
119  else
120  {
121  error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
122  ok = false;
123  break;
124  }
125  }
126  if (ok)
127  {
128  if (rs.setFromIK(jmg, req_poses, req.ik_link_names, req.attempts, req.timeout.toSec(), constraint))
129  {
130  robot_state::robotStateToRobotStateMsg(rs, solution, false);
131  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
132  }
133  else
134  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
135  }
136  }
137  }
138  }
139  else
140  error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
141 }
142 
143 bool move_group::MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Request& req,
144  moveit_msgs::GetPositionIK::Response& res)
145 {
146  context_->planning_scene_monitor_->updateFrameTransforms();
147 
148  // check if the planning scene needs to be kept locked; if so, call computeIK() in the scope of the lock
149  if (req.ik_request.avoid_collisions || !kinematic_constraints::isEmpty(req.ik_request.constraints))
150  {
151  planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
152  kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
153  robot_state::RobotState rs = ls->getCurrentState();
154  kset.add(req.ik_request.constraints, ls->getTransforms());
155  computeIK(req.ik_request, res.solution, res.error_code, rs,
156  boost::bind(&isIKSolutionValid, req.ik_request.avoid_collisions ?
157  static_cast<const planning_scene::PlanningSceneConstPtr&>(ls).get() :
158  NULL,
159  kset.empty() ? NULL : &kset, _1, _2, _3));
160  }
161  else
162  {
163  // compute unconstrained IK, no lock to planning scene maintained
164  robot_state::RobotState rs =
165  planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
166  computeIK(req.ik_request, res.solution, res.error_code, rs);
167  }
168 
169  return true;
170 }
171 
172 bool move_group::MoveGroupKinematicsService::computeFKService(moveit_msgs::GetPositionFK::Request& req,
173  moveit_msgs::GetPositionFK::Response& res)
174 {
175  if (req.fk_link_names.empty())
176  {
177  ROS_ERROR("No links specified for FK request");
178  res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
179  return true;
180  }
181 
182  context_->planning_scene_monitor_->updateFrameTransforms();
183 
184  const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
185  bool do_transform = !req.header.frame_id.empty() &&
186  !robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) &&
187  context_->planning_scene_monitor_->getTFClient();
188  bool tf_problem = false;
189 
190  robot_state::RobotState rs =
191  planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
192  robot_state::robotStateMsgToRobotState(req.robot_state, rs);
193  for (std::size_t i = 0; i < req.fk_link_names.size(); ++i)
194  if (rs.getRobotModel()->hasLinkModel(req.fk_link_names[i]))
195  {
196  res.pose_stamped.resize(res.pose_stamped.size() + 1);
197  tf::poseEigenToMsg(rs.getGlobalLinkTransform(req.fk_link_names[i]), res.pose_stamped.back().pose);
198  res.pose_stamped.back().header.frame_id = default_frame;
199  res.pose_stamped.back().header.stamp = ros::Time::now();
200  if (do_transform)
201  if (!performTransform(res.pose_stamped.back(), req.header.frame_id))
202  tf_problem = true;
203  res.fk_link_names.push_back(req.fk_link_names[i]);
204  }
205  if (tf_problem)
206  res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
207  else if (res.fk_link_names.size() == req.fk_link_names.size())
208  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
209  else
210  res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
211  return true;
212 }
213 
bool isEmpty(const moveit_msgs::Constraints &constr)
bool computeIKService(moveit_msgs::GetPositionIK::Request &req, moveit_msgs::GetPositionIK::Response &res)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
static const std::string FK_SERVICE_NAME
void computeIK(moveit_msgs::PositionIKRequest &req, moveit_msgs::RobotState &solution, moveit_msgs::MoveItErrorCodes &error_code, robot_state::RobotState &rs, const robot_state::GroupStateValidityCallbackFn &constraint=robot_state::GroupStateValidityCallbackFn()) const
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
static const std::string IK_SERVICE_NAME
static Time now()
#define ROS_ERROR(...)
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
bool isStateColliding(const std::string &group="", bool verbose=false)
bool computeFKService(moveit_msgs::GetPositionFK::Request &req, moveit_msgs::GetPositionFK::Response &res)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Sun Oct 18 2020 13:18:14