moveit2
The MoveIt Motion Planning Framework for ROS 2.
simple_sampler.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2020, PickNik 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 PickNik Inc. 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 
38 
40 {
41 namespace
42 {
43 constexpr double WAYPOINT_RADIAN_TOLERANCE = 0.2; // rad: L1-norm sum for all joints
44 } // namespace
45 
46 bool SimpleSampler::initialize([[maybe_unused]] const rclcpp::Node::SharedPtr& node,
47  const moveit::core::RobotModelConstPtr& robot_model, const std::string& group_name)
48 {
49  reference_trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, group_name);
50  next_waypoint_index_ = 0;
51  joint_group_ = robot_model->getJointModelGroup(group_name);
52  return true;
53 }
54 
55 moveit_msgs::action::LocalPlanner::Feedback
57 {
58  // Reset trajectory operator to delete old reference trajectory
59  reset();
60 
61  // Throw away old reference trajectory and use trajectory update
62  reference_trajectory_ = std::make_shared<robot_trajectory::RobotTrajectory>(new_trajectory);
63 
64  // Parametrize trajectory and calculate velocity and accelerations
65  time_parametrization_.computeTimeStamps(*reference_trajectory_);
66 
67  // Return empty feedback
68  return feedback_;
69 }
70 
72 {
73  // Reset index
74  next_waypoint_index_ = 0;
75  reference_trajectory_->clear();
76  return true;
77 }
78 moveit_msgs::action::LocalPlanner::Feedback
80  robot_trajectory::RobotTrajectory& local_trajectory)
81 {
82  if (reference_trajectory_->getWayPointCount() == 0)
83  {
84  feedback_.feedback = "unhandled_exception";
85  return feedback_;
86  }
87 
88  // Delete previous local trajectory
89  local_trajectory.clear();
90 
91  // Get next desired robot state
92  const moveit::core::RobotState next_desired_goal_state = reference_trajectory_->getWayPoint(next_waypoint_index_);
93 
94  // Check if state is reached
95  if (next_desired_goal_state.distance(current_state, joint_group_) <= WAYPOINT_RADIAN_TOLERANCE)
96  {
97  // Update index (and thus desired robot state)
98  next_waypoint_index_ = std::min(next_waypoint_index_ + 1, reference_trajectory_->getWayPointCount() - 1);
99  }
100 
101  // Construct local trajectory containing the next global trajectory waypoint
102  local_trajectory.addSuffixWayPoint(reference_trajectory_->getWayPoint(next_waypoint_index_),
103  reference_trajectory_->getWayPointDurationFromPrevious(next_waypoint_index_));
104 
105  // Return empty feedback
106  return feedback_;
107 }
108 
109 double SimpleSampler::getTrajectoryProgress([[maybe_unused]] const moveit::core::RobotState& current_state)
110 {
111  // Check if trajectory is unwinded
112  if (next_waypoint_index_ >= reference_trajectory_->getWayPointCount() - 1)
113  {
114  return 1.0;
115  }
116  return 0.0;
117 }
118 } // namespace moveit::hybrid_planning
119 
120 #include <pluginlib/class_list_macros.hpp>
121 
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:90
double distance(const RobotState &other) const
Return the sum of joint distances to "other" state. An L1 norm. Only considers active joints.
Definition: robot_state.h:1354
bool initialize([[maybe_unused]] const rclcpp::Node::SharedPtr &node, const moveit::core::RobotModelConstPtr &robot_model, const std::string &group_name) override
double getTrajectoryProgress([[maybe_unused]] const moveit::core::RobotState &current_state) override
moveit_msgs::action::LocalPlanner::Feedback addTrajectorySegment(const robot_trajectory::RobotTrajectory &new_trajectory) override
moveit_msgs::action::LocalPlanner::Feedback getLocalTrajectory(const moveit::core::RobotState &current_state, robot_trajectory::RobotTrajectory &local_trajectory) override
Maintain a sequence of waypoints and the time durations between these waypoints.
RobotTrajectory & addSuffixWayPoint(const moveit::core::RobotState &state, double dt)
Add a point to the trajectory.
bool computeTimeStamps(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0) const override
Compute a trajectory with waypoints spaced equally in time (according to resample_dt_)....
PLUGINLIB_EXPORT_CLASS(moveit::hybrid_planning::SimpleSampler, moveit::hybrid_planning::TrajectoryOperatorInterface)