ruckig_traj_smoothing.h
Go to the documentation of this file.
1 /*******************************************************************************
2  * BSD 3-Clause License
3  *
4  * Copyright (c) 2021, PickNik Robotics
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 are met:
9  *
10  * * Redistributions of source code must retain the above copyright notice, this
11  * list of conditions and the following disclaimer.
12  *
13  * * Redistributions in binary form must reproduce the above copyright notice,
14  * this list of conditions and the following disclaimer in the documentation
15  * and/or other materials provided with the distribution.
16  *
17  * * Neither the name of the copyright holder nor the names of its
18  * contributors may be used to endorse or promote products derived from
19  * this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
24  * ARE
25  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
26  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
27  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
28  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
30  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
31  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  *******************************************************************************/
33 
34 /* Author: Jack Center, Wyatt Rees, Andy Zelenak, Stephanie Eng */
35 
36 #pragma once
37 
38 #include <Eigen/Core>
39 #include <list>
40 #include <unordered_map>
42 #include <ruckig/ruckig.hpp>
43 
44 namespace trajectory_processing
45 {
46 class RuckigSmoothing
47 {
48 public:
57  static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
58  const double max_velocity_scaling_factor = 1.0,
59  const double max_acceleration_scaling_factor = 1.0, const bool mitigate_overshoot = false,
60  const double overshoot_threshold = 0.01);
61 
71  static bool applySmoothing(robot_trajectory::RobotTrajectory& trajectory,
72  const std::unordered_map<std::string, double>& velocity_limits,
73  const std::unordered_map<std::string, double>& acceleration_limits,
74  const std::unordered_map<std::string, double>& jerk_limits,
75  const bool mitigate_overshoot = false, const double overshoot_threshold = 0.01);
76 
77 private:
82  [[nodiscard]] static bool validateGroup(const robot_trajectory::RobotTrajectory& trajectory);
83 
91  [[nodiscard]] static bool getRobotModelBounds(const double max_velocity_scaling_factor,
92  const double max_acceleration_scaling_factor,
93  moveit::core::JointModelGroup const* const group,
94  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);
95 
103  static void getNextRuckigInput(const moveit::core::RobotStateConstPtr& current_waypoint,
104  const moveit::core::RobotStateConstPtr& next_waypoint,
105  const moveit::core::JointModelGroup* joint_group,
106  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);
107 
115  static void initializeRuckigState(const moveit::core::RobotState& first_waypoint,
116  const moveit::core::JointModelGroup* joint_group,
117  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input);
118 
131  static std::optional<robot_trajectory::RobotTrajectory>
133  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input, size_t batch_size = 100);
134 
142  [[nodiscard]] static bool runRuckig(robot_trajectory::RobotTrajectory& trajectory,
143  ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
144  const bool mitigate_overshoot = false, const double overshoot_threshold = 0.01);
145 
155  static void extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints,
156  const size_t num_dof, const std::vector<int>& move_group_idx,
157  const robot_trajectory::RobotTrajectory& original_trajectory,
159 
161  static bool checkOvershoot(ruckig::Trajectory<ruckig::DynamicDOFs, ruckig::StandardVector>& ruckig_trajectory,
162  const size_t num_dof, ruckig::InputParameter<ruckig::DynamicDOFs>& ruckig_input,
163  const double overshoot_threshold);
164 };
165 } // namespace trajectory_processing
moveit::core::JointModelGroup
Definition: joint_model_group.h:134
robot_trajectory.h
moveit::core::RobotState
Representation of a robot's state. This includes position, velocity, acceleration and effort.
Definition: robot_state.h:155
trajectory_processing::RuckigSmoothing::extendTrajectoryDuration
static void extendTrajectoryDuration(const double duration_extension_factor, size_t num_waypoints, const size_t num_dof, const std::vector< int > &move_group_idx, const robot_trajectory::RobotTrajectory &original_trajectory, robot_trajectory::RobotTrajectory &trajectory)
Extend the duration of every trajectory segment.
Definition: ruckig_traj_smoothing.cpp:397
robot_trajectory::RobotTrajectory
Maintain a sequence of waypoints and the time durations between these waypoints.
Definition: robot_trajectory.h:84
trajectory_processing::RuckigSmoothing::runRuckigInBatches
static std::optional< robot_trajectory::RobotTrajectory > runRuckigInBatches(const robot_trajectory::RobotTrajectory &trajectory, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, size_t batch_size=100)
Break the trajectory parameter into batches of reasonable size (~100), run Ruckig on each of them,...
Definition: ruckig_traj_smoothing.cpp:179
trajectory_processing::RuckigSmoothing::initializeRuckigState
static void initializeRuckigState(const moveit::core::RobotState &first_waypoint, const moveit::core::JointModelGroup *joint_group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input)
Initialize Ruckig position/vel/accel. This initializes ruckig_input and ruckig_output to the same val...
Definition: ruckig_traj_smoothing.cpp:423
trajectory_processing
Definition: iterative_spline_parameterization.h:43
trajectory_processing::RuckigSmoothing::applySmoothing
static bool applySmoothing(robot_trajectory::RobotTrajectory &trajectory, const double max_velocity_scaling_factor=1.0, const double max_acceleration_scaling_factor=1.0, const bool mitigate_overshoot=false, const double overshoot_threshold=0.01)
Apply jerk-limited smoothing to a trajectory.
Definition: ruckig_traj_smoothing.cpp:88
trajectory_processing::RuckigSmoothing::getNextRuckigInput
static void getNextRuckigInput(const moveit::core::RobotStateConstPtr &current_waypoint, const moveit::core::RobotStateConstPtr &next_waypoint, const moveit::core::JointModelGroup *joint_group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input)
Feed previous output back as input for next iteration. Get next target state from the next waypoint.
Definition: ruckig_traj_smoothing.cpp:450
trajectory_processing::RuckigSmoothing::validateGroup
static bool validateGroup(const robot_trajectory::RobotTrajectory &trajectory)
A utility function to check if the group is defined.
Definition: ruckig_traj_smoothing.cpp:240
trajectory_processing::RuckigSmoothing::getRobotModelBounds
static bool getRobotModelBounds(const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, moveit::core::JointModelGroup const *const group, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input)
A utility function to get bounds from a JointModelGroup and save them for Ruckig.
Definition: ruckig_traj_smoothing.cpp:251
trajectory_processing::RuckigSmoothing::runRuckig
static bool runRuckig(robot_trajectory::RobotTrajectory &trajectory, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, const bool mitigate_overshoot=false, const double overshoot_threshold=0.01)
A utility function to instantiate and run Ruckig for a series of waypoints.
Definition: ruckig_traj_smoothing.cpp:307
trajectory_processing::RuckigSmoothing::checkOvershoot
static bool checkOvershoot(ruckig::Trajectory< ruckig::DynamicDOFs, ruckig::StandardVector > &ruckig_trajectory, const size_t num_dof, ruckig::InputParameter< ruckig::DynamicDOFs > &ruckig_input, const double overshoot_threshold)
Check if a trajectory out of Ruckig overshoots the target state.
Definition: ruckig_traj_smoothing.cpp:485


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Sun Nov 3 2024 03:26:15