Merge branch 'iron' into mergify/bp/iron/pr-3105

moveit · Nov 16, 2024 · 6c6c7e1 · 6c6c7e1
2 parents 50febf0 + b094539
commit 6c6c7e1
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 11 deletions.
diff --git 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<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 };
@@ -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
@@ -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);

diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp
@@ -206,6 +206,7 @@ bool ROS2ControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter&
           const ControlInterfaces interfaces = parent_.getControlInterfaces(ci.joints_);
           emitter << YAML::Key << "command_interfaces" << YAML::Value << interfaces.command_interfaces;
           emitter << YAML::Key << "state_interfaces" << YAML::Value << interfaces.state_interfaces;
+          emitter << YAML::Key << "allow_nonzero_velocity_at_trajectory_end" << YAML::Value << true;
         }
       }
       emitter << YAML::EndMap;