Saving JTC example, but might not be needed

Author: URJala

ur_robot_driver/CMakeLists.txt

@@ -170,6 +170,7 @@ install(PROGRAMS
   examples/robot_class.py
   examples/passthrough.py
   examples/scaled_joint.py
+  examples/jtc.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

ur_robot_driver/examples/jtc.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+# Copyright 2024, Universal Robots
+#
+# 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 rclpy.node import Node
+from robot_class import Robot, Actions
+from control_msgs.action import FollowJointTrajectory
+from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
+import time
+
+if __name__ == "__main__":
+    rclpy.init()
+    node = Node("robot_driver_test")
+    robot = Robot(node)
+
+    robot.switch_motion_controller("scaled_joint_trajectory_controller")
+    robot.play()
+    goal = FollowJointTrajectory.Goal()
+    # The following list are arbitrary joint positions, change according to your own needs
+    pts = [[robot.HOME[i] + j for i in range(6)] for j in [0, 1, -1]]
+    vels = [
+        [
+            0.0,
+            0.0,
+            0.0,
+            1.0,
+            0.0,
+            0.0,
+        ],
+        [
+            0.0,
+            0.0,
+            0.0,
+            2.0,
+            0.0,
+            0.0,
+        ],
+        [0.0 for i in range(6)],
+    ]
+    time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)]
+    t = JointTrajectory()
+    t.joint_names = robot.joints
+    for idx, v in enumerate(vels):
+        p = JointTrajectoryPoint()
+        p.positions = pts[idx]
+        p.velocities = v
+        p.time_from_start = time_vec[idx]
+        t.points.append(p)
+
+    goal.trajectory = t
+    # Execute trajectory on robot, make sure that the robot is booted and the control script is running
+
+    res = robot.call_action(Actions.FOLLOW_TRAJECTORY, goal)
+    if not res.accepted:
+        print("goal not accepted")
+        quit()
+    time.sleep(1)
+    rel = robot.get_result(Actions.FOLLOW_TRAJECTORY, res)
+    print(rel)

ur_robot_driver/examples/robot_class.py

@@ -50,7 +50,14 @@
 TIMEOUT_WAIT_SERVICE_INITIAL = 60
 TIMEOUT_WAIT_ACTION = 10
 
-MOTION_CONTROLLERS = ["passthrough_trajectory_controller", "scaled_joint_trajectory_controller"]
+# Only one of these can be active at any given time
+MOTION_CONTROLLERS = [
+    "passthrough_trajectory_controller",
+    "scaled_joint_trajectory_controller",
+    "joint_trajectory_controller",
+    "freedrive_mode_controller",
+]
+
 
 ROBOT_JOINTS = [
     "shoulder_pan_joint",
@@ -60,10 +67,15 @@
     "wrist_2_joint",
     "wrist_3_joint",
 ]
+
 Action_tuple = namedtuple("Actions", ["name", "action_type"])
 
 
 class Actions(Enum):
+    JTC = Action_tuple(
+        "/joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory
+    )
+
     PASSTHROUGH_TRAJECTORY = Action_tuple(
         "/passthrough_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory
     )
@@ -114,9 +126,10 @@ class Robot:
     def __init__(self, node):
         self.node = node
         self.init_robot()
+        self.HOME = [0.0, -1.5708, 0.0, -1.5708, 0.0, 0.0]
 
     def init_robot(self):
