wrap_python_move_group.cpp, move_group.py: add algorithm option to re…

…time_trajectory in pyhon interface of move_group.
moveit · Jun 19, 2019 · 39f8d8a · 39f8d8a
1 parent 11f239f
commit 39f8d8a
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 5 deletions.
diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py
@@ -578,10 +578,10 @@ def set_support_surface_name(self, value):
         """ Set the support surface name for a place operation """
         self._g.set_support_surface_name(value)
 
-    def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0):
+    def retime_trajectory(self, ref_state_in, traj_in, velocity_scaling_factor=1.0, acceleration_scaling_factor=1.0, algorithm=""):
         ser_ref_state_in = conversions.msg_to_string(ref_state_in)
         ser_traj_in = conversions.msg_to_string(traj_in)
-        ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor, acceleration_scaling_factor)
+        ser_traj_out = self._g.retime_trajectory(ser_ref_state_in, ser_traj_in, velocity_scaling_factor, acceleration_scaling_factor, algorithm)
         traj_out = RobotTrajectory()
         traj_out.deserialize(ser_traj_out)
         return traj_out
diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp
@@ -41,6 +41,8 @@
 #include <moveit/robot_state/conversions.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <moveit/trajectory_processing/iterative_time_parameterization.h>
+#include <moveit/trajectory_processing/iterative_spline_parameterization.h>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.h>
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2/LinearMath/Quaternion.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
@@ -489,7 +491,8 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
   }
 
   std::string retimeTrajectory(const std::string& ref_state_str, const std::string& traj_str,
-                               double velocity_scaling_factor, double acceleration_scaling_factor)
+                               double velocity_scaling_factor, double acceleration_scaling_factor,
+                               std::string algorithm)
   {
     // Convert reference state message to object
     moveit_msgs::RobotState ref_state_msg;
@@ -504,8 +507,26 @@ class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer
       traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg);
 
       // Do the actual retiming
-      trajectory_processing::IterativeParabolicTimeParameterization time_param;
-      time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor);
+      if (algorithm == "" || algorithm == "iterative_time_parameterization")
+      {
+        trajectory_processing::IterativeParabolicTimeParameterization time_param;
+        time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor);
+      }
+      else if (algorithm == "iterative_spline_parameterization")
+      {
+        trajectory_processing::IterativeSplineParameterization time_param;
+        time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor);
+      }
+      else if (algorithm == "time_optimal_trajectory_generation")
+      {
+        trajectory_processing::TimeOptimalTrajectoryGeneration time_param;
+        time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor);
+      }
+      else
+      {
+        ROS_ERROR("Unknown algorithm: %s", algorithm.c_str());
+        return "";
+      }
 
       // Convert the retimed trajectory back into a message
       traj_obj.getRobotTrajectoryMsg(traj_msg);