moveit2
The MoveIt Motion Planning Framework for ROS 2.
command_list_manager.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2018 Pilz GmbH & Co. KG
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 Pilz GmbH & Co. KG 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 
36 
37 #include <rclcpp/logger.hpp>
38 #include <rclcpp/logging.hpp>
39 #include <cassert>
40 #include <functional>
41 #include <sstream>
42 
45 #include <moveit/utils/logger.hpp>
46 
47 #include "cartesian_limits_parameters.hpp"
52 
54 {
55 namespace
56 {
57 const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning";
58 rclcpp::Logger getLogger()
59 {
60  return moveit::getLogger("moveit.planners.pilz.command_list_manager");
61 }
62 } // namespace
63 
64 CommandListManager::CommandListManager(const rclcpp::Node::SharedPtr& node,
65  const moveit::core::RobotModelConstPtr& model)
66  : node_(node), model_(model)
67 {
68  // Obtain the aggregated joint limits
69  pilz_industrial_motion_planner::JointLimitsContainer aggregated_limit_active_joints;
70 
72  node_, PARAM_NAMESPACE_LIMITS, model_->getActiveJointModels());
73 
74  param_listener_ =
75  std::make_shared<cartesian_limits::ParamListener>(node, PARAM_NAMESPACE_LIMITS + ".cartesian_limits");
76  params_ = param_listener_->get_params();
77 
79  limits.setJointLimits(aggregated_limit_active_joints);
80  limits.setCartesianLimits(params_);
81 
82  plan_comp_builder_.setModel(model);
83  plan_comp_builder_.setBlender(std::unique_ptr<pilz_industrial_motion_planner::TrajectoryBlender>(
85 }
86 
87 RobotTrajCont CommandListManager::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
88  const planning_pipeline::PlanningPipelinePtr& planning_pipeline,
89  const moveit_msgs::msg::MotionSequenceRequest& req_list)
90 {
91  if (req_list.items.empty())
92  {
93  return RobotTrajCont();
94  }
95 
96  checkForNegativeRadii(req_list);
97  checkLastBlendRadiusZero(req_list);
98  checkStartStates(req_list);
99 
100  MotionResponseCont resp_cont{ solveSequenceItems(planning_scene, planning_pipeline, req_list) };
101 
102  assert(model_);
103  RadiiCont radii{ extractBlendRadii(*model_, req_list) };
104  checkForOverlappingRadii(resp_cont, radii);
105 
106  plan_comp_builder_.reset();
107  for (MotionResponseCont::size_type i = 0; i < resp_cont.size(); ++i)
108  {
109  plan_comp_builder_.append(planning_scene, resp_cont.at(i).trajectory,
110  // The blend radii has to be "attached" to
111  // the second part of a blend trajectory,
112  // therefore: "i-1".
113  (i > 0 ? radii.at(i - 1) : 0.));
114  }
115 
116  return plan_comp_builder_.build();
117 }
118 
119 bool CommandListManager::checkRadiiForOverlap(const robot_trajectory::RobotTrajectory& traj_A, const double radii_A,
120  const robot_trajectory::RobotTrajectory& traj_B,
121  const double radii_B) const
122 {
123  // No blending between trajectories from different groups
124  if (traj_A.getGroupName() != traj_B.getGroupName())
125  {
126  return false;
127  }
128 
129  auto sum_radii{ radii_A + radii_B };
130  if (sum_radii == 0.)
131  {
132  return false;
133  }
134 
135  const std::string& blend_frame{ getSolverTipFrame(model_->getJointModelGroup(traj_A.getGroupName())) };
136  auto distance_endpoints = (traj_A.getLastWayPoint().getFrameTransform(blend_frame).translation() -
137  traj_B.getLastWayPoint().getFrameTransform(blend_frame).translation())
138  .norm();
139  return distance_endpoints <= sum_radii;
140 }
141 
142 void CommandListManager::checkForOverlappingRadii(const MotionResponseCont& resp_cont, const RadiiCont& radii) const
143 {
144  if (resp_cont.empty())
145  {
146  return;
147  }
148  if (resp_cont.size() < 3)
149  {
150  return;
151  }
152 
153  for (MotionResponseCont::size_type i = 0; i < resp_cont.size() - 2; ++i)
154  {
155  if (checkRadiiForOverlap(*(resp_cont.at(i).trajectory), radii.at(i), *(resp_cont.at(i + 1).trajectory),
156  radii.at(i + 1)))
157  {
158  std::ostringstream os;
159  os << "Overlapping blend radii between command [" << i << "] and [" << i + 1 << "].";
160  throw OverlappingBlendRadiiException(os.str());
161  }
162  }
163 }
164 
165 CommandListManager::RobotState_OptRef
166 CommandListManager::getPreviousEndState(const MotionResponseCont& motion_plan_responses, const std::string& group_name)
167 {
168  for (MotionResponseCont::const_reverse_iterator it = motion_plan_responses.crbegin();
169  it != motion_plan_responses.crend(); ++it)
170  {
171  if (it->trajectory->getGroupName() == group_name)
172  {
173  return std::reference_wrapper(it->trajectory->getLastWayPoint());
174  }
175  }
176  return {};
177 }
178 
179 void CommandListManager::setStartState(const MotionResponseCont& motion_plan_responses, const std::string& group_name,
180  moveit_msgs::msg::RobotState& start_state)
181 {
182  RobotState_OptRef rob_state_op{ getPreviousEndState(motion_plan_responses, group_name) };
183  if (rob_state_op)
184  {
185  moveit::core::robotStateToRobotStateMsg(rob_state_op.value(), start_state, false);
186  start_state.is_diff = true;
187  }
188 }
189 
190 bool CommandListManager::isInvalidBlendRadii(const moveit::core::RobotModel& model,
191  const moveit_msgs::msg::MotionSequenceItem& item_A,
192  const moveit_msgs::msg::MotionSequenceItem& item_B)
193 {
194  // Zero blend radius is always valid
195  if (item_A.blend_radius == 0.)
196  {
197  return false;
198  }
199 
200  // No blending between different groups
201  if (item_A.req.group_name != item_B.req.group_name)
202  {
203  RCLCPP_WARN_STREAM(getLogger(), "Blending between different groups (in this case: \""
204  << item_A.req.group_name << "\" and \"" << item_B.req.group_name
205  << "\") not allowed");
206  return true;
207  }
208 
209  // No blending for groups without solver
210  if (!hasSolver(model.getJointModelGroup(item_A.req.group_name)))
211  {
212  RCLCPP_WARN_STREAM(getLogger(), "Blending for groups without solver not allowed");
213  return true;
214  }
215 
216  return false;
217 }
218 
219 CommandListManager::RadiiCont
220 CommandListManager::extractBlendRadii(const moveit::core::RobotModel& model,
221  const moveit_msgs::msg::MotionSequenceRequest& req_list)
222 {
223  RadiiCont radii(req_list.items.size(), 0.);
224  for (RadiiCont::size_type i = 0; i < (radii.size() - 1); ++i)
225  {
226  if (isInvalidBlendRadii(model, req_list.items.at(i), req_list.items.at(i + 1)))
227  {
228  RCLCPP_WARN_STREAM(getLogger(), "Invalid blend radii between commands: [" << i << "] and [" << i + 1
229  << "] => Blend radii set to zero");
230  continue;
231  }
232  radii.at(i) = req_list.items.at(i).blend_radius;
233  }
234  return radii;
235 }
236 
237 CommandListManager::MotionResponseCont
238 CommandListManager::solveSequenceItems(const planning_scene::PlanningSceneConstPtr& planning_scene,
239  const planning_pipeline::PlanningPipelinePtr& planning_pipeline,
240  const moveit_msgs::msg::MotionSequenceRequest& req_list) const
241 {
242  MotionResponseCont motion_plan_responses;
243  size_t curr_req_index{ 0 };
244  const size_t num_req{ req_list.items.size() };
245  for (const auto& seq_item : req_list.items)
246  {
247  planning_interface::MotionPlanRequest req{ seq_item.req };
248  setStartState(motion_plan_responses, req.group_name, req.start_state);
249 
251  if (!planning_pipeline->generatePlan(planning_scene, req, res))
252  {
253  RCLCPP_ERROR(getLogger(), "Generating a plan with planning pipeline failed.");
254  res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE;
255  }
256  if (res.error_code.val != res.error_code.SUCCESS)
257  {
258  std::ostringstream os;
259  os << "Could not solve request\n"; // TODO(henning): re-enable "---\n" << req << "\n---\n";
260  throw PlanningPipelineException(os.str(), res.error_code.val);
261  }
262  motion_plan_responses.emplace_back(res);
263  RCLCPP_DEBUG_STREAM(getLogger(), "Solved [" << ++curr_req_index << '/' << num_req << ']');
264  }
265  return motion_plan_responses;
266 }
267 
268 void CommandListManager::checkForNegativeRadii(const moveit_msgs::msg::MotionSequenceRequest& req_list)
269 {
270  if (!std::all_of(req_list.items.begin(), req_list.items.end(),
271  [](const moveit_msgs::msg::MotionSequenceItem& req) { return (req.blend_radius >= 0.); }))
272  {
273  throw NegativeBlendRadiusException("All blending radii MUST be non negative");
274  }
275 }
276 
277 void CommandListManager::checkStartStatesOfGroup(const moveit_msgs::msg::MotionSequenceRequest& req_list,
278  const std::string& group_name)
279 {
280  bool first_elem{ true };
281  for (const moveit_msgs::msg::MotionSequenceItem& item : req_list.items)
282  {
283  if (item.req.group_name != group_name)
284  {
285  continue;
286  }
287 
288  if (first_elem)
289  {
290  first_elem = false;
291  continue;
292  }
293 
294  if (!(item.req.start_state.joint_state.position.empty() && item.req.start_state.joint_state.velocity.empty() &&
295  item.req.start_state.joint_state.effort.empty() && item.req.start_state.joint_state.name.empty()))
296  {
297  std::ostringstream os;
298  os << "Only the first request is allowed to have a start state, but"
299  << " the requests for group: \"" << group_name << "\" violate the rule";
300  throw StartStateSetException(os.str());
301  }
302  }
303 }
304 
305 void CommandListManager::checkStartStates(const moveit_msgs::msg::MotionSequenceRequest& req_list)
306 {
307  if (req_list.items.size() <= 1)
308  {
309  return;
310  }
311 
312  GroupNamesCont group_names{ getGroupNames(req_list) };
313  for (const auto& curr_group_name : group_names)
314  {
315  checkStartStatesOfGroup(req_list, curr_group_name);
316  }
317 }
318 
319 CommandListManager::GroupNamesCont
320 CommandListManager::getGroupNames(const moveit_msgs::msg::MotionSequenceRequest& req_list)
321 {
322  GroupNamesCont group_names;
323  std::for_each(req_list.items.cbegin(), req_list.items.cend(),
324  [&group_names](const moveit_msgs::msg::MotionSequenceItem& item) {
325  if (std::find(group_names.cbegin(), group_names.cend(), item.req.group_name) == group_names.cend())
326  {
327  group_names.emplace_back(item.req.group_name);
328  }
329  });
330  return group_names;
331 }
332 
333 } // namespace pilz_industrial_motion_planner
Definition of a kinematic model. This class is not thread safe, however multiple instances can be cre...
Definition: robot_model.h:76
const JointModelGroup * getJointModelGroup(const std::string &name) const
Get a joint group from this model (by name)
const Eigen::Isometry3d & getFrameTransform(const std::string &frame_id, bool *frame_found=nullptr)
Get the transformation matrix from the model frame (root of model) to the frame identified by frame_i...
CommandListManager(const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModelConstPtr &model)
RobotTrajCont solve(const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_pipeline::PlanningPipelinePtr &planning_pipeline, const moveit_msgs::msg::MotionSequenceRequest &req_list)
Generates trajectories for the specified list of motion commands.
static JointLimitsContainer getAggregatedLimits(const rclcpp::Node::SharedPtr &node, const std::string &param_namespace, const std::vector< const moveit::core::JointModel * > &joint_models)
Aggregates(combines) the joint limits from joint model and node parameters. The rules for the combina...
Container for JointLimits, essentially a map with convenience functions. Adds the ability to as for l...
This class combines CartesianLimit and JointLimits into on single class.
void setCartesianLimits(cartesian_limits::Params &cartesian_limit)
Set cartesian limits.
void setJointLimits(JointLimitsContainer &joint_limits)
Set joint limits.
std::vector< robot_trajectory::RobotTrajectoryPtr > build() const
void reset()
Clears the trajectory container under construction.
void setModel(const moveit::core::RobotModelConstPtr &model)
Sets the robot model needed to create new trajectory elements.
void setBlender(std::unique_ptr< pilz_industrial_motion_planner::TrajectoryBlender > blender)
Sets the blender used to blend two trajectories.
void append(const planning_scene::PlanningSceneConstPtr &planning_scene, const robot_trajectory::RobotTrajectoryPtr &other, const double blend_radius)
Appends the specified trajectory to the trajectory container under construction.
Trajectory blender implementing transition window algorithm.
Maintain a sequence of waypoints and the time durations between these waypoints.
const std::string & getGroupName() const
const moveit::core::RobotState & getLastWayPoint() const
void robotStateToRobotStateMsg(const RobotState &state, moveit_msgs::msg::RobotState &robot_state, bool copy_attached_bodies=true)
Convert a MoveIt robot state to a robot state message.
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
Definition: logger.cpp:79
std::vector< robot_trajectory::RobotTrajectoryPtr > RobotTrajCont
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
This namespace includes the central class for representing planning contexts.
Response to a planning query.
moveit::core::MoveItErrorCode error_code
const std::string PARAM_NAMESPACE_LIMITS