Skip to content

Commit

Permalink
Add print statements to failing servo test
Browse files Browse the repository at this point in the history
  • Loading branch information
@sea-bass
sea-bass committed Oct 8, 2023
1 parent 0236685 commit 6b2f474
Show file tree
Hide file tree
Showing 3 changed files with 165 additions and 160 deletions.
16 changes: 8 additions & 8 deletions moveit_ros/moveit_servo/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()

Expand Down
5 changes: 5 additions & 0 deletions moveit_ros/moveit_servo/src/utils/common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -210,6 +210,8 @@ std::pair<double, StatusCode> 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.
Expand All @@ -229,6 +231,9 @@ std::pair<double, StatusCode> 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 -=
Expand Down
304 changes: 152 additions & 152 deletions moveit_ros/moveit_servo/tests/test_utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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<moveit::core::RobotState>(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<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);

// Move to singular state.
Eigen::Vector<double, 7> 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<moveit::core::RobotState>(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<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);
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<moveit::core::RobotState>(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<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);

// // Move to singular state.
// Eigen::Vector<double, 7> 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<moveit::core::RobotState>(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<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);
// ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_LEAVING_SINGULARITY);
// }

} // namespace

Expand Down

0 comments on commit 6b2f474

Please sign in to comment.