From 68a4c72b37027e3ec3d05959504a1ddca6332bbc Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 28 May 2025 13:47:13 +0200 Subject: [PATCH 1/2] [force mode controller] Fix the task frame orientation (#1379) * [force mode controller] Fix the task frame orientation Before, we were using a RPY notation. Chaning that to an angle-axis notation should fix things. * Update force_mode example to include a pose that is not in line with one of the main axes * Make test use an arbitraty orientation This should avoid a coincitental overlap if the transformation is wrong. (cherry picked from commit 8eb4288e3099a1c5458ca208d0240c666436fba1) # Conflicts: # ur_robot_driver/test/integration_test_force_mode.py --- ur_controllers/src/force_mode_controller.cpp | 8 +- ur_robot_driver/examples/force_mode.py | 290 ++++++++++++------ .../test/integration_test_force_mode.py | 135 +++++++- 3 files changed, 326 insertions(+), 107 deletions(-) diff --git a/ur_controllers/src/force_mode_controller.cpp b/ur_controllers/src/force_mode_controller.cpp index 3ecdd6cce..f52ec3469 100644 --- a/ur_controllers/src/force_mode_controller.cpp +++ b/ur_controllers/src/force_mode_controller.cpp @@ -260,9 +260,11 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request tf2::Quaternion quat_tf; tf2::convert(task_frame_transformed.pose.orientation, quat_tf); - tf2::Matrix3x3 rot_mat(quat_tf); - rot_mat.getRPY(force_mode_parameters.task_frame[3], force_mode_parameters.task_frame[4], - force_mode_parameters.task_frame[5]); + const double angle = quat_tf.getAngle(); + const auto axis = quat_tf.getAxis(); + force_mode_parameters.task_frame[3] = axis.x() * angle; // rx + force_mode_parameters.task_frame[4] = axis.y() * angle; // ry + force_mode_parameters.task_frame[5] = axis.z() * angle; // rz } catch (const tf2::TransformException& ex) { RCLCPP_ERROR(get_node()->get_logger(), "Could not transform %s to robot base: %s", req->task_frame.header.frame_id.c_str(), ex.what()); diff --git a/ur_robot_driver/examples/force_mode.py b/ur_robot_driver/examples/force_mode.py index 86ea05b70..f5b5c33c4 100755 --- a/ur_robot_driver/examples/force_mode.py +++ b/ur_robot_driver/examples/force_mode.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# Copyright 2024, Universal Robots A/S +# Copyright 2025, Universal Robots A/S # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions are met: @@ -27,116 +27,218 @@ # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE # POSSIBILITY OF SUCH DAMAGE. +import sys import time +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +import tf2_geometry_msgs # noqa # pylint: disable=unused-import + +from builtin_interfaces.msg import Duration + import rclpy +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import ExternalShutdownException, MultiThreadedExecutor from rclpy.node import Node from controller_manager_msgs.srv import SwitchController -from builtin_interfaces.msg import Duration from geometry_msgs.msg import Twist -from std_msgs.msg import Header from std_srvs.srv import Trigger +from visualization_msgs.msg import Marker + from geometry_msgs.msg import ( Point, - Quaternion, - Pose, PoseStamped, Wrench, Vector3, + Vector3Stamped, ) - from ur_msgs.srv import SetForceMode from examples import Robot + +class ForceModeExample(Node): + def __init__(self): + super().__init__("force_mode_example") + + self.robot = Robot(self) + # Add force mode service to service interfaces and re-init robot + self.robot.service_interfaces.update( + {"/force_mode_controller/start_force_mode": SetForceMode} + ) + self.robot.service_interfaces.update({"/force_mode_controller/stop_force_mode": Trigger}) + self.robot.init_robot() + + self.marker_publisher = self.create_publisher(Marker, "direction", 10) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + self.direction = Vector3(x=0.0, y=0.0, z=1.0) + + self.marker_published = False + self.force_mode_start_time = None + self.startup() + + self.timer_cb_group = MutuallyExclusiveCallbackGroup() + self.timer = self.create_timer(1.0, self.on_timer, self.timer_cb_group) + + def startup(self): + # Press play on the robot + self.robot.call_service("/dashboard_client/play", Trigger.Request()) + + time.sleep(0.5) + # Start controllers + self.robot.call_service( + "/controller_manager/switch_controller", + SwitchController.Request( + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + activate_controllers=[ + "passthrough_trajectory_controller", + "force_mode_controller", + ], + strictness=SwitchController.Request.BEST_EFFORT, + ), + ) + self.move_to_starting_pose() + + def on_timer(self): + if not self.marker_published: + self.publish_direction_marker() + self.marker_published = True + elif self.force_mode_start_time is None: + self.start_force_mode() + self.force_mode_start_time = self.get_clock().now() + self.get_logger().info("Force mode started.") + elif (self.get_clock().now() - self.force_mode_start_time).nanoseconds > 3e9: + self.get_logger().info("Stopping force mode after 3 seconds.") + self.robot.call_service("/force_mode_controller/stop_force_mode", Trigger.Request()) + sys.exit(0) + + def move_to_starting_pose(self): + # Move robot in to position + self.robot.send_trajectory( + waypts=[[-1.6, -1.55, -1.7, -1.0, 2.05, 0.5]], + time_vec=[Duration(sec=5, nanosec=0)], + action_client=self.robot.passthrough_trajectory_action_client, + ) + + def start_force_mode(self): + # Create task frame for force mode + frame_stamp = PoseStamped() + frame_stamp.header.frame_id = "tool0_controller" + frame_stamp.pose.position.x = 0.0 + frame_stamp.pose.position.y = 0.0 + frame_stamp.pose.position.z = 0.0 + frame_stamp.pose.orientation.x = 0.0 + frame_stamp.pose.orientation.y = 0.0 + frame_stamp.pose.orientation.z = 0.0 + frame_stamp.pose.orientation.w = 1.0 + + wrench_vec = Wrench( + force=Vector3(x=0.0, y=0.0, z=10.0), torque=Vector3(x=0.0, y=0.0, z=0.0) + ) + type_spec = SetForceMode.Request.NO_TRANSFORM + + # Specify max speeds and deviations of force mode + speed_limits = Twist() + speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) + speed_limits.angular = Vector3(x=0.0, y=0.0, z=0.0) + deviation_limits = [0.005, 0.005, 0.005, 0.005, 0.005, 0.005] + + # specify damping and gain scaling + damping_factor = 0.1 + gain_scale = 0.8 + + req = SetForceMode.Request() + req.task_frame = frame_stamp + req.selection_vector_x = False + req.selection_vector_y = False + req.selection_vector_z = True + req.selection_vector_rx = False + req.selection_vector_ry = False + req.selection_vector_rz = False + req.wrench = wrench_vec + req.type = type_spec + req.speed_limits = speed_limits + req.deviation_limits = deviation_limits + req.damping_factor = damping_factor + req.gain_scaling = gain_scale + + self.get_logger().info(f"Starting force mode with {req}") + self.robot.call_service("/force_mode_controller/start_force_mode", req) + + def publish_direction_marker(self): + """Publish a line strip going from the current TCP position to the desired direction.""" + to_frame_rel = "base" + from_frame_rel = "tool0_controller" + try: + t = self.tf_buffer.lookup_transform( + to_frame_rel, + from_frame_rel, + rclpy.time.Time(), + timeout=rclpy.time.Duration(seconds=10.0), + ) + self.get_logger().info( + f"[{t.transform.translation.x}, {t.transform.translation.y}, {t.transform.translation.z}]" + ) + direction_vec = Vector3Stamped() + direction_vec.header.frame_id = from_frame_rel + direction_vec.header.stamp = rclpy.time.Time() + direction_vec.vector = self.direction + transformed_direction = self.tf_buffer.transform( + direction_vec, to_frame_rel, timeout=rclpy.time.Duration(seconds=1.0) + ) + except TransformException as ex: + self.get_logger().info(f"Could not transform {to_frame_rel} to {from_frame_rel}: {ex}") + return + + marker = Marker() + marker.header.frame_id = "base" + marker.header.stamp = self.get_clock().now().to_msg() + marker.type = marker.ARROW + marker.id = 0 + marker.action = marker.ADD + marker.scale.x = 0.01 + marker.scale.y = 0.02 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + marker.points.append( + Point( + x=t.transform.translation.x, + y=t.transform.translation.y, + z=t.transform.translation.z, + ) + ) + marker.points.append( + Point( + x=t.transform.translation.x + transformed_direction.vector.x, + y=t.transform.translation.y + transformed_direction.vector.y, + z=t.transform.translation.z + transformed_direction.vector.z, + ) + ) + self.marker_publisher.publish(marker) + + if __name__ == "__main__": rclpy.init() - node = Node("robot_driver_test") - robot = Robot(node) - - # Add force mode service to service interfaces and re-init robot - robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode}) - robot.service_interfaces.update({"/force_mode_controller/stop_force_mode": Trigger}) - robot.init_robot() - time.sleep(0.5) - # Press play on the robot - robot.call_service("/dashboard_client/play", Trigger.Request()) - - time.sleep(0.5) - # Start controllers - robot.call_service( - "/controller_manager/switch_controller", - SwitchController.Request( - deactivate_controllers=["scaled_joint_trajectory_controller"], - activate_controllers=["passthrough_trajectory_controller", "force_mode_controller"], - strictness=SwitchController.Request.BEST_EFFORT, - ), - ) - - # Move robot in to position - robot.send_trajectory( - waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], - time_vec=[Duration(sec=5, nanosec=0)], - action_client=robot.passthrough_trajectory_action_client, - ) - - # Finished moving - # Create task frame for force mode - point = Point(x=0.0, y=0.0, z=0.0) - orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) - task_frame_pose = Pose() - task_frame_pose.position = point - task_frame_pose.orientation = orientation - header = Header(seq=1, frame_id="world") - header.stamp.sec = int(time.time()) + 1 - header.stamp.nanosec = 0 - frame_stamp = PoseStamped() - frame_stamp.header = header - frame_stamp.pose = task_frame_pose - - # Create compliance vector (which axes should be force controlled) - compliance = [False, False, True, False, False, False] - - # Create Wrench message for force mode - wrench_vec = Wrench(force=Vector3(x=0.0, y=0.0, z=-20.0), torque=Vector3(x=0.0, y=0.0, z=0.0)) - # Specify interpretation of task frame (no transform) - type_spec = SetForceMode.Request.NO_TRANSFORM - - # Specify max speeds and deviations of force mode - speed_limits = Twist() - speed_limits.linear = Vector3(x=0.0, y=0.0, z=1.0) - speed_limits.angular = Vector3(x=0.0, y=0.0, z=1.0) - deviation_limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0] - - # specify damping and gain scaling - damping_factor = 0.1 - gain_scale = 0.8 - - req = SetForceMode.Request() - req.task_frame = frame_stamp - req.selection_vector_x = compliance[0] - req.selection_vector_y = compliance[1] - req.selection_vector_z = compliance[2] - req.selection_vector_rx = compliance[3] - req.selection_vector_ry = compliance[4] - req.selection_vector_rz = compliance[5] - req.wrench = wrench_vec - req.type = type_spec - req.speed_limits = speed_limits - req.deviation_limits = deviation_limits - req.damping_factor = damping_factor - req.gain_scaling = gain_scale - - # Send request to controller - node.get_logger().info(f"Starting force mode with {req}") - robot.call_service("/force_mode_controller/start_force_mode", req) - robot.send_trajectory( - waypts=[[1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]], - time_vec=[Duration(sec=5, nanosec=0)], - action_client=robot.passthrough_trajectory_action_client, - ) - - time.sleep(3) - node.get_logger().info("Deactivating force mode controller.") - robot.call_service("/force_mode_controller/stop_force_mode", Trigger.Request()) + + node = ForceModeExample() + executor = MultiThreadedExecutor() + executor.add_node(node) + try: + rclpy.spin(node) + except (KeyboardInterrupt, ExternalShutdownException): + pass + + rclpy.shutdown() + + # time.sleep(0.5) + + # # Send request to controller diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index a1dc89f23..f5aaafa2e 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -41,7 +41,11 @@ from tf2_ros import TransformException from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener +import tf2_geometry_msgs # noqa: F401 # pylint: disable=unused-import +from builtin_interfaces.msg import Duration +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint import std_msgs from controller_manager_msgs.srv import SwitchController from geometry_msgs.msg import ( @@ -52,16 +56,19 @@ Twist, Wrench, Vector3, + Vector3Stamped, ) sys.path.append(os.path.dirname(__file__)) from test_common import ( # noqa: E402 + ActionInterface, ControllerManagerInterface, DashboardInterface, ForceModeInterface, IoStatusInterface, ConfigurationInterface, generate_driver_test_description, + ROBOT_JOINTS, ) TIMEOUT_EXECUTE_TRAJECTORY = 30 @@ -101,6 +108,11 @@ def init_robot(self): self._controller_manager_interface = ControllerManagerInterface(self.node) self._io_status_controller_interface = IoStatusInterface(self.node) self._configuration_controller_interface = ConfigurationInterface(self.node) + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) def setUp(self): self._dashboard_interface.start_robot() @@ -137,14 +149,18 @@ def test_force_mode_controller(self, tf_prefix): self._force_mode_controller_interface = ForceModeInterface(self.node) # Create task frame for force mode - point = Point(x=0.8, y=0.8, z=0.8) + point = Point(x=0.0, y=0.0, z=0.0) orientation = Quaternion(x=0.7071, y=0.0, z=0.0, w=0.7071) task_frame_pose = Pose() task_frame_pose.position = point task_frame_pose.orientation = orientation +<<<<<<< HEAD header = std_msgs.msg.Header(frame_id=tf_prefix + "base") header.stamp.sec = int(time.time()) + 1 header.stamp.nanosec = 0 +======= + header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "tool0_controller") +>>>>>>> 8eb4288 ([force mode controller] Fix the task frame orientation (#1379)) frame_stamp = PoseStamped() frame_stamp.header = header frame_stamp.pose = task_frame_pose @@ -154,7 +170,7 @@ def test_force_mode_controller(self, tf_prefix): # Create Wrench message for force mode wrench = Wrench() - wrench.force = Vector3(x=0.0, y=0.0, z=5.0) + wrench.force = Vector3(x=0.0, y=0.0, z=10.0) wrench.torque = Vector3(x=0.0, y=0.0, z=0.0) # Specify interpretation of task frame (no transform) @@ -193,27 +209,46 @@ def test_force_mode_controller(self, tf_prefix): time.sleep(5.0) trans_after = self.lookup_tcp_in_base(tf_prefix, self.node.get_clock().now()) + diff = Vector3Stamped( + vector=Vector3( + x=(trans_after.transform.translation.x - trans_before.transform.translation.x), + y=(trans_after.transform.translation.y - trans_before.transform.translation.y), + z=(trans_after.transform.translation.z - trans_before.transform.translation.z), + ), + header=trans_after.header, + ) + diff_in_tool0_controller = self.tf_buffer.transform( + diff, + tf_prefix + "tool0_controller", + timeout=rclpy.time.Duration(seconds=1.0), + ) # task frame and wrench determines the expected motion # In the example we used - # - a task frame rotated pi/2 deg around the base frame's x axis + # - a task frame rotated pi/2 deg around the tcp frame's x axis # - a wrench with a positive z component for the force - # => we should expect a motion in negative y of the base frame - self.assertTrue(trans_after.transform.translation.y < trans_before.transform.translation.y) + # => we should expect a motion in negative y of the tcp frame, since we didn't to any + # rotation + self.assertTrue( + diff_in_tool0_controller.vector.y < -0.03, + ) self.assertAlmostEqual( - trans_after.transform.translation.x, - trans_before.transform.translation.x, + diff_in_tool0_controller.vector.x, + 0.0, delta=0.001, + msg="X translation should not change", ) self.assertAlmostEqual( - trans_after.transform.translation.z, - trans_before.transform.translation.z, + diff_in_tool0_controller.vector.z, + 0.0, delta=0.001, + msg="Z translation should not change", ) self.assertTrue( are_quaternions_same( trans_after.transform.rotation, trans_before.transform.rotation, 0.001 - ) + ), + msg="Rotation should not change", ) res = self._force_mode_controller_interface.stop_force_mode() @@ -227,6 +262,86 @@ def test_force_mode_controller(self, tf_prefix): ).ok ) +<<<<<<< HEAD +======= + def test_force_mode_controller(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "force_mode_controller", + ], + ).ok + ) + waypts = [[-1.6, -1.55, -1.7, -1.0, 2.05, 0.5]] + time_vec = [Duration(sec=5, nanosec=0)] + test_trajectory = zip(time_vec, waypts) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in test_trajectory + ], + joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.run_force_mode(tf_prefix) + + def test_force_mode_controller_with_passthrough_controller(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + time.sleep(1) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "force_mode_controller", + ], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self.run_force_mode(tf_prefix) + +>>>>>>> 8eb4288 ([force mode controller] Fix the task frame orientation (#1379)) def test_illegal_force_mode_types(self, tf_prefix): self.assertTrue( self._controller_manager_interface.switch_controller( From f972f764c0b97546a0eae86b46e87d87955767f8 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 2 Jun 2025 11:07:33 +0000 Subject: [PATCH 2/2] [force mode controller] Fix the task frame orientation (#1379) Before, we were using a RPY notation. Chaning that to an angle-axis notation should fix things. * Update force_mode example to include a pose that is not in line with one of the main axes * Make test use an arbitraty orientation This should avoid a coincitental overlap if the transformation is wrong. (cherry picked from commit 8eb4288e3099a1c5458ca208d0240c666436fba1) # Conflicts: # ur_robot_driver/test/integration_test_force_mode.py --- .../test/integration_test_force_mode.py | 27 +++---------------- 1 file changed, 4 insertions(+), 23 deletions(-) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index f5aaafa2e..8b059b8fe 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -133,19 +133,9 @@ def lookup_tcp_in_base(self, tf_prefix, timepoint): pass return trans - def test_force_mode_controller(self, tf_prefix): - self.assertTrue( - self._controller_manager_interface.switch_controller( - strictness=SwitchController.Request.BEST_EFFORT, - activate_controllers=[ - "force_mode_controller", - ], - deactivate_controllers=[ - "scaled_joint_trajectory_controller", - "joint_trajectory_controller", - ], - ).ok - ) + # Implementation of force mode test to be reused + # todo: If we move to pytest this could be done using parametrization + def run_force_mode(self, tf_prefix): self._force_mode_controller_interface = ForceModeInterface(self.node) # Create task frame for force mode @@ -154,13 +144,7 @@ def test_force_mode_controller(self, tf_prefix): task_frame_pose = Pose() task_frame_pose.position = point task_frame_pose.orientation = orientation -<<<<<<< HEAD - header = std_msgs.msg.Header(frame_id=tf_prefix + "base") - header.stamp.sec = int(time.time()) + 1 - header.stamp.nanosec = 0 -======= - header = std_msgs.msg.Header(seq=1, frame_id=tf_prefix + "tool0_controller") ->>>>>>> 8eb4288 ([force mode controller] Fix the task frame orientation (#1379)) + header = std_msgs.msg.Header(frame_id=tf_prefix + "tool0_controller") frame_stamp = PoseStamped() frame_stamp.header = header frame_stamp.pose = task_frame_pose @@ -262,8 +246,6 @@ def test_force_mode_controller(self, tf_prefix): ).ok ) -<<<<<<< HEAD -======= def test_force_mode_controller(self, tf_prefix): self.assertTrue( self._controller_manager_interface.switch_controller( @@ -341,7 +323,6 @@ def test_force_mode_controller_with_passthrough_controller(self, tf_prefix): ) self.run_force_mode(tf_prefix) ->>>>>>> 8eb4288 ([force mode controller] Fix the task frame orientation (#1379)) def test_illegal_force_mode_types(self, tf_prefix): self.assertTrue( self._controller_manager_interface.switch_controller(