[Python] Add RetimeTrajectory to RobotTrajectory (#2411)

* [Python] Add RetimeTrajectory to RobotTrajectory

* Split retime trajecotry in multiple functions
Moved logic to trajectory_tools
Added Docstrings

* Removed retime function from python binding

---------

Co-authored-by: Sebastian Jahr <sebastian.jahr@picknik.ai>
(cherry picked from commit e7577ca)

moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h

@@ -37,9 +37,46 @@
 #pragma once
 
 #include <moveit_msgs/msg/robot_trajectory.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.h>
 
 namespace trajectory_processing
 {
+/**
+ * \brief Checks if a robot trajectory is empty.
+ * \param [in] trajectory The robot trajectory to check.
+ * \return True if the trajectory is empty, false otherwise.
+ */
 bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory);
+/**
+ * \brief Returns the number of waypoints in a robot trajectory.
+ * \param [in] trajectory The robot trajectory to count waypoints in.
+ * \return The number of waypoints in the trajectory.
+ */
 std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& trajectory);
+/**
+ * \brief Applies time parameterization to a robot trajectory using the Time-Optimal Trajectory Generation (TOTG)
+ * algorithm.
+ * \param [in,out] trajectory The robot trajectory to be time parameterized.
+ * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory.
+ * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory.
+ * \param [in] path_tolerance The path tolerance to use for time parameterization (default: 0.1).
+ * \param [in] resample_dt The time step to use for time parameterization (default: 0.1).
+ * \param [in] min_angle_change The minimum angle change to use for time parameterization (default: 0.001).
+ * \return True if time parameterization was successful, false otherwise.
+ */
+bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
+                                   const double& acceleration_scaling_factor, const double path_tolerance = 0.1,
+                                   const double resample_dt = 0.1, const double min_angle_change = 0.001);
+/**
+ * \brief Applies Ruckig smoothing to a robot trajectory.
+ * \param [in,out] trajectory The robot trajectory to be smoothed.
+ * \param [in] velocity_scaling_factor The factor by which to scale the maximum velocity of the trajectory.
+ * \param [in] acceleration_scaling_factor The factor by which to scale the maximum acceleration of the trajectory.
+ * \param [in] mitigate_overshoot Whether to mitigate overshoot during smoothing (default: false).
+ * \param [in] overshoot_threshold The maximum allowed overshoot during smoothing (default: 0.01).
+ * \return True if smoothing was successful, false otherwise.
+ */
+bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
+                          const double& acceleration_scaling_factor, const bool mitigate_overshoot = false,
+                          const double overshoot_threshold = 0.01);
 }  // namespace trajectory_processing

moveit_core/trajectory_processing/src/trajectory_tools.cpp

@@ -35,7 +35,8 @@
 /* Author: Ioan Sucan */
 
 #include <moveit/trajectory_processing/trajectory_tools.h>
-
+#include <moveit/trajectory_processing/ruckig_traj_smoothing.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 namespace trajectory_processing
 {
 bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory)
@@ -47,4 +48,19 @@ std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& tra
 {
   return std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
 }
+bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
+                                   const double& acceleration_scaling_factor, const double path_tolerance,
+                                   const double resample_dt, const double min_angle_change)
+{
+  TimeOptimalTrajectoryGeneration totg(path_tolerance, resample_dt, min_angle_change);
+  return totg.computeTimeStamps(trajectory, velocity_scaling_factor, acceleration_scaling_factor);
+}
+bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
+                          const double& acceleration_scaling_factor, const bool mitigate_overshoot,
+                          const double overshoot_threshold)
+{
+  RuckigSmoothing time_param;
+  return time_param.applySmoothing(trajectory, velocity_scaling_factor, acceleration_scaling_factor, mitigate_overshoot,
+                                   overshoot_threshold);
+}
 }  // namespace trajectory_processing

moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp

@@ -36,6 +36,7 @@
 
 #include "robot_trajectory.h"
 #include <moveit_py/moveit_py_utils/ros_msg_typecasters.h>
+#include <moveit/trajectory_processing/trajectory_tools.h>
 
 namespace moveit_py
 {
@@ -134,6 +135,35 @@ void init_robot_trajectory(py::module& m)
            Returns:
                list of float: The duration from previous of each waypoint in the trajectory.
            )")
+      .def("apply_totg_time_parameterization", &trajectory_processing::applyTOTGTimeParameterization,
+           py::arg("velocity_scaling_factor"), py::arg("acceleration_scaling_factor"), py::kw_only(),
+           py::arg("path_tolerance") = 0.1, py::arg("resample_dt") = 0.1, py::arg("min_angle_change") = 0.001,
+           R"(
+               Adds time parameterization to the trajectory using the Time-Optimal Trajectory Generation (TOTG) algorithm.
+           Args:
+               velocity_scaling_factor (float): The velocity scaling factor.
+               acceleration_scaling_factor (float): The acceleration scaling factor.
+               path_tolerance (float): The path tolerance to use for time parameterization (default: 0.1).
+               resample_dt (float): The time step to use for time parameterization (default: 0.1).
+               min_angle_change (float): The minimum angle change to use for time parameterization (default: 0.001).
+           )
+           Returns:
+               bool: True if the trajectory was successfully retimed, false otherwise.
+           )")
+      .def("apply_ruckig_smoothing", &trajectory_processing::applyRuckigSmoothing, py::arg("velocity_scaling_factor"),
+           py::arg("acceleration_scaling_factor"), py::kw_only(), py::arg("mitigate_overshoot") = false,
+           py::arg("overshoot_threshold") = 0.01,
+           R"(
+               Applies Ruckig smoothing to the trajectory.
+           Args:
+               velocity_scaling_factor (float): The velocity scaling factor.
+               acceleration_scaling_factor (float): The acceleration scaling factor.
+               mitigate_overshoot (bool): Whether to mitigate overshoot during smoothing (default: false).
+               overshoot_threshold (float): The maximum allowed overshoot during smoothing (default: 0.01
+           )
+           Returns:
+               bool: True if the trajectory was successfully retimed, false otherwise.
+           )")
       .def("get_robot_trajectory_msg", &moveit_py::bind_robot_trajectory::get_robot_trajectory_msg,
            py::arg("joint_filter") = std::vector<std::string>(),
            R"(

moveit_py/src/moveit/moveit_core/robot_trajectory/robot_trajectory.h

@@ -41,6 +41,7 @@
 #include <pybind11/eigen.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <moveit_msgs/msg/robot_trajectory.hpp>
+#include <rclcpp/rclcpp.hpp>
 
 namespace py = pybind11;