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 938fa29dc3..8c887c27a1 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 @@ -416,49 +416,48 @@ TEST(time_optimal_trajectory_generation, test_totg_with_torque_limits) // Request a trajectory. This will serve as the baseline. // Then decrease the joint torque limits and re-parameterize. The trajectory duration should be shorter. - const std::string urdf = - "" - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - "" - "" - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - " " - "" - ""; + const std::string urdf = "" + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + "" + " " + " " + " " + " " + "" + "" + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + " " + "" + ""; const std::string srdf = "" @@ -493,33 +492,25 @@ TEST(time_optimal_trajectory_generation, test_totg_with_torque_limits) gravity.z = -9.81; return gravity; }(); - const std::vector joint_torque_limits{ 250 }; // in N*m + const std::vector joint_torque_limits{ 250 }; // in N*m const double accel_limit_decrement_factor = 0.1; - const std::unordered_map velocity_limits = {{"joint_a", 3}}; - const std::unordered_map acceleration_limits = {{"joint_a", 3}}; + const std::unordered_map velocity_limits = { { "joint_a", 3 } }; + const std::unordered_map acceleration_limits = { { "joint_a", 3 } }; TimeOptimalTrajectoryGeneration totg(0.1 /* path tolerance */, 0.01 /* resample dt */, 0.001 /* min angle change */); - bool totg_success = totg.computeTimeStampsWithTorqueLimits(trajectory, - gravity_vector, - joint_torque_limits, - accel_limit_decrement_factor, - velocity_limits, - acceleration_limits, - 1.0 /* accel scaling */, - 1.0 /* vel scaling */); + bool totg_success = + totg.computeTimeStampsWithTorqueLimits(trajectory, gravity_vector, joint_torque_limits, + accel_limit_decrement_factor, velocity_limits, acceleration_limits, + 1.0 /* accel scaling */, 1.0 /* vel scaling */); ASSERT_TRUE(totg_success) << "Failed to compute timestamps"; double first_duration = trajectory.getDuration(); // Now decrease joint torque limits and re-time-parameterize. The trajectory duration should be longer. - const std::vector lower_torque_limits{ 1 }; // in N*m - totg_success = totg.computeTimeStampsWithTorqueLimits(trajectory, - gravity_vector, - lower_torque_limits, - accel_limit_decrement_factor, - velocity_limits, - acceleration_limits, - 1.0 /* accel scaling */, - 1.0 /* vel scaling */); + const std::vector lower_torque_limits{ 1 }; // in N*m + totg_success = + totg.computeTimeStampsWithTorqueLimits(trajectory, gravity_vector, lower_torque_limits, + accel_limit_decrement_factor, velocity_limits, acceleration_limits, + 1.0 /* accel scaling */, 1.0 /* vel scaling */); ASSERT_TRUE(totg_success) << "Failed to compute timestamps"; double second_duration = trajectory.getDuration(); EXPECT_GT(second_duration, first_duration) << "The second time parameterization should result in a longer duration";