moveit2
The MoveIt Motion Planning Framework for ROS 2.
plan_components_builder.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 <cassert>
38 
40 
42 {
43 std::vector<robot_trajectory::RobotTrajectoryPtr> PlanComponentsBuilder::build() const
44 {
45  std::vector<robot_trajectory::RobotTrajectoryPtr> res_vec{ traj_cont_ };
46  if (traj_tail_)
47  {
48  assert(!res_vec.empty());
49  appendWithStrictTimeIncrease(*(res_vec.back()), *traj_tail_);
50  }
51  return res_vec;
52 }
53 
54 void PlanComponentsBuilder::appendWithStrictTimeIncrease(robot_trajectory::RobotTrajectory& result,
56 {
57  if (result.empty())
58  {
59  result.append(source, 0.0);
60  return;
61  }
63  result.getGroupName(), ROBOT_STATE_EQUALITY_EPSILON))
64  {
65  result.append(source, source.getWayPointDurationFromStart(0));
66  return;
67  }
68 
69  for (size_t i = 1; i < source.getWayPointCount(); ++i)
70  {
72  }
73 }
74 
75 void PlanComponentsBuilder::blend(const planning_scene::PlanningSceneConstPtr& planning_scene,
76  const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius)
77 {
78  if (!blender_)
79  {
80  throw NoBlenderSetException("No blender set");
81  }
82 
83  assert(other->getGroupName() == traj_tail_->getGroupName());
84 
86 
87  blend_request.first_trajectory = traj_tail_;
88  blend_request.second_trajectory = other;
89  blend_request.blend_radius = blend_radius;
90  blend_request.group_name = traj_tail_->getGroupName();
91  blend_request.link_name = getSolverTipFrame(model_->getJointModelGroup(blend_request.group_name));
92 
94  if (!blender_->blend(planning_scene, blend_request, blend_response))
95  {
96  throw BlendingFailedException("Blending failed");
97  }
98 
99  // Append the new trajectory elements
100  appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.first_trajectory);
101  appendWithStrictTimeIncrease(*(traj_cont_.back()), *blend_response.blend_trajectory);
102 
103  // Store the last new trajectory element for future processing
104  traj_tail_ = blend_response.second_trajectory; // first for next blending segment
105 }
106 
107 void PlanComponentsBuilder::append(const planning_scene::PlanningSceneConstPtr& planning_scene,
108  const robot_trajectory::RobotTrajectoryPtr& other, const double blend_radius)
109 {
110  if (!model_)
111  {
112  throw NoRobotModelSetException("No robot model set");
113  }
114 
115  if (!traj_tail_)
116  {
117  traj_tail_ = other;
118  // Reserve space in container for new trajectory
119  traj_cont_.emplace_back(std::make_shared<robot_trajectory::RobotTrajectory>(model_, other->getGroupName()));
120  return;
121  }
122 
123  // Create new trajectory for every group change
124  if (other->getGroupName() != traj_tail_->getGroupName())
125  {
126  appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_);
127  traj_tail_ = other;
128  // Create new container element
129  traj_cont_.emplace_back(std::make_shared<robot_trajectory::RobotTrajectory>(model_, other->getGroupName()));
130  return;
131  }
132 
133  // No blending
134  if (blend_radius <= 0.0)
135  {
136  appendWithStrictTimeIncrease(*(traj_cont_.back()), *traj_tail_);
137  traj_tail_ = other;
138  return;
139  }
140 
141  blend(planning_scene, other, blend_radius);
142 }
143 
144 } // namespace pilz_industrial_motion_planner
std::vector< robot_trajectory::RobotTrajectoryPtr > build() const
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.
Maintain a sequence of waypoints and the time durations between these waypoints.
const std::string & getGroupName() const
double getWayPointDurationFromStart(std::size_t index) const
Returns the duration after start that a waypoint will be reached.
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
const moveit::core::RobotState & getFirstWayPoint() const
RobotTrajectory & append(const RobotTrajectory &source, double dt, size_t start_index=0, size_t end_index=std::numeric_limits< std::size_t >::max())
Add a specified part of a trajectory to the end of the current trajectory. The default (when start_in...
double getWayPointDurationFromPrevious(std::size_t index) const
const moveit::core::RobotState & getWayPoint(std::size_t index) const
const moveit::core::RobotState & getLastWayPoint() const
bool isRobotStateEqual(const moveit::core::RobotState &state1, const moveit::core::RobotState &state2, const std::string &joint_group_name, double epsilon)
Check if the two robot states have the same joint position/velocity/acceleration.
This namespace includes the central class for representing planning contexts.