diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index e00b8a7eb3..022aa89c52 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -144,14 +144,14 @@ if(BUILD_TESTING) ament_target_dependencies(moveit_servo_utils_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_ros_test(tests/launch/servo_utils.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - ament_add_gtest_executable(moveit_servo_cpp_integration_test tests/test_integration.cpp tests/servo_cpp_fixture.hpp) - target_link_libraries(moveit_servo_cpp_integration_test moveit_servo_lib_cpp) - ament_target_dependencies(moveit_servo_cpp_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(tests/launch/servo_cpp_integration.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - ament_add_gtest_executable(moveit_servo_ros_integration_test tests/test_ros_integration.cpp tests/servo_ros_fixture.hpp) - ament_target_dependencies(moveit_servo_ros_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(tests/launch/servo_ros_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # ament_add_gtest_executable(moveit_servo_cpp_integration_test tests/test_integration.cpp tests/servo_cpp_fixture.hpp) + # target_link_libraries(moveit_servo_cpp_integration_test moveit_servo_lib_cpp) + # ament_target_dependencies(moveit_servo_cpp_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # add_ros_test(tests/launch/servo_cpp_integration.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + # ament_add_gtest_executable(moveit_servo_ros_integration_test tests/test_ros_integration.cpp tests/servo_ros_fixture.hpp) + # ament_target_dependencies(moveit_servo_ros_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # add_ros_test(tests/launch/servo_ros_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index fe28852a69..3d0d974621 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -210,6 +210,8 @@ std::pair velocityScalingFactorForSingularity(const moveit:: } // Double check the direction using dot product. + std::cout << "Moving towards singularity vector dot product: " << vector_towards_singularity.dot(target_delta_x) + << std::endl; const bool moving_towards_singularity = vector_towards_singularity.dot(target_delta_x) > 0; // Compute upper condition variable threshold based on if we are moving towards or away from singularity. @@ -229,6 +231,9 @@ std::pair velocityScalingFactorForSingularity(const moveit:: double velocity_scale = 1.0; const bool is_above_lower_limit = current_condition_number > lower_singularity_threshold; const bool is_below_hard_stop_limit = current_condition_number < hard_stop_singularity_threshold; + std::cout << "Condition number: " << current_condition_number << std::endl; + std::cout << "Lower threshold: " << lower_singularity_threshold << std::endl; + std::cout << "Hard stop threshold: " << hard_stop_singularity_threshold << std::endl; if (is_above_lower_limit && is_below_hard_stop_limit) { velocity_scale -= diff --git a/moveit_ros/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp index d54eb08be5..c0da90f472 100644 --- a/moveit_ros/moveit_servo/tests/test_utils.cpp +++ b/moveit_ros/moveit_servo/tests/test_utils.cpp @@ -49,98 +49,98 @@ namespace { -TEST(ServoUtilsUnitTests, JointLimitVelocityScaling) -{ - using moveit::core::loadTestingRobotModel; - moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); - moveit::core::JointBoundsVector joint_bounds = robot_model->getActiveJointModelsBounds(); - - // Get the upper bound for the velocities of each joint. - Eigen::VectorXd incoming_velocities(joint_bounds.size()); - for (size_t i = 0; i < joint_bounds.size(); i++) - { - const auto joint_bound = (*joint_bounds[i])[0]; - if (joint_bound.velocity_bounded_) - { - incoming_velocities(i) = joint_bound.max_velocity_; - } - } - - // Create incoming velocities with only joint 1 and joint 2 over limit by a factor of 0.1 and 0.05 - // Scale down all other joint velocities by 0.3 to keep it within limits. - incoming_velocities(0) *= 1.1; - incoming_velocities(1) *= 1.05; - incoming_velocities.tail<5>() *= 0.7; - - // The resulting scaling factor selected should be approximately 0.95238 - double user_velocity_override = 0.0; - double scaling_factor = - moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override); - constexpr double tol = 0.001; - ASSERT_NEAR(scaling_factor, 0.95238, tol); -} - -TEST(ServoUtilsUnitTests, validVector) -{ - Eigen::VectorXd valid_vector(7); - valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - EXPECT_TRUE(moveit_servo::isValidCommand(valid_vector)); -} - -TEST(ServoUtilsUnitTests, invalidVector) -{ - Eigen::VectorXd invalid_vector(6); - invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; - EXPECT_FALSE(moveit_servo::isValidCommand(invalid_vector)); -} - -TEST(ServoUtilsUnitTests, validTwist) -{ - Eigen::VectorXd valid_vector(6); - valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; - moveit_servo::TwistCommand valid_twist{ "panda_link0", valid_vector }; - EXPECT_TRUE(moveit_servo::isValidCommand(valid_twist)); -} - -TEST(ServoUtilsUnitTests, emptyTwistFrame) -{ - Eigen::VectorXd invalid_vector(6); - invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; - moveit_servo::TwistCommand invalid_twist; - invalid_twist.velocities = invalid_vector; - EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); -} - -TEST(ServoUtilsUnitTests, invalidTwistVelocities) -{ - Eigen::VectorXd invalid_vector(6); - invalid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, std::nan(""); - moveit_servo::TwistCommand invalid_twist{ "panda_link0", invalid_vector }; - EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); -} - -TEST(ServoUtilsUnitTests, validIsometry) -{ - Eigen::Isometry3d valid_isometry; - valid_isometry.setIdentity(); - EXPECT_TRUE(moveit_servo::isValidCommand(valid_isometry)); -} - -TEST(ServoUtilsUnitTests, invalidIsometry) -{ - Eigen::Isometry3d invalid_isometry; - invalid_isometry.setIdentity(); - invalid_isometry.translation().z() = std::nan(""); - EXPECT_FALSE(moveit_servo::isValidCommand(invalid_isometry)); -} - -TEST(ServoUtilsUnitTests, validPose) -{ - Eigen::Isometry3d valid_isometry; - valid_isometry.setIdentity(); - moveit_servo::PoseCommand valid_pose{ "panda_link0", valid_isometry }; - EXPECT_TRUE(moveit_servo::isValidCommand(valid_pose)); -} +// TEST(ServoUtilsUnitTests, JointLimitVelocityScaling) +// { +// using moveit::core::loadTestingRobotModel; +// moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); +// moveit::core::JointBoundsVector joint_bounds = robot_model->getActiveJointModelsBounds(); + +// // Get the upper bound for the velocities of each joint. +// Eigen::VectorXd incoming_velocities(joint_bounds.size()); +// for (size_t i = 0; i < joint_bounds.size(); i++) +// { +// const auto joint_bound = (*joint_bounds[i])[0]; +// if (joint_bound.velocity_bounded_) +// { +// incoming_velocities(i) = joint_bound.max_velocity_; +// } +// } + +// // Create incoming velocities with only joint 1 and joint 2 over limit by a factor of 0.1 and 0.05 +// // Scale down all other joint velocities by 0.3 to keep it within limits. +// incoming_velocities(0) *= 1.1; +// incoming_velocities(1) *= 1.05; +// incoming_velocities.tail<5>() *= 0.7; + +// // The resulting scaling factor selected should be approximately 0.95238 +// double user_velocity_override = 0.0; +// double scaling_factor = +// moveit_servo::jointLimitVelocityScalingFactor(incoming_velocities, joint_bounds, user_velocity_override); +// constexpr double tol = 0.001; +// ASSERT_NEAR(scaling_factor, 0.95238, tol); +// } + +// TEST(ServoUtilsUnitTests, validVector) +// { +// Eigen::VectorXd valid_vector(7); +// valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; +// EXPECT_TRUE(moveit_servo::isValidCommand(valid_vector)); +// } + +// TEST(ServoUtilsUnitTests, invalidVector) +// { +// Eigen::VectorXd invalid_vector(6); +// invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; +// EXPECT_FALSE(moveit_servo::isValidCommand(invalid_vector)); +// } + +// TEST(ServoUtilsUnitTests, validTwist) +// { +// Eigen::VectorXd valid_vector(6); +// valid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0; +// moveit_servo::TwistCommand valid_twist{ "panda_link0", valid_vector }; +// EXPECT_TRUE(moveit_servo::isValidCommand(valid_twist)); +// } + +// TEST(ServoUtilsUnitTests, emptyTwistFrame) +// { +// Eigen::VectorXd invalid_vector(6); +// invalid_vector << 0.0, 0.0, 0.0, 0.0, std::nan(""), 0.0; +// moveit_servo::TwistCommand invalid_twist; +// invalid_twist.velocities = invalid_vector; +// EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); +// } + +// TEST(ServoUtilsUnitTests, invalidTwistVelocities) +// { +// Eigen::VectorXd invalid_vector(6); +// invalid_vector << 0.0, 0.0, 0.0, 0.0, 0.0, std::nan(""); +// moveit_servo::TwistCommand invalid_twist{ "panda_link0", invalid_vector }; +// EXPECT_FALSE(moveit_servo::isValidCommand(invalid_twist)); +// } + +// TEST(ServoUtilsUnitTests, validIsometry) +// { +// Eigen::Isometry3d valid_isometry; +// valid_isometry.setIdentity(); +// EXPECT_TRUE(moveit_servo::isValidCommand(valid_isometry)); +// } + +// TEST(ServoUtilsUnitTests, invalidIsometry) +// { +// Eigen::Isometry3d invalid_isometry; +// invalid_isometry.setIdentity(); +// invalid_isometry.translation().z() = std::nan(""); +// EXPECT_FALSE(moveit_servo::isValidCommand(invalid_isometry)); +// } + +// TEST(ServoUtilsUnitTests, validPose) +// { +// Eigen::Isometry3d valid_isometry; +// valid_isometry.setIdentity(); +// moveit_servo::PoseCommand valid_pose{ "panda_link0", valid_isometry }; +// EXPECT_TRUE(moveit_servo::isValidCommand(valid_pose)); +// } TEST(ServoUtilsUnitTests, ApproachingSingularityScaling) { @@ -168,66 +168,66 @@ TEST(ServoUtilsUnitTests, ApproachingSingularityScaling) ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY); } -TEST(ServoUtilsUnitTests, HaltForSingularityScaling) -{ - using moveit::core::loadTestingRobotModel; - moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); - moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); - - servo::Params servo_params; - servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); - - 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 }; - 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); - - // Move to singular state. - Eigen::Vector singular_state{ -0.0001, 0.5690, 0.0005, -0.7782, 0.0, 1.3453, 0.7845 }; - robot_state->setJointGroupActivePositions(joint_model_group, singular_state); - scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::HALT_FOR_SINGULARITY); -} - -TEST(ServoUtilsUnitTests, LeavingSingularityScaling) -{ - using moveit::core::loadTestingRobotModel; - moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); - moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); - - servo::Params servo_params; - servo_params.move_group_name = "panda_arm"; - const moveit::core::JointModelGroup* joint_model_group = - robot_state->getJointModelGroup(servo_params.move_group_name); - - 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 }; - 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 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); - ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY); -} +// TEST(ServoUtilsUnitTests, HaltForSingularityScaling) +// { +// using moveit::core::loadTestingRobotModel; +// moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); +// moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + +// servo::Params servo_params; +// servo_params.move_group_name = "panda_arm"; +// const moveit::core::JointModelGroup* joint_model_group = +// robot_state->getJointModelGroup(servo_params.move_group_name); + +// 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 }; +// 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); + +// // Move to singular state. +// Eigen::Vector singular_state{ -0.0001, 0.5690, 0.0005, -0.7782, 0.0, 1.3453, 0.7845 }; +// robot_state->setJointGroupActivePositions(joint_model_group, singular_state); +// scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params); +// ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::HALT_FOR_SINGULARITY); +// } + +// TEST(ServoUtilsUnitTests, LeavingSingularityScaling) +// { +// using moveit::core::loadTestingRobotModel; +// moveit::core::RobotModelPtr robot_model = loadTestingRobotModel("panda"); +// moveit::core::RobotStatePtr robot_state = std::make_shared(robot_model); + +// servo::Params servo_params; +// servo_params.move_group_name = "panda_arm"; +// const moveit::core::JointModelGroup* joint_model_group = +// robot_state->getJointModelGroup(servo_params.move_group_name); + +// 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 }; +// 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 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); +// ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY); +// } } // namespace