diff --git a/src/add_toppra_time_parameterization.cpp b/src/add_toppra_time_parameterization.cpp index d20f92e..3735d38 100644 --- 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();