From 227a579b408a6e1c05d0b4076bdb70adb77ee173 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 9 Dec 2022 12:14:00 -0600 Subject: [PATCH] Require vel/accel limits for TOTG --- .../time_optimal_trajectory_generation.cpp | 35 +++-- ...est_time_optimal_trajectory_generation.cpp | 142 ++++++------------ .../utils/src/robot_model_test_utils.cpp | 6 +- 3 files changed, 67 insertions(+), 116 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 index be6249411e..e605d46bb7 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -924,7 +924,6 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT const moveit::core::VariableBounds& bounds = rmodel.getVariableBounds(vars[j]); // Limits need to be non-zero, otherwise we never exit - max_velocity[j] = 1.0; if (bounds.velocity_bounded_) { if (bounds.max_velocity_ <= 0.0) @@ -938,12 +937,12 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } else { - RCLCPP_WARN_STREAM_ONCE( - LOGGER, "Joint velocity limits are not defined. Using the default " - << max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml."); + RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint " + << vars[j].c_str() + << "! You can define velocity limits in the URDF or joint_limits.yaml"); + return false; } - max_acceleration[j] = 1.0; if (bounds.acceleration_bounded_) { if (bounds.max_acceleration_ < 0.0) @@ -957,10 +956,11 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps(robot_trajectory::RobotT } else { - RCLCPP_WARN_STREAM_ONCE(LOGGER, - "Joint acceleration limits are not defined. Using the default " - << max_acceleration[j] - << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml."); + RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint " + << vars[j].c_str() + << "! You can define acceleration limits in the URDF or " + "joint_limits.yaml"); + return false; } } @@ -1016,10 +1016,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( if (!set_velocity_limit) { - max_velocity[j] = 1.0; - RCLCPP_WARN_STREAM_ONCE( - LOGGER, "Joint velocity limits are not defined. Using the default " - << max_velocity[j] << " rad/s. You can define velocity limits in the URDF or joint_limits.yaml."); + RCLCPP_ERROR_STREAM(LOGGER, "No velocity limit was defined for joint " + << vars[j].c_str() + << "! You can define velocity limits in the URDF or " + "joint_limits.yaml"); } // ACCELERATION LIMIT @@ -1046,11 +1046,10 @@ bool TimeOptimalTrajectoryGeneration::computeTimeStamps( } if (!set_acceleration_limit) { - max_acceleration[j] = 1.0; - RCLCPP_WARN_STREAM_ONCE(LOGGER, - "Joint acceleration limits are not defined. Using the default " - << max_acceleration[j] - << " rad/s^2. You can define acceleration limits in the URDF or joint_limits.yaml."); + RCLCPP_ERROR_STREAM(LOGGER, "No acceleration limit was defined for joint " + << vars[j].c_str() + << "! You can define acceleration limits in the URDF or " + "joint_limits.yaml"); } } diff --git a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp index cb2059d6b6..60572db42a 100644 --- a/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp @@ -44,6 +44,29 @@ using trajectory_processing::Path; using trajectory_processing::TimeOptimalTrajectoryGeneration; using trajectory_processing::Trajectory; +namespace +{ +// The URDF used in moveit::core::loadTestingRobotModel() does not contain acceleration limits, +// so add them here. +// TODO(andyz): Function won't be needed once this issue has been addressed: +// https://github.com/ros/urdfdom/issues/177 +void set_acceleration_limits(const moveit::core::RobotModelPtr& robot_model) +{ + const std::vector joint_models = robot_model->getActiveJointModels(); + for (auto& joint_model : joint_models) + { + std::vector joint_bounds_msg(joint_model->getVariableBoundsMsg()); + // TODO: update individual bounds + for (auto& joint_bound : joint_bounds_msg) + { + joint_bound.has_acceleration_limits = true; + joint_bound.max_acceleration = 1.0; + } + joint_model->setVariableBounds(joint_bounds_msg); + } +} +} // namespace + TEST(time_optimal_trajectory_generation, test1) { Eigen::VectorXd waypoint(4); @@ -153,13 +176,14 @@ TEST(time_optimal_trajectory_generation, test3) } // Test the version of computeTimeStamps that takes custom velocity/acceleration limits -TEST(time_optimal_trajectory_generation, test_custom_limits) +TEST(time_optimal_trajectory_generation, testCustomLimits) { constexpr auto robot_name{ "panda" }; constexpr auto group_name{ "panda_arm" }; auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; + set_acceleration_limits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); @@ -174,8 +198,14 @@ TEST(time_optimal_trajectory_generation, test_custom_limits) TimeOptimalTrajectoryGeneration totg; // Custom velocity & acceleration limits for some joints - std::unordered_map vel_limits{ { "panda_joint1", 1.3 } }; - std::unordered_map accel_limits{ { "panda_joint2", 2.3 }, { "panda_joint3", 3.3 } }; + std::unordered_map vel_limits{ { "panda_joint1", 1.3 }, { "panda_joint2", 2.3 }, + { "panda_joint3", 3.3 }, { "panda_joint4", 4.3 }, + { "panda_joint5", 5.3 }, { "panda_joint6", 6.3 }, + { "panda_joint7", 7.3 } }; + std::unordered_map accel_limits{ { "panda_joint1", 1.3 }, { "panda_joint2", 2.3 }, + { "panda_joint3", 3.3 }, { "panda_joint4", 4.3 }, + { "panda_joint5", 5.3 }, { "panda_joint6", 6.3 }, + { "panda_joint7", 7.3 } }; ASSERT_TRUE(totg.computeTimeStamps(trajectory, vel_limits, accel_limits)) << "Failed to compute time stamps"; } @@ -256,14 +286,16 @@ TEST(time_optimal_trajectory_generation, testLargeAccel) } } -// Test parameterizing a trajectory would always produce a trajectory with output end waypoint same as the input end waypoint +// Test parameterizing a trajectory would always produce a trajectory with output end waypoint same as the input end +// waypoint TEST(time_optimal_trajectory_generation, testLastWaypoint) { constexpr auto robot_name{ "panda" }; - constexpr auto group_name{ "hand" }; + constexpr auto group_name{ "panda_arm" }; auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; + set_acceleration_limits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); @@ -274,96 +306,14 @@ TEST(time_optimal_trajectory_generation, testLastWaypoint) waypoint_state.setJointGroupPositions(group, waypoint); trajectory.addSuffixWayPoint(waypoint_state, 0.1); }; - add_waypoint({ 0.000000000, 0.000000000 }); - add_waypoint({ 0.000396742, 0.000396742 }); - add_waypoint({ 0.000793484, 0.000793484 }); - add_waypoint({ 0.001190226, 0.001190226 }); - add_waypoint({ 0.001586968, 0.001586968 }); - add_waypoint({ 0.001983710, 0.001983710 }); - add_waypoint({ 0.002380452, 0.002380452 }); - add_waypoint({ 0.002777194, 0.002777194 }); - add_waypoint({ 0.003173936, 0.003173936 }); - add_waypoint({ 0.003570678, 0.003570678 }); - add_waypoint({ 0.003967420, 0.003967420 }); - add_waypoint({ 0.004364162, 0.004364162 }); - add_waypoint({ 0.004760904, 0.004760904 }); - add_waypoint({ 0.005157646, 0.005157646 }); - add_waypoint({ 0.005554388, 0.005554388 }); - add_waypoint({ 0.005951130, 0.005951130 }); - add_waypoint({ 0.006347872, 0.006347872 }); - add_waypoint({ 0.006744614, 0.006744614 }); - add_waypoint({ 0.007141356, 0.007141356 }); - add_waypoint({ 0.007538098, 0.007538098 }); - add_waypoint({ 0.007934840, 0.007934840 }); - add_waypoint({ 0.008331582, 0.008331582 }); - add_waypoint({ 0.008728324, 0.008728324 }); - add_waypoint({ 0.009125066, 0.009125066 }); - add_waypoint({ 0.009521808, 0.009521808 }); - add_waypoint({ 0.009918550, 0.009918550 }); - add_waypoint({ 0.010315292, 0.010315292 }); - add_waypoint({ 0.010712034, 0.010712034 }); - add_waypoint({ 0.011108776, 0.011108776 }); - add_waypoint({ 0.011505518, 0.011505518 }); - add_waypoint({ 0.011902261, 0.011902261 }); - add_waypoint({ 0.012299003, 0.012299003 }); - add_waypoint({ 0.012695745, 0.012695745 }); - add_waypoint({ 0.013092487, 0.013092487 }); - add_waypoint({ 0.013489229, 0.013489229 }); - add_waypoint({ 0.013885971, 0.013885971 }); - add_waypoint({ 0.014282713, 0.014282713 }); - add_waypoint({ 0.014679455, 0.014679455 }); - add_waypoint({ 0.015076197, 0.015076197 }); - add_waypoint({ 0.015472939, 0.015472939 }); - add_waypoint({ 0.015869681, 0.015869681 }); - add_waypoint({ 0.016266423, 0.016266423 }); - add_waypoint({ 0.016663165, 0.016663165 }); - add_waypoint({ 0.017059907, 0.017059907 }); - add_waypoint({ 0.017456649, 0.017456649 }); - add_waypoint({ 0.017853391, 0.017853391 }); - add_waypoint({ 0.018250133, 0.018250133 }); - add_waypoint({ 0.018646875, 0.018646875 }); - add_waypoint({ 0.019043617, 0.019043617 }); - add_waypoint({ 0.019440359, 0.019440359 }); - add_waypoint({ 0.019837101, 0.019837101 }); - add_waypoint({ 0.020233843, 0.020233843 }); - add_waypoint({ 0.020630585, 0.020630585 }); - add_waypoint({ 0.021027327, 0.021027327 }); - add_waypoint({ 0.021424069, 0.021424069 }); - add_waypoint({ 0.021820811, 0.021820811 }); - add_waypoint({ 0.022217553, 0.022217553 }); - add_waypoint({ 0.022614295, 0.022614295 }); - add_waypoint({ 0.023011037, 0.023011037 }); - add_waypoint({ 0.023407779, 0.023407779 }); - add_waypoint({ 0.023804521, 0.023804521 }); - add_waypoint({ 0.024201263, 0.024201263 }); - add_waypoint({ 0.024598005, 0.024598005 }); - add_waypoint({ 0.024994747, 0.024994747 }); - add_waypoint({ 0.025391489, 0.025391489 }); - add_waypoint({ 0.025788231, 0.025788231 }); - add_waypoint({ 0.026184973, 0.026184973 }); - add_waypoint({ 0.026581715, 0.026581715 }); - add_waypoint({ 0.026978457, 0.026978457 }); - add_waypoint({ 0.027375199, 0.027375199 }); - add_waypoint({ 0.027771941, 0.027771941 }); - add_waypoint({ 0.028168683, 0.028168683 }); - add_waypoint({ 0.028565425, 0.028565425 }); - add_waypoint({ 0.028962167, 0.028962167 }); - add_waypoint({ 0.029358909, 0.029358909 }); - add_waypoint({ 0.029755651, 0.029755651 }); - add_waypoint({ 0.030152393, 0.030152393 }); - add_waypoint({ 0.030549135, 0.030549135 }); - add_waypoint({ 0.030945877, 0.030945877 }); - add_waypoint({ 0.031342619, 0.031342619 }); - add_waypoint({ 0.031739361, 0.031739361 }); - add_waypoint({ 0.032136103, 0.032136103 }); - add_waypoint({ 0.032532845, 0.032532845 }); - add_waypoint({ 0.032929587, 0.032929587 }); - add_waypoint({ 0.033326329, 0.033326329 }); - add_waypoint({ 0.033723071, 0.033723071 }); - add_waypoint({ 0.034119813, 0.034119813 }); - add_waypoint({ 0.034516555, 0.034516555 }); - - const std::vector expected_last_waypoint = { 0.034913297, 0.034913297 }; + add_waypoint({ 0.000000000, 0.000000000, 0, 0, 0, 0, 0 }); + add_waypoint({ 0.009521808, 0.009521808, 0, 0, 0, 0, 0 }); + add_waypoint({ 0.011902261, 0.011902261, 0, 0, 0, 0, 0 }); + add_waypoint({ 0.016663165, 0.016663165, 0, 0, 0, 0, 0 }); + add_waypoint({ 0.026184973, 0.026184973, 0, 0, 0, 0, 0 }); + add_waypoint({ 0.034516555, 0.034516555, 0, 0, 0, 0, 0 }); + + const std::vector expected_last_waypoint = { 0.034516555, 0.034516555, 0, 0, 0, 0, 0 }; add_waypoint(expected_last_waypoint); TimeOptimalTrajectoryGeneration totg; @@ -384,6 +334,7 @@ TEST(time_optimal_trajectory_generation, testPluginAPI) auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; + set_acceleration_limits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); @@ -506,6 +457,7 @@ TEST(time_optimal_trajectory_generation, testFixedNumWaypoints) auto robot_model = moveit::core::loadTestingRobotModel(robot_name); ASSERT_TRUE(robot_model) << "Failed to load robot model" << robot_name; + set_acceleration_limits(robot_model); auto group = robot_model->getJointModelGroup(group_name); ASSERT_TRUE(group) << "Failed to load joint model group " << group_name; moveit::core::RobotState waypoint_state(robot_model); diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index 55f5fb27b1..23792d7f63 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -34,14 +34,14 @@ /* Author: Bryce Willey */ +#include #include +#include #include -#include #include -#include -#include #include #include +#include namespace moveit {