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 index 50000bfdda..583fb3799c 100644 --- 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 @@ -77,6 +77,10 @@ class IterativeSplineParameterization : public TimeParameterization bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const override; + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits) 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. 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 index 390476def6..d44b0232cf 100644 --- 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 @@ -52,6 +52,10 @@ class IterativeParabolicTimeParameterization : public TimeParameterization bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const override; + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits) 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 diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h index cbbc6cd739..b8b6d771ab 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h @@ -169,7 +169,15 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const override; + bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits) const; + private: + bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory, + const Eigen::VectorXd& max_velocity, + const Eigen::VectorXd& max_acceleration) const; + const double path_tolerance_; const double resample_dt_; const double min_angle_change_; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h index 3a64960e54..001e8e8543 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h @@ -14,5 +14,8 @@ class TimeParameterization virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, const double max_velocity_scaling_factor = 1.0, const double max_acceleration_scaling_factor = 1.0) const = 0; + virtual bool computeTimeStamps(robot_trajectory::RobotTrajectory& trajectory, + const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits) const = 0; }; } // 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 index 1b1646e6a9..8dd0e6c227 100644 --- a/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp @@ -346,6 +346,14 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT return true; } +bool IterativeSplineParameterization::computeTimeStamps( + robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits) const +{ + RCLCPP_ERROR(LOGGER, "ISTP does not support this version of computeTimeStamps. Try TOTG instead?"); + return false; +} + //////// Internal functions ////////////// /* diff --git a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp index c3c37153ed..8ecfed7fcd 100644 --- a/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp +++ b/moveit_core/trajectory_processing/src/iterative_time_parameterization.cpp @@ -485,4 +485,12 @@ bool IterativeParabolicTimeParameterization::computeTimeStamps(robot_trajectory: updateTrajectory(trajectory, time_diff); return true; } + +bool IterativeParabolicTimeParameterization::computeTimeStamps( + robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits) const +{ + RCLCPP_ERROR(LOGGER, "IPTP does not support this version of computeTimeStamps. Try TOTG instead?"); + return false; +} } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 294f2e94ec..6a91ed382b 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -911,17 +911,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT max_acceleration_scaling_factor, acceleration_scaling_factor); } - // This lib does not actually work properly when angles wrap around, so we need to unwind the path first - trajectory.unwind(); - - // This is pretty much copied from IterativeParabolicTimeParameterization::applyVelocityConstraints const std::vector& vars = group->getVariableNames(); - const std::vector& idx = group->getVariableIndexList(); const moveit::core::RobotModel& rmodel = group->getParentModel(); const unsigned num_joints = group->getVariableCount(); - const unsigned num_points = trajectory.getWayPointCount(); - // Get the limits (we do this at same time, unlike IterativeParabolicTimeParameterization) + // Get the vel/accel limits Eigen::VectorXd max_velocity(num_joints); Eigen::VectorXd max_acceleration(num_joints); for (size_t j = 0; j < num_joints; ++j) @@ -932,7 +926,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT max_velocity[j] = 1.0; if (bounds.velocity_bounded_) { - if (bounds.max_velocity_ < std::numeric_limits::epsilon()) + if (bounds.max_velocity_ <= 0.0) { RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", bounds.max_velocity_, vars[j].c_str()); @@ -951,7 +945,7 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT max_acceleration[j] = 1.0; if (bounds.acceleration_bounded_) { - if (bounds.max_acceleration_ < std::numeric_limits::epsilon()) + if (bounds.max_acceleration_ < 0.0) { RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", bounds.max_acceleration_, vars[j].c_str()); @@ -969,6 +963,117 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } } + return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration); +} + +bool TimeOptimalTrajectoryGeneration::computeTimeStamps( + robot_trajectory::RobotTrajectory& trajectory, const std::unordered_map& velocity_limits, + const std::unordered_map& acceleration_limits) const +{ + if (trajectory.empty()) + return true; + + // Get the default joint limits from the robot model, then overwrite any that are provided as arguments + 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 unsigned num_joints = group->getVariableCount(); + const std::vector& vars = group->getVariableNames(); + const moveit::core::RobotModel& rmodel = group->getParentModel(); + + Eigen::VectorXd max_velocity(num_joints); + Eigen::VectorXd max_acceleration(num_joints); + for (size_t j = 0; j < num_joints; ++j) + { + const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); + + // VELOCITY LIMIT + // Check if a custom limit was specified as an argument + bool set_velocity_limit = false; + auto it = velocity_limits.find(vars[j]); + if (it != velocity_limits.end()) + { + max_velocity[j] = it->second; + set_velocity_limit = true; + } + + if (bounds.velocity_bounded_ && !set_velocity_limit) + { + // Set the default velocity limit, from robot model + if (bounds.max_velocity_ < 0.0) + { + RCLCPP_ERROR(LOGGER, "Invalid max_velocity %f specified for '%s', must be greater than 0.0", + bounds.max_velocity_, vars[j].c_str()); + return false; + } + max_velocity[j] = std::min(std::fabs(bounds.max_velocity_), std::fabs(bounds.min_velocity_)); + set_velocity_limit = true; + } + + if (!set_velocity_limit) + { + max_velocity[j] = 1.0; + RCLCPP_WARN_STREAM_ONCE( + LOGGER, "Joint velocity limits are not defined. Using the default " + << max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml."); + } + + // ACCELERATION LIMIT + // Check if a custom limit was specified as an argument + bool set_acceleration_limit = false; + it = acceleration_limits.find(vars[j]); + if (it != acceleration_limits.end()) + { + max_acceleration[j] = it->second; + set_acceleration_limit = true; + } + + if (bounds.acceleration_bounded_ && !set_acceleration_limit) + { + // Set the default acceleration limit, from robot model + if (bounds.max_acceleration_ < 0.0) + { + RCLCPP_ERROR(LOGGER, "Invalid max_acceleration %f specified for '%s', must be greater than 0.0", + bounds.max_acceleration_, vars[j].c_str()); + return false; + } + max_acceleration[j] = std::min(std::fabs(bounds.max_acceleration_), std::fabs(bounds.min_acceleration_)); + set_acceleration_limit = true; + } + if (!set_acceleration_limit) + { + max_acceleration[j] = 1.0; + RCLCPP_WARN_STREAM_ONCE(LOGGER, + "Joint acceleration limits are not defined. Using the default " + << max_acceleration[j] + << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml."); + } + } + + return doTimeParameterizationCalculations(trajectory, max_velocity, max_acceleration); +} + +bool TimeOptimalTrajectoryGeneration::doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory, + const Eigen::VectorXd& max_velocity, + const Eigen::VectorXd& max_acceleration) const +{ + // This lib does not actually work properly when angles wrap around, so we need to unwind the path first + trajectory.unwind(); + + 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 unsigned num_points = trajectory.getWayPointCount(); + const std::vector& idx = group->getVariableIndexList(); + const unsigned num_joints = group->getVariableCount(); + // Have to convert into Eigen data structs and remove repeated points // (https://github.com/tobiaskunz/trajectories/issues/3) std::list points; diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index 4f2aabf72e..c41b40f294 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -152,6 +152,33 @@ TEST(time_optimal_trajectory_generation, test3) EXPECT_DOUBLE_EQ(90.0, trajectory.getPosition(trajectory.getDuration())[3]); } +// Test the version of computeTimeStamps that takes custom velocity/acceleration limits +TEST(time_optimal_trajectory_generation, test_custom_limits) +{ + constexpr auto robot_name{ "panda" }; + constexpr auto group_name{ "panda_arm" }; + + auto robot_model = moveit::core::loadTestingRobotModel(robot_name); + ASSERT_TRUE((bool)robot_model) << "Failed to load robot model" << robot_name; + auto group = robot_model->getJointModelGroup(group_name); + ASSERT_TRUE((bool)group) << "Failed to load joint model group " << group_name; + moveit::core::RobotState waypoint_state(robot_model); + waypoint_state.setToDefaultValues(); + + const double delta_t = 0.1; + robot_trajectory::RobotTrajectory trajectory(robot_model, group); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.5, -3.52, 1.35, -2.51, -0.88, 0.63, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, delta_t); + waypoint_state.setJointGroupPositions(group, std::vector{ -0.45, -3.2, 1.2, -2.4, -0.8, 0.6, 0.0 }); + trajectory.addSuffixWayPoint(waypoint_state, delta_t); + + TimeOptimalTrajectoryGeneration totg; + // Custom velocity & acceleration limits for some joints + std::unordered_map vel_limits{ { "panda_joint1", 1.3 } }; + std::unordered_map accel_limits{ { "panda_joint2", 2.3 }, { "panda_joint3", 3.3 } }; + ASSERT_TRUE(totg.computeTimeStamps(trajectory, vel_limits, accel_limits)) << "Failed to compute time stamps"; +} + // Test that totg algorithm doesn't give large acceleration TEST(time_optimal_trajectory_generation, testLargeAccel) {