diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index dffacbcaa0..17bbd33841 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -2,6 +2,7 @@ set(MOVEIT_LIB_NAME moveit_trajectory_processing) add_library(${MOVEIT_LIB_NAME} src/iterative_time_parameterization.cpp + src/iterative_spline_parameterization.cpp src/trajectory_tools.cpp ) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION 0.7.3) @@ -14,3 +15,10 @@ install(TARGETS ${MOVEIT_LIB_NAME} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) + +if(CATKIN_ENABLE_TESTING) + find_package(moveit_resources REQUIRED) + include_directories(${moveit_resources_INCLUDE_DIRS}) + catkin_add_gtest(test_time_parameterization test/test_time_parameterization.cpp) + target_link_libraries(test_time_parameterization ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME}) +endif() 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 new file mode 100644 index 0000000000..c3cb24cb12 --- /dev/null +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h @@ -0,0 +1,89 @@ +/********************************************************************* + * 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 */ + +#ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__ +#define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__ + +#include +#include +#include +#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: + IterativeSplineParameterization(bool add_points = true); + ~IterativeSplineParameterization(); + + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, + const double max_acceleration_scaling_factor = 1.0) const; + +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. +}; +} + +#endif diff --git a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp new file mode 100644 index 0000000000..3b70cf1c3c --- /dev/null +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -0,0 +1,587 @@ +/********************************************************************* + * 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 + +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 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 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); +static double global_adjustment_factor(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); + +// 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, int num_joints, const int num_points, + std::vector& time_diff); + +IterativeSplineParameterization::IterativeSplineParameterization(bool add_points) : add_points_(add_points) +{ +} + +IterativeSplineParameterization::~IterativeSplineParameterization() +{ +} + +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 robot_model::JointModelGroup* group = trajectory.getGroup(); + if (!group) + { + logError("It looks like the planner did not set the group the plan was computed for"); + return false; + } + const robot_model::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) + logDebug("A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.", velocity_scaling_factor); + else + logWarn("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) + logDebug("A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.", + acceleration_scaling_factor); + else + logWarn("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) + { + robot_state::RobotState point = trajectory.getWayPoint(1); + robot_state::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 robot_model::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) + { + logError("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) + { + logError("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) + { + logError("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) + { + logError("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) + { + logError("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) + { + logError("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) + { + logError("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, num_joints, num_points, 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)->setVariablePosition(idx[j], t2[j].positions[i]); + trajectory.getWayPointPtr(i)->setVariableVelocity(idx[j], t2[j].velocities[i]); + trajectory.getWayPointPtr(i)->setVariableAcceleration(idx[j], t2[j].accelerations[i]); + } + } + + 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 propogation 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; + } +} + +/* + 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; +} + +// 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 dt[], const double x[], 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; + + // fit_cubic_spline(n, dt, x, x1, x2); + + 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, int num_joints, const int num_points, + std::vector& time_diff) +{ + double gtfactor = 1.0; + for (int j = 0; j < num_joints; j++) + { + double tfactor; + tfactor = global_adjustment_factor(num_points, &time_diff[0], &t2[j].positions[0], &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 (int i = 0; i < num_points - 1; i++) + time_diff[i] *= gtfactor; + + for (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]); + } +} +} diff --git a/moveit_core/trajectory_processing/test/test_time_parameterization.cpp b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp new file mode 100644 index 0000000000..2e77e3630f --- /dev/null +++ b/moveit_core/trajectory_processing/test/test_time_parameterization.cpp @@ -0,0 +1,219 @@ +/********************************************************************* + * 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 +#include + +// Function declarations +moveit::core::RobotModelConstPtr loadModel(); + +// Static variables used in all tests +moveit::core::RobotModelConstPtr rmodel = loadModel(); +robot_trajectory::RobotTrajectory trajectory(rmodel, "right_arm"); + +// Load pr2. Take a look at test/ in planning_scene, robot_mode, +// and robot_state for inspiration. +moveit::core::RobotModelConstPtr loadModel() +{ + moveit::core::RobotModelConstPtr robot_model; + urdf::ModelInterfaceSharedPtr urdf_model; + srdf::ModelSharedPtr srdf_model; + boost::filesystem::path res_path(MOVEIT_TEST_RESOURCES_DIR); + std::string xml_string; + std::fstream xml_file((res_path / "pr2_description/urdf/robot.xml").string().c_str(), std::fstream::in); + EXPECT_TRUE(xml_file.is_open()); + while (xml_file.good()) + { + std::string line; + std::getline(xml_file, line); + xml_string += (line + "\n"); + } + xml_file.close(); + urdf_model = urdf::parseURDF(xml_string); + srdf_model.reset(new srdf::Model()); + srdf_model->initFile(*urdf_model, (res_path / "pr2_description/srdf/robot.xml").string()); + robot_model.reset(new moveit::core::RobotModel(urdf_model, srdf_model)); + return robot_model; +} + +// Initialize one-joint, 3 points exactly the same. +int initRepeatedPointTrajectory(robot_trajectory::RobotTrajectory& trajectory) +{ + const int num = 3; + unsigned i; + + const robot_model::JointModelGroup* group = trajectory.getGroup(); + if (!group) + { + logError("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 +// Can specify init/final velocity/acceleration, +// but not all time parameterization methods may accept it. +int initStraightTrajectory(robot_trajectory::RobotTrajectory& trajectory, double vel_i = 0.0, double vel_f = 0.0, + double acc_i = 0.0, double acc_f = 0.0) +{ + const int num = 10; + const double max = 2.0; + unsigned i; + + const robot_model::JointModelGroup* group = trajectory.getGroup(); + if (!group) + { + logError("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; +} + +void printTrajectory(robot_trajectory::RobotTrajectory& trajectory) +{ + const robot_model::JointModelGroup* group = trajectory.getGroup(); + const std::vector& idx = group->getVariableIndexList(); + unsigned int count = trajectory.getWayPointCount(); + + std::cout << "trajectory length is " << trajectory.getWayPointDurationFromStart(count - 1) << " seconds." + << std::endl; + std::cout << " Trajectory Points" << std::endl; + for (unsigned i = 0; i < count; i++) + { + robot_state::RobotStatePtr point = trajectory.getWayPointPtr(i); + printf(" waypoint %2d time %6.2f pos %6.2f vel %6.2f acc %6.2f ", i, trajectory.getWayPointDurationFromStart(i), + point->getVariablePosition(idx[0]), point->getVariableVelocity(idx[0]), + point->getVariableAcceleration(idx[0])); + if (i > 0) + { + robot_state::RobotStatePtr prev = trajectory.getWayPointPtr(i - 1); + printf("jrk %6.2f", + (point->getVariableAcceleration(idx[0]) - prev->getVariableAcceleration(idx[0])) / + (trajectory.getWayPointDurationFromStart(i) - trajectory.getWayPointDurationFromStart(i - 1))); + } + printf("\n"); + } +} + +TEST(TestTimeParameterization, TestIterativeParabolic) +{ + trajectory_processing::IterativeParabolicTimeParameterization time_parameterization; + EXPECT_EQ(initStraightTrajectory(trajectory), 0); + + ros::WallTime wt = ros::WallTime::now(); + EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); + std::cout << "IterativeParabolicTimeParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl; + 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); + + ros::WallTime wt = ros::WallTime::now(); + EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); + std::cout << "IterativeSplineParameterization took " << (ros::WallTime::now() - wt).toSec() << std::endl; + 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); + + ros::WallTime wt = ros::WallTime::now(); + EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); + std::cout << "IterativeSplineParameterization with added points took " << (ros::WallTime::now() - wt).toSec() + << std::endl; + 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); + + ros::WallTime wt = ros::WallTime::now(); + EXPECT_TRUE(time_parameterization.computeTimeStamps(trajectory)); + // std::cout << " took " << (ros::WallTime::now() - wt).toSec() << std::endl; + printTrajectory(trajectory); + ASSERT_LT(trajectory.getWayPointDurationFromStart(trajectory.getWayPointCount() - 1), 0.001); +} + +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 4e083c1943..1f240937ec 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -6,7 +6,8 @@ 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_time_parameterization.cpp + src/add_iterative_spline_parameterization.cpp) add_library(${MOVEIT_LIB_NAME} ${SOURCE_FILES}) set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION 0.7.3) 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 new file mode 100644 index 0000000000..0707c5b055 --- /dev/null +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/add_iterative_spline_parameterization.cpp @@ -0,0 +1,80 @@ +/********************************************************************* + * 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 +#include + +namespace default_planner_request_adapters +{ +class AddIterativeSplineParameterization : public planning_request_adapter::PlanningRequestAdapter +{ +public: + AddIterativeSplineParameterization() : planning_request_adapter::PlanningRequestAdapter() + { + } + + virtual std::string getDescription() const + { + return "Add Time Parameterization"; + } + + virtual 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 + { + bool result = planner(planning_scene, req, res); + if (result && res.trajectory_) + { + ROS_DEBUG("Running '%s'", getDescription().c_str()); + if (!time_param_.computeTimeStamps(*res.trajectory_, req.max_velocity_scaling_factor, + req.max_acceleration_scaling_factor)) + ROS_WARN("Time parametrization for the solution path failed."); + } + + return result; + } + +private: + trajectory_processing::IterativeSplineParameterization time_param_; +}; +} + +CLASS_LOADER_REGISTER_CLASS(default_planner_request_adapters::AddIterativeSplineParameterization, + planning_request_adapter::PlanningRequestAdapter); diff --git a/moveit_ros/planning/planning_request_adapters_plugin_description.xml b/moveit_ros/planning/planning_request_adapters_plugin_description.xml index c267bcf473..bcc78ef618 100644 --- a/moveit_ros/planning/planning_request_adapters_plugin_description.xml +++ b/moveit_ros/planning/planning_request_adapters_plugin_description.xml @@ -30,4 +30,9 @@ + + + + +