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 
40 #include <tf2_eigen/tf2_eigen.h>
42 
43 namespace move_group
44 {
45 MoveGroupKinematicsService::MoveGroupKinematicsService() : MoveGroupCapability("KinematicsService")
46 {
47 }
48 
50 {
51  fk_service_ =
53  ik_service_ =
55 }
56 
57 namespace
58 {
59 bool isIKSolutionValid(const planning_scene::PlanningScene* planning_scene,
62  const double* ik_solution)
63 {
64  state->setJointGroupPositions(jmg, ik_solution);
65  state->update();
66  return (!planning_scene || !planning_scene->isStateColliding(*state, jmg->getName())) &&
67  (!constraint_set || constraint_set->decide(*state).satisfied);
68 }
69 } // namespace
70 
71 void MoveGroupKinematicsService::computeIK(moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution,
72  moveit_msgs::MoveItErrorCodes& error_code, moveit::core::RobotState& rs,
73  const moveit::core::GroupStateValidityCallbackFn& constraint) const
74 {
75  const moveit::core::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name);
76  if (jmg)
77  {
78  if (!moveit::core::isEmpty(req.robot_state))
79  {
80  moveit::core::robotStateMsgToRobotState(req.robot_state, rs);
81  }
82  const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
83 
84  if (req.pose_stamped_vector.empty() || req.pose_stamped_vector.size() == 1)
85  {
86  geometry_msgs::PoseStamped req_pose =
87  req.pose_stamped_vector.empty() ? req.pose_stamped : req.pose_stamped_vector[0];
88  std::string ik_link = (!req.pose_stamped_vector.empty()) ?
89  (req.ik_link_names.empty() ? "" : req.ik_link_names[0]) :
90  req.ik_link_name;
91 
92  if (performTransform(req_pose, default_frame))
93  {
94  bool result_ik = false;
95  if (ik_link.empty())
96  result_ik = rs.setFromIK(jmg, req_pose.pose, req.timeout.toSec(), constraint);
97  else
98  result_ik = rs.setFromIK(jmg, req_pose.pose, ik_link, req.timeout.toSec(), constraint);
99 
100  if (result_ik)
101  {
102  moveit::core::robotStateToRobotStateMsg(rs, solution, false);
103  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
104  }
105  else
106  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
107  }
108  else
109  error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
110  }
111  else
112  {
113  if (req.pose_stamped_vector.size() != req.ik_link_names.size())
114  error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
115  else
116  {
117  bool ok = true;
118  EigenSTL::vector_Isometry3d req_poses(req.pose_stamped_vector.size());
119  for (std::size_t k = 0; k < req.pose_stamped_vector.size(); ++k)
120  {
121  geometry_msgs::PoseStamped msg = req.pose_stamped_vector[k];
122  if (performTransform(msg, default_frame))
123  tf2::fromMsg(msg.pose, req_poses[k]);
124  else
125  {
126  error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
127  ok = false;
128  break;
129  }
130  }
131  if (ok)
132  {
133  if (rs.setFromIK(jmg, req_poses, req.ik_link_names, req.timeout.toSec(), constraint))
134  {
135  moveit::core::robotStateToRobotStateMsg(rs, solution, false);
136  error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
137  }
138  else
139  error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
140  }
141  }
142  }
143  }
144  else
145  error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
146 }
147 
148 bool MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Request& req,
149  moveit_msgs::GetPositionIK::Response& res)
150 {
151  context_->planning_scene_monitor_->updateFrameTransforms();
152 
153  // check if the planning scene needs to be kept locked; if so, call computeIK() in the scope of the lock
154  if (req.ik_request.avoid_collisions || !moveit::core::isEmpty(req.ik_request.constraints))
155  {
156  planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
157  kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
158  moveit::core::RobotState rs = ls->getCurrentState();
159  kset.add(req.ik_request.constraints, ls->getTransforms());
160  computeIK(req.ik_request, res.solution, res.error_code, rs,
161  boost::bind(&isIKSolutionValid,
162  req.ik_request.avoid_collisions ?
163  static_cast<const planning_scene::PlanningSceneConstPtr&>(ls).get() :
164  nullptr,
165  kset.empty() ? nullptr : &kset, _1, _2, _3));
166  }
167  else
168  {
169  // compute unconstrained IK, no lock to planning scene maintained
171  planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
172  computeIK(req.ik_request, res.solution, res.error_code, rs);
173  }
174 
175  return true;
176 }
177 
178 bool MoveGroupKinematicsService::computeFKService(moveit_msgs::GetPositionFK::Request& req,
179  moveit_msgs::GetPositionFK::Response& res)
180 {
181  if (req.fk_link_names.empty())
182  {
183  ROS_ERROR_NAMED(getName(), "No links specified for FK request");
184  res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
185  return true;
186  }
187 
188  context_->planning_scene_monitor_->updateFrameTransforms();
189 
190  const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
191  bool do_transform = !req.header.frame_id.empty() &&
192  !moveit::core::Transforms::sameFrame(req.header.frame_id, default_frame) &&
193  context_->planning_scene_monitor_->getTFClient();
194  bool tf_problem = false;
195 
197  planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
198  moveit::core::robotStateMsgToRobotState(req.robot_state, rs);
199  for (std::size_t i = 0; i < req.fk_link_names.size(); ++i)
200  if (rs.getRobotModel()->hasLinkModel(req.fk_link_names[i]))
201  {
202  res.pose_stamped.resize(res.pose_stamped.size() + 1);
203  res.pose_stamped.back().pose = tf2::toMsg(rs.getGlobalLinkTransform(req.fk_link_names[i]));
204  res.pose_stamped.back().header.frame_id = default_frame;
205  res.pose_stamped.back().header.stamp = ros::Time::now();
206  if (do_transform)
207  if (!performTransform(res.pose_stamped.back(), req.header.frame_id))
208  tf_problem = true;
209  res.fk_link_names.push_back(req.fk_link_names[i]);
210  }
211  if (tf_problem)
212  res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
213  else if (res.fk_link_names.size() == req.fk_link_names.size())
214  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
215  else
216  res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
217  return true;
218 }
219 } // namespace move_group
220 
moveit::core::GroupStateValidityCallbackFn
boost::function< bool(RobotState *robot_state, const JointModelGroup *joint_group, const double *joint_group_variable_values)> GroupStateValidityCallbackFn
move_group::MoveGroupCapability::performTransform
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
Definition: move_group_capability.cpp:177
move_group::MoveGroupCapability
Definition: move_group_capability.h:89
move_group::MoveGroupCapability::context_
MoveGroupContextPtr context_
Definition: move_group_capability.h:132
move_group::MoveGroupCapability::getName
const std::string & getName() const
Definition: move_group_capability.h:104
tf2_eigen.h
tf2::fromMsg
void fromMsg(const A &, B &b)
move_group::MoveGroupKinematicsService::computeIK
void computeIK(moveit_msgs::PositionIKRequest &req, moveit_msgs::RobotState &solution, moveit_msgs::MoveItErrorCodes &error_code, moveit::core::RobotState &rs, const moveit::core::GroupStateValidityCallbackFn &constraint=moveit::core::GroupStateValidityCallbackFn()) const
Definition: kinematics_service_capability.cpp:103
planning_scene::PlanningScene
kinematic_constraints::KinematicConstraintSet::decide
ConstraintEvaluationResult decide(const moveit::core::RobotState &state, bool verbose=false) const
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
move_group::FK_SERVICE_NAME
static const std::string FK_SERVICE_NAME
Definition: capability_names.h:86
ROS_ERROR_NAMED
#define ROS_ERROR_NAMED(name,...)
moveit::core::RobotState
moveit::core::RobotState::getGlobalLinkTransform
const Eigen::Isometry3d & getGlobalLinkTransform(const std::string &link_name)
moveit::core::RobotState::setFromIK
bool setFromIK(const JointModelGroup *group, const geometry_msgs::Pose &pose, double timeout=0.0, const GroupStateValidityCallbackFn &constraint=GroupStateValidityCallbackFn(), const kinematics::KinematicsQueryOptions &options=kinematics::KinematicsQueryOptions())
kinematic_constraints::ConstraintEvaluationResult::satisfied
bool satisfied
move_group::IK_SERVICE_NAME
static const std::string IK_SERVICE_NAME
Definition: capability_names.h:85
move_group::MoveGroupKinematicsService::computeFKService
bool computeFKService(moveit_msgs::GetPositionFK::Request &req, moveit_msgs::GetPositionFK::Response &res)
Definition: kinematics_service_capability.cpp:210
ok
ROSCPP_DECL bool ok()
moveit::core::JointModelGroup::getName
const std::string & getName() const
message_checks.h
moveit::core::isEmpty
bool isEmpty(const geometry_msgs::Pose &msg)
kinematics_service_capability.h
move_group::MoveGroupKinematicsService::MoveGroupKinematicsService
MoveGroupKinematicsService()
Definition: kinematics_service_capability.cpp:77
moveit::core::RobotState::update
void update(bool force=false)
move_group::MoveGroupCapability::root_node_handle_
ros::NodeHandle root_node_handle_
Definition: move_group_capability.h:129
moveit::core::RobotState::getRobotModel
const RobotModelConstPtr & getRobotModel() const
kinematic_constraints::KinematicConstraintSet
move_group
Definition: capability_names.h:41
class_loader.hpp
move_group::MoveGroupKinematicsService::initialize
void initialize() override
Definition: kinematics_service_capability.cpp:81
move_group::MoveGroupKinematicsService::ik_service_
ros::ServiceServer ik_service_
Definition: kinematics_service_capability.h:126
moveit::core::Transforms::sameFrame
static bool sameFrame(const std::string &frame1, const std::string &frame2)
move_group::MoveGroupKinematicsService
Definition: kinematics_service_capability.h:77
planning_scene_monitor::LockedPlanningSceneRO
moveit::core::JointModelGroup
tf2::toMsg
B toMsg(const A &a)
conversions.h
CLASS_LOADER_REGISTER_CLASS
CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddIterativeSplineParameterization, planning_request_adapter::PlanningRequestAdapter)
moveit::core::RobotState::setJointGroupPositions
void setJointGroupPositions(const std::string &joint_group_name, const double *gstate)
moveit::core::RobotState::getJointModelGroup
const JointModelGroup * getJointModelGroup(const std::string &group) const
planning_scene
move_group::MoveGroupKinematicsService::fk_service_
ros::ServiceServer fk_service_
Definition: kinematics_service_capability.h:125
capability_names.h
moveit::core::robotStateToRobotStateMsg
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::RobotState &robot_state, bool copy_attached_bodies=true)
moveit::core::robotStateMsgToRobotState
bool robotStateMsgToRobotState(const moveit_msgs::RobotState &robot_state, RobotState &state, bool copy_attached_bodies=true)
move_group::MoveGroupKinematicsService::computeIKService
bool computeIKService(moveit_msgs::GetPositionIK::Request &req, moveit_msgs::GetPositionIK::Response &res)
Definition: kinematics_service_capability.cpp:180
ros::Time::now
static Time now()


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Tue Oct 12 2021 02:25:39