From b459bd1c202d4615a1eb6c733f9090b754627ce5 Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 2 May 2024 11:52:05 +0000 Subject: [PATCH] Created more examples for the use of the controller. Controller now supports passing through accelerations and velocities. --- examples/example_movej.py | 252 +++++++++++++++++ examples/example_spline.py | 264 ++++++++++++++++++ examples/examples.py | 17 +- examples/load_switch_controller_example.py | 236 ++++++++++++++++ .../passthrough_trajectory_controller.hpp | 11 +- .../src/passthrough_trajectory_controller.cpp | 189 +++++++++---- ur_robot_driver/CMakeLists.txt | 3 + .../ur_robot_driver/hardware_interface.hpp | 3 + ur_robot_driver/launch/ur_control.launch.py | 7 +- ur_robot_driver/src/hardware_interface.cpp | 84 +++--- 10 files changed, 964 insertions(+), 102 deletions(-) create mode 100644 examples/example_movej.py create mode 100644 examples/example_spline.py create mode 100644 examples/load_switch_controller_example.py diff --git a/examples/example_movej.py b/examples/example_movej.py new file mode 100644 index 000000000..2376212e5 --- /dev/null +++ b/examples/example_movej.py @@ -0,0 +1,252 @@ +#!/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 control_msgs.action import JointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ( + UnloadController, + LoadController, + ConfigureController, + SwitchController, + ListControllers, +) +import time + +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.init_robot() + + def init_robot(self): + service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/controller_manager/load_controller": LoadController, + "/controller_manager/unload_controller": UnloadController, + "/controller_manager/configure_controller": ConfigureController, + "/controller_manager/switch_controller": SwitchController, + "/controller_manager/list_controllers": ListControllers, + } + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in service_interfaces.items() + } + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + 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 = JTmsg() + 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, JointTrajectory.Goal(trajectory=joint_trajectory) + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result + + 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()}") + + def load_passthrough_controller(self): + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names = [] + # Find loaded controllers + for controller in list_response.controller: + names.append(controller.name) + # Check whether the passthrough controller is already loaded + try: + names.index("passthrough_trajectory_controller") + except ValueError: + print("Loading controller") + load_request = LoadController.Request() + load_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/load_controller", load_request) + configure_request = ConfigureController.Request() + configure_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/configure_controller", configure_request) + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names.clear() + # Update the list of controller names. + for controller in list_response.controller: + names.append(controller.name) + id = names.index("passthrough_trajectory_controller") + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + if list_response.controller[id].state == "inactive": + switch_request.activate_controllers = ["passthrough_trajectory_controller"] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + try: + id = names.index("scaled_joint_trajectory_controller") + if list_response.controller[id].state == "active": + switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] + except ValueError: + switch_request.deactivate_controllers = [] + finally: + switch_request.strictness = 1 # 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) + self.call_service("/controller_manager/switch_controller", switch_request) + # Try unloading the scaled joint trajectory controller + try: + names.index("scaled_joint_trajectory_controller") + unload_request = UnloadController.Request() + unload_request.name = "scaled_joint_trajectory_controller" + self.call_service("/controller_manager/unload_controller", unload_request) + except ValueError: + print("scaled_joint_trajectory_controller not loaded, skipping unload") + # Connect to the passthrough controller action + finally: + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # 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) + self.call_service("/controller_manager/switch_controller", switch_request) + + +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, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + ] + 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) diff --git a/examples/example_spline.py b/examples/example_spline.py new file mode 100644 index 000000000..059333da7 --- /dev/null +++ b/examples/example_spline.py @@ -0,0 +1,264 @@ +#!/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 control_msgs.action import JointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ( + UnloadController, + LoadController, + ConfigureController, + SwitchController, + ListControllers, +) +import time + +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.init_robot() + + def init_robot(self): + service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/controller_manager/load_controller": LoadController, + "/controller_manager/unload_controller": UnloadController, + "/controller_manager/configure_controller": ConfigureController, + "/controller_manager/switch_controller": SwitchController, + "/controller_manager/list_controllers": ListControllers, + } + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in service_interfaces.items() + } + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + 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, vels, accels): + """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 = JTmsg() + joint_trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.velocities = vels[i] + point.accelerations = accels[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, JointTrajectory.Goal(trajectory=joint_trajectory) + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result + + 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()}") + + def load_passthrough_controller(self): + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names = [] + # Find loaded controllers + for controller in list_response.controller: + names.append(controller.name) + # Check whether the passthrough controller is already loaded + try: + names.index("passthrough_trajectory_controller") + except ValueError: + print("Loading controller") + load_request = LoadController.Request() + load_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/load_controller", load_request) + configure_request = ConfigureController.Request() + configure_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/configure_controller", configure_request) + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names.clear() + # Update the list of controller names. + for controller in list_response.controller: + names.append(controller.name) + id = names.index("passthrough_trajectory_controller") + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + if list_response.controller[id].state == "inactive": + switch_request.activate_controllers = ["passthrough_trajectory_controller"] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + try: + id = names.index("scaled_joint_trajectory_controller") + if list_response.controller[id].state == "active": + switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] + except ValueError: + switch_request.deactivate_controllers = [] + finally: + switch_request.strictness = 1 # 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) + self.call_service("/controller_manager/switch_controller", switch_request) + # Try unloading the scaled joint trajectory controller + try: + names.index("scaled_joint_trajectory_controller") + unload_request = UnloadController.Request() + unload_request.name = "scaled_joint_trajectory_controller" + self.call_service("/controller_manager/unload_controller", unload_request) + except ValueError: + print("scaled_joint_trajectory_controller not loaded, skipping unload") + # Connect to the passthrough controller action + finally: + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # 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) + self.call_service("/controller_manager/switch_controller", switch_request) + + +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, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + ] + 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], + ] + 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], + ] + 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, vels, accels) diff --git a/examples/examples.py b/examples/examples.py index e097f068d..79364c97a 100644 --- a/examples/examples.py +++ b/examples/examples.py @@ -215,6 +215,19 @@ def load_passthrough_controller(self): ) time.sleep(2) + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # 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) + self.call_service("/controller_manager/switch_controller", switch_request) + if __name__ == "__main__": rclpy.init() @@ -224,9 +237,9 @@ def load_passthrough_controller(self): # The following list are arbitrary joint positions, change according to your own needs waypts = [ - [-0.47, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], - [-0.47, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], ] time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] diff --git a/examples/load_switch_controller_example.py b/examples/load_switch_controller_example.py new file mode 100644 index 000000000..c5a5a6ceb --- /dev/null +++ b/examples/load_switch_controller_example.py @@ -0,0 +1,236 @@ +#!/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 control_msgs.action import JointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ( + UnloadController, + LoadController, + ConfigureController, + SwitchController, + ListControllers, +) +import time + +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.init_robot() + + def init_robot(self): + service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/controller_manager/load_controller": LoadController, + "/controller_manager/unload_controller": UnloadController, + "/controller_manager/configure_controller": ConfigureController, + "/controller_manager/switch_controller": SwitchController, + "/controller_manager/list_controllers": ListControllers, + } + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in service_interfaces.items() + } + + 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 = JTmsg() + 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, JointTrajectory.Goal(trajectory=joint_trajectory) + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result + + 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()}") + + def load_passthrough_controller(self): + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names = [] + # Find loaded controllers + for controller in list_response.controller: + names.append(controller.name) + # Check whether the passthrough controller is already loaded + try: + names.index("passthrough_trajectory_controller") + except ValueError: + print("Loading controller") + load_request = LoadController.Request() + load_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/load_controller", load_request) + configure_request = ConfigureController.Request() + configure_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/configure_controller", configure_request) + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names.clear() + # Update the list of controller names. + for controller in list_response.controller: + names.append(controller.name) + id = names.index("passthrough_trajectory_controller") + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + if list_response.controller[id].state == "inactive": + switch_request.activate_controllers = ["passthrough_trajectory_controller"] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + try: + id = names.index("scaled_joint_trajectory_controller") + if list_response.controller[id].state == "active": + switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] + except ValueError: + switch_request.deactivate_controllers = [] + finally: + switch_request.strictness = 1 # 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) + self.call_service("/controller_manager/switch_controller", switch_request) + # Try unloading the scaled joint trajectory controller + try: + names.index("scaled_joint_trajectory_controller") + unload_request = UnloadController.Request() + unload_request.name = "scaled_joint_trajectory_controller" + self.call_service("/controller_manager/unload_controller", unload_request) + except ValueError: + print("scaled_joint_trajectory_controller not loaded, skipping unload") + # Connect to the passthrough controller action + finally: + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # 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) + self.call_service("/controller_manager/switch_controller", switch_request) + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + robot.load_passthrough_controller() diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 78eddc92f..c6cf9f269 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -67,10 +67,11 @@ enum CommandInterfaces PASSTHROUGH_POINT_WRITTEN = 1, PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2, PASSTHROUGH_TRAJECTORY_CANCEL = 3, - PASSTHROUGH_TRAJECTORY_POSITIONS_ = 4, - PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 10, - PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 16, - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 22 + PASSTHROUGH_TRAJECTORY_DIMENSIONS = 4, + PASSTHROUGH_TRAJECTORY_POSITIONS_ = 5, + PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 11, + PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 17, + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 23 }; class PassthroughTrajectoryController : public controller_interface::ControllerInterface @@ -110,12 +111,14 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI void execute(const std::shared_ptr> goal_handle); + int check_dimensions(std::shared_ptr goal); trajectory_msgs::msg::JointTrajectory active_joint_traj_; uint32_t current_point_; bool trajectory_active_; uint64_t active_trajectory_elapsed_time_; uint64_t max_trajectory_time_; double scaling_factor_; + std::shared_ptr> current_handle; }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 049a11052..81935f287 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -48,10 +48,7 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() { passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); - - /*std::cout << "-------------------------------Initialised passthrough " - "controller-------------------------------------------------" - << std::endl;*/ + current_point_ = 0; return controller_interface::CallbackReturn::SUCCESS; } @@ -61,9 +58,6 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre { start_action_server(); trajectory_active_ = false; - /*std::cout << "-------------------------------Configured passthrough " - "controller-------------------------------------------------" - << std::endl;*/ return ControllerInterface::on_configure(previous_state); } @@ -87,12 +81,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st conf.names.push_back(passthrough_params_.speed_scaling_interface_name); - // std::cout << conf.names[0] << " / " << conf.names.size() << std::endl; - - /*std::cout << "------------------------------- Configured passthrough " - "controller state interface -------------------------------------------------" - << std::endl;*/ - return conf; } @@ -103,7 +91,7 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co const std::string tf_prefix = passthrough_params_.tf_prefix; - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_present"); + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_point_written"); @@ -111,6 +99,8 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_cancel"); + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_dimensions"); + for (size_t i = 0; i < 6; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + std::to_string(i)); @@ -126,42 +116,35 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"); - /*std::cout << "------------------------------- Configured passthrough " - "controller command interface -------------------------------------------------" - << std::endl;*/ - return config; } controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { - /*std::cout << "-------------------------------Activated passthrough " - "controller-------------------------------------------------" - << std::endl;*/ return ControllerInterface::on_activate(state); } controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& period) { - static int delay = 500; - // Debug message - if (!delay) { - std::cout << "-------------------------------Updated passthrough " - "controller-------------------------------------------------" - << std::endl; - delay--; - } - if (delay) - delay--; - if (trajectory_active_) { + // static int delay = 1500; + // if (!delay) { + // std::cout << "Speed scaling : " << state_interfaces_[0].get_value() << std::endl; + // std::cout << "Trajectory executing : " << trajectory_active_ << std::endl; + // delay = 1500; + // } + + // if (delay) + // delay--; + + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 3.0) { scaling_factor_ = state_interfaces_[0].get_value(); active_trajectory_elapsed_time_ += static_cast(scaling_factor_ * ((period.seconds() * pow(10, 9)) + period.nanoseconds())); - if (active_trajectory_elapsed_time_ > max_trajectory_time_) { - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " - "if it is not."); + if (active_trajectory_elapsed_time_ > max_trajectory_time_ && trajectory_active_) { + RCLCPP_WARN(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " + "if it is not."); trajectory_active_ = false; } } @@ -172,50 +155,118 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory to forward to robot"); - // Precondition: Running controller if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectories. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } + + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); + return rclcpp_action::GoalResponse::REJECT; + } // Check dimensions of the trajectory + if (check_dimensions(goal) == 0) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are wrong."); + return rclcpp_action::GoalResponse::REJECT; + } else { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_DIMENSIONS].set_value( + static_cast(check_dimensions(goal))); + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} +// This function should be told how many robot joints there are. Not just run with 6 or 0. +int PassthroughTrajectoryController::check_dimensions( + std::shared_ptr goal) +{ for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + std::string msg; if (goal->trajectory.points[i].positions.size() != 6) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have positions " "for all joints of the robot. (6 joint positions per point)"); - return rclcpp_action::GoalResponse::REJECT; + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].positions.size()) + " positions."; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + return 0; + } + if (goal->trajectory.points[i].velocities.size() != 0 && goal->trajectory.points[i].velocities.size() != 6) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have velocities " + "for all joints of the robot. (6 joint velocities per point)"); + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + " velocities."; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + return 0; + } + if (goal->trajectory.points[i].accelerations.size() != 0 && goal->trajectory.points[i].accelerations.size() != 6) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have " + "accelerations " + "for all joints of the robot. (6 joint accelerations per point)"); + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + return 0; } } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + if (goal->trajectory.points[0].velocities.size() == 6 && goal->trajectory.points[0].accelerations.size() == 0) { + /*If there are only positions and velocities defined */ + return 2; + } else if (goal->trajectory.points[0].velocities.size() == 0 && + goal->trajectory.points[0].accelerations.size() == 6) { + /*If there are only positions and accelerations defined */ + return 3; + } else if (goal->trajectory.points[0].velocities.size() == 6 && + goal->trajectory.points[0].accelerations.size() == 6) { + /*If there are both positions, velocities and accelerations defined */ + return 4; + } else { + /*If there are only positions defined */ + return 1; + } } rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( const std::shared_ptr> goal_handle) { - RCLCPP_INFO(get_node()->get_logger(), "Canceling active trajectory because cancel callback received."); + RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory because cancel callback received."); command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); current_point_ = 0; command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(1.0); std::shared_ptr result = std::make_shared(); goal_handle->canceled(result); + trajectory_active_ = false; return rclcpp_action::CancelResponse::ACCEPT; } void PassthroughTrajectoryController::goal_accepted_callback( const std::shared_ptr> goal_handle) { + current_handle = goal_handle; RCLCPP_INFO(get_node()->get_logger(), "Accepted new trajectory."); trajectory_active_ = true; active_trajectory_elapsed_time_ = 0; current_point_ = 0; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(0.0); + active_joint_traj_ = goal_handle->get_goal()->trajectory; + max_trajectory_time_ = (active_joint_traj_.points.back().time_from_start.sec * pow(10, 9)) + active_joint_traj_.points.back().time_from_start.nanosec; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS].set_value( active_joint_traj_.points.size()); + + std::cout << "Size of positions: " << active_joint_traj_.points[0].positions.size() << std::endl; + + std::cout << "Size of velocities: " << active_joint_traj_.points[0].velocities.size() << std::endl; + + std::cout << "Size of accelerations: " << active_joint_traj_.points[0].accelerations.size() << std::endl; + + command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(1.0); command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); + std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } .detach(); return; @@ -224,11 +275,10 @@ void PassthroughTrajectoryController::goal_accepted_callback( void PassthroughTrajectoryController::execute( const std::shared_ptr> goal_handle) { - std::cout << "------------------Entered Execute loop--------------------------" << std::endl; - rclcpp::Rate loop_rate(500); + rclcpp::Rate loop_rate(200); while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { - std::cout << "------------------Entered cancel--------------------------" << std::endl; + RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); std::shared_ptr result = std::make_shared(); goal_handle->abort(result); @@ -244,31 +294,50 @@ void PassthroughTrajectoryController::execute( return; } - // Write a new point to the command interface, if the previous point has been written to the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ < active_joint_traj_.points.size()) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( - active_joint_traj_.points[current_point_].time_from_start.sec + - (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); - for (int i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_point_].positions[i]); + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { + // Write a new point to the command interface, if the previous point has been written to the hardware interface. + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && + current_point_ < active_joint_traj_.points.size()) { + // Write the time_from_start parameter. + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + active_joint_traj_.points[current_point_].time_from_start.sec + + (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + // Write the positions for each joint of the robot + for (int i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + active_joint_traj_.points[current_point_].positions[i]); + // Optionally, also write velocities and accelerations for each joint. + if (active_joint_traj_.points[current_point_].velocities.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( + active_joint_traj_.points[current_point_].velocities[i]); + } + + if (active_joint_traj_.points[current_point_].accelerations.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( + active_joint_traj_.points[current_point_].accelerations[i]); + } + } + // Tell hardware interface that this point is ready to be read. + command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); + current_point_++; + } + // Check if all points have been written to the hardware + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && + current_point_ == active_joint_traj_.points.size()) { + RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " + "execute!"); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); } - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); - current_point_++; - } - // Check if all points have been written to the hardware - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ == active_joint_traj_.points.size() && - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajecotry will now execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); } + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { std::shared_ptr result = std::make_shared(); goal_handle->succeed(result); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); + std::cout << "It took this long: " << active_trajectory_elapsed_time_ << std::endl; command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); + trajectory_active_ = false; return; } loop_rate.sleep(); diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 4cfbc383a..1231dd8c4 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -146,6 +146,9 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS scripts/tool_communication.py ../examples/examples.py + ../examples/load_switch_controller_example.py + ../examples/example_movej.py + ../examples/example_spline.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 2b977c0e6..c506f985d 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -197,6 +197,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double passthrough_trajectory_cancel_; double passthrough_point_written_; double passthrough_trajectory_number_of_points_; + double passthrough_trajectory_dimensions_; std::array passthrough_trajectory_positions_; std::array passthrough_trajectory_velocities_; std::array passthrough_trajectory_accelerations_; @@ -224,6 +225,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double robot_program_running_copy_; bool passthrough_trajectory_executing_; std::vector> trajectory_joint_positions_; + std::vector> trajectory_joint_velocities_; + std::vector> trajectory_joint_accelerations_; std::vector trajectory_times_; PausingState pausing_state_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index aabe2f9c2..e29269ce7 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -157,9 +157,12 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "scaled_joint_trajectory_controller", + ] + controllers_inactive = [ + "forward_position_controller", "passthrough_trajectory_controller", ] - controllers_inactive = ["forward_position_controller", "scaled_joint_trajectory_controller"] controller_spawners = [controller_spawner(controllers_active)] + [ controller_spawner(controllers_inactive, active=False) @@ -325,7 +328,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", - default_value="passthrough_trajectory_controller", + default_value="scaled_joint_trajectory_controller", description="Initially loaded robot controller.", ) ) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index edb075bcb..592d91ce1 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -95,6 +95,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; passthrough_trajectory_executing_ = false; + passthrough_trajectory_transfer_state_ = 0.0; + trajectory_joint_positions_.clear(); + trajectory_joint_velocities_.clear(); + trajectory_joint_accelerations_.clear(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -313,6 +317,9 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_cancel", &passthrough_trajectory_cancel_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "passthrough_controller", "passthrough_trajectory_dimensions", &passthrough_trajectory_dimensions_)); + for (size_t i = 0; i < 6; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_positions_" + std::to_string(i), @@ -914,6 +921,10 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { passthrough_trajectory_controller_running_ = false; + passthrough_trajectory_cancel_ = 1; + trajectory_joint_positions_.clear(); + trajectory_joint_accelerations_.clear(); + trajectory_joint_velocities_.clear(); std::cout << "---------------------Stopped passthrough controller---------------" << std::endl; } @@ -947,55 +958,60 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; - if (passthrough_trajectory_transfer_state_ == 1.0) { + if (passthrough_trajectory_transfer_state_ == 1.0 && passthrough_point_written_ == 0.0) { trajectory_joint_positions_.push_back(passthrough_trajectory_positions_); + trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time); last_time = passthrough_trajectory_time_from_start_; passthrough_point_written_ = 1.0; + if (passthrough_trajectory_dimensions_ == 2.0 || passthrough_trajectory_dimensions_ == 4.0) { + trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); + } + if (passthrough_trajectory_dimensions_ == 3.0 || passthrough_trajectory_dimensions_ == 4.0) { + trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_); + } } else if (passthrough_trajectory_transfer_state_ == 2.0) { + std::cout << " Passing through " + std::to_string(trajectory_joint_positions_.size()) + " Points to the robot." + << std::endl; + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, trajectory_joint_positions_.size()); - for (int i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]); + if (passthrough_trajectory_dimensions_ == 1.0) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]); + } + } else if (passthrough_trajectory_dimensions_ == 2.0) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], + trajectory_times_[i]); + } + } else if (passthrough_trajectory_dimensions_ == 3.0) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_accelerations_[i], + trajectory_times_[i]); + } + } else if (passthrough_trajectory_dimensions_ == 4.0) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], + trajectory_joint_accelerations_[i], trajectory_times_[i]); + } } + trajectory_joint_positions_.clear(); + trajectory_joint_accelerations_.clear(); + trajectory_joint_velocities_.clear(); passthrough_trajectory_transfer_state_ = 3.0; } - - // if (passthrough_trajectory_transfer_state_ == 1.0) { - // if (!passthrough_trajectory_executing_) { - // ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - // static_cast(passthrough_trajectory_number_of_points_)); - // passthrough_trajectory_executing_ = true; - // std::cout << "Received points to passthrough: " << passthrough_trajectory_number_of_points_ << std::endl; - // } - // if (passthrough_trajectory_executing_ && passthrough_point_written_ == 0.0) { - // std::cout << "Writing point to robot with time parameter: " << passthrough_trajectory_time_from_start_ - // << std::endl; - - // bool status = ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false, - // passthrough_trajectory_time_from_start_ - last_time); - - // std::cout << "Status of write: " << status << std::endl; - // if (!status) { - // std::cout << "Write failed, cancelling trajectory" << std::endl; - // passthrough_trajectory_cancel_ = 1.0; - // std::cout << "----------------------------Cancelling trajectory in hardware interface------------------" - // << std::endl; - // ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); - // } else { - // last_time = passthrough_trajectory_time_from_start_; - // passthrough_point_written_ = 1.0; - // } - // } - // } else { - // passthrough_trajectory_executing_ = false; - // } } void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) { std::cout << "-------------------Triggered trajectory callback!--------------------------" << std::endl; - passthrough_trajectory_transfer_state_ = 4.0; + if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) { + passthrough_trajectory_transfer_state_ = 4.0; + std::cout << "-------------------Trajectory was successful!--------------------------" << std::endl; + } else { + passthrough_trajectory_cancel_ = 1.0; + } std::cout << "Result is: " << int(result) << std::endl; passthrough_trajectory_executing_ = false; return;