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 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_bringup/launch/helix_bringup.launch.py b/helix_bringup/launch/helix_bringup.launch.py index bbaea3c..f3c601c 100644 --- a/helix_bringup/launch/helix_bringup.launch.py +++ b/helix_bringup/launch/helix_bringup.launch.py @@ -79,13 +79,15 @@ 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) ld.add_action(joint_state_publisher_node) @@ -93,6 +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..846054d --- /dev/null +++ b/helix_transmission/config/helix_transmission.config.yml @@ -0,0 +1,11 @@ +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 # [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/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/__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..8d4da20 --- /dev/null +++ b/helix_transmission/helix_transmission/tendon_transmission.py @@ -0,0 +1,216 @@ +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=[ + ('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), + ('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') + + self.last_motor_joint_positions = None + + # Subscription/publication for motor<->tendon transmission + self.tendon_command_sub = self.create_subscription( + Float64MultiArray, + '~/command', + self.tendon_to_motor_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_to_motor_command_pub = self.create_publisher( + Float64MultiArray, + '/motor_head_joint_position_controller/commands', + 10) + + self.tendon_state_pub = self.create_publisher( + JointState, + '~/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() + + # 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) + + 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): + 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 = ((command_limited * self.MOTOR_ORIENTS) \ + / self.PULLEY_RADIUS + self.MOTOR_OFFSETS).tolist() + self.tendon_to_motor_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] + 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 + 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) + + # 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') + 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.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") + 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 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: + 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 = new_offsets + yaml.dump({'motor_offsets': self.MOTOR_OFFSETS}, file) + return True + except (FileNotFoundError, yaml.YAMLError, TypeError): + self.get_logger().info('Failed to write motor offsets to file') + return False + + +def main(args=None): + rclpy.init(args=args) + + tendon_transmission_node = TendonTransmissionNode() + + # 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() + + +if __name__ == '__main__': + main() diff --git a/helix_transmission/package.xml b/helix_transmission/package.xml new file mode 100644 index 0000000..d13768a --- /dev/null +++ b/helix_transmission/package.xml @@ -0,0 +1,20 @@ + + + + helix_transmission + 0.0.0 + Transmission providing abstraction between Helix motors and tendons + sebtiburzio + TODO: License declaration + + rclpy + numpy + yaml + sensor_msgs + std_msgs + controller_manager_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..0bc086c --- /dev/null +++ b/helix_transmission/setup.py @@ -0,0 +1,29 @@ +import os +from glob import glob +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']), + (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, + 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