Update controller to match updated service definition

Also add example of using force mode controller

ur_controllers/src/force_mode_controller.cpp

@@ -217,12 +217,12 @@ bool ForceModeController::setForceMode(const ur_msgs::srv::SetForceMode::Request
 
   /* The limits specifies the maximum allowed speed along/around compliant axes. For non-compliant axes this value is
    * the maximum allowed deviation between actual tcp position and the one that has been programmed. */
-  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits.twist.linear.x);
-  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits.twist.linear.y);
-  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits.twist.linear.z);
-  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits.twist.angular.x);
-  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits.twist.angular.y);
-  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits.twist.angular.z);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_X].set_value(req->limits[0]);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Y].set_value(req->limits[1]);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_Z].set_value(req->limits[2]);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RX].set_value(req->limits[3]);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RY].set_value(req->limits[4]);
+  command_interfaces_[CommandInterfaces::FORCE_MODE_LIMITS_RZ].set_value(req->limits[5]);
 
   /* The type decides how the robot interprets the force frame (the one defined in task_frame). See ur_script manual for
    * explanation, under force_mode. */

ur_robot_driver/CMakeLists.txt

@@ -149,6 +149,8 @@ install(PROGRAMS
   scripts/tool_communication.py
   scripts/wait_for_robot_description
   scripts/start_ursim.sh
+  examples/examples.py
+  examples/force_mode.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

ur_robot_driver/examples/examples.py

@@ -0,0 +1,175 @@
+#!/usr/bin/env python3
+# Copyright 2024, 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:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the {copyright_holder} nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import rclpy
+from builtin_interfaces.msg import Duration
+from control_msgs.action import FollowJointTrajectory
+
+from rclpy.action import ActionClient
+from rclpy.node import Node
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+from ur_msgs.srv import SetIO
+from controller_manager_msgs.srv import SwitchController
+from std_srvs.srv import Trigger
+
+TIMEOUT_WAIT_SERVICE = 10
+TIMEOUT_WAIT_SERVICE_INITIAL = 60
+TIMEOUT_WAIT_ACTION = 10
+
+ROBOT_JOINTS = [
+    "shoulder_pan_joint",
+    "shoulder_lift_joint",
+    "elbow_joint",
+    "wrist_1_joint",
+    "wrist_2_joint",
+    "wrist_3_joint",
+]
+
+
+# Helper functions
+def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE):
+    client = node.create_client(srv_type, srv_name)
+    if client.wait_for_service(timeout) is False:
+        raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}")
+
+    node.get_logger().info(f"Successfully connected to service '{srv_name}'")
+    return client
+
+
+def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION):
+    client = ActionClient(node, action_type, action_name)
+    if client.wait_for_server(timeout) is False:
+        raise Exception(
+            f"Could not reach action server '{action_name}' within timeout of {timeout}"
+        )
+
+    node.get_logger().info(f"Successfully connected to action '{action_name}'")
+    return client
+
+
+class Robot:
+    def __init__(self, node):
+        self.node = node
+        self.service_interfaces = {
+            "/io_and_status_controller/set_io": SetIO,
+            "/dashboard_client/play": Trigger,
+            "/controller_manager/switch_controller": SwitchController,
+        }
+        self.init_robot()
+
+    def init_robot(self):
+        self.service_clients = {
+            srv_name: waitForService(self.node, srv_name, srv_type)
+            for (srv_name, srv_type) in self.service_interfaces.items()
+        }
+
+        self.jtc_action_client = waitForAction(
+            self.node,
+            "/scaled_joint_trajectory_controller/follow_joint_trajectory",
+            FollowJointTrajectory,
+        )
+
+    def set_io(self, pin, value):
+        """Test to set an IO."""
+        set_io_req = SetIO.Request()
+        set_io_req.fun = 1
+        set_io_req.pin = pin
+        set_io_req.state = value
+
+        self.call_service("/io_and_status_controller/set_io", set_io_req)
+
+    def send_trajectory(self, waypts, time_vec):
+        """Send robot trajectory."""
+        if len(waypts) != len(time_vec):
+            raise Exception("waypoints vector and time vec should be same length")
+
+        # Construct test trajectory
+        joint_trajectory = JointTrajectory()
+        joint_trajectory.joint_names = ROBOT_JOINTS
+        for i in range(len(waypts)):
+            point = JointTrajectoryPoint()
+            point.positions = waypts[i]
+            point.time_from_start = time_vec[i]
+            joint_trajectory.points.append(point)
+
+        # Sending trajectory goal
+        goal_response = self.call_action(
+            self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory)
+        )
+        if not goal_response.accepted:
+            raise Exception("trajectory was not accepted")
+
+        # Verify execution
+        result = self.get_result(self.jtc_action_client, goal_response)
+        return result.error_code == FollowJointTrajectory.Result.SUCCESSFUL
+
+    def call_service(self, srv_name, request):
+        future = self.service_clients[srv_name].call_async(request)
+        rclpy.spin_until_future_complete(self.node, future)
+        if future.result() is not None:
+            return future.result()
+        else:
+            raise Exception(f"Exception while calling service: {future.exception()}")
+
+    def call_action(self, ac_client, g):
+        future = ac_client.send_goal_async(g)
+        rclpy.spin_until_future_complete(self.node, future)
+
+        if future.result() is not None:
+            return future.result()
+        else:
+            raise Exception(f"Exception while calling action: {future.exception()}")
+
+    def get_result(self, ac_client, goal_response):
+        future_res = ac_client._get_result_async(goal_response)
+        rclpy.spin_until_future_complete(self.node, future_res)
+        if future_res.result() is not None:
+            return future_res.result().result
+        else:
+            raise Exception(f"Exception while calling action: {future_res.exception()}")
+
+
+if __name__ == "__main__":
+    rclpy.init()
+    node = Node("robot_driver_test")
+    robot = Robot(node)
+
+    # The following list are arbitrary joint positions, change according to your own needs
+    waypts = [
+        [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
+        [-1.2, -1.4, -1.9, -1.2, 1.5951, -0.0311],
+        [-1.6006, -1.7272, -2.2030, -0.8079, 1.5951, -0.0311],
+    ]
+    time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)]
+
+    # Execute trajectory on robot, make sure that the robot is booted and the control script is running
+    robot.send_trajectory(waypts, time_vec)
+
+    # Set digital output 1 to true
+    robot.set_io(1, 1.0)

