From 893ea0c4d878c96b257466c164d2a2b656998b79 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Fri, 16 Feb 2024 17:23:38 +0100 Subject: [PATCH 1/8] Add tendon transmission node. Needs testing. No offsetting yet. --- helix_bringup/launch/helix_bringup.launch.py | 8 ++ .../helix_transmission/__init__.py | 0 .../helix_transmission/tendon_transmission.py | 75 +++++++++++++++++++ helix_transmission/package.xml | 18 +++++ .../resource/helix_transmission | 0 helix_transmission/setup.cfg | 4 + helix_transmission/setup.py | 25 +++++++ run.sh | 1 + 8 files changed, 131 insertions(+) create mode 100644 helix_transmission/helix_transmission/__init__.py create mode 100644 helix_transmission/helix_transmission/tendon_transmission.py create mode 100644 helix_transmission/package.xml create mode 100644 helix_transmission/resource/helix_transmission create mode 100644 helix_transmission/setup.cfg create mode 100644 helix_transmission/setup.py diff --git a/helix_bringup/launch/helix_bringup.launch.py b/helix_bringup/launch/helix_bringup.launch.py index bbaea3c..a4e6d5e 100644 --- a/helix_bringup/launch/helix_bringup.launch.py +++ b/helix_bringup/launch/helix_bringup.launch.py @@ -87,6 +87,13 @@ def generate_launch_description(): # ) # ) + tendon_transmission_node = Node( + package="helix_transmission", + executable="tendon_transmission_node", + name="tendon_transmission_node", + output="screen", + ) + ld.add_action(robot_state_publisher) ld.add_action(joint_state_publisher_node) ld.add_action(helix_ros2_control_node) @@ -94,5 +101,6 @@ def generate_launch_description(): ld.add_action(motor_head_helix_joint_effort_controller_node) ld.add_action(motor_head_joint_state_broadcaster_node) # ld.add_action(delay_joint_controller_after_broadcaster) + ld.add_action(tendon_transmission_node) return ld \ No newline at end of file diff --git a/helix_transmission/helix_transmission/__init__.py b/helix_transmission/helix_transmission/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/helix_transmission/helix_transmission/tendon_transmission.py b/helix_transmission/helix_transmission/tendon_transmission.py new file mode 100644 index 0000000..4b73278 --- /dev/null +++ b/helix_transmission/helix_transmission/tendon_transmission.py @@ -0,0 +1,75 @@ +import rclpy +from rclpy.node import Node +import numpy as np + +from sensor_msgs.msg import JointState +from std_msgs.msg import Float64MultiArray + +class TendonTransmissionNode(Node): + + def __init__(self): + super().__init__('tendon_transmission_node') + + self.PULLEY_RADIUS = 0.01 # [m] + # For Motor 0,1,2,3,4,5 increasing rotor position/positive torque -> contraction + # For Motor 6,7,8 decreasing rotor position/negative torque -> contraction + self.MOTOR_ORIENTS = np.array([1, 1, 1, 1, 1, 1, -1, -1, -1], dtype=np.float64) + self.MOTOR_OFFSETS = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=np.float64) + + self.tendon_command_sub = self.create_subscription( + Float64MultiArray, + '/helix_arm_tendons/command', + self.tendon_command_cb, + 10) + self.tendon_command_sub + + self.motor_state_sub = self.create_subscription( + JointState, + '/motor_head_joint_state_broadcaster/joint_states', + self.motor_state_cb, + 10) + self.motor_state_sub + + self.tendon_command_pub = self.create_publisher( + Float64MultiArray, + '/motor_head_joint_position_controller/commands_test', + 10) + + self.tendon_state_pub = self.create_publisher( + JointState, + '/helix_arm_tendons/tendon_states', + 10) + + def tendon_command_cb(self, msg): + motor_command = Float64MultiArray() + motor_command.data = ((np.array(msg.data, dtype=np.float64) * self.MOTOR_ORIENTS) \ + / self.PULLEY_RADIUS + self.MOTOR_OFFSETS).tolist() + self.tendon_command_pub.publish(motor_command) + + def motor_state_cb(self, msg): + tendon_state = JointState() + motor_names = sorted(msg.name) + motor_angs = [msg.position[msg.name.index(motor_name)] for motor_name in motor_names] + motor_angvels = [msg.velocity[msg.name.index(motor_name)] for motor_name in motor_names] + motor_currents = [msg.effort[msg.name.index(motor_name)] for motor_name in motor_names] + tendon_state.name = motor_names + tendon_state.position = ((np.array(motor_angs, dtype=np.float64) - self.MOTOR_OFFSETS) \ + * self.PULLEY_RADIUS * self.MOTOR_ORIENTS).tolist() + tendon_state.velocity = (np.array(motor_angvels, dtype=np.float64) \ + * self.PULLEY_RADIUS * self.MOTOR_ORIENTS).tolist() + tendon_state.effort = motor_currents + self.tendon_state_pub.publish(tendon_state) + + +def main(args=None): + rclpy.init(args=args) + + tendon_transmission_node = TendonTransmissionNode() + rclpy.spin(tendon_transmission_node) + + tendon_transmission_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/helix_transmission/package.xml b/helix_transmission/package.xml new file mode 100644 index 0000000..80710d2 --- /dev/null +++ b/helix_transmission/package.xml @@ -0,0 +1,18 @@ + + + + helix_transmission + 0.0.0 + Transmission providing abstraction between Helix motors and tendons + sebtiburzio + TODO: License declaration + + rclpy + numpy + sensor_msgs + std_msgs + + + ament_python + + diff --git a/helix_transmission/resource/helix_transmission b/helix_transmission/resource/helix_transmission new file mode 100644 index 0000000..e69de29 diff --git a/helix_transmission/setup.cfg b/helix_transmission/setup.cfg new file mode 100644 index 0000000..ba735c3 --- /dev/null +++ b/helix_transmission/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/helix_transmission +[install] +install_scripts=$base/lib/helix_transmission diff --git a/helix_transmission/setup.py b/helix_transmission/setup.py new file mode 100644 index 0000000..310693c --- /dev/null +++ b/helix_transmission/setup.py @@ -0,0 +1,25 @@ +from setuptools import find_packages, setup + +package_name = 'helix_transmission' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='sebtiburzio', + maintainer_email='sebtiburzio@gmail.com', + description='Transmission providing abstraction between Helix motors and tendons', + license='TODO: License declaration', + entry_points={ + 'console_scripts': [ + 'tendon_transmission_node = helix_transmission.tendon_transmission:main' + ], + }, +) diff --git a/run.sh b/run.sh index d8f03f1..57b078f 100755 --- a/run.sh +++ b/run.sh @@ -7,5 +7,6 @@ export HOST_UID=$(id -u) docker compose -f $SCRIPT_DIR/docker-compose.yml run \ --volume $SCRIPT_DIR/helix_bringup:/colcon_ws/src/helix_bringup \ --volume $SCRIPT_DIR/helix_description:/colcon_ws/src/helix_description \ +--volume $SCRIPT_DIR/helix_transmission:/colcon_ws/src/helix_transmission \ --volume $SCRIPT_DIR/dynamixel_hardware:/colcon_ws/src/dynamixel_hardware \ ros-helix bash From 1d266e8adaf06b9ef2f682122c966f1ccf5af741 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Mon, 19 Feb 2024 18:06:17 +0100 Subject: [PATCH 2/8] Add loading tendon control parameters from config file Add limits to tendon control Add tendon zero position calibration. Manually determined calibration values only --- helix_bringup/launch/helix_bringup.launch.py | 10 +---- .../config/helix_transmission.config.yml | 10 +++++ helix_transmission/config/tendon_calib.yml | 11 ++++++ .../helix_transmission/tendon_transmission.py | 37 +++++++++++++++---- helix_transmission/setup.py | 4 ++ 5 files changed, 57 insertions(+), 15 deletions(-) create mode 100644 helix_transmission/config/helix_transmission.config.yml create mode 100644 helix_transmission/config/tendon_calib.yml diff --git a/helix_bringup/launch/helix_bringup.launch.py b/helix_bringup/launch/helix_bringup.launch.py index a4e6d5e..f3c601c 100644 --- a/helix_bringup/launch/helix_bringup.launch.py +++ b/helix_bringup/launch/helix_bringup.launch.py @@ -79,19 +79,14 @@ def generate_launch_description(): output="screen", ) - # # Delay start of motor_head_joint_position_controller_node after `motor_head_joint_state_broadcaster_node` - # delay_joint_controller_after_broadcaster = RegisterEventHandler( - # event_handler=OnProcessExit( - # target_action=motor_head_joint_state_broadcaster_node, - # on_exit=[motor_head_joint_position_controller_node], - # ) - # ) + helix_transmission_params = os.path.join(get_package_share_directory('helix_transmission'),'config','helix_transmission.config.yml') tendon_transmission_node = Node( package="helix_transmission", executable="tendon_transmission_node", name="tendon_transmission_node", output="screen", + parameters = [helix_transmission_params], ) ld.add_action(robot_state_publisher) @@ -100,7 +95,6 @@ def generate_launch_description(): ld.add_action(motor_head_joint_position_controller_node) ld.add_action(motor_head_helix_joint_effort_controller_node) ld.add_action(motor_head_joint_state_broadcaster_node) - # ld.add_action(delay_joint_controller_after_broadcaster) ld.add_action(tendon_transmission_node) return ld \ No newline at end of file diff --git a/helix_transmission/config/helix_transmission.config.yml b/helix_transmission/config/helix_transmission.config.yml new file mode 100644 index 0000000..db9c99b --- /dev/null +++ b/helix_transmission/config/helix_transmission.config.yml @@ -0,0 +1,10 @@ +tendon_transmission_node: + ros__parameters: + pulley_radius: 0.01 # [m] + # For Motor 0,1,2,3,4,5 increasing rotor position/positive torque -> contraction + # For Motor 6,7,8 decreasing rotor position/negative torque -> contraction + motor_orients: [1, 1, 1, 1, 1, 1, -1, -1, -1] + tendon_min_lim: -0.1 + tendon_max_lim: 0.1 + # TODO - a better solution for the calibration file + tendon_calib_file_path: "/colcon_ws/src/helix_transmission/config/tendon_calib.yml" \ No newline at end of file diff --git a/helix_transmission/config/tendon_calib.yml b/helix_transmission/config/tendon_calib.yml new file mode 100644 index 0000000..6fdf78d --- /dev/null +++ b/helix_transmission/config/tendon_calib.yml @@ -0,0 +1,11 @@ +motor_offsets: +- -2.820990800857544 +- -2.980524778366089 +- -5.218602657318115 +- -8.886350631713867 +- -6.9934186935424805 +- -2.9897286891937256 +- 1.1443496942520142 +- -8.996797561645508 +- -9.438584327697754 + diff --git a/helix_transmission/helix_transmission/tendon_transmission.py b/helix_transmission/helix_transmission/tendon_transmission.py index 4b73278..34fdf53 100644 --- a/helix_transmission/helix_transmission/tendon_transmission.py +++ b/helix_transmission/helix_transmission/tendon_transmission.py @@ -1,6 +1,7 @@ import rclpy from rclpy.node import Node import numpy as np +import yaml from sensor_msgs.msg import JointState from std_msgs.msg import Float64MultiArray @@ -10,11 +11,29 @@ class TendonTransmissionNode(Node): def __init__(self): super().__init__('tendon_transmission_node') - self.PULLEY_RADIUS = 0.01 # [m] - # For Motor 0,1,2,3,4,5 increasing rotor position/positive torque -> contraction - # For Motor 6,7,8 decreasing rotor position/negative torque -> contraction - self.MOTOR_ORIENTS = np.array([1, 1, 1, 1, 1, 1, -1, -1, -1], dtype=np.float64) + self.declare_parameters( + namespace='', + parameters=[ + ('pulley_radius',rclpy.Parameter.Type.DOUBLE), + ('motor_orients',rclpy.Parameter.Type.INTEGER_ARRAY), + ('tendon_min_lim',rclpy.Parameter.Type.DOUBLE), + ('tendon_max_lim',rclpy.Parameter.Type.DOUBLE), + ('tendon_calib_file_path',rclpy.Parameter.Type.STRING) + ]) + + self.PULLEY_RADIUS = self.get_parameter('pulley_radius').value + self.MOTOR_ORIENTS = np.array(self.get_parameter('motor_orients').value, dtype=np.float64) + self.TENDON_LIMITS = np.array([self.get_parameter('tendon_min_lim').value, + self.get_parameter('tendon_max_lim').value]) self.MOTOR_OFFSETS = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=np.float64) + try: + with open(self.get_parameter('tendon_calib_file_path').value, 'r') as file: + data = yaml.safe_load(file) + if 'motor_offsets' in data: + self.MOTOR_OFFSETS = data['motor_offsets'] + self.get_logger().info('No offset data found in calibration file, offsets set to 0.') + except (FileNotFoundError, yaml.YAMLError, TypeError): + self.get_logger().info('Failed to load calibration file, offsets set to 0.') self.tendon_command_sub = self.create_subscription( Float64MultiArray, @@ -32,7 +51,7 @@ def __init__(self): self.tendon_command_pub = self.create_publisher( Float64MultiArray, - '/motor_head_joint_position_controller/commands_test', + '/motor_head_joint_position_controller/commands', 10) self.tendon_state_pub = self.create_publisher( @@ -41,8 +60,12 @@ def __init__(self): 10) def tendon_command_cb(self, msg): + command = np.array(msg.data, dtype=np.float64) + command_limited = np.clip(command, self.TENDON_LIMITS[0], self.TENDON_LIMITS[1]) + if not np.array_equal(command, command_limited): + self.get_logger().info('Some tendon commands exceeded limits and were clipped.') motor_command = Float64MultiArray() - motor_command.data = ((np.array(msg.data, dtype=np.float64) * self.MOTOR_ORIENTS) \ + motor_command.data = ((command_limited * self.MOTOR_ORIENTS) \ / self.PULLEY_RADIUS + self.MOTOR_OFFSETS).tolist() self.tendon_command_pub.publish(motor_command) @@ -53,7 +76,7 @@ def motor_state_cb(self, msg): motor_angvels = [msg.velocity[msg.name.index(motor_name)] for motor_name in motor_names] motor_currents = [msg.effort[msg.name.index(motor_name)] for motor_name in motor_names] tendon_state.name = motor_names - tendon_state.position = ((np.array(motor_angs, dtype=np.float64) - self.MOTOR_OFFSETS) \ + tendon_state.position = ((np.array(motor_angs, dtype=np.float64) + self.MOTOR_OFFSETS) \ * self.PULLEY_RADIUS * self.MOTOR_ORIENTS).tolist() tendon_state.velocity = (np.array(motor_angvels, dtype=np.float64) \ * self.PULLEY_RADIUS * self.MOTOR_ORIENTS).tolist() diff --git a/helix_transmission/setup.py b/helix_transmission/setup.py index 310693c..0bc086c 100644 --- a/helix_transmission/setup.py +++ b/helix_transmission/setup.py @@ -1,3 +1,5 @@ +import os +from glob import glob from setuptools import find_packages, setup package_name = 'helix_transmission' @@ -10,6 +12,8 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch/*launch.[pxy][yma]*'))), + (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*config.[pxy][yma]*'))), ], install_requires=['setuptools'], zip_safe=True, From 69725234ed83a1a40fe8076656760edddd6a9002 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Tue, 20 Feb 2024 16:22:28 +0100 Subject: [PATCH 3/8] Add set holding current service --- dynamixel_hardware | 2 +- .../config/helix_transmission.config.yml | 5 +- .../helix_transmission/tendon_transmission.py | 63 +++++++++++++++++-- 3 files changed, 61 insertions(+), 9 deletions(-) diff --git a/dynamixel_hardware b/dynamixel_hardware index 590e5b1..081f76c 160000 --- a/dynamixel_hardware +++ b/dynamixel_hardware @@ -1 +1 @@ -Subproject commit 590e5b114f66a76cb4a7cf5d3437da693c05d8c3 +Subproject commit 081f76c524d40155849924474a8803a73fc8c01c diff --git a/helix_transmission/config/helix_transmission.config.yml b/helix_transmission/config/helix_transmission.config.yml index db9c99b..846054d 100644 --- a/helix_transmission/config/helix_transmission.config.yml +++ b/helix_transmission/config/helix_transmission.config.yml @@ -4,7 +4,8 @@ tendon_transmission_node: # For Motor 0,1,2,3,4,5 increasing rotor position/positive torque -> contraction # For Motor 6,7,8 decreasing rotor position/negative torque -> contraction motor_orients: [1, 1, 1, 1, 1, 1, -1, -1, -1] - tendon_min_lim: -0.1 - tendon_max_lim: 0.1 + tendon_min_lim: -0.1 # [m] + tendon_max_lim: 0.1 # [m] + holding_current: 70.0 # [mA] # TODO - a better solution for the calibration file tendon_calib_file_path: "/colcon_ws/src/helix_transmission/config/tendon_calib.yml" \ No newline at end of file diff --git a/helix_transmission/helix_transmission/tendon_transmission.py b/helix_transmission/helix_transmission/tendon_transmission.py index 34fdf53..cfc0a56 100644 --- a/helix_transmission/helix_transmission/tendon_transmission.py +++ b/helix_transmission/helix_transmission/tendon_transmission.py @@ -1,16 +1,21 @@ import rclpy from rclpy.node import Node +from rclpy.executors import MultiThreadedExecutor +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup import numpy as np import yaml from sensor_msgs.msg import JointState from std_msgs.msg import Float64MultiArray +from std_srvs.srv import Trigger +from controller_manager_msgs.srv import SwitchController class TendonTransmissionNode(Node): def __init__(self): super().__init__('tendon_transmission_node') + # Config from helix_transmission.config.yml self.declare_parameters( namespace='', parameters=[ @@ -18,27 +23,32 @@ def __init__(self): ('motor_orients',rclpy.Parameter.Type.INTEGER_ARRAY), ('tendon_min_lim',rclpy.Parameter.Type.DOUBLE), ('tendon_max_lim',rclpy.Parameter.Type.DOUBLE), + ('holding_current',rclpy.Parameter.Type.DOUBLE), ('tendon_calib_file_path',rclpy.Parameter.Type.STRING) ]) - self.PULLEY_RADIUS = self.get_parameter('pulley_radius').value self.MOTOR_ORIENTS = np.array(self.get_parameter('motor_orients').value, dtype=np.float64) self.TENDON_LIMITS = np.array([self.get_parameter('tendon_min_lim').value, self.get_parameter('tendon_max_lim').value]) + self.HOLDING_CURRENT = self.get_parameter('holding_current').value + + # Motor offsets from local tendon calibration file self.MOTOR_OFFSETS = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0], dtype=np.float64) try: with open(self.get_parameter('tendon_calib_file_path').value, 'r') as file: data = yaml.safe_load(file) if 'motor_offsets' in data: self.MOTOR_OFFSETS = data['motor_offsets'] + else: self.get_logger().info('No offset data found in calibration file, offsets set to 0.') except (FileNotFoundError, yaml.YAMLError, TypeError): self.get_logger().info('Failed to load calibration file, offsets set to 0.') + # Subscription/publication for motor<->tendon transmission self.tendon_command_sub = self.create_subscription( Float64MultiArray, '/helix_arm_tendons/command', - self.tendon_command_cb, + self.tendon_to_motor_command_cb, 10) self.tendon_command_sub @@ -49,7 +59,7 @@ def __init__(self): 10) self.motor_state_sub - self.tendon_command_pub = self.create_publisher( + self.tendon_to_motor_command_pub = self.create_publisher( Float64MultiArray, '/motor_head_joint_position_controller/commands', 10) @@ -58,8 +68,24 @@ def __init__(self): JointState, '/helix_arm_tendons/tendon_states', 10) + + # Publisher for motor current holding current + self.motor_effort_command_pub = self.create_publisher( + Float64MultiArray, + '/motor_head_joint_effort_controller/commands', + 10) + + # Client/service for setting holding current + client_cb_group = MutuallyExclusiveCallbackGroup() + self.controller_switch_cli = self.create_client( + SwitchController, '/controller_manager/switch_controller', callback_group=client_cb_group) + + service_cb_group = MutuallyExclusiveCallbackGroup() + self.set_holding_current_srv = self.create_service( + Trigger, '~/set_holding_current_srv', self.set_holding_current_cb, callback_group=service_cb_group) - def tendon_command_cb(self, msg): + # Callbacks for motor<->tendon transmission + def tendon_to_motor_command_cb(self, msg): command = np.array(msg.data, dtype=np.float64) command_limited = np.clip(command, self.TENDON_LIMITS[0], self.TENDON_LIMITS[1]) if not np.array_equal(command, command_limited): @@ -67,7 +93,7 @@ def tendon_command_cb(self, msg): motor_command = Float64MultiArray() motor_command.data = ((command_limited * self.MOTOR_ORIENTS) \ / self.PULLEY_RADIUS + self.MOTOR_OFFSETS).tolist() - self.tendon_command_pub.publish(motor_command) + self.tendon_to_motor_command_pub.publish(motor_command) def motor_state_cb(self, msg): tendon_state = JointState() @@ -83,12 +109,37 @@ def motor_state_cb(self, msg): tendon_state.effort = motor_currents self.tendon_state_pub.publish(tendon_state) + # Callback for setting holding current + def set_holding_current_cb(self, request, response): + while not self.controller_switch_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waiting for controller switch service') + controller_switch_req = SwitchController.Request() + controller_switch_req.activate_controllers = ['motor_head_joint_effort_controller'] + controller_switch_req.deactivate_controllers = ['motor_head_joint_position_controller'] + controller_switch_req.strictness = SwitchController.Request.STRICT + controller_switch_future = self.controller_switch_cli.call_async(controller_switch_req) + while self.executor.spin_until_future_complete(controller_switch_future): + self.get_logger().info("Waiting for controller switch to complete") + if controller_switch_future.result().ok == False: + self.get_logger().error('Failed to switch to effort controller.') + response.success = False + response.message = 'Failed to switch to effort controller.' + return response + self.motor_effort_command_pub.publish( + Float64MultiArray(data = self.HOLDING_CURRENT * self.MOTOR_ORIENTS)) + response.success = True + return response + def main(args=None): rclpy.init(args=args) tendon_transmission_node = TendonTransmissionNode() - rclpy.spin(tendon_transmission_node) + + # Unable to complete controller switch service call from within holding current service callback + # MultiThreadedExectuor solution based on https://answers.ros.org/question/412149/ + executor = MultiThreadedExecutor() + rclpy.spin(tendon_transmission_node, executor=executor) tendon_transmission_node.destroy_node() rclpy.shutdown() From 84ae07bc4dbfa878684ba4e794b9c82bfccdc597 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Tue, 20 Feb 2024 16:32:30 +0100 Subject: [PATCH 4/8] Update package depends --- helix_transmission/package.xml | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/helix_transmission/package.xml b/helix_transmission/package.xml index 80710d2..d13768a 100644 --- a/helix_transmission/package.xml +++ b/helix_transmission/package.xml @@ -8,9 +8,11 @@ TODO: License declaration rclpy - numpy + numpy + yaml sensor_msgs std_msgs + controller_manager_msgs ament_python From c49fe1326d1693178f9d0b6b326937129adc8d64 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Tue, 20 Feb 2024 18:05:27 +0100 Subject: [PATCH 5/8] Add set motor offsets service --- .../helix_transmission/tendon_transmission.py | 39 +++++++++++++++---- 1 file changed, 31 insertions(+), 8 deletions(-) diff --git a/helix_transmission/helix_transmission/tendon_transmission.py b/helix_transmission/helix_transmission/tendon_transmission.py index cfc0a56..44fb308 100644 --- a/helix_transmission/helix_transmission/tendon_transmission.py +++ b/helix_transmission/helix_transmission/tendon_transmission.py @@ -40,9 +40,11 @@ def __init__(self): if 'motor_offsets' in data: self.MOTOR_OFFSETS = data['motor_offsets'] else: - self.get_logger().info('No offset data found in calibration file, offsets set to 0.') + self.get_logger().info('No offset data found in calibration file, offsets set to 0') except (FileNotFoundError, yaml.YAMLError, TypeError): - self.get_logger().info('Failed to load calibration file, offsets set to 0.') + self.get_logger().info('Failed to load calibration file, offsets set to 0') + + self.last_motor_joint_positions = None # Subscription/publication for motor<->tendon transmission self.tendon_command_sub = self.create_subscription( @@ -77,19 +79,24 @@ def __init__(self): # Client/service for setting holding current client_cb_group = MutuallyExclusiveCallbackGroup() + self.controller_switch_cli = self.create_client( SwitchController, '/controller_manager/switch_controller', callback_group=client_cb_group) service_cb_group = MutuallyExclusiveCallbackGroup() + self.set_holding_current_srv = self.create_service( - Trigger, '~/set_holding_current_srv', self.set_holding_current_cb, callback_group=service_cb_group) + Trigger, '~/set_holding_current', self.set_holding_current_cb, callback_group=service_cb_group) + + self.set_motor_offsets_srv = self.create_service( + Trigger, '~/set_motor_offsets', self.set_motor_offsets_cb, callback_group=service_cb_group) # Callbacks for motor<->tendon transmission def tendon_to_motor_command_cb(self, msg): command = np.array(msg.data, dtype=np.float64) command_limited = np.clip(command, self.TENDON_LIMITS[0], self.TENDON_LIMITS[1]) if not np.array_equal(command, command_limited): - self.get_logger().info('Some tendon commands exceeded limits and were clipped.') + self.get_logger().info('Some tendon commands exceeded limits and were clipped') motor_command = Float64MultiArray() motor_command.data = ((command_limited * self.MOTOR_ORIENTS) \ / self.PULLEY_RADIUS + self.MOTOR_OFFSETS).tolist() @@ -99,17 +106,18 @@ def motor_state_cb(self, msg): tendon_state = JointState() motor_names = sorted(msg.name) motor_angs = [msg.position[msg.name.index(motor_name)] for motor_name in motor_names] + self.last_motor_joint_positions = motor_angs motor_angvels = [msg.velocity[msg.name.index(motor_name)] for motor_name in motor_names] motor_currents = [msg.effort[msg.name.index(motor_name)] for motor_name in motor_names] tendon_state.name = motor_names - tendon_state.position = ((np.array(motor_angs, dtype=np.float64) + self.MOTOR_OFFSETS) \ + tendon_state.position = ((np.array(motor_angs, dtype=np.float64) - self.MOTOR_OFFSETS) \ * self.PULLEY_RADIUS * self.MOTOR_ORIENTS).tolist() tendon_state.velocity = (np.array(motor_angvels, dtype=np.float64) \ * self.PULLEY_RADIUS * self.MOTOR_ORIENTS).tolist() tendon_state.effort = motor_currents self.tendon_state_pub.publish(tendon_state) - # Callback for setting holding current + # Callbacks for calibration def set_holding_current_cb(self, request, response): while not self.controller_switch_cli.wait_for_service(timeout_sec=1.0): self.get_logger().info('Waiting for controller switch service') @@ -121,14 +129,29 @@ def set_holding_current_cb(self, request, response): while self.executor.spin_until_future_complete(controller_switch_future): self.get_logger().info("Waiting for controller switch to complete") if controller_switch_future.result().ok == False: - self.get_logger().error('Failed to switch to effort controller.') + self.get_logger().error('Failed to switch to effort controller') response.success = False - response.message = 'Failed to switch to effort controller.' + response.message = 'Failed to switch to effort controller' return response self.motor_effort_command_pub.publish( Float64MultiArray(data = self.HOLDING_CURRENT * self.MOTOR_ORIENTS)) response.success = True return response + + def set_motor_offsets_cb(self, request, response): + try: + with open(self.get_parameter('tendon_calib_file_path').value + '.bak', 'w') as backup: + yaml.dump({'motor_offsets': self.MOTOR_OFFSETS}, backup) + with open(self.get_parameter('tendon_calib_file_path').value, 'w') as file: + self.MOTOR_OFFSETS = self.last_motor_joint_positions + yaml.dump({'motor_offsets': self.MOTOR_OFFSETS}, file) + response.success = True + return response + except (FileNotFoundError, yaml.YAMLError, TypeError): + self.get_logger().info('Failed to write motor offsets to file') + response.success = False + response.message = 'Failed to write motor offsets to file' + return response def main(args=None): From 9ec33c5020c5555be1fbecdda282735d92659663 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Wed, 21 Feb 2024 14:51:28 +0100 Subject: [PATCH 6/8] Add calibration check service for after motor controller power cycle --- .../helix_transmission/tendon_transmission.py | 34 +++++++++++++++---- 1 file changed, 27 insertions(+), 7 deletions(-) diff --git a/helix_transmission/helix_transmission/tendon_transmission.py b/helix_transmission/helix_transmission/tendon_transmission.py index 44fb308..6fdae06 100644 --- a/helix_transmission/helix_transmission/tendon_transmission.py +++ b/helix_transmission/helix_transmission/tendon_transmission.py @@ -90,6 +90,9 @@ def __init__(self): self.set_motor_offsets_srv = self.create_service( Trigger, '~/set_motor_offsets', self.set_motor_offsets_cb, callback_group=service_cb_group) + + self.check_calibration_srv = self.create_service( + Trigger, '~/check_calibration', self.check_calibration_cb, callback_group=service_cb_group) # Callbacks for motor<->tendon transmission def tendon_to_motor_command_cb(self, msg): @@ -106,7 +109,7 @@ def motor_state_cb(self, msg): tendon_state = JointState() motor_names = sorted(msg.name) motor_angs = [msg.position[msg.name.index(motor_name)] for motor_name in motor_names] - self.last_motor_joint_positions = motor_angs + self.last_motor_joint_positions = np.array(motor_angs, dtype=np.float64) motor_angvels = [msg.velocity[msg.name.index(motor_name)] for motor_name in motor_names] motor_currents = [msg.effort[msg.name.index(motor_name)] for motor_name in motor_names] tendon_state.name = motor_names @@ -139,19 +142,36 @@ def set_holding_current_cb(self, request, response): return response def set_motor_offsets_cb(self, request, response): + write_succeeded = self.write_motor_offsets(self.last_motor_joint_positions.tolist()) + if write_succeeded: + response.success = True + else: + response.success = False + response.message = 'Failed to set motor offsets file' + return response + + def check_calibration_cb(self, request, response): + recalibrated_offsets = self.MOTOR_OFFSETS \ + + 2*np.pi * np.round((self.last_motor_joint_positions-self.MOTOR_OFFSETS) / (2*np.pi)) + write_succeeded = self.write_motor_offsets(recalibrated_offsets.tolist()) + if write_succeeded: + response.success = True + else: + response.success = False + response.message = 'Failed to update motor offsets file' + return response + + def write_motor_offsets(self, new_offsets): try: with open(self.get_parameter('tendon_calib_file_path').value + '.bak', 'w') as backup: yaml.dump({'motor_offsets': self.MOTOR_OFFSETS}, backup) with open(self.get_parameter('tendon_calib_file_path').value, 'w') as file: - self.MOTOR_OFFSETS = self.last_motor_joint_positions + self.MOTOR_OFFSETS = new_offsets yaml.dump({'motor_offsets': self.MOTOR_OFFSETS}, file) - response.success = True - return response + return True except (FileNotFoundError, yaml.YAMLError, TypeError): self.get_logger().info('Failed to write motor offsets to file') - response.success = False - response.message = 'Failed to write motor offsets to file' - return response + return False def main(args=None): From 74479ba305b26a6b0c72d15039475101230533b1 Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Wed, 21 Feb 2024 14:52:55 +0100 Subject: [PATCH 7/8] Add local calibratin files to gitignore --- .gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 .gitignore diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..a4be63f --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +helix_transmission/config/tendon_calib.yml +helix_transmission/config/tendon_calib.yml.bak From 436bd8fb18c63b9b3b84246614ad87da5ff3b7ab Mon Sep 17 00:00:00 2001 From: Seb Tiburzio Date: Mon, 26 Feb 2024 09:29:07 +0100 Subject: [PATCH 8/8] Add temporary unwind service for releasing tension --- .../helix_transmission/tendon_transmission.py | 30 +++++++++++++++++-- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/helix_transmission/helix_transmission/tendon_transmission.py b/helix_transmission/helix_transmission/tendon_transmission.py index 6fdae06..8d4da20 100644 --- a/helix_transmission/helix_transmission/tendon_transmission.py +++ b/helix_transmission/helix_transmission/tendon_transmission.py @@ -49,7 +49,7 @@ def __init__(self): # Subscription/publication for motor<->tendon transmission self.tendon_command_sub = self.create_subscription( Float64MultiArray, - '/helix_arm_tendons/command', + '~/command', self.tendon_to_motor_command_cb, 10) self.tendon_command_sub @@ -68,7 +68,7 @@ def __init__(self): self.tendon_state_pub = self.create_publisher( JointState, - '/helix_arm_tendons/tendon_states', + '~/tendon_states', 10) # Publisher for motor current holding current @@ -85,9 +85,13 @@ def __init__(self): service_cb_group = MutuallyExclusiveCallbackGroup() + # TODO - make custom services work over rosbridge so can have one parametrised set current service self.set_holding_current_srv = self.create_service( Trigger, '~/set_holding_current', self.set_holding_current_cb, callback_group=service_cb_group) + self.unwind_srv = self.create_service( + Trigger, '~/unwind', self.unwind_cb, callback_group=service_cb_group) + self.set_motor_offsets_srv = self.create_service( Trigger, '~/set_motor_offsets', self.set_motor_offsets_cb, callback_group=service_cb_group) @@ -127,7 +131,7 @@ def set_holding_current_cb(self, request, response): controller_switch_req = SwitchController.Request() controller_switch_req.activate_controllers = ['motor_head_joint_effort_controller'] controller_switch_req.deactivate_controllers = ['motor_head_joint_position_controller'] - controller_switch_req.strictness = SwitchController.Request.STRICT + controller_switch_req.strictness = SwitchController.Request.BEST_EFFORT controller_switch_future = self.controller_switch_cli.call_async(controller_switch_req) while self.executor.spin_until_future_complete(controller_switch_future): self.get_logger().info("Waiting for controller switch to complete") @@ -141,6 +145,26 @@ def set_holding_current_cb(self, request, response): response.success = True return response + def unwind_cb(self, request, response): + while not self.controller_switch_cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('Waiting for controller switch service') + controller_switch_req = SwitchController.Request() + controller_switch_req.activate_controllers = ['motor_head_joint_effort_controller'] + controller_switch_req.deactivate_controllers = ['motor_head_joint_position_controller'] + controller_switch_req.strictness = SwitchController.Request.STRICT + controller_switch_future = self.controller_switch_cli.call_async(controller_switch_req) + while self.executor.spin_until_future_complete(controller_switch_future): + self.get_logger().info("Waiting for controller switch to complete") + if controller_switch_future.result().ok == False: + self.get_logger().error('Failed to switch to effort controller') + response.success = False + response.message = 'Failed to switch to effort controller' + return response + self.motor_effort_command_pub.publish( + Float64MultiArray(data = -3.0 * self.MOTOR_ORIENTS)) + response.success = True + return response + def set_motor_offsets_cb(self, request, response): write_succeeded = self.write_motor_offsets(self.last_motor_joint_positions.tolist()) if write_succeeded: