From 16bbc5ffcbc2d39e6b8048f74ea2372ec8bd9c91 Mon Sep 17 00:00:00 2001 From: AndyZe Date: Fri, 9 Dec 2022 11:54:22 -0600 Subject: [PATCH] Reduce num_waypoints in a test. Use "panda_arm" for all tests --- ...est_time_optimal_trajectory_generation.cpp | 102 ++---------------- 1 file changed, 10 insertions(+), 92 deletions(-) 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 1bf5b38da06..60572db42af 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 @@ -50,7 +50,7 @@ namespace // 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(moveit::core::RobotModelPtr robot_model) +void set_acceleration_limits(const moveit::core::RobotModelPtr& robot_model) { const std::vector joint_models = robot_model->getActiveJointModels(); for (auto& joint_model : joint_models) @@ -291,7 +291,7 @@ TEST(time_optimal_trajectory_generation, testLargeAccel) 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; @@ -306,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;