From 23226cda7fdecb2c45f77444ec220efb16b9de08 Mon Sep 17 00:00:00 2001 From: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> Date: Mon, 9 Oct 2023 03:11:50 -0400 Subject: [PATCH] Fix Servo singularity scaling unit tests (#2414) * Fix Servo singularity scaling unit tests * Fix Servo singularity scaling unit tests * Simplify tests * updateLinkTransforms is not needed after all (cherry picked from commit e2b8ac2f2cb53c4f72ffe81eebc3986bd9582c24) --- moveit_ros/moveit_servo/tests/test_utils.cpp | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/moveit_ros/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp index d54eb08be5..bd6eb9fcfc 100644 --- a/moveit_ros/moveit_servo/tests/test_utils.cpp +++ b/moveit_ros/moveit_servo/tests/test_utils.cpp @@ -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 cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; // Home state Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; @@ -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 cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; // Home state @@ -205,9 +205,9 @@ 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 cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; + Eigen::Vector cartesian_delta{ -0.005, 0.0, 0.0, 0.0, 0.0, 0.0 }; // Home state Eigen::Vector state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 }; @@ -215,14 +215,7 @@ TEST(ServoUtilsUnitTests, LeavingSingularityScaling) 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 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 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);