Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix Servo singularity scaling unit tests #2414

Merged
merged 5 commits into from
Oct 9, 2023
Merged

Fix Servo singularity scaling unit tests #2414

Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
e8b406e
Fix Servo singularity scaling unit tests
sea-bass Oct 9, 2023
dd8f9a3
Fix Servo singularity scaling unit tests
sea-bass Oct 9, 2023
6fe3423
Merge branch 'update-link-tforms-in-servo-tests' of https://github.co…
sea-bass Oct 9, 2023
9632cef
Simplify tests
sea-bass Oct 9, 2023
bc110b3
updateLinkTransforms is not needed after all
sea-bass Oct 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 4 additions & 11 deletions moveit_ros/moveit_servo/tests/test_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -152,8 +152,8 @@ TEST(ServoUtilsUnitTests, ApproachingSingularityScaling)
servo_params.move_group_name = "panda_arm";
const moveit::core::JointModelGroup* joint_model_group =
robot_state->getJointModelGroup(servo_params.move_group_name);
robot_state->setToDefaultValues();

rclcpp::sleep_for(std::chrono::milliseconds(500));
Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
// Home state
Eigen::Vector<double, 7> state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
Expand All @@ -178,8 +178,8 @@ TEST(ServoUtilsUnitTests, HaltForSingularityScaling)
servo_params.move_group_name = "panda_arm";
const moveit::core::JointModelGroup* joint_model_group =
robot_state->getJointModelGroup(servo_params.move_group_name);
robot_state->setToDefaultValues();

rclcpp::sleep_for(std::chrono::milliseconds(500));
Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };

// Home state
Expand All @@ -205,24 +205,17 @@ TEST(ServoUtilsUnitTests, LeavingSingularityScaling)
servo_params.move_group_name = "panda_arm";
const moveit::core::JointModelGroup* joint_model_group =
robot_state->getJointModelGroup(servo_params.move_group_name);
robot_state->setToDefaultValues();

rclcpp::sleep_for(std::chrono::milliseconds(500));
Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
Eigen::Vector<double, 6> cartesian_delta{ -0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };

// Home state
Eigen::Vector<double, 7> state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
robot_state->setJointGroupActivePositions(joint_model_group, state_ready);
auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params);
ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING);

// Approach singularity
Eigen::Vector<double, 7> state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 };
robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity);
scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params);
ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY);

// Move away from singularity
cartesian_delta(0) *= -1;
Eigen::Vector<double, 7> state_leaving_singularity{ 0.0, 0.3458, 0.0, -1.1424, 0.0, 1.4865, 0.785 };
robot_state->setJointGroupActivePositions(joint_model_group, state_leaving_singularity);
scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params);
Expand Down
Loading