TEMP print limits

moveit · Oct 4, 2024 · 9f5cd20 · 9f5cd20
1 parent ad8da70
commit 9f5cd20
Showing 1 changed file with 5 additions and 0 deletions.
diff --git a/src/add_toppra_time_parameterization.cpp b/src/add_toppra_time_parameterization.cpp
@@ -179,6 +179,11 @@ class AddToppraTimeParameterization : public planning_interface::PlanningRespons
     getVelocityBounds(joint_model_group, plant, lower_velocity_limits, upper_velocity_limits);
     getAccelerationBounds(joint_model_group, plant, lower_acceleration_limits, upper_acceleration_limits);
 
+    RCLCPP_INFO_STREAM(getLogger(), "Lower velocity limits: " << lower_velocity_limits.transpose());
+    RCLCPP_INFO_STREAM(getLogger(), "Upper velocity limits: " << upper_velocity_limits.transpose());
+    RCLCPP_INFO_STREAM(getLogger(), "Lower acceleration limits: " << lower_acceleration_limits.transpose());
+    RCLCPP_INFO_STREAM(getLogger(), "Upper acceleration limits: " << upper_acceleration_limits.transpose());
+
     toppra.AddJointVelocityLimit(lower_velocity_limits, upper_velocity_limits);
     toppra.AddJointAccelerationLimit(lower_acceleration_limits, upper_acceleration_limits);
     auto optimized_trajectory = toppra.SolvePathParameterization();