-
+        self.joints = ROBOT_JOINTS
         self.service_clients = {
             service.name: waitForService(self.node, service.value.name, service.value.service_type)
             for (service) in Services
@@ -151,14 +164,7 @@ def set_io(self, pin, value):
 
     def follow_trajectory(self, waypts: list[list[float]], time_vec: list[float]):
         # No other motion controllers can be active at the same time as the scaled joint controller
-        self.switch_controllers(
-            ["scaled_joint_trajectory_controller"],
-            [
-                "passthrough_trajectory_controller",
-                "force_mode_controller",
-                "freedrive_mode_controller",
-            ],
-        )
+        self.switch_motion_controller("scaled_joint_trajectory_controller")
         """Send robot trajectory."""
         if len(waypts) != len(time_vec):
             raise Exception("waypoints vector and time vec should be same length")
@@ -193,9 +199,7 @@ def passthrough_trajectory(
         goal_time_tolerance=Duration(sec=1),
     ):
         # The scaled joint controller can't be active at the same time as the passthrough controller
-        self.switch_controllers(
-            ["passthrough_trajectory_controller"], ["scaled_joint_trajectory_controller"]
-        )
+        self.switch_motion_controller("passthrough_trajectory_controller")
         """Send trajectory through the passthrough controller."""
         if len(waypts) != len(time_vec):
             raise Exception("waypoints vector and time vec should be same length.")
@@ -231,8 +235,8 @@ def call_service(self, Service: Services, request):
         else:
             raise Exception(f"Exception while calling service: {future.exception()}")
 
-    def call_action(self, Action: Actions, g):
-        future = self.action_clients[Action.name].send_goal_async(g)
+    def call_action(self, Action: Actions, goal):
+        future = self.action_clients[Action.name].send_goal_async(goal)
         rclpy.spin_until_future_complete(self.node, future)
 
         if future.result() is not None:
@@ -248,10 +252,14 @@ def get_result(self, Action: Actions, goal_response):
         else:
             raise Exception(f"Exception while calling action: {future_res.exception()}")
 
-    def load_controller(self, controller_name: str):
+    def list_controllers(self):
         list_response = self.call_service(Services.List_Controllers, ListControllers.Request())
-        names = []
+        return list_response
+
+    def load_controller(self, controller_name: str):
+        list_response = self.list_controllers()
         # Find loaded controllers
+        names = []
         for controller in list_response.controller:
             names.append(controller.name)
         # Check whether the controller is already loaded
@@ -263,7 +271,7 @@ def load_controller(self, controller_name: str):
             self.call_service(Services.Load_Controller, load_request)
             configure_request = ConfigureController.Request(name=controller_name)
             self.call_service(Services.Configure_Controller, configure_request)
-            list_response = self.call_service(Services.List_Controllers, ListControllers.Request())
+            list_response = self.list_controllers()
             names.clear()
             # Update the list of controller names.
             for controller in list_response.controller:
@@ -273,17 +281,33 @@ def load_controller(self, controller_name: str):
         finally:
             print(f"Currently loaded controllers: {names}")
 
-    def switch_controllers(self, active: list[str] = [], inactive: list[str] = []) -> bool:
+    def switch_controllers(self, activate: list[str] = [], deactivate: list[str] = []) -> bool:
+        controllers = self.list_controllers()
+        activate_ = []
+        deactivate_ = []
+        for controller in controllers.controller:
+            if controller.name in activate and controller.state == "inactive":
+                activate_.append(controller.name)
+            elif controller.name in deactivate and controller.state == "active":
+                deactivate_.append(controller.name)
+
         switch_request = SwitchController.Request()
-        switch_request.activate_controllers = active
-        switch_request.deactivate_controllers = inactive
+        switch_request.activate_controllers = activate_
+        switch_request.deactivate_controllers = deactivate_
         switch_request.strictness = (
             SwitchController.Request.BEST_EFFORT
         )  # Best effort switching, will not terminate program if controller is already running
         switch_request.activate_asap = False
         switch_request.timeout = Duration(sec=2, nanosec=0)
         return self.call_service(Services.Switch_Controller, switch_request)
 
+    def switch_motion_controller(self, motion_controller: str):
+        if motion_controller not in MOTION_CONTROLLERS:
+            print("Requested motion controller does not exist.")
+        return self.switch_controllers(
+            [motion_controller], [i for i in MOTION_CONTROLLERS if motion_controller not in i]
+        )
+
     def start_force_mode(self, req: SetForceMode.Request):
         return self.call_service(Services.start_force_mode, req)