Skip to content

Commit

Permalink
Pre-commit
Browse files Browse the repository at this point in the history
  • Loading branch information
@AndyZe
AndyZe committed May 2, 2023
1 parent 3ca562c commit bb3c1db
Showing 1 changed file with 54 additions and 63 deletions.
117 changes: 54 additions & 63 deletions moveit_core/trajectory_processing/test/test_time_optimal_trajectory_generation.cpp
View file
Expand Up @@ -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 =
"<?xml version=\"1.0\" ?>"
"<robot name=\"one_robot\">"
"<link name=\"base_link\">"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<joint name=\"joint_a\" type=\"continuous\">"
" <axis xyz=\"0.7071 0.7071 0\"/>"
" <parent link=\"base_link\"/>"
" <child link=\"link_a\"/>"
" <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
"</joint>"
"<link name=\"link_a\">"
" <inertial>"
" <mass value=\"1000.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";
const std::string urdf = "<?xml version=\"1.0\" ?>"
"<robot name=\"one_robot\">"
"<link name=\"base_link\">"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"<joint name=\"joint_a\" type=\"continuous\">"
" <axis xyz=\"0.7071 0.7071 0\"/>"
" <parent link=\"base_link\"/>"
" <child link=\"link_a\"/>"
" <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
"</joint>"
"<link name=\"link_a\">"
" <inertial>"
" <mass value=\"1000.0\"/>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
" <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
" </inertial>"
" <collision>"
" <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </collision>"
" <visual>"
" <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
" <geometry>"
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
"</link>"
"</robot>";

const std::string srdf =
"<?xml version=\"1.0\" ?>"
Expand Down Expand Up @@ -493,33 +492,25 @@ TEST(time_optimal_trajectory_generation, test_totg_with_torque_limits)
gravity.z = -9.81;
return gravity;
}();
const std::vector<double> joint_torque_limits{ 250 }; // in N*m
const std::vector<double> joint_torque_limits{ 250 }; // in N*m
const double accel_limit_decrement_factor = 0.1;
const std::unordered_map<std::string, double> velocity_limits = {{"joint_a", 3}};
const std::unordered_map<std::string, double> acceleration_limits = {{"joint_a", 3}};
const std::unordered_map<std::string, double> velocity_limits = { { "joint_a", 3 } };
const std::unordered_map<std::string, double> 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<double> 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<double> 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";
Expand Down

0 comments on commit bb3c1db

Please sign in to comment.