Merge pull request #10 from helix-robotics-ag/seb/add_temp_zero_curr_cb

Working (but temporary) tendon calibration interface

helix_transmission/config/helix_transmission.config.yml

@@ -1,9 +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]
+    # Helix v1:
+    # For Motor 0,1,2,3,4,5 decreasing rotor position/positive torque -> contraction
+    # For Motor 6,7,8 increasing rotor position/negative torque -> contraction 
+    motor_orients: [-1, -1, -1, -1, -1, -1, 1, 1, 1]  # !!!NOTE: OPPOSITE FOR HELIX v0!!!
     tendon_min_lim: -0.1  # [m]
     tendon_max_lim: 0.1  # [m]
     holding_current: 70.0  # [mA]

helix_transmission/helix_transmission/tendon_transmission.py

@@ -47,12 +47,12 @@ def __init__(self):
         self.last_motor_joint_positions = None
 
         # Subscription/publication for motor<->tendon transmission
-        self.tendon_command_sub = self.create_subscription(
+        self.tendon_commands_sub = self.create_subscription(
             Float64MultiArray, 
-            '~/command', 
+            '~/commands', 
             self.tendon_to_motor_command_cb,
             10)
-        self.tendon_command_sub
+        self.tendon_commands_sub
 
         self.motor_state_sub = self.create_subscription(
             JointState, 
@@ -86,11 +86,20 @@ def __init__(self):
         service_cb_group = MutuallyExclusiveCallbackGroup()
 
         # TODO - make custom services work over rosbridge so can have one parametrised set current service
+        self.switch_to_current_control = self.create_service(
+            Trigger, '~/switch_to_current_control', self.switch_to_current_control_cb, callback_group=service_cb_group)
+
+        self.switch_to_position_control = self.create_service(
+            Trigger, '~/switch_to_position_control', self.switch_to_position_control_cb, callback_group=service_cb_group)
+
         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)
+            Trigger, '~/set_unwind_current', self.set_unwind_current_cb, callback_group=service_cb_group)
+
+        self.unwind_srv = self.create_service(
+            Trigger, '~/set_zero_current', self.set_zero_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)
@@ -124,8 +133,8 @@ def motor_state_cb(self, msg):
         tendon_state.effort = motor_currents
         self.tendon_state_pub.publish(tendon_state)
 
-    # Callbacks for calibration
-    def set_holding_current_cb(self, request, response):
+    # Callbacks for controller switching
+    def switch_to_current_control_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()
@@ -140,31 +149,46 @@ def set_holding_current_cb(self, request, response):
             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):
+
+    def switch_to_position_control_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_req.activate_controllers = ['motor_head_joint_position_controller']
+        controller_switch_req.deactivate_controllers = ['motor_head_joint_effort_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')
+            self.get_logger().error('Failed to switch to position controller')
             response.success = False
-            response.message = 'Failed to switch to effort controller'
+            response.message = 'Failed to switch to position controller'
             return response
+        response.success = True
+        return response
+
+    # Callbacks for calibration
+    def set_holding_current_cb(self, request, response):
+        self.motor_effort_command_pub.publish(
+            Float64MultiArray(data = self.HOLDING_CURRENT * self.MOTOR_ORIENTS))
+        response.success = True
+        return response
+
+    def set_unwind_current_cb(self, request, response):
         self.motor_effort_command_pub.publish(
             Float64MultiArray(data = -3.0 * self.MOTOR_ORIENTS))
         response.success = True
         return response
 
+    def set_zero_current_cb(self, request, response):
+        self.motor_effort_command_pub.publish(
+            Float64MultiArray(data = 0.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: