Added ARM Odrive Firmware

odrive_interface/src/node_odrive_arm.py

@@ -0,0 +1,381 @@
+# Author: mn297
+import threading
+from threading import Lock
+from odrive_interface.msg import MotorState, MotorError
+from ODriveJoint import *
+from std_msgs.msg import Float32MultiArray
+from odrive.utils import dump_errors
+from odrive.enums import AxisState, ODriveError, ProcedureResult
+from enum import Enum
+import rospy
+import os
+import sys
+
+currentdir = os.path.dirname(os.path.realpath(__file__))
+sys.path.append(currentdir)
+
+
+# unused at the moment
+class FeedbackMode(Enum):
+    FROM_ODRIVE = "fb_from_odrive"
+    FROM_OUTSHAFT = "fb_from_outshaft"
+
+
+# Unused at the moment
+arm_zero_offsets = {
+    "rover_arm_elbow": 0,
+    "rover_arm_shoulder": 45.0,
+    "rover_arm_waist": 0,
+}
+
+current_mode = FeedbackMode.FROM_OUTSHAFT
+
+# VARIABLES -------------------------------------------------------------------
+# Dictionary of ODriveJoint objects, key is the joint name in string format, value is the ODriveJoint object
+
+
+# Predefine the order of joints for publishing feedback
+joint_order = ["rover_arm_elbow", "rover_arm_shoulder", "rover_arm_waist"]
+
+
+class NodeODriveInterfaceArm:
+    def __init__(self):
+        self.is_homed = False
+        self.is_calibrated = False
+
+        # CONFIGURATION ---------------------------------------------------------------
+        # Serial number of the ODrive controlling the joint
+        self.joint_serial_numbers = {
+            # 0x383834583539 = 61814047520057 in decimal
+            "rover_arm_elbow": "383834583539",
+            # 0x386434413539 = 62003024573753 in decimal
+            "rover_arm_shoulder": "386434413539",
+            "rover_arm_waist": "0",
+        }
+        self.joint_dict = {
+            "rover_arm_elbow": None,
+            "rover_arm_shoulder": None,
+            "rover_arm_waist": None,
+        }
+
+        # FEEDBACK VARIABLES
+        self.joint_pos_outshaft_dict = {
+            "rover_arm_elbow": 0,
+            "rover_arm_shoulder": 0,
+            "rover_arm_waist": 0,
+        }
+
+        self.locks = {joint_name: Lock()
+                      for joint_name in self.joint_dict.keys()}
+
+        # Subscriptions
+        rospy.init_node("odrive_interface_arm")
+        self.outshaft_pos_fb_subscriber = rospy.Subscriber(
+            "/armBrushlessFb",
+            Float32MultiArray,
+            self.handle_outshaft_fb,
+        )
+        # Cmd comes from the external control node, we convert it to setpoint and apply it to the ODrive
+        self.arm_joint_cmd_subscriber = rospy.Subscriber(
+            "/armBrushlessCmd", Float32MultiArray, self.handle_arm_cmd
+        )
+
+        # Publishers
+        self.odrive_state_publisher = rospy.Publisher(
+            "/odrive_status", MotorState, queue_size=1
+        )
+        self.odrive_pos_fb_publisher = rospy.Publisher(
+            "/odrive_armBrushlessFb", Float32MultiArray, queue_size=1
+        )
+
+        # Frequency of the ODrive I/O
+        self.rate = rospy.Rate(100)
+        self.run()
+
+    # Update encoder angle from external encoders
+    def handle_outshaft_fb(self, msg):
+        if not self.is_calibrated:
+            return
+        if not self.is_homed:
+            self.joint_pos_outshaft_dict["rover_arm_elbow"] = msg.data[0]
+            self.joint_pos_outshaft_dict["rover_arm_shoulder"] = msg.data[1]
+            self.joint_pos_outshaft_dict["rover_arm_waist"] = msg.data[2]
+
+            # Home the joints to 0 position
+            for joint_name, joint_obj in self.joint_dict.items():
+                try:
+                    if not joint_obj.odrv:
+                        continue
+                    temp = (
+                        self.joint_pos_outshaft_dict[joint_name]
+                        * joint_obj.gear_ratio
+                        / 360
+                    )
+                    joint_obj.odrv.axis0.set_abs_pos(temp)
+                except:
+                    print(
+                        f"""Cannot home joint: {
+                            joint_name} to position: {
+                            self.joint_pos_outshaft_dict[joint_name]}"""
+                    )
+            self.is_homed = True
+
+    # Receive setpoint from external control node
+    def handle_arm_cmd(self, msg):
+        if not self.is_calibrated:
+            print("ODrive not calibrated. Ignoring command.")
+            return
+        self.joint_dict["rover_arm_elbow"].pos_cmd = msg.data[0]
+        self.joint_dict["rover_arm_shoulder"].pos_cmd = msg.data[1]
+        self.joint_dict["rover_arm_waist"].pos_cmd = msg.data[2]
+
+    def reconnect_joint(self, joint_name, joint_obj):
+        # Attempt to reconnect...
+        # Update joint_obj.odrv as necessary...
+        joint_obj.reconnect()
+
+        # Once done, reset the flag
+        with self.locks[joint_name]:
+            joint_obj.is_reconnecting = False
+
+    def calibrate_and_enter_closed_loop(self, joint_obj):
+        if joint_obj.odrv is not None:
+            print(f"CALIBRATING joint {joint_obj.name}...")
+            joint_obj.calibrate()
+
+            print(
+                f"ENTERING CLOSED LOOP CONTROL for joint {joint_obj.name}...")
+            joint_obj.enter_closed_loop_control()
+
+    def run(self):
+        threads = []
+        calibrate_threads = []
+        enter_closed_loop_threads = []
+
+        # SETUP ODRIVE CONNECTIONS -----------------------------------------------------
+        for key, value in self.joint_serial_numbers.items():
+            # Instantiate ODriveJoint class whether or not the connection attempt was made/successful
+            self.joint_dict[key] = ODriveJoint(name=key, serial_number=value)
+
+            if value == 0:
+                print(
+                    f"""Skipping connection for joint: {
+                        key} due to serial_number being 0"""
+                )
+                continue
+
+            self.joint_dict[key].reconnect()
+
+        print("Connection step completed.")
+
+        for joint_name, joint_obj in self.joint_dict.items():
+            if joint_obj.odrv is None:
+                continue
+            print(f"Creating thread for joint {joint_obj.name}")
+            t = threading.Thread(
+                target=joint_obj.calibrate,
+                # args=(joint_obj,),
+            )
+            calibrate_threads.append(t)
+            t.start()
+
+        # Wait for all threads to complete
+        for t in calibrate_threads:
+            t.join()
+
+        print("Calibration step completed.")
+
+        for joint_name, joint_obj in self.joint_dict.items():
+            if joint_obj.odrv is None:
+                continue
+            print(f"Creating thread for joint {joint_obj.name}")
+            t = threading.Thread(
+                target=joint_obj.enter_closed_loop_control,
+                # args=(joint_obj,),
+            )
+            enter_closed_loop_threads.append(t)
+            t.start()
+
+        # Wait for all threads to complete
+        for t in enter_closed_loop_threads:
+            t.join()
+
+        self.is_calibrated = True
+
+        # Set the direction of the motors
+        self.joint_dict["rover_arm_elbow"].gear_ratio = 100
+        self.joint_dict["rover_arm_shoulder"].gear_ratio = 100
+        self.joint_dict["rover_arm_waist"].gear_ratio = 1
+
+        print("Entering closed loop control step completed.")
+
+        # MAIN LOOP
+        while not rospy.is_shutdown():
+            # PRINT TIMESTAMP
+            print(f"""Time: {rospy.get_time()}""")
+
+            # ODRIVE POSITION FEEDBACK, different from the outshaft feedback
+            feedback = Float32MultiArray()
+            for joint_name, joint_obj in self.joint_dict.items():
+                if not joint_obj.odrv:
+                    continue
+                try:
+                    # Assuming .odrv.axis0.pos_vel_mapper.pos_abs and .gear_ratio are correct
+                    feedback.data.append(
+                        360
+                        * (
+                            joint_obj.odrv.axis0.pos_vel_mapper.pos_abs
+                            / joint_obj.gear_ratio
+                        )
+                    )
+                # Default value in case lose connection to ODrive
+                except fibre.libfibre.ObjectLostError:
+                    joint_obj.odrv = None
+                    feedback.data.append(0.0)
+                except:
+                    print(f"""Cannot get feedback from joint: {joint_name}""")
+                    feedback.data.append(0.0)
+
+            # Publish
+            self.odrive_pos_fb_publisher.publish(feedback)
+
+            # APPLY Position Cmd
+            for joint_name, joint_obj in self.joint_dict.items():
+                if not joint_obj.odrv:
+                    continue
+                try:
+                    # setpoint in radians
+                    setpoint = joint_obj.pos_cmd * joint_obj.gear_ratio / 360
+                    joint_obj.odrv.axis0.controller.input_pos = setpoint
+                except fibre.libfibre.ObjectLostError:
+                    joint_obj.odrv = None
+                except:
+                    print(
+                        f"""Cannot apply setpoint {
+                            setpoint} to joint: {joint_name}"""
+                    )
+
+            # PRINT POSITIONS TO CONSOLE
+            print_joint_state_from_dict(self.joint_dict)
+
+            # SEND ODRIVE INFO AND HANDLE ERRORS
+            for joint_name, joint_obj in self.joint_dict.items():
+                with self.locks[joint_name]:  # Use a lock specific to each joint
+                    if joint_obj.odrv is not None:
+                        # Check if 'odrv' object has 'axis0' attribute before accessing it
+                        if hasattr(joint_obj.odrv, 'axis0') and joint_obj.odrv.axis0.current_state != AxisState.CLOSED_LOOP_CONTROL:
+                            if not joint_obj.is_reconnecting:
+                                print(
+                                    f"{joint_name} is not in closed loop control, recalibrating...")
+                                joint_obj.is_reconnecting = True
+                                t = threading.Thread(
+                                    target=self.calibrate_and_enter_closed_loop, args=(joint_obj,))
+                                t.start()
+                        else:
+                            # Handle case where 'axis0' is not available
+                            print(
+                                f"Cannot check current state for {joint_name}, 'axis0' attribute missing.")
+                    else:
+                        # ODrive interface is None, indicating a disconnection or uninitialized state
+                        if not joint_obj.is_reconnecting:
+                            print(f"RECONNECTING {joint_name}...")
+                            joint_obj.is_reconnecting = True
+                            t = threading.Thread(
+                                target=self.reconnect_joint, args=(joint_name, joint_obj))
+                            t.start()
+
+                # try:
+                #     # TODO ODriveStatus.msg
+
+                #     # ERROR HANDLING
+                #     if joint_obj.odrv.axis0.active_errors != 0:
+                #         # Tell the rover to stop
+                #         for joint_name, joint_obj in self.joint_dict.items():
+                #             if not joint_obj.odrv:
+                #                 continue
+                #             joint_obj.odrv.axis0.controller.input_vel = 0
+
+                #         # Wait until it actually stops
+                #         motor_stopped = False
+                #         while not motor_stopped:
+                #             motor_stopped = True
+                #             for joint_name, joint_obj in self.joint_dict.items():
+                #                 if not joint_obj.odrv:
+                #                     continue
+                #                 if (
+                #                     abs(joint_obj.odrv.encoder_estimator0.vel_estimate)
+                #                     >= 0.01
+                #                 ):
+                #                     motor_stopped = False
+                #             if rospy.is_shutdown():
+                #                 print(
+                #                     "Shutdown prompt received. Setting all motors to idle state."
+                #                 )
+                #                 for joint_name, joint_obj in self.joint_dict.items():
+                #                     if not joint_obj.odrv:
+                #                         continue
+                #                     joint_obj.odrv.axis0.requested_state = (
+                #                         AxisState.IDLE
+                #                     )
+                #                 break
+
+                #         # Wait for two seconds while all the transient currents and voltages calm down
+                #         rospy.sleep(5)
+
+                #         # Now try to recover from the error. This will always succeed the first time, but if
+                #         # the error persists, the ODrive will not allow the transition to closed loop control, and
+                #         # re-throw the error.
+                #         joint_obj.odrv.clear_errors()
+                #         rospy.sleep(0.5)
+                #         joint_obj.odrv.axis0.requested_state = (
+                #             AxisState.CLOSED_LOOP_CONTROL
+                #         )
+                #         rospy.sleep(0.5)
+
+                #         # If the motor cannot recover successfully publish a message about the error, then print to console
+                #         if joint_obj.odrv.axis0.active_errors != 0:
+                #             error_fb = MotorError()
+                #             error_fb.id = joint_obj.serial_number
+                #             error_fb.error = ODriveError(
+                #                 joint_obj.odrv.axis0.active_errors
+                #             ).name
+                #             self.error_publisher.publish(error_fb)
+                #             print(
+                #                 f"""\nError(s) occurred. Motor ID: {
+                #                     error_fb.id}, Error(s): {error_fb.error}"""
+                #             )
+
+                #             # Finally, hang the node and keep trying to recover until the error is gone or the shutdown signal is received
+                #             print(
+                #                 f"""\nMotor {error_fb.id} Cannot recover from error(s) {
+                #                     error_fb.error}. R to retry, keyboard interrupt to shut down node."""
+                #             )
+                #             while not rospy.is_shutdown():
+                #                 prompt = input(">").upper()
+                #                 if prompt == "R":
+                #                     joint_obj.odrv.clear_errors()
+                #                     rospy.sleep(0.5)
+                #                     joint_obj.odrv.axis0.requested_state = (
+                #                         AxisState.CLOSED_LOOP_CONTROL
+                #                     )
+                #                     rospy.sleep(0.5)
+                #                     if joint_obj.odrv.axis0.active_errors == 0:
+                #                         break
+                #                     else:
+                #                         print("Recovery failed. Try again?")
+                # except AttributeError:
+                #     print(f"""{joint_name} is not connected""")
+            print()
+            self.rate.sleep()
+
+        # On shutdown, bring motors to idle state
+        print("Shutdown prompt received. Setting all motors to idle state.")
+        for joint_name, joint_obj in self.joint_dict.items():
+            if not joint_obj.odrv:
+                continue
+            joint_obj.odrv.axis0.requested_state = AxisState.IDLE
+
+
+if __name__ == "__main__":
+    driver = NodeODriveInterfaceArm()
+    rospy.spin()