Moved example move, updated cmake

Author: URJala

ur_robot_driver/CMakeLists.txt

@@ -159,8 +159,13 @@ ament_python_install_package(${PROJECT_NAME})
 install(PROGRAMS
   scripts/tool_communication.py
   scripts/wait_for_robot_description
-  scripts/example_move.py
   scripts/start_ursim.sh
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# Install examples
+install(PROGRAMS
+  examples/example_move.py
   examples/force_mode.py
   examples/robot_class.py
   examples/passthrough.py

ur_robot_driver/scripts/example_move.py renamed to ur_robot_driver/examples/example_move.py

@@ -46,6 +46,14 @@
         [-0.5, -2.0, -0.5, -2.0, -0.4, -1.0],
         [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406],
     ]
+    time_vec = [
+        Duration(sec=2, nanosec=0),
+        Duration(sec=6, nanosec=0),
+        Duration(sec=10, nanosec=0),
+        Duration(sec=14, nanosec=0),
+        Duration(sec=18, nanosec=0),
+    ]
+
     # Velocities and accelerations can be omitted
     vels = [
         [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
@@ -61,35 +69,10 @@
         [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
         [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
     ]
-    bad_vels = [
-        [0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-    ]
-    bad_accels = [
-        [0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-        [0.1, 0.1, 0.1, 0.1, 0.1, 0.1],
-    ]
-    time_vec = [
-        Duration(sec=2, nanosec=0),
-        Duration(sec=6, nanosec=0),
-        Duration(sec=10, nanosec=0),
-        Duration(sec=14, nanosec=0),
-        Duration(sec=18, nanosec=0),
-    ]
 
     # Execute trajectory on robot, make sure that the robot is booted and the control script is running
     robot.play()
     # Trajectory using positions, velocities and accelerations
     robot.passthrough_trajectory(waypts, time_vec, vels, accels)
     # Trajectory using only positions
     robot.passthrough_trajectory(waypts, time_vec)
-
-    robot.passthrough_trajectory(waypts, time_vec, bad_vels, accels)
-
-    robot.passthrough_trajectory(waypts, time_vec, vels, bad_accels)

ur_robot_driver/examples/passthrough.py

@@ -35,7 +35,6 @@
 from rclpy.node import Node
 from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint
 from ur_msgs.srv import SetIO, SetForceMode
-from ur_msgs.action import TrajectoryUntil, ToolContact
 from controller_manager_msgs.srv import (
     UnloadController,
     LoadController,
@@ -51,6 +50,8 @@
 TIMEOUT_WAIT_SERVICE_INITIAL = 60
 TIMEOUT_WAIT_ACTION = 10
 
+MOTION_CONTROLLERS = ["passthrough_trajectory_controller", "scaled_joint_trajectory_controller"]
+
 ROBOT_JOINTS = [
     "shoulder_pan_joint",
     "shoulder_lift_joint",
@@ -148,7 +149,7 @@ def set_io(self, pin, value):
 
         self.call_service(Services.Set_IO, set_io_req)
 
-    def follow_trajectory(self, waypts, time_vec):
+    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"],
@@ -185,10 +186,10 @@ def follow_trajectory(self, waypts, time_vec):
 
     def passthrough_trajectory(
         self,
-        waypts: list[float],
+        waypts: list[list[float]],
         time_vec: list[float],
-        vels: list[float] = [],
-        accels: list[float] = [],
+        vels: list[list[float]] = [],
+        accels: list[list[float]] = [],
         goal_time_tolerance=Duration(sec=1),
     ):
         # The scaled joint controller can't be active at the same time as the passthrough controller
@@ -219,9 +220,7 @@ def passthrough_trajectory(
             raise Exception("trajectory was not accepted")
 
         # Verify execution
-        result = self.get_result(
-            Actions.PASSTHROUGH_TRAJECTORY, goal_response
-        )
+        result = self.get_result(Actions.PASSTHROUGH_TRAJECTORY, goal_response)
         return result
 
     def call_service(self, Service: Services, request):
@@ -255,7 +254,7 @@ def load_controller(self, controller_name: str):
         # Find loaded controllers
         for controller in list_response.controller:
             names.append(controller.name)
-        # Check whether the passthrough controller is already loaded
+        # Check whether the controller is already loaded
         try:
             names.index(controller_name)
         except ValueError:
@@ -264,9 +263,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 = robot.call_service(
-                Services.List_Controllers, ListControllers.Request()
-            )
+            list_response = self.call_service(Services.List_Controllers, ListControllers.Request())
             names.clear()
             # Update the list of controller names.
             for controller in list_response.controller:
@@ -286,10 +283,10 @@ def switch_controllers(self, active: list[str] = [], inactive: list[str] = []) -
         switch_request.activate_asap = False
         switch_request.timeout = Duration(sec=2, nanosec=0)
         return self.call_service(Services.Switch_Controller, switch_request)
-    
+
     def start_force_mode(self, req: SetForceMode.Request):
         return self.call_service(Services.start_force_mode, req)
-    
+
     def stop_force_mode(self):
         return self.call_service(Services.stop_force_mode, Trigger.Request())
 
@@ -299,15 +296,15 @@ def stop_force_mode(self):
     node = Node("robot_driver_test")
     print("Available actions:")
     for action in Actions:
-        print(action.name)
-        print(action.value.name)
-        print(action.value.action_type)
+        print("Action Enum name: ", action.name)
+        print("Action Ros name: ", action.value.name)
+        print("Action Ros type: ", action.value.action_type)
     print("Available services:")
     for service in Services:
-        print(service.name)
-        print(service.value.name)
-        print(service.value.service_type)
-    
+        print("Service Enum name: ", service.name)
+        print("Service Ros name: ", service.value.name)
+        print("Service Ros type: ", service.value.service_type)
+
     # robot = Robot(node)
     # robot.switch_controllers(
     #     ["passthrough_trajectory_controller"], ["scaled_joint_trajectory_controller"]