diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index 9961101048e..99d42a3b8ad 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 50000bfdda0..00000000000 --- 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 390476def65..00000000000 --- 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 1b1646e6a92..00000000000 --- 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 c3c37153ed1..00000000000 --- 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 70c01d39e21..00000000000 --- 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_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index 27b92d0d1ee..fae5b461827 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 de4df4abd34..00000000000 --- 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 387caafdc3a..00000000000 --- 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)