Iterating on computeTimeStampsWithTorqueLimits()

moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h

@@ -246,8 +246,10 @@ class TimeOptimalTrajectoryGeneration : public TimeParameterization
                                          const geometry_msgs::Vector3& gravity_vector,
                                          const std::vector<double>& joint_torque_limits,
                                          double accel_limit_decrement_factor,
-                                         const double max_velocity_scaling_factor = 1.0,
-                                         const double max_acceleration_scaling_factor = 1.0) const;
+                                         const std::unordered_map<std::string, double>& velocity_limits,
+                                         std::unordered_map<std::string, double>& acceleration_limits,
+                                         const double max_velocity_scaling_factor,
+                                         const double max_acceleration_scaling_factor) const;
 
 private:
   bool doTimeParameterizationCalculations(robot_trajectory::RobotTrajectory& trajectory,

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -1051,7 +1051,9 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(
 bool TimeOptimalTrajectoryGeneration::computeTimeStampsWithTorqueLimits(
     robot_trajectory::RobotTrajectory& trajectory, const geometry_msgs::Vector3& gravity_vector,
     const std::vector<double>& joint_torque_limits, double accel_limit_decrement_factor,
-    const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor) const
+    const std::unordered_map<std::string, double>& velocity_limits,
+    std::unordered_map<std::string, double>& acceleration_limits, const double max_velocity_scaling_factor,
+    const double max_acceleration_scaling_factor) const
 {
   // 1. Call computeTimeStamps() to time-parameterize the trajectory with given vel/accel limits.
   // 2. Run forward dynamics to check if torque limits are violated at any waypoint.
@@ -1089,17 +1091,26 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStampsWithTorqueLimits(
 
   // Create a copy of the trajectory that is not modified while iterating
   robot_trajectory::RobotTrajectory original_trajectory(trajectory, true /* deep copy */);
+  bool iteration_needed = true;
+  size_t num_iterations = 0;
+  const size_t max_iterations = 10;
 
-  while (ros::ok())
+  while (ros::ok() && iteration_needed && num_iterations < max_iterations)
   {
+    ++num_iterations;
+    iteration_needed = false;
+
     trajectory = robot_trajectory::RobotTrajectory(original_trajectory, true /* deep copy */);
-    computeTimeStamps(trajectory, max_velocity_scaling_factor, max_acceleration_scaling_factor);
+    computeTimeStamps(trajectory, velocity_limits, acceleration_limits, max_velocity_scaling_factor,
+                      max_acceleration_scaling_factor);
 
     std::vector<double> joint_positions(dof);
     std::vector<double> joint_velocities(dof);
     std::vector<double> joint_accelerations(dof);
     std::vector<double> joint_torques(dof);
 
+    const std::vector<const moveit::core::JointModel*>& joint_models = group->getActiveJointModels();
+
     // Check if any torque limits are violated
     for (size_t waypoint_idx = 0; waypoint_idx < trajectory.getWayPointCount(); ++waypoint_idx)
     {
@@ -1121,11 +1132,15 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStampsWithTorqueLimits(
       {
         if (std::fabs(joint_torques.at(joint_idx)) > joint_torque_limits.at(joint_idx))
         {
-          ;
+          // There are some edge cases where decreasing acceleration could actually increase joint torque. For example,
+          // if gravity is accelerating the joint. In that case, the joint would be fighting against gravity more.
+          // TODO(andyz): Check if decreasing acceleration of this joint actually decreases joint torque. Check KDL API.
+          acceleration_limits.at(joint_models.at(joint_idx)->getName()) *= accel_limit_decrement_factor;
+          iteration_needed = true;
         }
-      }
-    }
-  }  // while (ros::ok())
+      }  // for each joint
+    }    // for each waypoint
+  }      // while (ros::ok())
 
   return true;
 }