From 2a10d855b3a722a97258d8d6c6ecd1d525040b9f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 20 Apr 2022 21:04:40 -0500 Subject: [PATCH 1/3] Iterative parabolic parameterization fails for nonzero initial/final conditions --- .../test/test_time_parameterization.cpp | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index f96c7a0e69..7838f9c6c6 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -108,6 +108,33 @@ int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory) return 0; } +// Initialize one-joint, 2-point trajectory with nonzero initial & final vel/accel +int initNonzeroInitialFinalTrajectory(robot_trajectory::RobotTrajectory& trajectory) +{ + const int num_points = 2; + + const moveit::core::JointModelGroup* group = trajectory.getGroup(); + if (!group) + { + RCLCPP_ERROR(LOGGER, "Need to set the group"); + return -1; + } + + const std::vector& idx = group->getVariableIndexList(); + moveit::core::RobotState state(trajectory.getRobotModel()); + + trajectory.clear(); + for (unsigned i = 0; i < num_points; ++i) + { + state.setVariablePosition(idx[0], 0.0); + state.setVariableVelocity(idx[0], 0.1); + state.setVariableAcceleration(idx[0], 0.2); + trajectory.addSuffixWayPoint(state, 0.0); + } + + return 0; +} + void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) { const moveit::core::JointModelGroup* group = trajectory.getGroup(); @@ -164,6 +191,14 @@ TEST(TestTimeParameterization, TestRepeatedPoint) ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 0.001); } +TEST(TestTimeParameterization, NonzeroInitialVelAccel) +{ + trajectory_processing::IterativeParabolicTimeParameterization time_parameterization; + EXPECT_EQ(initNonzeroInitialFinalTrajectory(TRAJECTORY), 0); + EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); + printTrajectory(TRAJECTORY); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); From 1e86575c268faa93e269083cecceb1063d24614f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 20 Apr 2022 21:16:43 -0500 Subject: [PATCH 2/3] Iterative spline parameterization fails, too --- .../test/test_time_parameterization.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp index 7838f9c6c6..70c01d39e2 100644 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -126,9 +126,9 @@ int initNonzeroInitialFinalTrajectory(robot_trajectory::RobotTrajectory& traject trajectory.clear(); for (unsigned i = 0; i < num_points; ++i) { - state.setVariablePosition(idx[0], 0.0); - state.setVariableVelocity(idx[0], 0.1); - state.setVariableAcceleration(idx[0], 0.2); + state.setVariablePosition(idx[0], 0.01 * i); + state.setVariableVelocity(idx[0], 0.1 * i); + state.setVariableAcceleration(idx[0], 0.2 * i); trajectory.addSuffixWayPoint(state, 0.0); } @@ -193,7 +193,7 @@ TEST(TestTimeParameterization, TestRepeatedPoint) TEST(TestTimeParameterization, NonzeroInitialVelAccel) { - trajectory_processing::IterativeParabolicTimeParameterization time_parameterization; + trajectory_processing::IterativeSplineParameterization time_parameterization(true); EXPECT_EQ(initNonzeroInitialFinalTrajectory(TRAJECTORY), 0); EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); printTrajectory(TRAJECTORY); From d46389a67ee8ef8045a2a58b47f68732b308f28f Mon Sep 17 00:00:00 2001 From: AndyZe Date: Wed, 20 Apr 2022 22:24:00 -0500 Subject: [PATCH 3/3] Delete Iterative Spline & Iterative Parabola algorithms --- .../trajectory_processing/CMakeLists.txt | 6 - .../iterative_spline_parameterization.h | 84 --- .../iterative_time_parameterization.h | 68 -- .../src/iterative_spline_parameterization.cpp | 600 ------------------ .../src/iterative_time_parameterization.cpp | 488 -------------- .../test/test_time_parameterization.cpp | 206 ------ .../src/chomp_planning_context.cpp | 1 - .../src/chomp_optimizer_adapter.cpp | 1 - .../cartesian_path_service_capability.cpp | 4 +- .../CMakeLists.txt | 2 - .../add_iterative_spline_parameterization.cpp | 87 --- .../src/add_time_parameterization.cpp | 86 --- .../src/motion_planning_frame_planning.cpp | 8 +- 13 files changed, 6 insertions(+), 1635 deletions(-) delete mode 100644 moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h delete mode 100644 moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h delete mode 100644 moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp delete mode 100644 moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp delete mode 100644 moveit_core/trajectory_processing/test/test_time_parameterization.cpp delete mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp delete mode 100644 moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index 9961101048..99d42a3b8a 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -1,8 +1,6 @@ set(MOVEIT_LIB_NAME moveit_trajectory_processing) add_library(${MOVEIT_LIB_NAME} SHARED - src/iterative_time_parameterization.cpp - src/iterative_spline_parameterization.cpp src/ruckig_traj_smoothing.cpp src/trajectory_tools.cpp src/time_optimal_trajectory_generation.cpp @@ -34,10 +32,6 @@ if(BUILD_TESTING) set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_trajectory;${CMAKE_CURRENT_BINARY_DIR}/../utils") endif() - ament_add_gtest(test_time_parameterization test/test_time_parameterization.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - target_link_libraries(test_time_parameterization moveit_test_utils ${MOVEIT_LIB_NAME}) - ament_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp) target_link_libraries(test_time_optimal_trajectory_generation moveit_test_utils ${MOVEIT_LIB_NAME}) diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h deleted file mode 100644 index 50000bfdda..0000000000 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h +++ /dev/null @@ -1,84 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * Copyright (c) 2017, Ken Anderson - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ken Anderson */ - -#pragma once - -#include -#include "rclcpp/rclcpp.hpp" -#include - -namespace trajectory_processing -{ -/// \brief This class sets the timestamps of a trajectory -/// to enforce velocity, acceleration constraints. -/// Initial/final velocities and accelerations may be specified in the trajectory. -/// Velocity and acceleration limits are specified in the model. -/// -/// This algorithm repeatedly fits a cubic spline, adjusts the timing intervals, -/// and repeats until all constraints are satisfied. -/// When finished, each trajectory waypoint will have the time set, -/// as well as the velocities and accelerations for each joint. -/// Since we fit to a cubic spline, the position, velocity, and -/// acceleration will be continuous and within bounds. -/// The jerk will be discontinuous. -/// -/// To match the velocity and acceleration at the endpoints, -/// the second and second-last point locations need to move. -/// By default, two extra points are added to leave the original trajectory unaffected. -/// If points are not added, the trajectory could potentially be faster, -/// but the 2nd and 2nd-last points should be re-checked for collisions. -/// -/// Migration notes: If migrating from Iterative Parabolic Time Parameterization, -/// be aware that the velocity and acceleration limits are more strictly enforced -/// using this technique. -/// This means that time-parameterizing the same trajectory with the same -/// velocity and acceleration limits, will result in a longer trajectory. -/// If this is a problem, try retuning (increasing) the limits. -/// -class IterativeSplineParameterization : public TimeParameterization -{ -public: - IterativeSplineParameterization(bool add_points = true); - - bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const override; - -private: - bool add_points_; /// @brief If true, add two points to trajectory (first and last segments). - /// If false, move the 2nd and 2nd-last points. -}; -} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h deleted file mode 100644 index 390476def6..0000000000 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h +++ /dev/null @@ -1,68 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2011, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ken Anderson */ - -#pragma once - -#include -#include "rclcpp/rclcpp.hpp" -#include - -namespace trajectory_processing -{ -/// \brief This class modifies the timestamps of a trajectory to respect -/// velocity and acceleration constraints. -class IterativeParabolicTimeParameterization : public TimeParameterization -{ -public: - IterativeParabolicTimeParameterization(unsigned int max_iterations = 100, double max_time_change_per_it = .01); - - bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, - const double max_acceleration_scaling_factor = 1.0) const override; - -private: - unsigned int max_iterations_; /// @brief maximum number of iterations to find solution - double max_time_change_per_it_; /// @brief maximum allowed time change per iteration in seconds - - void applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory, std::vector& time_diff, - const double max_velocity_scaling_factor) const; - - void applyAccelerationConstraints(robot_trajectory::RobotTrajectory& rob_trajectory, std::vector& time_diff, - const double max_acceleration_scaling_factor) const; - - double findT1(const double d1, const double d2, double t1, const double t2, const double a_max) const; - double findT2(const double d1, const double d2, const double t1, double t2, const double a_max) const; -}; -} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp deleted file mode 100644 index 1b1646e6a9..0000000000 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ /dev/null @@ -1,600 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Ken Anderson - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ken Anderson */ - -#include -#include -#include - -static const double VLIMIT = 1.0; // default if not specified in model -static const double ALIMIT = 1.0; // default if not specified in model - -namespace trajectory_processing -{ -static const rclcpp::Logger LOGGER = - rclcpp::get_logger("moveit_trajectory_processing.iterative_spline_parameterization"); - -static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[]); -static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[], - const double x2_i, const double x2_f); -static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity); -static double global_adjustment_factor(const int n, double x1[], double x2[], const double max_velocity, - const double min_velocity, const double max_acceleration, - const double min_acceleration); - -// The path of a single joint: positions, velocities, and accelerations -struct SingleJointTrajectory -{ - std::vector positions_; // joint's position at time[x] - std::vector velocities_; - std::vector accelerations_; - double initial_acceleration_; - double final_acceleration_; - double min_velocity_; - double max_velocity_; - double min_acceleration_; - double max_acceleration_; -}; - -void globalAdjustment(std::vector& t2, robot_trajectory::RobotTrajectory& trajectory, - std::vector& time_diff); - -IterativeSplineParameterization::IterativeSplineParameterization(bool add_points) : add_points_(add_points) -{ -} - -bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, - const double max_velocity_scaling_factor, - const double max_acceleration_scaling_factor) const -{ - if (trajectory.empty()) - return true; - - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - if (!group) - { - RCLCPP_ERROR(LOGGER, "It looks like the planner did not set " - "the group the plan was computed for"); - return false; - } - const moveit::core::RobotModel& rmodel = group->getParentModel(); - const std::vector& idx = group->getVariableIndexList(); - const std::vector& vars = group->getVariableNames(); - double velocity_scaling_factor = 1.0; - double acceleration_scaling_factor = 1.0; - unsigned int num_points = trajectory.getWayPointCount(); - unsigned int num_joints = group->getVariableCount(); - - // Set scaling factors - if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0) - velocity_scaling_factor = max_velocity_scaling_factor; - else if (max_velocity_scaling_factor == 0.0) - { - RCLCPP_DEBUG(LOGGER, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.", - velocity_scaling_factor); - } - else - { - RCLCPP_WARN(LOGGER, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.", - max_velocity_scaling_factor, velocity_scaling_factor); - } - if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0) - acceleration_scaling_factor = max_acceleration_scaling_factor; - else if (max_acceleration_scaling_factor == 0.0) - { - RCLCPP_DEBUG(LOGGER, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.", - acceleration_scaling_factor); - } - else - { - RCLCPP_WARN(LOGGER, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.", - max_acceleration_scaling_factor, acceleration_scaling_factor); - } - // No wrapped angles. - trajectory.unwind(); - - if (add_points_) - { - // Insert 2nd and 2nd-last points - // (required to force acceleration to specified values at endpoints) - if (trajectory.getWayPointCount() >= 2) - { - moveit::core::RobotState point = trajectory.getWayPoint(1); - moveit::core::RobotStatePtr p0, p1; - - // 2nd point is 90% of p0, and 10% of p1 - p0 = trajectory.getWayPointPtr(0); - p1 = trajectory.getWayPointPtr(1); - for (unsigned int j = 0; j < num_joints; ++j) - { - point.setVariablePosition(idx[j], - (9 * p0->getVariablePosition(idx[j]) + 1 * p1->getVariablePosition(idx[j])) / 10); - } - trajectory.insertWayPoint(1, point, 0.0); - num_points++; - - // 2nd-last point is 10% of p0, and 90% of p1 - p0 = trajectory.getWayPointPtr(num_points - 2); - p1 = trajectory.getWayPointPtr(num_points - 1); - for (unsigned int j = 0; j < num_joints; ++j) - { - point.setVariablePosition(idx[j], - (1 * p0->getVariablePosition(idx[j]) + 9 * p1->getVariablePosition(idx[j])) / 10); - } - trajectory.insertWayPoint(num_points - 1, point, 0.0); - num_points++; - } - } - - // JointTrajectory indexes in [point][joint] order. - // We need [joint][point] order to solve efficiently, - // so convert form here. - - std::vector t2(num_joints); - - for (unsigned int j = 0; j < num_joints; ++j) - { - // Copy positions - t2[j].positions_.resize(num_points, 0.0); - for (unsigned int i = 0; i < num_points; ++i) - { - t2[j].positions_[i] = trajectory.getWayPointPtr(i)->getVariablePosition(idx[j]); - } - - // Initialize velocities - t2[j].velocities_.resize(num_points, 0.0); - // Copy initial/final velocities if specified - if (trajectory.getWayPointPtr(0)->hasVelocities()) - t2[j].velocities_[0] = trajectory.getWayPointPtr(0)->getVariableVelocity(idx[j]); - if (trajectory.getWayPointPtr(num_points - 1)->hasVelocities()) - t2[j].velocities_[num_points - 1] = trajectory.getWayPointPtr(num_points - 1)->getVariableVelocity(idx[j]); - - // Initialize accelerations - t2[j].accelerations_.resize(num_points, 0.0); - t2[j].initial_acceleration_ = 0.0; - t2[j].final_acceleration_ = 0.0; - // Copy initial/final accelerations if specified - if (trajectory.getWayPointPtr(0)->hasAccelerations()) - t2[j].initial_acceleration_ = trajectory.getWayPointPtr(0)->getVariableAcceleration(idx[j]); - t2[j].accelerations_[0] = t2[j].initial_acceleration_; - if (trajectory.getWayPointPtr(num_points - 1)->hasAccelerations()) - t2[j].final_acceleration_ = trajectory.getWayPointPtr(num_points - 1)->getVariableAcceleration(idx[j]); - t2[j].accelerations_[num_points - 1] = t2[j].final_acceleration_; - - // Set bounds based on model, or default limits - const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); - t2[j].max_velocity_ = VLIMIT; - t2[j].min_velocity_ = -VLIMIT; - if (bounds.velocity_bounded_) - { - t2[j].max_velocity_ = bounds.max_velocity_; - t2[j].min_velocity_ = bounds.min_velocity_; - if (t2[j].min_velocity_ == 0.0) - t2[j].min_velocity_ = -t2[j].max_velocity_; - } - t2[j].max_velocity_ *= velocity_scaling_factor; - t2[j].min_velocity_ *= velocity_scaling_factor; - - t2[j].max_acceleration_ = ALIMIT; - t2[j].min_acceleration_ = -ALIMIT; - if (bounds.acceleration_bounded_) - { - t2[j].max_acceleration_ = bounds.max_acceleration_; - t2[j].min_acceleration_ = bounds.min_acceleration_; - if (t2[j].min_acceleration_ == 0.0) - t2[j].min_acceleration_ = -t2[j].max_acceleration_; - } - t2[j].max_acceleration_ *= acceleration_scaling_factor; - t2[j].min_acceleration_ *= acceleration_scaling_factor; - - // Error out if bounds don't make sense - if (t2[j].max_velocity_ <= 0.0 || t2[j].max_acceleration_ <= 0.0) - { - RCLCPP_ERROR(LOGGER, - "Joint %d max velocity %f and max acceleration %f must be greater than zero " - "or a solution won't be found.\n", - j, t2[j].max_velocity_, t2[j].max_acceleration_); - return false; - } - if (t2[j].min_velocity_ >= 0.0 || t2[j].min_acceleration_ >= 0.0) - { - RCLCPP_ERROR(LOGGER, - "Joint %d min velocity %f and min acceleration %f must be less than zero " - "or a solution won't be found.\n", - j, t2[j].min_velocity_, t2[j].min_acceleration_); - return false; - } - } - - // Error check - if (num_points < 4) - { - RCLCPP_ERROR(LOGGER, "number of waypoints %d, needs to be greater than 3.\n", num_points); - return false; - } - for (unsigned int j = 0; j < num_joints; ++j) - { - if (t2[j].velocities_[0] > t2[j].max_velocity_ || t2[j].velocities_[0] < t2[j].min_velocity_) - { - RCLCPP_ERROR(LOGGER, "Initial velocity %f out of bounds\n", t2[j].velocities_[0]); - return false; - } - else if (t2[j].velocities_[num_points - 1] > t2[j].max_velocity_ || - t2[j].velocities_[num_points - 1] < t2[j].min_velocity_) - { - RCLCPP_ERROR(LOGGER, "Final velocity %f out of bounds\n", t2[j].velocities_[num_points - 1]); - return false; - } - else if (t2[j].accelerations_[0] > t2[j].max_acceleration_ || t2[j].accelerations_[0] < t2[j].min_acceleration_) - { - RCLCPP_ERROR(LOGGER, "Initial acceleration %f out of bounds\n", t2[j].accelerations_[0]); - return false; - } - else if (t2[j].accelerations_[num_points - 1] > t2[j].max_acceleration_ || - t2[j].accelerations_[num_points - 1] < t2[j].min_acceleration_) - { - RCLCPP_ERROR(LOGGER, "Final acceleration %f out of bounds\n", t2[j].accelerations_[num_points - 1]); - return false; - } - } - - // Initialize times - // start with valid velocities, then expand intervals - // epsilon to prevent divide-by-zero - std::vector time_diff(trajectory.getWayPointCount() - 1, std::numeric_limits::epsilon()); - for (unsigned int j = 0; j < num_joints; ++j) - init_times(num_points, &time_diff[0], &t2[j].positions_[0], t2[j].max_velocity_, t2[j].min_velocity_); - - // Stretch intervals until close to the bounds - while (1) - { - int loop = 0; - - // Calculate the interval stretches due to acceleration - std::vector time_factor(num_points - 1, 1.00); - for (unsigned j = 0; j < num_joints; ++j) - { - // Move points to satisfy initial/final acceleration - if (add_points_) - { - adjust_two_positions(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], - &t2[j].accelerations_[0], t2[j].initial_acceleration_, t2[j].final_acceleration_); - } - - fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]); - for (unsigned i = 0; i < num_points; ++i) - { - const double acc = t2[j].accelerations_[i]; - double atfactor = 1.0; - if (acc > t2[j].max_acceleration_) - atfactor = sqrt(acc / t2[j].max_acceleration_); - if (acc < t2[j].min_acceleration_) - atfactor = sqrt(acc / t2[j].min_acceleration_); - if (atfactor > 1.01) // within 1% - loop = 1; - atfactor = (atfactor - 1.0) / 16.0 + 1.0; // 1/16th - if (i > 0) - time_factor[i - 1] = std::max(time_factor[i - 1], atfactor); - if (i < num_points - 1) - time_factor[i] = std::max(time_factor[i], atfactor); - } - } - - if (loop == 0) - break; // finished - - // Stretch - for (unsigned i = 0; i < num_points - 1; ++i) - time_diff[i] *= time_factor[i]; - } - - // Final adjustment forces the trajectory within bounds - globalAdjustment(t2, trajectory, time_diff); - - // Convert back to JointTrajectory form - for (unsigned int i = 1; i < num_points; ++i) - trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]); - for (unsigned int i = 0; i < num_points; ++i) - { - for (unsigned int j = 0; j < num_joints; ++j) - { - trajectory.getWayPointPtr(i)->setVariableVelocity(idx[j], t2[j].velocities_[i]); - trajectory.getWayPointPtr(i)->setVariableAcceleration(idx[j], t2[j].accelerations_[i]); - } - - // Only update position of additionally inserted points (at second and next-to-last position) - if (add_points_ && (i == 1 || i == num_points - 2)) - { - for (unsigned int j = 0; j < num_joints; ++j) - trajectory.getWayPointPtr(i)->setVariablePosition(idx[j], t2[j].positions_[i]); - trajectory.getWayPointPtr(i)->update(); - } - } - - return true; -} - -//////// Internal functions ////////////// - -/* - Fit a 'clamped' cubic spline over a series of points. - A cubic spline ensures continuous function across positions, - 1st derivative (velocities), and 2nd derivative (accelerations). - 'Clamped' means the first derivative at the endpoints is specified. - - Fitting a cubic spline involves solving a series of linear equations. - The general form for each segment is: - (tj-t_(j-1))*x"_(j-1) + 2*(t_(j+1)-t_(j-1))*x"j + (t_(j+1)-tj)*x"_j+1) = - (x_(j+1)-xj)/(t_(j+1)-tj) - (xj-x_(j-1))/(tj-t_(j-1)) - And the first and last segment equations are clamped to specified values: x1_i and x1_f. - - Represented in matrix form: - [ 2*(t1-t0) (t1-t0) 0 ][x0"] [(x1-x0)/(t1-t0) - t1_i ] - [ t1-t0 2*(t2-t0) t2-t1 ][x1"] [(x2-x1)/(t2-t1) - (x1-x0)/(t1-t0)] - [ t2-t1 2*(t3-t1) t3-t2 ][x2"] = 6 * [(x3-x2)/(t3/t2) - (x2-x1)/(t2-t1)] - [ ... ... ... ][...] [... ] - [ 0 tN-t_(N-1) 2*(tN-t_(N-1)) ][xN"] [t1_f - (xN-x_(N-1))/(tN-t_(N-1)) ] - - This matrix is tridiagonal, which can be solved solved in O(N) time - using the tridiagonal algorithm. - There is a forward propagation pass followed by a backsubstitution pass. - - n is the number of points - dt contains the time difference between each point (size=n-1) - x contains the positions (size=n) - x1 contains the 1st derivative (velocities) (size=n) - x1[0] and x1[n-1] MUST be specified. - x2 contains the 2nd derivative (accelerations) (size=n) - x1 and x2 are filled in by the algorithm. -*/ - -static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[]) -{ - int i; - const double x1_i = x1[0], x1_f = x1[n - 1]; - - // Tridiagonal alg - forward sweep - // x1 and x2 used to store the temporary coefficients c and d - // (will get overwritten during backsubstitution) - double *c = x1, *d = x2; - c[0] = 0.5; - d[0] = 3.0 * ((x[1] - x[0]) / dt[0] - x1_i) / dt[0]; - for (i = 1; i <= n - 2; ++i) - { - const double dt2 = dt[i - 1] + dt[i]; - const double a = dt[i - 1] / dt2; - const double denom = 2.0 - a * c[i - 1]; - c[i] = (1.0 - a) / denom; - d[i] = 6.0 * ((x[i + 1] - x[i]) / dt[i] - (x[i] - x[i - 1]) / dt[i - 1]) / dt2; - d[i] = (d[i] - a * d[i - 1]) / denom; - } - const double denom = dt[n - 2] * (2.0 - c[n - 2]); - d[n - 1] = 6.0 * (x1_f - (x[n - 1] - x[n - 2]) / dt[n - 2]); - d[n - 1] = (d[n - 1] - dt[n - 2] * d[n - 2]) / denom; - - // Tridiagonal alg - backsubstitution sweep - // 2nd derivative - x2[n - 1] = d[n - 1]; - for (i = n - 2; i >= 0; i--) - x2[i] = d[i] - c[i] * x2[i + 1]; - - // 1st derivative - x1[0] = x1_i; - for (i = 1; i < n - 1; ++i) - x1[i] = (x[i + 1] - x[i]) / dt[i] - (2 * x2[i] + x2[i + 1]) * dt[i] / 6.0; - x1[n - 1] = x1_f; -} - -/* - Modify the value of x[1] and x[N-2] - so that 2nd derivative starts and ends at specified value. - This involves fitting the spline twice, - then solving for the specified value. - - x2_i and x2_f are the (initial and final) 2nd derivative at 0 and N-1 -*/ - -static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[], - const double x2_i, const double x2_f) -{ - x[1] = x[0]; - x[n - 2] = x[n - 3]; - fit_cubic_spline(n, dt, x, x1, x2); - double a0 = x2[0]; - double b0 = x2[n - 1]; - - x[1] = x[2]; - x[n - 2] = x[n - 1]; - fit_cubic_spline(n, dt, x, x1, x2); - double a2 = x2[0]; - double b2 = x2[n - 1]; - - // we can solve this with linear equation (use two-point form) - if (a2 != a0) - x[1] = x[0] + ((x[2] - x[0]) / (a2 - a0)) * (x2_i - a0); - if (b2 != b0) - x[n - 2] = x[n - 3] + ((x[n - 1] - x[n - 3]) / (b2 - b0)) * (x2_f - b0); -} - -/* - Find time required to go max velocity on each segment. - Increase a segment's time interval if the current time isn't long enough. -*/ - -static void init_times(const int n, double dt[], const double x[], const double max_velocity, const double min_velocity) -{ - int i; - for (i = 0; i < n - 1; ++i) - { - double time; - double dx = x[i + 1] - x[i]; - if (dx >= 0.0) - time = (dx / max_velocity); - else - time = (dx / min_velocity); - time += std::numeric_limits::epsilon(); // prevent divide-by-zero - - if (dt[i] < time) - dt[i] = time; - } -} - -#if 0 // unused function -/* - Fit a spline, then check each interval to see if bounds are met. - If all bounds met (no time adjustments made), return 0. - If bounds not met (time adjustments made), slightly increase the - surrounding time intervals and return 1. - - n is the number of points - dt contains the time difference between each point (size=n-1) - x contains the positions (size=n) - x1 contains the 1st derivative (velocities) (size=n) - x1[0] and x1[n-1] MUST be specified. - x2 contains the 2nd derivative (accelerations) (size=n) - max_velocity is the max velocity for this joint. - min_velocity is the min velocity for this joint. - max_acceleration is the max acceleration for this joint. - min_acceleration is the min acceleration for this joint. - tfactor is the time adjustment (multiplication) factor. - x1 and x2 are filled in by the algorithm. -*/ - -static int fit_spline_and_adjust_times(const int n, double dt[], const double x[], double x1[], double x2[], - const double max_velocity, const double min_velocity, - const double max_acceleration, const double min_acceleration, - const double tfactor) -{ - int i, ret = 0; - - fit_cubic_spline(n, dt, x, x1, x2); - - // Instantaneous velocity is calculated at each point - for (i = 0; i < n - 1; ++i) - { - const double vel = x1[i]; - const double vel2 = x1[i + 1]; - if (vel > max_velocity || vel < min_velocity || vel2 > max_velocity || vel2 < min_velocity) - { - dt[i] *= tfactor; - ret = 1; - } - } - // Instantaneous acceleration is calculated at each point - if (ret == 0) - { - for (i = 0; i < n - 1; ++i) - { - const double acc = x2[i]; - const double acc2 = x2[i + 1]; - if (acc > max_acceleration || acc < min_acceleration || acc2 > max_acceleration || acc2 < min_acceleration) - { - dt[i] *= tfactor; - ret = 1; - } - } - } - - return ret; -} -#endif - -// return global expansion multiplicative factor required -// to force within bounds. -// Assumes that the spline is already fit -// (fit_cubic_spline must have been called before this). -static double global_adjustment_factor(const int n, double x1[], double x2[], const double max_velocity, - const double min_velocity, const double max_acceleration, - const double min_acceleration) -{ - int i; - double tfactor2 = 1.00; - - for (i = 0; i < n; ++i) - { - double tfactor; - tfactor = x1[i] / max_velocity; - if (tfactor2 < tfactor) - tfactor2 = tfactor; - tfactor = x1[i] / min_velocity; - if (tfactor2 < tfactor) - tfactor2 = tfactor; - - if (x2[i] >= 0) - { - tfactor = sqrt(fabs(x2[i] / max_acceleration)); - if (tfactor2 < tfactor) - tfactor2 = tfactor; - } - else - { - tfactor = sqrt(fabs(x2[i] / min_acceleration)); - if (tfactor2 < tfactor) - tfactor2 = tfactor; - } - } - - return tfactor2; -} - -// Expands the entire trajectory to fit exactly within bounds -void globalAdjustment(std::vector& t2, robot_trajectory::RobotTrajectory& trajectory, - std::vector& time_diff) -{ - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - - const unsigned int num_points = trajectory.getWayPointCount(); - const unsigned int num_joints = group->getVariableCount(); - - double gtfactor = 1.0; - for (unsigned int j = 0; j < num_joints; ++j) - { - double tfactor; - tfactor = global_adjustment_factor(num_points, &t2[j].velocities_[0], &t2[j].accelerations_[0], t2[j].max_velocity_, - t2[j].min_velocity_, t2[j].max_acceleration_, t2[j].min_acceleration_); - if (tfactor > gtfactor) - gtfactor = tfactor; - } - - // printf("# Global adjustment: %0.4f%%\n", 100.0 * (gtfactor - 1.0)); - for (unsigned int i = 0; i < num_points - 1; ++i) - time_diff[i] *= gtfactor; - - for (unsigned int j = 0; j < num_joints; ++j) - { - fit_cubic_spline(num_points, &time_diff[0], &t2[j].positions_[0], &t2[j].velocities_[0], &t2[j].accelerations_[0]); - } -} -} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp deleted file mode 100644 index c3c37153ed..0000000000 --- a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp +++ /dev/null @@ -1,488 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2009, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ken Anderson */ - -#include -#include -#include - -namespace trajectory_processing -{ -static const double DEFAULT_VEL_MAX = 1.0; -static const double DEFAULT_ACCEL_MAX = 1.0; -static const double ROUNDING_THRESHOLD = 0.01; - -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.iterative_time_parameterization"); - -IterativeParabolicTimeParameterization::IterativeParabolicTimeParameterization(unsigned int max_iterations, - double max_time_change_per_it) - : max_iterations_(max_iterations), max_time_change_per_it_(max_time_change_per_it) -{ -} - -#if 0 // unused functions -namespace -{ -void printPoint(const trajectory_msgs::msg::JointTrajectoryPoint& point, std::size_t i) -{ - RCLCPP_DEBUG(LOGGER, " time [%zu]= %f", i, point.time_from_start.sec + point.time_from_start.nanosec / 1e9); - if (point.positions.size() >= 7) - { - RCLCPP_DEBUG(LOGGER, " pos_ [%zu]= %f %f %f %f %f %f %f", i, point.positions[0], point.positions[1], - point.positions[2], point.positions[3], point.positions[4], point.positions[5], point.positions[6]); - } - if (point.velocities.size() >= 7) - { - RCLCPP_DEBUG(LOGGER, " vel_ [%zu]= %f %f %f %f %f %f %f", i, point.velocities[0], point.velocities[1], - point.velocities[2], point.velocities[3], point.velocities[4], point.velocities[5], - point.velocities[6]); - } - if (point.accelerations.size() >= 7) - { - RCLCPP_DEBUG(LOGGER, " acc_ [%zu]= %f %f %f %f %f %f %f", i, point.accelerations[0], point.accelerations[1], - point.accelerations[2], point.accelerations[3], point.accelerations[4], point.accelerations[5], - point.accelerations[6]); - } -} - -void printStats(const trajectory_msgs::msg::JointTrajectory& trajectory, - const std::vector& limits) -{ - RCLCPP_DEBUG(LOGGER, "jointNames= %s %s %s %s %s %s %s", limits[0].joint_name.c_str(), limits[1].joint_name.c_str(), - limits[2].joint_name.c_str(), limits[3].joint_name.c_str(), limits[4].joint_name.c_str(), - limits[5].joint_name.c_str(), limits[6].joint_name.c_str()); - RCLCPP_DEBUG(LOGGER, "maxVelocities= %f %f %f %f %f %f %f", limits[0].max_velocity, limits[1].max_velocity, - limits[2].max_velocity, limits[3].max_velocity, limits[4].max_velocity, limits[5].max_velocity, - limits[6].max_velocity); - RCLCPP_DEBUG(LOGGER, "maxAccelerations= %f %f %f %f %f %f %f", limits[0].max_acceleration, limits[1].max_acceleration, - limits[2].max_acceleration, limits[3].max_acceleration, limits[4].max_acceleration, - limits[5].max_acceleration, limits[6].max_acceleration); - // for every point in time: - for (std::size_t i = 0; i < trajectory.points.size(); ++i) - printPoint(trajectory.points[i], i); -} -} // namespace -#endif - -// Applies velocity -void IterativeParabolicTimeParameterization::applyVelocityConstraints(robot_trajectory::RobotTrajectory& rob_trajectory, - std::vector& time_diff, - const double max_velocity_scaling_factor) const -{ - const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); - const std::vector& vars = group->getVariableNames(); - const std::vector& idx = group->getVariableIndexList(); - const moveit::core::RobotModel& rmodel = group->getParentModel(); - const int num_points = rob_trajectory.getWayPointCount(); - - double velocity_scaling_factor = 1.0; - - if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0) - velocity_scaling_factor = max_velocity_scaling_factor; - else if (max_velocity_scaling_factor == 0.0) - { - RCLCPP_DEBUG(LOGGER, "A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.", - velocity_scaling_factor); - } - else - { - RCLCPP_WARN(LOGGER, "Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.", - max_velocity_scaling_factor, velocity_scaling_factor); - } - for (int i = 0; i < num_points - 1; ++i) - { - const moveit::core::RobotStatePtr& curr_waypoint = rob_trajectory.getWayPointPtr(i); - const moveit::core::RobotStatePtr& next_waypoint = rob_trajectory.getWayPointPtr(i + 1); - - for (std::size_t j = 0; j < vars.size(); ++j) - { - double v_max = DEFAULT_VEL_MAX; - const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]); - if (b.velocity_bounded_) - v_max = - std::min(fabs(b.max_velocity_ * velocity_scaling_factor), fabs(b.min_velocity_ * velocity_scaling_factor)); - const double dq1 = curr_waypoint->getVariablePosition(idx[j]); - const double dq2 = next_waypoint->getVariablePosition(idx[j]); - const double t_min = std::abs(dq2 - dq1) / v_max; - if (t_min > time_diff[i]) - time_diff[i] = t_min; - } - } -} - -// Iteratively expand dt1 interval by a constant factor until within acceleration constraint -// In the future we may want to solve to quadratic equation to get the exact timing interval. -// To do this, use the CubicTrajectory::quadSolve() function in cubic_trajectory.h -double IterativeParabolicTimeParameterization::findT1(const double dq1, const double dq2, double dt1, const double dt2, - const double a_max) const -{ - const double mult_factor = 1.01; - double v1 = (dq1) / dt1; - double v2 = (dq2) / dt2; - double a = 2.0 * (v2 - v1) / (dt1 + dt2); - - while (std::abs(a) > a_max) - { - v1 = (dq1) / dt1; - v2 = (dq2) / dt2; - a = 2.0 * (v2 - v1) / (dt1 + dt2); - dt1 *= mult_factor; - } - - return dt1; -} - -double IterativeParabolicTimeParameterization::findT2(const double dq1, const double dq2, const double dt1, double dt2, - const double a_max) const -{ - const double mult_factor = 1.01; - double v1 = (dq1) / dt1; - double v2 = (dq2) / dt2; - double a = 2.0 * (v2 - v1) / (dt1 + dt2); - - while (std::abs(a) > a_max) - { - v1 = (dq1) / dt1; - v2 = (dq2) / dt2; - a = 2.0 * (v2 - v1) / (dt1 + dt2); - dt2 *= mult_factor; - } - - return dt2; -} - -namespace -{ -// Takes the time differences, and updates the timestamps, velocities and accelerations -// in the trajectory. -void updateTrajectory(robot_trajectory::RobotTrajectory& rob_trajectory, const std::vector& time_diff) -{ - // Error check - if (time_diff.empty()) - return; - - double time_sum = 0.0; - - moveit::core::RobotStatePtr prev_waypoint; - moveit::core::RobotStatePtr curr_waypoint; - moveit::core::RobotStatePtr next_waypoint; - - const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); - const std::vector& vars = group->getVariableNames(); - const std::vector& idx = group->getVariableIndexList(); - - int num_points = rob_trajectory.getWayPointCount(); - - rob_trajectory.setWayPointDurationFromPrevious(0, time_sum); - - // Times - for (int i = 1; i < num_points; ++i) - // Update the time between the waypoints in the robot_trajectory. - rob_trajectory.setWayPointDurationFromPrevious(i, time_diff[i - 1]); - - // Return if there is only one point in the trajectory! - if (num_points <= 1) - return; - - // Accelerations - for (int i = 0; i < num_points; ++i) - { - curr_waypoint = rob_trajectory.getWayPointPtr(i); - - if (i > 0) - prev_waypoint = rob_trajectory.getWayPointPtr(i - 1); - - if (i < num_points - 1) - next_waypoint = rob_trajectory.getWayPointPtr(i + 1); - - for (std::size_t j = 0; j < vars.size(); ++j) - { - double q1; - double q2; - double q3; - double dt1; - double dt2; - - if (i == 0) - { - // First point - q1 = next_waypoint->getVariablePosition(idx[j]); - q2 = curr_waypoint->getVariablePosition(idx[j]); - q3 = q1; - - dt1 = dt2 = time_diff[i]; - } - else if (i < num_points - 1) - { - // middle points - q1 = prev_waypoint->getVariablePosition(idx[j]); - q2 = curr_waypoint->getVariablePosition(idx[j]); - q3 = next_waypoint->getVariablePosition(idx[j]); - - dt1 = time_diff[i - 1]; - dt2 = time_diff[i]; - } - else - { - // last point - q1 = prev_waypoint->getVariablePosition(idx[j]); - q2 = curr_waypoint->getVariablePosition(idx[j]); - q3 = q1; - - dt1 = dt2 = time_diff[i - 1]; - } - - double v1, v2, a; - - bool start_velocity = false; - if (dt1 == 0.0 || dt2 == 0.0) - { - v1 = 0.0; - v2 = 0.0; - a = 0.0; - } - else - { - if (i == 0) - { - if (curr_waypoint->hasVelocities()) - { - start_velocity = true; - v1 = curr_waypoint->getVariableVelocity(idx[j]); - } - } - v1 = start_velocity ? v1 : (q2 - q1) / dt1; - // v2 = (q3-q2)/dt2; - v2 = start_velocity ? v1 : (q3 - q2) / dt2; // Needed to ensure continuous velocity for first point - a = 2.0 * (v2 - v1) / (dt1 + dt2); - } - - curr_waypoint->setVariableVelocity(idx[j], (v2 + v1) / 2.0); - curr_waypoint->setVariableAcceleration(idx[j], a); - } - } -} -} // namespace - -// Applies Acceleration constraints -void IterativeParabolicTimeParameterization::applyAccelerationConstraints( - robot_trajectory::RobotTrajectory& rob_trajectory, std::vector& time_diff, - const double max_acceleration_scaling_factor) const -{ - moveit::core::RobotStatePtr prev_waypoint; - moveit::core::RobotStatePtr curr_waypoint; - moveit::core::RobotStatePtr next_waypoint; - - const moveit::core::JointModelGroup* group = rob_trajectory.getGroup(); - const std::vector& vars = group->getVariableNames(); - const std::vector& idx = group->getVariableIndexList(); - const moveit::core::RobotModel& rmodel = group->getParentModel(); - - const int num_points = rob_trajectory.getWayPointCount(); - const unsigned int num_joints = group->getVariableCount(); - int num_updates = 0; - int iteration = 0; - bool backwards = false; - double q1; - double q2; - double q3; - double dt1; - double dt2; - double v1; - double v2; - double a; - - double acceleration_scaling_factor = 1.0; - - if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0) - acceleration_scaling_factor = max_acceleration_scaling_factor; - else if (max_acceleration_scaling_factor == 0.0) - { - RCLCPP_DEBUG(LOGGER, "A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.", - acceleration_scaling_factor); - } - else - { - RCLCPP_WARN(LOGGER, "Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.", - max_acceleration_scaling_factor, acceleration_scaling_factor); - } - do - { - num_updates = 0; - iteration++; - - // In this case we iterate through the joints on the outer loop. - // This is so that any time interval increases have a chance to get propagated through the trajectory - for (unsigned int j = 0; j < num_joints; ++j) - { - // Loop forwards, then backwards - for (int count = 0; count < 2; ++count) - { - for (int i = 0; i < num_points - 1; ++i) - { - int index = backwards ? (num_points - 1) - i : i; - - curr_waypoint = rob_trajectory.getWayPointPtr(index); - - if (index > 0) - prev_waypoint = rob_trajectory.getWayPointPtr(index - 1); - - if (index < num_points - 1) - next_waypoint = rob_trajectory.getWayPointPtr(index + 1); - - // Get acceleration limits - double a_max = DEFAULT_ACCEL_MAX; - const moveit::core::VariableBounds& b = rmodel.getVariableBounds(vars[j]); - if (b.acceleration_bounded_) - a_max = std::min(fabs(b.max_acceleration_ * acceleration_scaling_factor), - fabs(b.min_acceleration_ * acceleration_scaling_factor)); - - if (index == 0) - { - // First point - q1 = next_waypoint->getVariablePosition(idx[j]); - q2 = curr_waypoint->getVariablePosition(idx[j]); - q3 = next_waypoint->getVariablePosition(idx[j]); - - dt1 = dt2 = time_diff[index]; - assert(!backwards); - } - else if (index < num_points - 1) - { - // middle points - q1 = prev_waypoint->getVariablePosition(idx[j]); - q2 = curr_waypoint->getVariablePosition(idx[j]); - q3 = next_waypoint->getVariablePosition(idx[j]); - - dt1 = time_diff[index - 1]; - dt2 = time_diff[index]; - } - else - { - // last point - careful, there are only numpoints-1 time intervals - q1 = prev_waypoint->getVariablePosition(idx[j]); - q2 = curr_waypoint->getVariablePosition(idx[j]); - q3 = prev_waypoint->getVariablePosition(idx[j]); - - dt1 = dt2 = time_diff[index - 1]; - assert(backwards); - } - - if (dt1 == 0.0 || dt2 == 0.0) - { - v1 = 0.0; - v2 = 0.0; - a = 0.0; - } - else - { - bool start_velocity = false; - if (index == 0) - { - if (curr_waypoint->hasVelocities()) - { - start_velocity = true; - v1 = curr_waypoint->getVariableVelocity(idx[j]); - } - } - v1 = start_velocity ? v1 : (q2 - q1) / dt1; - v2 = (q3 - q2) / dt2; - a = 2.0 * (v2 - v1) / (dt1 + dt2); - } - - if (fabs(a) > a_max + ROUNDING_THRESHOLD) - { - if (!backwards) - { - dt2 = std::min(dt2 + max_time_change_per_it_, findT2(q2 - q1, q3 - q2, dt1, dt2, a_max)); - time_diff[index] = dt2; - } - else - { - dt1 = std::min(dt1 + max_time_change_per_it_, findT1(q2 - q1, q3 - q2, dt1, dt2, a_max)); - time_diff[index - 1] = dt1; - } - num_updates++; - - if (dt1 == 0.0 || dt2 == 0.0) - { - v1 = 0.0; - v2 = 0.0; - a = 0.0; - } - else - { - v1 = (q2 - q1) / dt1; - v2 = (q3 - q2) / dt2; - a = 2 * (v2 - v1) / (dt1 + dt2); - } - } - } - backwards = !backwards; - } - } - // RCLCPP_DEBUG(LOGGER, "applyAcceleration: num_updates=%i", - // num_updates); - } while (num_updates > 0 && iteration < static_cast(max_iterations_)); -} - -bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, - const double max_velocity_scaling_factor, - const double max_acceleration_scaling_factor) const -{ - if (trajectory.empty()) - return true; - - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - if (!group) - { - RCLCPP_ERROR(LOGGER, "It looks like the planner did not set " - "the group the plan was computed for"); - return false; - } - - // this lib does not actually work properly when angles wrap around, so we need to unwind the path first - trajectory.unwind(); - - const int num_points = trajectory.getWayPointCount(); - std::vector time_diff(num_points - 1, 0.0); // the time difference between adjacent points - - applyVelocityConstraints(trajectory, time_diff, max_velocity_scaling_factor); - applyAccelerationConstraints(trajectory, time_diff, max_acceleration_scaling_factor); - - updateTrajectory(trajectory, time_diff); - return true; -} -} // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp deleted file mode 100644 index 70c01d39e2..0000000000 --- a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2017, Ken Anderson - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ken Anderson */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include "rclcpp/rclcpp.hpp" - -// Logger -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_trajectory_processing.test.test_time_parameterization"); - -// Static variables used in all tests -moveit::core::RobotModelConstPtr RMODEL = moveit::core::loadTestingRobotModel("pr2"); -robot_trajectory::RobotTrajectory TRAJECTORY(RMODEL, "right_arm"); - -// Initialize one-joint, 3 points exactly the same. -int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory& trajectory) -{ - const int num = 3; - unsigned i; - - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - if (!group) - { - RCLCPP_ERROR(LOGGER, "Need to set the group"); - return -1; - } - // leave initial velocity/acceleration unset - const std::vector& idx = group->getVariableIndexList(); - moveit::core::RobotState state(trajectory.getRobotModel()); - - trajectory.clear(); - for (i = 0; i < num; ++i) - { - state.setVariablePosition(idx[0], 1.0); - trajectory.addSuffixWayPoint(state, 0.0); - } - - return 0; -} - -// Initialize one-joint, straight-line trajectory -int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory) -{ - const int num = 10; - const double max = 2.0; - unsigned i; - - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - if (!group) - { - RCLCPP_ERROR(LOGGER, "Need to set the group"); - return -1; - } - // leave initial velocity/acceleration unset - const std::vector& idx = group->getVariableIndexList(); - moveit::core::RobotState state(trajectory.getRobotModel()); - - trajectory.clear(); - for (i = 0; i < num; ++i) - { - state.setVariablePosition(idx[0], i * max / num); - trajectory.addSuffixWayPoint(state, 0.0); - } - - // leave final velocity/acceleration unset - state.setVariablePosition(idx[0], max); - trajectory.addSuffixWayPoint(state, 0.0); - - return 0; -} - -// Initialize one-joint, 2-point trajectory with nonzero initial & final vel/accel -int initNonzeroInitialFinalTrajectory(robot_trajectory::RobotTrajectory& trajectory) -{ - const int num_points = 2; - - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - if (!group) - { - RCLCPP_ERROR(LOGGER, "Need to set the group"); - return -1; - } - - const std::vector& idx = group->getVariableIndexList(); - moveit::core::RobotState state(trajectory.getRobotModel()); - - trajectory.clear(); - for (unsigned i = 0; i < num_points; ++i) - { - state.setVariablePosition(idx[0], 0.01 * i); - state.setVariableVelocity(idx[0], 0.1 * i); - state.setVariableAcceleration(idx[0], 0.2 * i); - trajectory.addSuffixWayPoint(state, 0.0); - } - - return 0; -} - -void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) -{ - const moveit::core::JointModelGroup* group = trajectory.getGroup(); - const std::vector& idx = group->getVariableIndexList(); - trajectory.print(std::cout, { idx[0] }); -} - -TEST(TestTimeParameterization, TestIterativeParabolic) -{ - trajectory_processing::IterativeParabolicTimeParameterization time_parameterization; - EXPECT_EQ(initStraightTrajectory(TRAJECTORY), 0); - - auto wt = std::chrono::system_clock::now(); - EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); - - std::cout << "IterativeParabolicTimeParameterization took " << (std::chrono::system_clock::now() - wt).count() - << '\n'; - printTrajectory(TRAJECTORY); - ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 3.0); -} - -TEST(TestTimeParameterization, TestIterativeSpline) -{ - trajectory_processing::IterativeSplineParameterization time_parameterization(false); - EXPECT_EQ(initStraightTrajectory(TRAJECTORY), 0); - - auto wt = std::chrono::system_clock::now(); - EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); - std::cout << "IterativeSplineParameterization took " << (std::chrono::system_clock::now() - wt).count() << '\n'; - printTrajectory(TRAJECTORY); - ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); -} - -TEST(TestTimeParameterization, TestIterativeSplineAddPoints) -{ - trajectory_processing::IterativeSplineParameterization time_parameterization(true); - EXPECT_EQ(initStraightTrajectory(TRAJECTORY), 0); - - auto wt = std::chrono::system_clock::now(); - EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); - std::cout << "IterativeSplineParameterization with added points took " - << (std::chrono::system_clock::now() - wt).count() << '\n'; - printTrajectory(TRAJECTORY); - ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 5.0); -} - -TEST(TestTimeParameterization, TestRepeatedPoint) -{ - trajectory_processing::IterativeSplineParameterization time_parameterization(true); - EXPECT_EQ(initRepeatedPointTrajectory(TRAJECTORY), 0); - - EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); - printTrajectory(TRAJECTORY); - ASSERT_LT(TRAJECTORY.getWayPointDurationFromStart(TRAJECTORY.getWayPointCount() - 1), 0.001); -} - -TEST(TestTimeParameterization, NonzeroInitialVelAccel) -{ - trajectory_processing::IterativeSplineParameterization time_parameterization(true); - EXPECT_EQ(initNonzeroInitialFinalTrajectory(TRAJECTORY), 0); - EXPECT_TRUE(time_parameterization.computeTimeStamps(TRAJECTORY)); - printTrajectory(TRAJECTORY); -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp index 65b7040743..6a2d2f2eb9 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_planning_context.cpp @@ -36,7 +36,6 @@ #include #include -#include #include "rclcpp/rclcpp.hpp" diff --git a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp index e9378a6a2e..ed18a603ba 100644 --- a/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp +++ b/moveit_planners/chomp/chomp_optimizer_adapter/src/chomp_optimizer_adapter.cpp @@ -40,7 +40,6 @@ #include #include #include -#include #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 6e24559bfc..88459ebace 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -49,7 +49,7 @@ #include #include #include -#include +#include namespace { @@ -174,7 +174,7 @@ bool MoveGroupCartesianPathService::computeService(const std::shared_ptrsolution); diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index 27b92d0d1e..fae5b46182 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -6,9 +6,7 @@ set(SOURCE_FILES src/fix_start_state_collision.cpp src/fix_start_state_path_constraints.cpp src/fix_workspace_bounds.cpp - src/add_time_parameterization.cpp src/add_ruckig_traj_smoothing.cpp - src/add_iterative_spline_parameterization.cpp src/add_time_optimal_parameterization.cpp src/resolve_constraint_frames.cpp ) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp deleted file mode 100644 index de4df4abd3..0000000000 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp +++ /dev/null @@ -1,87 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * Copyright (c) 2017, Ken Anderson - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ken Anderson, based off add_time_parameterization.cpp by Ioan Sucan */ - -#include -#include -#include - -namespace default_planner_request_adapters -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_iterative_spline_parameterization"); - -class AddIterativeSplineParameterization : public planning_request_adapter::PlanningRequestAdapter -{ -public: - AddIterativeSplineParameterization() : planning_request_adapter::PlanningRequestAdapter() - { - } - - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override - { - } - - std::string getDescription() const override - { - return "Add Time Parameterization"; - } - - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override - { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) - { - RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); - if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, - req.max_acceleration_scaling_factor)) - { - RCLCPP_WARN(LOGGER, "Time parametrization for the solution path failed."); - result = false; - } - } - - return result; - } - -private: - trajectory_processing::IterativeSplineParameterization time_param_; -}; -} // namespace default_planner_request_adapters - -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddIterativeSplineParameterization, - planning_request_adapter::PlanningRequestAdapter) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp deleted file mode 100644 index 387caafdc3..0000000000 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/add_time_parameterization.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Ioan Sucan */ - -#include -#include -#include - -namespace default_planner_request_adapters -{ -static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_ros.add_time_parameterization"); - -class AddTimeParameterization : public planning_request_adapter::PlanningRequestAdapter -{ -public: - AddTimeParameterization() : planning_request_adapter::PlanningRequestAdapter() - { - } - - void initialize(const rclcpp::Node::SharedPtr& /* node */, const std::string& /* parameter_namespace */) override - { - } - - std::string getDescription() const override - { - return "Add Time Parameterization"; - } - - bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res, - std::vector& /*added_path_index*/) const override - { - bool result = planner(planning_scene, req, res); - if (result && res.trajectory_) - { - RCLCPP_DEBUG(LOGGER, "Running '%s'", getDescription().c_str()); - if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, - req.max_acceleration_scaling_factor)) - { - RCLCPP_WARN(LOGGER, "Time parametrization for the solution path failed."); - result = false; - } - } - - return result; - } - -private: - trajectory_processing::IterativeParabolicTimeParameterization time_param_; -}; -} // namespace default_planner_request_adapters - -CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddTimeParameterization, - planning_request_adapter::PlanningRequestAdapter) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp index 19ef6e2fc4..e97f5ded53 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_planning.cpp @@ -48,7 +48,7 @@ #else #include #endif -#include +#include #include "ui_motion_planning_rviz_plugin_frame.h" @@ -156,9 +156,9 @@ bool MotionPlanningFrame::computeCartesianPlan() // https://groups.google.com/forum/#!topic/moveit-users/MOoFxy2exT4 robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName()); rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(), trajectory); - trajectory_processing::IterativeParabolicTimeParameterization iptp; - bool success = - iptp.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(), ui_->acceleration_scaling_factor->value()); + trajectory_processing::TimeOptimalTrajectoryGeneration time_parameterization; + bool success = time_parameterization.computeTimeStamps(rt, ui_->velocity_scaling_factor->value(), + ui_->acceleration_scaling_factor->value()); RCLCPP_INFO(LOGGER, "Computing time stamps %s", success ? "SUCCEEDED" : "FAILED"); // Store trajectory in current_plan_