removed minimum max vel/accel for joints in totg, added parameterizat…

…ion failure if limits less than/equal to zero
moveit · Nov 2, 2021 · 934894d · 934894d
1 parent 03f85d5
commit 934894d
Showing 1 changed file with 10 additions and 2 deletions.
diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp
@@ -932,17 +932,25 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT
     max_velocity[j] = 1.0;
     if (bounds.velocity_bounded_)
     {
+      if (bounds.max_velocity_ < std::numeric_limits<double>::epsilon())
+      {
+        ROS_ERROR_NAMED(LOGNAME, "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_)) * velocity_scaling_factor;
-      max_velocity[j] = std::max(0.001, max_velocity[j]);
     }
 
     max_acceleration[j] = 1.0;
     if (bounds.acceleration_bounded_)
     {
+      if (bounds.max_acceleration_ < std::numeric_limits<double>::epsilon())
+      {
+        ROS_ERROR_NAMED(LOGNAME, "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_)) *
                             acceleration_scaling_factor;
-      max_acceleration[j] = std::max(0.001, max_acceleration[j]);
     }
   }