From 9f5cd20763f1edcec0b9a10452438630cb21eb79 Mon Sep 17 00:00:00 2001 From: Sebastian Jahr Date: Fri, 4 Oct 2024 19:57:53 +0200 Subject: [PATCH] TEMP print limits --- src/add_toppra_time_parameterization.cpp | 5 +++++ 1 file changed, 5 insertions(+) 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();