ur_robot_driver/examples/force_mode.py

@@ -0,0 +1,136 @@
+#!/usr/bin/env python3
+# Copyright 2024, 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:
+#
+#    * Redistributions of source code must retain the above copyright
+#      notice, this list of conditions and the following disclaimer.
+#
+#    * Redistributions in binary form must reproduce the above copyright
+#      notice, this list of conditions and the following disclaimer in the
+#      documentation and/or other materials provided with the distribution.
+#
+#    * Neither the name of the {copyright_holder} nor the names of its
+#      contributors may be used to endorse or promote products derived from
+#      this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import time
+
+import rclpy
+from rclpy.node import Node
+from controller_manager_msgs.srv import SwitchController
+from builtin_interfaces.msg import Duration
+from std_msgs.msg import Header
+from std_srvs.srv import Trigger
+
+from geometry_msgs.msg import (
+    Point,
+    Quaternion,
+    Pose,
+    PoseStamped,
+    Wrench,
+    WrenchStamped,
+    Vector3,
+)
+
+from ur_msgs.srv import SetForceMode
+
+from examples import Robot
+
+if __name__ == "__main__":
+    rclpy.init()
+    node = Node("robot_driver_test")
+    robot = Robot(node)
+
+    # Activate force mode controller
+    robot.call_service(
+        "/controller_manager/switch_controller",
+        SwitchController.Request(
+            activate_controllers=["force_mode_controller", "scaled_joint_trajectory_controller"],
+            strictness=SwitchController.Request.BEST_EFFORT,
+        ),
+    )
+
+    # Add force mode service to service interfaces and re-init robot
+    robot.service_interfaces.update({"/force_mode_controller/start_force_mode": SetForceMode})
+    robot.init_robot()
+    time.sleep(2)
+    # Press play on the robot
+    robot.call_service("/dashboard_client/play", Trigger.Request())
+
+    # Move robot in to position
+    robot.send_trajectory(
+        waypts=[[-1.5707, -1.5707, -1.5707, -1.5707, 1.5707, 0]],
+        time_vec=[Duration(sec=6, nanosec=0)],
+    )
+
+    # 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=0.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=-10.0), torque=Vector3(x=0.0, y=0.0, z=0.0))
+    wrench_stamp = WrenchStamped(header=header, wrench=wrench_vec)
+    # Specify interpretation of task frame (no transform)
+    type_spec = SetForceMode.Request.NO_TRANSFORM
+
+    # Specify max speeds and deviations of force mode
+    limits = [1.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+    # specify damping and gain scaling
+    damping_factor = 0.025
+    gain_scale = 0.5
+
+    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_stamp
+    req.type = type_spec
+    req.limits = limits
+    req.damping_factor = damping_factor
+    req.gain_scaling = gain_scale
+
+    # Send request to controller
+    robot.call_service("/force_mode_controller/start_force_mode", req)
+
+    time.sleep(15)
+    # Deactivate force mode controller
+    robot.call_service(
+        "/controller_manager/switch_controller",
+        SwitchController.Request(
+            deactivate_controllers=["force_mode_controller"],
+            strictness=SwitchController.Request.BEST_EFFORT,
+        ),
+    )

ur_robot_driver/test/robot_driver.py

@@ -48,9 +48,7 @@
     Quaternion,
     Point,
     WrenchStamped,
-    TwistStamped,
     Wrench,
-    Twist,
     Vector3,
 )
 from ur_msgs.msg import IOStates
@@ -186,15 +184,10 @@ def test_force_mode_controller(self, tf_prefix):
         type_spec = 2
 
         # Specify max speeds and deviations of force mode
-        limits = Twist()
-        limits.linear = Vector3(x=1.1, y=1.1, z=1.1)
-        limits.angular = limits.linear
-        limits_stamp = TwistStamped()
-        limits_stamp.header = header
-        limits_stamp.twist = limits
+        limits = [1.0, 1.0, 1.0, 0.5, 1.0, 1.0, 1.0]
 
         # specify damping and gain scaling
-        damping_factor = 0.8
+        damping_factor = 0.1
         gain_scale = 0.8
 
         # Send request to controller
@@ -208,7 +201,7 @@ def test_force_mode_controller(self, tf_prefix):
             selection_vector_rz=compliance[5],
             wrench=wrench_stamp,
             type=type_spec,
-            limits=limits_stamp,
+            limits=limits,
             damping_factor=damping_factor,
             gain_scaling=gain_scale,
         )