Modules

ActuatorBase

Bases: ABC

Source code in opensourceleg/actuators/base.py

class ActuatorBase(ABC):
    def __init__(
        self,
        tag: str,
        gear_ratio: float,
        motor_constants: MOTOR_CONSTANTS,
        frequency: int = 1000,
        offline: bool = False,
        *args,
        **kwargs,
    ) -> None:
        self._MOTOR_CONSTANTS: MOTOR_CONSTANTS = motor_constants
        self._gear_ratio: float = gear_ratio
        self._tag: str = tag
        self._frequency: int = frequency
        self._data: Any = None
        self._is_offline: bool = offline
        self._is_homed: bool = False

        self._mode: CONTROL_MODES = CONTROL_MODES.IDLE

        self._motor_zero_position: float = 0.0
        self._motor_position_offset: float = 0.0

        self._joint_zero_position: float = 0.0
        self._joint_position_offset: float = 0.0
        self._joint_direction: int = 1

        self._is_open: bool = False
        self._is_streaming: bool = False

        self._original_methods: dict[str, MethodWithRequiredModes] = {}

        self._set_original_methods()
        self._set_mutated_methods()

    def __enter__(self):
        self.start()
        return self

    def __exit__(self, exc_type, exc_value, exc_traceback):
        self.stop()

    def _restricted_method(self, method_name: str, *args, **kwargs):
        LOGGER.error(f"{method_name}() is not available in {self._mode.name} mode.")
        return None

    def _set_original_methods(self):
        for method_name in CONTROL_MODE_METHODS:
            try:
                method = getattr(self, method_name)
                if callable(method) and hasattr(method, "_required_modes"):
                    self._original_methods[method_name] = method
            except AttributeError:
                LOGGER.debug(msg=f"[{self.tag}] {method_name}() is not implemented in {self.tag}.")

    def _set_mutated_methods(self):
        for method_name, method in self._original_methods.items():
            if self._mode in method._required_modes:
                setattr(self, method_name, method)
            else:
                setattr(self, method_name, partial(self._restricted_method, method_name))

    @property
    @abstractmethod
    def _CONTROL_MODE_CONFIGS(self) -> CONTROL_MODE_CONFIGS:
        pass

    @abstractmethod
    def start(self) -> None:
        pass

    @abstractmethod
    def stop(self) -> None:
        pass

    @abstractmethod
    def update(self) -> None:
        pass

    def _get_control_mode_config(self, mode: CONTROL_MODES) -> Optional[ControlModeConfig]:
        return cast(
            Optional[ControlModeConfig],
            getattr(self._CONTROL_MODE_CONFIGS, mode.name),
        )

    def set_control_mode(self, mode: CONTROL_MODES) -> None:
        if self.mode == mode:
            LOGGER.debug(msg=f"[{self.tag}] Already in {self.mode.name} control mode.")
            return

        current_config = self._get_control_mode_config(self.mode)
        if current_config:
            current_config.exit_callback(self)

        self._mode = mode

        new_config = self._get_control_mode_config(self.mode)
        if new_config:
            new_config.entry_callback(self)

        self._set_mutated_methods()

    @abstractmethod
    @requires(CONTROL_MODES.VOLTAGE)
    def set_motor_voltage(self, value: float) -> None:
        pass

    @abstractmethod
    @requires(CONTROL_MODES.CURRENT)
    def set_motor_current(self, value: float) -> None:
        pass

    @abstractmethod
    @requires(CONTROL_MODES.POSITION)
    def set_motor_position(self, value: float) -> None:
        pass

    @requires(
        CONTROL_MODES.POSITION
    )  # This needs to be tested as set_motor_position is already decorated with requires
    def set_output_position(self, value: float) -> None:
        self.set_motor_position(value=value * self.gear_ratio)

    @abstractmethod
    @requires(CONTROL_MODES.TORQUE)
    def set_motor_torque(self, value: float) -> None:
        pass

    @abstractmethod
    @requires(CONTROL_MODES.TORQUE)
    def set_joint_torque(self, value: float) -> None:
        pass

    @abstractmethod
    @requires(CONTROL_MODES.CURRENT)
    def set_current_gains(self, kp: float, ki: float, kd: float, ff: float) -> None:
        pass

    @abstractmethod
    @requires(CONTROL_MODES.POSITION)
    def set_position_gains(self, kp: float, ki: float, kd: float, ff: float) -> None:
        pass

    @abstractmethod
    @requires(CONTROL_MODES.IMPEDANCE)
    def set_impedance_gains(self, kp: float, ki: float, kd: float, k: float, b: float, ff: float) -> None:
        pass

    @abstractmethod
    def home(self) -> None:
        pass

    def set_motor_zero_position(self, value: float) -> None:
        """Sets motor zero position in radians"""
        self._motor_zero_position = value

    def set_motor_position_offset(self, value: float) -> None:
        """Sets joint offset position in radians"""
        self._motor_position_offset = value

    def set_joint_zero_position(self, value: float) -> None:
        """Sets joint zero position in radians"""
        self._joint_zero_position = value

    def set_joint_position_offset(self, value: float) -> None:
        """Sets joint offset position in radians"""
        self._joint_position_offset = value

    def set_joint_direction(self, value: int) -> None:
        """Sets joint direction to 1 or -1"""
        self._joint_direction = value

    @property
    @abstractmethod
    def motor_position(self) -> float:
        pass

    @property
    def output_position(self) -> float:
        """
        Position of the output in radians.
        This is calculated by scaling the motor angle with the gear ratio.
        Note that this method does not consider compliance from an SEA.
        """
        return self.motor_position / self.gear_ratio

    @property
    @abstractmethod
    def motor_velocity(self) -> float:
        pass

    @property
    def output_velocity(self) -> float:
        """
        Velocity of the output in radians.
        This is calculated by scaling the motor angle with the gear ratio.
        Note that this method does not consider compliance from an SEA.
        """
        return self.motor_velocity / self.gear_ratio

    @property
    @abstractmethod
    def motor_voltage(self) -> float:
        pass

    @property
    @abstractmethod
    def motor_current(self) -> float:
        pass

    @property
    @abstractmethod
    def motor_torque(self) -> float:
        pass

    @property
    def MOTOR_CONSTANTS(self) -> MOTOR_CONSTANTS:
        return self._MOTOR_CONSTANTS

    @property
    def mode(self) -> CONTROL_MODES:
        return self._mode

    @property
    def tag(self) -> str:
        return self._tag

    @property
    def is_homed(self) -> bool:
        return self._is_homed

    @property
    def frequency(self) -> int:
        return self._frequency

    @property
    def is_offline(self) -> bool:
        return self._is_offline

    @property
    def gear_ratio(self) -> float:
        return self._gear_ratio

    @property
    def max_case_temperature(self) -> float:
        return self._MOTOR_CONSTANTS.MAX_CASE_TEMPERATURE

    @property
    @abstractmethod
    def case_temperature(self) -> float:
        pass

    @property
    @abstractmethod
    def winding_temperature(self) -> float:
        pass

    @property
    def max_winding_temperature(self) -> float:
        return self._MOTOR_CONSTANTS.MAX_WINDING_TEMPERATURE

    @property
    def motor_zero_position(self) -> float:
        return self._motor_zero_position

    @property
    def motor_position_offset(self) -> float:
        return self._motor_position_offset

    @property
    def joint_zero_position(self) -> float:
        return self._joint_zero_position

    @property
    def joint_position_offset(self) -> float:
        return self._joint_position_offset

    @property
    def joint_direction(self) -> int:
        return self._joint_direction

    @property
    def is_open(self) -> bool:
        return self._is_open

    @property
    def is_streaming(self) -> bool:
        return self._is_streaming

output_position: float property

Position of the output in radians. This is calculated by scaling the motor angle with the gear ratio. Note that this method does not consider compliance from an SEA.

output_velocity: float property

Velocity of the output in radians. This is calculated by scaling the motor angle with the gear ratio. Note that this method does not consider compliance from an SEA.

set_joint_direction(value)

Sets joint direction to 1 or -1

Source code in opensourceleg/actuators/base.py

def set_joint_direction(self, value: int) -> None:
    """Sets joint direction to 1 or -1"""
    self._joint_direction = value

set_joint_position_offset(value)

Sets joint offset position in radians

Source code in opensourceleg/actuators/base.py

def set_joint_position_offset(self, value: float) -> None:
    """Sets joint offset position in radians"""
    self._joint_position_offset = value

set_joint_zero_position(value)

Sets joint zero position in radians

Source code in opensourceleg/actuators/base.py

def set_joint_zero_position(self, value: float) -> None:
    """Sets joint zero position in radians"""
    self._joint_zero_position = value

set_motor_position_offset(value)

Sets joint offset position in radians

Source code in opensourceleg/actuators/base.py

def set_motor_position_offset(self, value: float) -> None:
    """Sets joint offset position in radians"""
    self._motor_position_offset = value

set_motor_zero_position(value)

Sets motor zero position in radians

Source code in opensourceleg/actuators/base.py

def set_motor_zero_position(self, value: float) -> None:
    """Sets motor zero position in radians"""
    self._motor_zero_position = value

ActuatorConnectionException

Bases: Exception

Actuator Connection Exception

Attributes

message (str): Error message

Source code in opensourceleg/logging/exceptions.py

class ActuatorConnectionException(Exception):
    """Actuator Connection Exception

    Attributes
    ----------
    message (str): Error message

    """

    def __init__(self, tag: str) -> None:
        super().__init__(f"{tag} is not connected")

ActuatorStreamException

Bases: Exception

Actuator Stream Exception

Attributes

message (str): Error message

Source code in opensourceleg/logging/exceptions.py

class ActuatorStreamException(Exception):
    """Actuator Stream Exception

    Attributes
    ----------
    message (str): Error message

    """

    def __init__(self, tag: str) -> None:
        super().__init__(f"{tag} is not streaming, please call start() method before sending commands")

DephyActuator

Bases: Device, ActuatorBase

Source code in opensourceleg/actuators/dephy.py

class DephyActuator(Device, ActuatorBase):
    def __init__(
        self,
        tag: str = "DephyActuator",
        firmware_version: str = "7.2.0",
        port: str = "/dev/ttyACM0",
        gear_ratio: float = 1.0,
        baud_rate: int = 230400,
        frequency: int = 500,
        debug_level: int = 4,
        dephy_log: bool = False,
        offline: bool = False,
        stop_motor_on_disconnect: bool = False,
    ) -> None:
        ActuatorBase.__init__(
            self,
            tag=tag,
            gear_ratio=gear_ratio,
            motor_constants=DEPHY_ACTUATOR_CONSTANTS,
            frequency=frequency,
            offline=offline,
        )

        self._debug_level: int = debug_level if dephy_log else 6
        self._dephy_log: bool = dephy_log

        if self.is_offline:
            self.port = port
            self._is_streaming: bool = False
            self._is_open: bool = False
        else:
            Device.__init__(
                self,
                firmwareVersion=firmware_version,
                port=port,
                baudRate=baud_rate,
                stopMotorOnDisconnect=stop_motor_on_disconnect,
            )

        self._thermal_model: ThermalModel = ThermalModel(
            temp_limit_windings=self.max_winding_temperature,
            soft_border_C_windings=10,
            temp_limit_case=self.max_case_temperature,
            soft_border_C_case=10,
        )
        self._thermal_scale: float = 1.0

        self._mode = CONTROL_MODES.IDLE

    def __repr__(self) -> str:
        return f"{self.tag}[DephyLegacyActuator]"

    @property
    def _CONTROL_MODE_CONFIGS(self) -> CONTROL_MODE_CONFIGS:
        return DEPHY_CONTROL_MODE_CONFIGS

    @check_actuator_connection
    def start(self) -> None:
        try:
            self.open()
            self._is_open = True
        except OSError:
            print("\n")
            LOGGER.error(
                msg=f"[{self.__repr__()}] Need admin previleges to open the port '{self.port}'. \n\nPlease run the script with 'sudo' command or add the user to the dialout group.\n"
            )
            os._exit(status=1)

        self.start_streaming(self._frequency)
        time.sleep(0.2)
        self._is_streaming = True

        self._data = self.read()
        self.set_control_mode(CONTROL_MODES.VOLTAGE)

    @check_actuator_stream
    @check_actuator_open
    def stop(self) -> None:
        self.stop_motor()
        self.set_control_mode(mode=CONTROL_MODES.IDLE)
        self._is_streaming = False
        self._is_open = False
        self.close()

    def update(self) -> None:
        self._data = self.read()

        self._thermal_model.T_c = self.case_temperature
        self._thermal_scale = self._thermal_model.update_and_get_scale(
            dt=1 / self.frequency,
            motor_current=self.motor_current,
        )
        if self.case_temperature >= self.max_case_temperature:
            LOGGER.error(
                msg=f"[{str.upper(self.tag)}] Case thermal limit {self.max_case_temperature} reached. Stopping motor."
            )
            # self.stop()
            raise ThermalLimitException()

        if self.winding_temperature >= self.max_winding_temperature:
            LOGGER.error(
                msg=f"[{str.upper(self.tag)}] Winding thermal limit {self.max_winding_temperature} reached. Stopping motor."
            )
            raise ThermalLimitException()
        # Check for thermal fault, bit 2 of the execute status byte
        if self._data["status_ex"] & 0b00000010 == 0b00000010:
            self.stop()
            raise RuntimeError("Actpack Thermal Limit Tripped")

    def home(
        self,
        homing_voltage: int = 2000,
        homing_frequency: Optional[int] = None,
        homing_direction: int = -1,
        joint_direction: int = -1,
        joint_position_offset: float = 0.0,
        motor_position_offset: float = 0.0,
        current_threshold: int = 5000,
        velocity_threshold: float = 0.001,
    ) -> None:
        """

        This method homes the actuator and the corresponding joint by moving it to the zero position.
        The zero position is defined as the position where the joint is fully extended. This method will
        also load the encoder map if it exists. The encoder map is a polynomial that maps the encoder counts
        to joint position in radians. This is useful for more accurate joint position estimation.
        Args:
            homing_voltage (int): Voltage in mV to use for homing. Default is 2000 mV.
            homing_frequency (int): Frequency in Hz to use for homing. Default is the actuator's frequency.
            homing_direction (int): Direction to move the actuator during homing. Default is -1.
            joint_direction (int): Direction to move the joint during homing. Default is -1.
            joint_position_offset (float): Offset in radians to add to the joint position. Default is 0.0.
            motor_position_offset (float): Offset in radians to add to the motor position. Default is 0.0.
            current_threshold (int): Current threshold in mA to stop homing the joint or actuator. This is used to detect if the actuator or joint has hit a hard stop. Default is 5000 mA.
            velocity_threshold (float): Velocity threshold in rad/s to stop homing the joint or actuator. This is also used to detect if the actuator or joint has hit a hard stop. Default is 0.001 rad/s.

        """
        is_homing = True
        homing_frequency = homing_frequency if homing_frequency is not None else self.frequency

        self.set_control_mode(mode=CONTROL_MODES.VOLTAGE)

        self.set_motor_voltage(value=homing_direction * homing_voltage)  # mV, negative for counterclockwise

        _motor_encoder_array = []
        _joint_encoder_array = []

        time.sleep(0.1)

        try:
            while is_homing:
                self.update()
                time.sleep(1 / homing_frequency)

                _motor_encoder_array.append(self.motor_position)
                _joint_encoder_array.append(self.joint_position)

                if abs(self.output_velocity) <= velocity_threshold or abs(self.motor_current) >= current_threshold:
                    self.set_motor_voltage(value=0)
                    is_homing = False

        except KeyboardInterrupt:
            self.set_motor_voltage(value=0)
            LOGGER.info(msg=f"[{self.__repr__()}] Homing interrupted.")
            return
        except Exception as e:
            self.set_motor_voltage(value=0)
            LOGGER.error(msg=f"[{self.__repr__()}] Homing failed: {e}")
            return

        self.set_motor_zero_position(value=self.motor_position)
        self.set_joint_zero_position(value=self.joint_position)

        time.sleep(0.1)
        self.set_joint_direction(joint_direction)
        self.set_motor_position_offset(motor_position_offset)
        self.set_joint_position_offset(joint_position_offset)

        self._is_homed = True
        LOGGER.info(f"[{self.__repr__()}] Homing complete.")

        if os.path.isfile(path=f"./{self.tag}_encoder_map.npy"):
            coefficients = np.load(file=f"./{self.tag}_encoder_map.npy")
            self.set_encoder_map(np.polynomial.polynomial.Polynomial(coef=coefficients))
        else:
            LOGGER.debug(
                msg=f"[{self.__repr__()}] No encoder map found. Please call the make_encoder_map method to create one. The encoder map is used to estimate joint position more accurately."
            )

    def make_encoder_map(self, overwrite=False) -> None:
        """
        This method makes a lookup table to calculate the position measured by the joint encoder.
        This is necessary because the magnetic output encoders are nonlinear.
        By making the map while the joint is unloaded, joint position calculated by motor position * gear ratio
        should be the same as the true joint position.

        Output from this function is a file containing a_i values parameterizing the map

        Eqn: position = sum from i=0^5 (a_i*counts^i)

        Author: Kevin Best
                U-M Locolab | Neurobionics Lab
                Gitub: tkevinbest, https://github.com/tkevinbest
        """

        if not self.is_homed:
            LOGGER.warning(msg=f"[{self.__repr__()}] Please home the {self.tag} joint before making the encoder map.")
            return

        if os.path.exists(f"./{self.tag}_encoder_map.npy") and not overwrite:
            LOGGER.info(msg=f"[{self.__repr__()}] Encoder map exists. Skipping encoder map creation.")
            return

        self.set_control_mode(mode=CONTROL_MODES.CURRENT)
        self.set_current_gains()
        time.sleep(0.1)
        self.set_current_gains()

        self.set_joint_torque(value=0.0)
        time.sleep(0.1)
        self.set_joint_torque(value=0.0)

        _joint_encoder_array = []
        _output_position_array = []

        LOGGER.info(
            msg=f"[{self.__repr__()}] Please manually move the {self.tag} joint numerous times through its full range of motion for 10 seconds. \n{input('Press any key when you are ready to start.')}"
        )

        _start_time: float = time.time()

        try:
            while time.time() - _start_time < 10:
                LOGGER.info(
                    msg=f"[{self.__repr__()}] Mapping the {self.tag} joint encoder: {10 - time.time() + _start_time} seconds left."
                )
                self.update()
                _joint_encoder_array.append(self.joint_encoder_counts)
                _output_position_array.append(self.output_position)
                time.sleep(1 / self.frequency)

        except KeyboardInterrupt:
            LOGGER.warning(msg="Encoder map interrupted.")
            return

        LOGGER.info(msg=f"[{self.__repr__()}] You may now stop moving the {self.tag} joint.")

        _power = np.arange(4.0)
        _a_mat = np.array(_joint_encoder_array).reshape(-1, 1) ** _power
        _beta = np.linalg.lstsq(_a_mat, _output_position_array, rcond=None)
        _coeffs = _beta[0]

        self.set_encoder_map(np.polynomial.polynomial.Polynomial(coef=_coeffs))

        np.save(file=f"./{self.tag}_encoder_map.npy", arr=_coeffs)
        LOGGER.info(msg=f"[{self.__repr__()}] Encoder map saved to './{self.tag}_encoder_map.npy'.")

    def set_motor_torque(self, value: float) -> None:
        """
        Sets the motor torque in Nm. This is the torque that is applied to the motor rotor, not the joint or output.
        Args:
            value (float): The torque to set in Nm.
        """
        self.set_motor_current(
            value / self.MOTOR_CONSTANTS.NM_PER_MILLIAMP,
        )

    def set_joint_torque(self, value: float) -> None:
        """
        Set the joint torque of the joint.
        This is the torque that is applied to the joint, not the motor.

        Args:
            value (float): torque in N_m
        """
        self.set_motor_torque(value=value / self.gear_ratio)

    @deprecated_with_routing(alternative_func=set_joint_torque)
    def set_output_torque(self, value: float) -> None:
        """
        Set the output torque of the joint.
        This is the torque that is applied to the joint, not the motor.

        Args:
            value (float): torque in N_m
        """
        self.set_motor_torque(value=value / self.gear_ratio)

    def set_motor_current(
        self,
        value: float,
    ):
        """
        Sets the motor current in mA.

        Args:
            value (float): The current to set in mA.
        """
        self.command_motor_current(value=int(value))

    @deprecated_with_routing(alternative_func=set_motor_current)
    def set_current(self, value: float) -> None:
        self.command_motor_current(value=int(value))

    def set_motor_voltage(self, value: float) -> None:
        """
        Sets the motor voltage in mV.

        Args:
            value (float): The voltage to set in mV.
        """
        self.command_motor_voltage(value=int(value))

    @deprecated_with_routing(alternative_func=set_motor_voltage)
    def set_voltage(self, value: float) -> None:
        self.command_motor_voltage(value=int(value))

    def set_motor_position(self, value: float) -> None:
        """
        Sets the motor position in radians.
        If in impedance mode, this sets the equilibrium angle in radians.

        Args:
            value (float): The position to set
        """
        self.command_motor_position(
            value=int(
                (value + self.motor_zero_position + self.motor_position_offset) / self.MOTOR_CONSTANTS.RAD_PER_COUNT
            ),
        )

    def set_position_gains(
        self,
        kp: float = DEFAULT_POSITION_GAINS.kp,
        ki: float = DEFAULT_POSITION_GAINS.ki,
        kd: float = DEFAULT_POSITION_GAINS.kd,
        ff: float = DEFAULT_POSITION_GAINS.ff,
    ) -> None:
        """
        Sets the position gains in arbitrary Dephy units.

        Args:
            kp (float): The proportional gain
            ki (float): The integral gain
            kd (float): The derivative gain
            ff (float): The feedforward gain
        """
        self.set_gains(
            kp=int(kp),
            ki=int(ki),
            kd=int(kd),
            k=0,
            b=0,
            ff=int(ff),
        )

    def set_current_gains(
        self,
        kp: float = DEFAULT_CURRENT_GAINS.kp,
        ki: float = DEFAULT_CURRENT_GAINS.ki,
        kd: float = DEFAULT_CURRENT_GAINS.kd,
        ff: float = DEFAULT_CURRENT_GAINS.ff,
    ) -> None:
        """
        Sets the current gains in arbitrary Dephy units.

        Args:
            kp (float): The proportional gain
            ki (float): The integral gain
            kd (float): The derivative gain
            ff (float): The feedforward gain
        """
        self.set_gains(
            kp=int(kp),
            ki=int(ki),
            kd=int(kd),
            k=0,
            b=0,
            ff=int(ff),
        )

    def set_output_impedance(
        self,
        kp: float = DEFAULT_IMPEDANCE_GAINS.kp,
        ki: float = DEFAULT_IMPEDANCE_GAINS.ki,
        kd: float = DEFAULT_IMPEDANCE_GAINS.kd,
        k: float = 100.0,
        b: float = 3.0,
        ff: float = 128,
    ) -> None:
        """
        Set the impedance gains of the joint in real units: Nm/rad and Nm/rad/s.
        This sets the impedance at the output and automatically scales based on gear raitos.

        Conversion:
            K_motor = K_joint / (gear_ratio ** 2)
            B_motor = B_joint / (gear_ratio ** 2)

        Args:
            kp (float): Proportional gain. Defaults to 40.
            ki (float): Integral gain. Defaults to 400.
            kd (float): Derivative gain. Defaults to 0.
            k (float): Spring constant. Defaults to 100 Nm/rad.
            b (float): Damping constant. Defaults to 3.0 Nm/rad/s.
            ff (float): Feedforward gain. Defaults to 128.
        """
        self.set_motor_impedance(
            kp=kp,
            ki=ki,
            kd=kd,
            k=k / (self.gear_ratio**2),
            b=b / (self.gear_ratio**2),
            ff=ff,
        )

    def set_impedance_gains(
        self,
        kp: float = DEFAULT_IMPEDANCE_GAINS.kp,
        ki: float = DEFAULT_IMPEDANCE_GAINS.ki,
        kd: float = DEFAULT_IMPEDANCE_GAINS.kd,
        k: float = DEFAULT_IMPEDANCE_GAINS.k,
        b: float = DEFAULT_IMPEDANCE_GAINS.b,
        ff: float = DEFAULT_IMPEDANCE_GAINS.ff,
    ) -> None:
        """
        Sets the impedance gains in arbitrary actpack units.
        See Dephy's webpage for conversions or use other library methods that handle conversion for you.

        Args:
            kp (float): The proportional gain
            ki (float): The integral gain
            kd (float): The derivative gain
            k (float): The spring constant
            b (float): The damping constant
            ff (float): The feedforward gain
        """
        self.set_gains(
            kp=int(kp),
            ki=int(ki),
            kd=int(kd),
            k=int(k),
            b=int(b),
            ff=int(ff),
        )

    def set_motor_impedance(
        self,
        kp: float = DEFAULT_IMPEDANCE_GAINS.kp,
        ki: float = DEFAULT_IMPEDANCE_GAINS.ki,
        kd: float = DEFAULT_IMPEDANCE_GAINS.kd,
        k: float = 0.08922,
        b: float = 0.0038070,
        ff: float = DEFAULT_IMPEDANCE_GAINS.ff,
    ) -> None:
        """
        Set the impedance gains of the motor in real units: Nm/rad and Nm/rad/s.

        Args:
            kp (float): Proportional gain. Defaults to 40.
            ki (float): Integral gain. Defaults to 400.
            kd (float): Derivative gain. Defaults to 0.
            k (float): Spring constant. Defaults to 0.08922 Nm/rad.
            b (float): Damping constant. Defaults to 0.0038070 Nm/rad/s.
            ff (float): Feedforward gain. Defaults to 128.
        """
        self.set_impedance_gains(
            kp=kp,
            ki=ki,
            kd=kd,
            k=int(k * self.MOTOR_CONSTANTS.NM_PER_RAD_TO_K),
            b=int(b * self.MOTOR_CONSTANTS.NM_S_PER_RAD_TO_B),
            ff=ff,
        )

    def set_encoder_map(self, encoder_map) -> None:
        """Sets the joint encoder map"""
        self._encoder_map = encoder_map

    @property
    def encoder_map(self):
        """Polynomial coefficients defining the joint encoder map from counts to radians."""
        if getattr(self, "_encoder_map", None) is not None:
            return self._encoder_map
        else:
            LOGGER.warning(msg="Encoder map is not set. Please call the make_encoder_map method to create one.")
            return None

    @property
    def motor_voltage(self) -> float:
        """Q-axis motor voltage in mV."""
        if self._data is not None:
            return float(self._data["mot_volt"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_current(self) -> float:
        if self._data is not None:
            return float(self._data["mot_cur"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_torque(self) -> float:
        if self._data is not None:
            return float(self._data["mot_cur"] * self.MOTOR_CONSTANTS.NM_PER_MILLIAMP)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_position(self) -> float:
        if self._data is not None:
            return (
                float(self._data["mot_ang"] * self.MOTOR_CONSTANTS.RAD_PER_COUNT)
                - self.motor_zero_position
                - self.motor_position_offset
            )
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_encoder_counts(self) -> int:
        """Raw reading from motor encoder in counts."""
        if self._data is not None:
            return int(self._data["mot_ang"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0."
            )
            return 0

    @property
    def joint_encoder_counts(self) -> int:
        """Raw reading from joint encoder in counts."""
        if self._data is not None:
            return int(self._data["ank_ang"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0."
            )
            return 0

    @property
    def motor_velocity(self) -> float:
        if self._data is not None:
            return int(self._data["mot_vel"]) * RAD_PER_DEG
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_acceleration(self) -> float:
        if self._data is not None:
            return float(self._data["mot_acc"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def battery_voltage(self) -> float:
        """Battery voltage in mV."""
        if self._data is not None:
            return float(self._data["batt_volt"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def battery_current(self) -> float:
        """Battery current in mA."""
        if self._data is not None:
            return float(self._data["batt_curr"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def joint_position(self) -> float:
        if self._data is not None:
            return (
                float(self._data["ank_ang"] * self.MOTOR_CONSTANTS.RAD_PER_COUNT)
                - self.joint_zero_position
                - self.joint_position_offset
            ) * self.joint_direction
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def joint_velocity(self) -> float:
        if self._data is not None:
            return float(self._data["ank_vel"] * RAD_PER_DEG) * self.joint_direction
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def joint_torque(self) -> float:
        """
        Torque at the joint output in Nm.
        This is calculated using motor current, k_t, and the gear ratio.
        """
        return self.motor_torque * self.gear_ratio

    @property
    def case_temperature(self) -> float:
        if self._data is not None:
            return float(self._data["temperature"])
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def winding_temperature(self) -> float:
        """
        ESTIMATED temperature of the windings in celsius.
        This is calculated based on the thermal model using motor current.
        """
        if self._data is not None:
            return float(self._thermal_model.T_w)
        else:
            return 0.0

    @property
    def genvars(self):
        """Dephy's 'genvars' object."""
        if self._data is not None:
            return np.array(
                object=[
                    self._data["genvar_0"],
                    self._data["genvar_1"],
                    self._data["genvar_2"],
                    self._data["genvar_3"],
                    self._data["genvar_4"],
                    self._data["genvar_5"],
                ]
            )
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning zeros"
            )
            return np.zeros(shape=6)

    @property
    def accelx(self) -> float:
        """
        Acceleration in x direction in m/s^2.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data["accelx"] * M_PER_SEC_SQUARED_ACCLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def accely(self) -> float:
        """
        Acceleration in y direction in m/s^2.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data["accely"] * M_PER_SEC_SQUARED_ACCLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def accelz(self) -> float:
        """
        Acceleration in z direction in m/s^2.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data["accelz"] * M_PER_SEC_SQUARED_ACCLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def gyrox(self) -> float:
        """
        Angular velocity in x direction in rad/s.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data["gyrox"] * RAD_PER_SEC_GYROLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def gyroy(self) -> float:
        """
        Angular velocity in y direction in rad/s.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data["gyroy"] * RAD_PER_SEC_GYROLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def gyroz(self) -> float:
        """
        Angular velocity in z direction in rad/s.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data["gyroz"] * RAD_PER_SEC_GYROLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def thermal_scaling_factor(self) -> float:
        """
        Scale factor to use in torque control, in [0,1].
        If you scale the torque command by this factor, the motor temperature will never exceed max allowable temperature.
        For a proof, see paper referenced in thermal model.
        """
        return self._thermal_scale

accelx: float property

Acceleration in x direction in m/s^2. Measured using actpack's onboard IMU.

accely: float property

Acceleration in y direction in m/s^2. Measured using actpack's onboard IMU.

accelz: float property

Acceleration in z direction in m/s^2. Measured using actpack's onboard IMU.

battery_current: float property

Battery current in mA.

battery_voltage: float property

Battery voltage in mV.

encoder_map property

Polynomial coefficients defining the joint encoder map from counts to radians.

genvars property

Dephy's 'genvars' object.

gyrox: float property

Angular velocity in x direction in rad/s. Measured using actpack's onboard IMU.

gyroy: float property

Angular velocity in y direction in rad/s. Measured using actpack's onboard IMU.

gyroz: float property

Angular velocity in z direction in rad/s. Measured using actpack's onboard IMU.

joint_encoder_counts: int property

Raw reading from joint encoder in counts.

joint_torque: float property

Torque at the joint output in Nm. This is calculated using motor current, k_t, and the gear ratio.

motor_encoder_counts: int property

Raw reading from motor encoder in counts.

motor_voltage: float property

Q-axis motor voltage in mV.

thermal_scaling_factor: float property

Scale factor to use in torque control, in [0,1]. If you scale the torque command by this factor, the motor temperature will never exceed max allowable temperature. For a proof, see paper referenced in thermal model.

winding_temperature: float property

ESTIMATED temperature of the windings in celsius. This is calculated based on the thermal model using motor current.

home(homing_voltage=2000, homing_frequency=None, homing_direction=-1, joint_direction=-1, joint_position_offset=0.0, motor_position_offset=0.0, current_threshold=5000, velocity_threshold=0.001)

This method homes the actuator and the corresponding joint by moving it to the zero position. The zero position is defined as the position where the joint is fully extended. This method will also load the encoder map if it exists. The encoder map is a polynomial that maps the encoder counts to joint position in radians. This is useful for more accurate joint position estimation. Args: homing_voltage (int): Voltage in mV to use for homing. Default is 2000 mV. homing_frequency (int): Frequency in Hz to use for homing. Default is the actuator's frequency. homing_direction (int): Direction to move the actuator during homing. Default is -1. joint_direction (int): Direction to move the joint during homing. Default is -1. joint_position_offset (float): Offset in radians to add to the joint position. Default is 0.0. motor_position_offset (float): Offset in radians to add to the motor position. Default is 0.0. current_threshold (int): Current threshold in mA to stop homing the joint or actuator. This is used to detect if the actuator or joint has hit a hard stop. Default is 5000 mA. velocity_threshold (float): Velocity threshold in rad/s to stop homing the joint or actuator. This is also used to detect if the actuator or joint has hit a hard stop. Default is 0.001 rad/s.

Source code in opensourceleg/actuators/dephy.py

def home(
    self,
    homing_voltage: int = 2000,
    homing_frequency: Optional[int] = None,
    homing_direction: int = -1,
    joint_direction: int = -1,
    joint_position_offset: float = 0.0,
    motor_position_offset: float = 0.0,
    current_threshold: int = 5000,
    velocity_threshold: float = 0.001,
) -> None:
    """

    This method homes the actuator and the corresponding joint by moving it to the zero position.
    The zero position is defined as the position where the joint is fully extended. This method will
    also load the encoder map if it exists. The encoder map is a polynomial that maps the encoder counts
    to joint position in radians. This is useful for more accurate joint position estimation.
    Args:
        homing_voltage (int): Voltage in mV to use for homing. Default is 2000 mV.
        homing_frequency (int): Frequency in Hz to use for homing. Default is the actuator's frequency.
        homing_direction (int): Direction to move the actuator during homing. Default is -1.
        joint_direction (int): Direction to move the joint during homing. Default is -1.
        joint_position_offset (float): Offset in radians to add to the joint position. Default is 0.0.
        motor_position_offset (float): Offset in radians to add to the motor position. Default is 0.0.
        current_threshold (int): Current threshold in mA to stop homing the joint or actuator. This is used to detect if the actuator or joint has hit a hard stop. Default is 5000 mA.
        velocity_threshold (float): Velocity threshold in rad/s to stop homing the joint or actuator. This is also used to detect if the actuator or joint has hit a hard stop. Default is 0.001 rad/s.

    """
    is_homing = True
    homing_frequency = homing_frequency if homing_frequency is not None else self.frequency

    self.set_control_mode(mode=CONTROL_MODES.VOLTAGE)

    self.set_motor_voltage(value=homing_direction * homing_voltage)  # mV, negative for counterclockwise

    _motor_encoder_array = []
    _joint_encoder_array = []

    time.sleep(0.1)

    try:
        while is_homing:
            self.update()
            time.sleep(1 / homing_frequency)

            _motor_encoder_array.append(self.motor_position)
            _joint_encoder_array.append(self.joint_position)

            if abs(self.output_velocity) <= velocity_threshold or abs(self.motor_current) >= current_threshold:
                self.set_motor_voltage(value=0)
                is_homing = False

    except KeyboardInterrupt:
        self.set_motor_voltage(value=0)
        LOGGER.info(msg=f"[{self.__repr__()}] Homing interrupted.")
        return
    except Exception as e:
        self.set_motor_voltage(value=0)
        LOGGER.error(msg=f"[{self.__repr__()}] Homing failed: {e}")
        return

    self.set_motor_zero_position(value=self.motor_position)
    self.set_joint_zero_position(value=self.joint_position)

    time.sleep(0.1)
    self.set_joint_direction(joint_direction)
    self.set_motor_position_offset(motor_position_offset)
    self.set_joint_position_offset(joint_position_offset)

    self._is_homed = True
    LOGGER.info(f"[{self.__repr__()}] Homing complete.")

    if os.path.isfile(path=f"./{self.tag}_encoder_map.npy"):
        coefficients = np.load(file=f"./{self.tag}_encoder_map.npy")
        self.set_encoder_map(np.polynomial.polynomial.Polynomial(coef=coefficients))
    else:
        LOGGER.debug(
            msg=f"[{self.__repr__()}] No encoder map found. Please call the make_encoder_map method to create one. The encoder map is used to estimate joint position more accurately."
        )

make_encoder_map(overwrite=False)

This method makes a lookup table to calculate the position measured by the joint encoder. This is necessary because the magnetic output encoders are nonlinear. By making the map while the joint is unloaded, joint position calculated by motor position * gear ratio should be the same as the true joint position.

Output from this function is a file containing a_i values parameterizing the map

Eqn: position = sum from i=0^5 (a_i*counts^i)

Kevin Best

U-M Locolab | Neurobionics Lab Gitub: tkevinbest, https://github.com/tkevinbest

Source code in opensourceleg/actuators/dephy.py

def make_encoder_map(self, overwrite=False) -> None:
    """
    This method makes a lookup table to calculate the position measured by the joint encoder.
    This is necessary because the magnetic output encoders are nonlinear.
    By making the map while the joint is unloaded, joint position calculated by motor position * gear ratio
    should be the same as the true joint position.

    Output from this function is a file containing a_i values parameterizing the map

    Eqn: position = sum from i=0^5 (a_i*counts^i)

    Author: Kevin Best
            U-M Locolab | Neurobionics Lab
            Gitub: tkevinbest, https://github.com/tkevinbest
    """

    if not self.is_homed:
        LOGGER.warning(msg=f"[{self.__repr__()}] Please home the {self.tag} joint before making the encoder map.")
        return

    if os.path.exists(f"./{self.tag}_encoder_map.npy") and not overwrite:
        LOGGER.info(msg=f"[{self.__repr__()}] Encoder map exists. Skipping encoder map creation.")
        return

    self.set_control_mode(mode=CONTROL_MODES.CURRENT)
    self.set_current_gains()
    time.sleep(0.1)
    self.set_current_gains()

    self.set_joint_torque(value=0.0)
    time.sleep(0.1)
    self.set_joint_torque(value=0.0)

    _joint_encoder_array = []
    _output_position_array = []

    LOGGER.info(
        msg=f"[{self.__repr__()}] Please manually move the {self.tag} joint numerous times through its full range of motion for 10 seconds. \n{input('Press any key when you are ready to start.')}"
    )

    _start_time: float = time.time()

    try:
        while time.time() - _start_time < 10:
            LOGGER.info(
                msg=f"[{self.__repr__()}] Mapping the {self.tag} joint encoder: {10 - time.time() + _start_time} seconds left."
            )
            self.update()
            _joint_encoder_array.append(self.joint_encoder_counts)
            _output_position_array.append(self.output_position)
            time.sleep(1 / self.frequency)

    except KeyboardInterrupt:
        LOGGER.warning(msg="Encoder map interrupted.")
        return

    LOGGER.info(msg=f"[{self.__repr__()}] You may now stop moving the {self.tag} joint.")

    _power = np.arange(4.0)
    _a_mat = np.array(_joint_encoder_array).reshape(-1, 1) ** _power
    _beta = np.linalg.lstsq(_a_mat, _output_position_array, rcond=None)
    _coeffs = _beta[0]

    self.set_encoder_map(np.polynomial.polynomial.Polynomial(coef=_coeffs))

    np.save(file=f"./{self.tag}_encoder_map.npy", arr=_coeffs)
    LOGGER.info(msg=f"[{self.__repr__()}] Encoder map saved to './{self.tag}_encoder_map.npy'.")

set_current_gains(kp=DEFAULT_CURRENT_GAINS.kp, ki=DEFAULT_CURRENT_GAINS.ki, kd=DEFAULT_CURRENT_GAINS.kd, ff=DEFAULT_CURRENT_GAINS.ff)

Sets the current gains in arbitrary Dephy units.

Parameters:

Name	Type	Description	Default
kp	float	The proportional gain	kp
ki	float	The integral gain	ki
kd	float	The derivative gain	kd
ff	float	The feedforward gain	ff

Source code in opensourceleg/actuators/dephy.py

def set_current_gains(
    self,
    kp: float = DEFAULT_CURRENT_GAINS.kp,
    ki: float = DEFAULT_CURRENT_GAINS.ki,
    kd: float = DEFAULT_CURRENT_GAINS.kd,
    ff: float = DEFAULT_CURRENT_GAINS.ff,
) -> None:
    """
    Sets the current gains in arbitrary Dephy units.

    Args:
        kp (float): The proportional gain
        ki (float): The integral gain
        kd (float): The derivative gain
        ff (float): The feedforward gain
    """
    self.set_gains(
        kp=int(kp),
        ki=int(ki),
        kd=int(kd),
        k=0,
        b=0,
        ff=int(ff),
    )

set_encoder_map(encoder_map)

Sets the joint encoder map

Source code in opensourceleg/actuators/dephy.py

def set_encoder_map(self, encoder_map) -> None:
    """Sets the joint encoder map"""
    self._encoder_map = encoder_map

set_impedance_gains(kp=DEFAULT_IMPEDANCE_GAINS.kp, ki=DEFAULT_IMPEDANCE_GAINS.ki, kd=DEFAULT_IMPEDANCE_GAINS.kd, k=DEFAULT_IMPEDANCE_GAINS.k, b=DEFAULT_IMPEDANCE_GAINS.b, ff=DEFAULT_IMPEDANCE_GAINS.ff)

Sets the impedance gains in arbitrary actpack units. See Dephy's webpage for conversions or use other library methods that handle conversion for you.

Parameters:

Name	Type	Description	Default
kp	float	The proportional gain	kp
ki	float	The integral gain	ki
kd	float	The derivative gain	kd
k	float	The spring constant	k
b	float	The damping constant	b
ff	float	The feedforward gain	ff

Source code in opensourceleg/actuators/dephy.py

def set_impedance_gains(
    self,
    kp: float = DEFAULT_IMPEDANCE_GAINS.kp,
    ki: float = DEFAULT_IMPEDANCE_GAINS.ki,
    kd: float = DEFAULT_IMPEDANCE_GAINS.kd,
    k: float = DEFAULT_IMPEDANCE_GAINS.k,
    b: float = DEFAULT_IMPEDANCE_GAINS.b,
    ff: float = DEFAULT_IMPEDANCE_GAINS.ff,
) -> None:
    """
    Sets the impedance gains in arbitrary actpack units.
    See Dephy's webpage for conversions or use other library methods that handle conversion for you.

    Args:
        kp (float): The proportional gain
        ki (float): The integral gain
        kd (float): The derivative gain
        k (float): The spring constant
        b (float): The damping constant
        ff (float): The feedforward gain
    """
    self.set_gains(
        kp=int(kp),
        ki=int(ki),
        kd=int(kd),
        k=int(k),
        b=int(b),
        ff=int(ff),
    )

set_joint_torque(value)

Set the joint torque of the joint. This is the torque that is applied to the joint, not the motor.

Parameters:

Name	Type	Description	Default
value	float	torque in N_m	required

Source code in opensourceleg/actuators/dephy.py

def set_joint_torque(self, value: float) -> None:
    """
    Set the joint torque of the joint.
    This is the torque that is applied to the joint, not the motor.

    Args:
        value (float): torque in N_m
    """
    self.set_motor_torque(value=value / self.gear_ratio)

set_motor_current(value)

Sets the motor current in mA.

Parameters:

Name	Type	Description	Default
value	float	The current to set in mA.	required

Source code in opensourceleg/actuators/dephy.py

def set_motor_current(
    self,
    value: float,
):
    """
    Sets the motor current in mA.

    Args:
        value (float): The current to set in mA.
    """
    self.command_motor_current(value=int(value))

set_motor_impedance(kp=DEFAULT_IMPEDANCE_GAINS.kp, ki=DEFAULT_IMPEDANCE_GAINS.ki, kd=DEFAULT_IMPEDANCE_GAINS.kd, k=0.08922, b=0.003807, ff=DEFAULT_IMPEDANCE_GAINS.ff)

Set the impedance gains of the motor in real units: Nm/rad and Nm/rad/s.

Parameters:

Name	Type	Description	Default
kp	float	Proportional gain. Defaults to 40.	kp
ki	float	Integral gain. Defaults to 400.	ki
kd	float	Derivative gain. Defaults to 0.	kd
k	float	Spring constant. Defaults to 0.08922 Nm/rad.	0.08922
b	float	Damping constant. Defaults to 0.0038070 Nm/rad/s.	0.003807
ff	float	Feedforward gain. Defaults to 128.	ff

Source code in opensourceleg/actuators/dephy.py

def set_motor_impedance(
    self,
    kp: float = DEFAULT_IMPEDANCE_GAINS.kp,
    ki: float = DEFAULT_IMPEDANCE_GAINS.ki,
    kd: float = DEFAULT_IMPEDANCE_GAINS.kd,
    k: float = 0.08922,
    b: float = 0.0038070,
    ff: float = DEFAULT_IMPEDANCE_GAINS.ff,
) -> None:
    """
    Set the impedance gains of the motor in real units: Nm/rad and Nm/rad/s.

    Args:
        kp (float): Proportional gain. Defaults to 40.
        ki (float): Integral gain. Defaults to 400.
        kd (float): Derivative gain. Defaults to 0.
        k (float): Spring constant. Defaults to 0.08922 Nm/rad.
        b (float): Damping constant. Defaults to 0.0038070 Nm/rad/s.
        ff (float): Feedforward gain. Defaults to 128.
    """
    self.set_impedance_gains(
        kp=kp,
        ki=ki,
        kd=kd,
        k=int(k * self.MOTOR_CONSTANTS.NM_PER_RAD_TO_K),
        b=int(b * self.MOTOR_CONSTANTS.NM_S_PER_RAD_TO_B),
        ff=ff,
    )

set_motor_position(value)

Sets the motor position in radians. If in impedance mode, this sets the equilibrium angle in radians.

Parameters:

Name	Type	Description	Default
value	float	The position to set	required

Source code in opensourceleg/actuators/dephy.py

def set_motor_position(self, value: float) -> None:
    """
    Sets the motor position in radians.
    If in impedance mode, this sets the equilibrium angle in radians.

    Args:
        value (float): The position to set
    """
    self.command_motor_position(
        value=int(
            (value + self.motor_zero_position + self.motor_position_offset) / self.MOTOR_CONSTANTS.RAD_PER_COUNT
        ),
    )

set_motor_torque(value)

Sets the motor torque in Nm. This is the torque that is applied to the motor rotor, not the joint or output. Args: value (float): The torque to set in Nm.

Source code in opensourceleg/actuators/dephy.py

def set_motor_torque(self, value: float) -> None:
    """
    Sets the motor torque in Nm. This is the torque that is applied to the motor rotor, not the joint or output.
    Args:
        value (float): The torque to set in Nm.
    """
    self.set_motor_current(
        value / self.MOTOR_CONSTANTS.NM_PER_MILLIAMP,
    )

set_motor_voltage(value)

Sets the motor voltage in mV.

Parameters:

Name	Type	Description	Default
value	float	The voltage to set in mV.	required

Source code in opensourceleg/actuators/dephy.py

def set_motor_voltage(self, value: float) -> None:
    """
    Sets the motor voltage in mV.

    Args:
        value (float): The voltage to set in mV.
    """
    self.command_motor_voltage(value=int(value))

set_output_impedance(kp=DEFAULT_IMPEDANCE_GAINS.kp, ki=DEFAULT_IMPEDANCE_GAINS.ki, kd=DEFAULT_IMPEDANCE_GAINS.kd, k=100.0, b=3.0, ff=128)

Set the impedance gains of the joint in real units: Nm/rad and Nm/rad/s. This sets the impedance at the output and automatically scales based on gear raitos.

Conversion

K_motor = K_joint / (gear_ratio ** 2) B_motor = B_joint / (gear_ratio ** 2)

Parameters:

Name	Type	Description	Default
kp	float	Proportional gain. Defaults to 40.	kp
ki	float	Integral gain. Defaults to 400.	ki
kd	float	Derivative gain. Defaults to 0.	kd
k	float	Spring constant. Defaults to 100 Nm/rad.	100.0
b	float	Damping constant. Defaults to 3.0 Nm/rad/s.	3.0
ff	float	Feedforward gain. Defaults to 128.	128

Source code in opensourceleg/actuators/dephy.py

def set_output_impedance(
    self,
    kp: float = DEFAULT_IMPEDANCE_GAINS.kp,
    ki: float = DEFAULT_IMPEDANCE_GAINS.ki,
    kd: float = DEFAULT_IMPEDANCE_GAINS.kd,
    k: float = 100.0,
    b: float = 3.0,
    ff: float = 128,
) -> None:
    """
    Set the impedance gains of the joint in real units: Nm/rad and Nm/rad/s.
    This sets the impedance at the output and automatically scales based on gear raitos.

    Conversion:
        K_motor = K_joint / (gear_ratio ** 2)
        B_motor = B_joint / (gear_ratio ** 2)

    Args:
        kp (float): Proportional gain. Defaults to 40.
        ki (float): Integral gain. Defaults to 400.
        kd (float): Derivative gain. Defaults to 0.
        k (float): Spring constant. Defaults to 100 Nm/rad.
        b (float): Damping constant. Defaults to 3.0 Nm/rad/s.
        ff (float): Feedforward gain. Defaults to 128.
    """
    self.set_motor_impedance(
        kp=kp,
        ki=ki,
        kd=kd,
        k=k / (self.gear_ratio**2),
        b=b / (self.gear_ratio**2),
        ff=ff,
    )

set_output_torque(value)

Set the output torque of the joint. This is the torque that is applied to the joint, not the motor.

Parameters:

Name	Type	Description	Default
value	float	torque in N_m	required

Source code in opensourceleg/actuators/dephy.py

@deprecated_with_routing(alternative_func=set_joint_torque)
def set_output_torque(self, value: float) -> None:
    """
    Set the output torque of the joint.
    This is the torque that is applied to the joint, not the motor.

    Args:
        value (float): torque in N_m
    """
    self.set_motor_torque(value=value / self.gear_ratio)

set_position_gains(kp=DEFAULT_POSITION_GAINS.kp, ki=DEFAULT_POSITION_GAINS.ki, kd=DEFAULT_POSITION_GAINS.kd, ff=DEFAULT_POSITION_GAINS.ff)

Sets the position gains in arbitrary Dephy units.

Parameters:

Name	Type	Description	Default
kp	float	The proportional gain	kp
ki	float	The integral gain	ki
kd	float	The derivative gain	kd
ff	float	The feedforward gain	ff

Source code in opensourceleg/actuators/dephy.py

def set_position_gains(
    self,
    kp: float = DEFAULT_POSITION_GAINS.kp,
    ki: float = DEFAULT_POSITION_GAINS.ki,
    kd: float = DEFAULT_POSITION_GAINS.kd,
    ff: float = DEFAULT_POSITION_GAINS.ff,
) -> None:
    """
    Sets the position gains in arbitrary Dephy units.

    Args:
        kp (float): The proportional gain
        ki (float): The integral gain
        kd (float): The derivative gain
        ff (float): The feedforward gain
    """
    self.set_gains(
        kp=int(kp),
        ki=int(ki),
        kd=int(kd),
        k=0,
        b=0,
        ff=int(ff),
    )

DephyLegacyActuator

Bases: DephyActuator

Source code in opensourceleg/actuators/dephy.py

class DephyLegacyActuator(DephyActuator):
    def __init__(
        self,
        tag: str = "DephyActuator",
        port: str = "/dev/ttyACM0",
        gear_ratio: float = 1.0,
        baud_rate: int = 230400,
        frequency: int = 500,
        debug_level: int = 4,
        dephy_log: bool = False,
        offline: bool = False,
    ) -> None:
        ActuatorBase.__init__(
            self,
            tag=tag,
            gear_ratio=gear_ratio,
            motor_constants=DEPHY_ACTUATOR_CONSTANTS,
            frequency=frequency,
            offline=offline,
        )

        self._debug_level: int = debug_level if dephy_log else 6
        self._dephy_log: bool = dephy_log

        if self.is_offline:
            self.port = port
            self._is_streaming: bool = False
            self._is_open: bool = False
        else:
            # def set_is_streaming(self, value):
            #     self._is_streaming = value

            # def set_is_open(self, value):
            #     self._is_open = value

            # type(self).is_streaming = property(fset=set_is_streaming)
            # type(self).is_open = property(fset=set_is_open)

            Device.__init__(self, port=port, baud_rate=baud_rate)

        self._thermal_model: ThermalModel = ThermalModel(
            temp_limit_windings=self.max_winding_temperature,
            soft_border_C_windings=10,
            temp_limit_case=self.max_case_temperature,
            soft_border_C_case=10,
        )
        self._thermal_scale: float = 1.0

        self._mode = CONTROL_MODES.IDLE

    def __repr__(self) -> str:
        return f"{self.tag}[DephyActuator]"

    @property
    def _CONTROL_MODE_CONFIGS(self) -> CONTROL_MODE_CONFIGS:
        return DEPHY_LEGACY_CONTROL_MODE_CONFIGS

    @check_actuator_connection
    def start(self) -> None:
        try:
            self.open(
                freq=self._frequency,
                log_level=self._debug_level,
                log_enabled=self._dephy_log,
            )
        except OSError:
            print("\n")
            LOGGER.error(
                msg=f"[{self.__repr__()}] Need admin previleges to open the port '{self.port}'. \n\nPlease run the script with 'sudo' command or add the user to the dialout group.\n"
            )
            os._exit(status=1)

        self._data = self.read()

        # TODO: Verify if we need this sleep here
        time.sleep(0.1)
        self.set_control_mode(CONTROL_MODES.VOLTAGE)

    @check_actuator_stream
    @check_actuator_open
    def stop(self) -> None:
        self.set_control_mode(mode=CONTROL_MODES.VOLTAGE)
        self.set_motor_voltage(value=0)

        self.set_control_mode(mode=CONTROL_MODES.IDLE)
        time.sleep(0.1)
        self.close()

    def update(self) -> None:
        self._data = self.read()

        self._thermal_model.T_c = self.case_temperature
        self._thermal_scale = self._thermal_model.update_and_get_scale(
            dt=1 / self.frequency,
            motor_current=self.motor_current,
        )
        if self.case_temperature >= self.max_case_temperature:
            LOGGER.error(
                msg=f"[{str.upper(self.tag)}] Case thermal limit {self.max_case_temperature} reached. Stopping motor."
            )
            raise ThermalLimitException()

        if self.winding_temperature >= self.max_winding_temperature:
            LOGGER.error(
                msg=f"[{str.upper(self.tag)}] Winding thermal limit {self.max_winding_temperature} reached. Stopping motor."
            )
            raise ThermalLimitException()
        # Check for thermal fault, bit 2 of the execute status byte

        if self._data.status_ex & 0b00000010 == 0b00000010:
            LOGGER.error(
                msg=f"[{str.upper(self.tag)}] Thermal Fault: Winding temperature: {self.winding_temperature}; Case temperature: {self.case_temperature}."
            )
            raise ThermalLimitException("Internal thermal limit tripped.")

    def set_motor_current(
        self,
        value: float,
    ):
        """
        Sets the motor current in mA.

        Args:
            value (float): The current to set in mA.
        """
        self.send_motor_command(ctrl_mode=c_int(self.mode.value), value=int(value))

    @deprecated_with_routing(alternative_func=set_motor_current)
    def set_current(self, value: float) -> None:
        self.send_motor_command(ctrl_mode=c_int(self.mode.value), value=int(value))

    def set_motor_voltage(self, value: float) -> None:
        """
        Sets the motor voltage in mV.

        Args:
            value (float): The voltage to set in mV.
        """
        self.send_motor_command(ctrl_mode=c_int(self.mode.value), value=int(value))

    @deprecated_with_routing(alternative_func=set_motor_voltage)
    def set_voltage(self, value: float) -> None:
        self.send_motor_command(ctrl_mode=c_int(self.mode.value), value=int(value))

    def set_motor_position(self, value: float) -> None:
        """
        Sets the motor position in radians.
        If in impedance mode, this sets the equilibrium angle in radians.

        Args:
            value (float): The position to set
        """
        self.send_motor_command(
            ctrl_mode=c_int(self.mode.value),
            value=int(
                (value + self.motor_zero_position + self.motor_position_offset) / self.MOTOR_CONSTANTS.RAD_PER_COUNT
            ),
        )

    @property
    def motor_voltage(self) -> float:
        """Q-axis motor voltage in mV."""
        if self._data is not None:
            return float(self._data.mot_volt)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_current(self) -> float:
        if self._data is not None:
            return float(self._data.mot_cur)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_torque(self) -> float:
        """
        Torque at the motor output in Nm.
        """
        if self._data is not None:
            return float(self._data.mot_cur * self.MOTOR_CONSTANTS.NM_PER_MILLIAMP)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_position(self) -> float:
        """
        Motor position in radians.
        """
        if self._data is not None:
            return (
                float(self._data.mot_ang * self.MOTOR_CONSTANTS.RAD_PER_COUNT)
                - self.motor_zero_position
                - self.motor_position_offset
            )
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_encoder_counts(self) -> int:
        """Raw reading from motor encoder in counts."""
        if self._data is not None:
            return int(self._data.mot_ang)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0."
            )
            return 0

    @property
    def joint_encoder_counts(self) -> int:
        """Raw reading from joint encoder in counts."""
        if self._data is not None:
            return int(self._data.ank_ang)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0."
            )
            return 0

    @property
    def motor_velocity(self) -> float:
        """
        Motor velocity in rad/s.
        """
        if self._data is not None:
            return int(self._data.mot_vel) * RAD_PER_DEG
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def motor_acceleration(self) -> float:
        """
        Motor acceleration in rad/s^2.
        """
        if self._data is not None:
            return float(self._data.mot_acc)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def battery_voltage(self) -> float:
        """Battery voltage in mV."""
        if self._data is not None:
            return float(self._data.batt_volt)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def battery_current(self) -> float:
        """Battery current in mA."""
        if self._data is not None:
            return float(self._data.batt_curr)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def joint_position(self) -> float:
        """
        Joint position in radians.
        """
        if self._data is not None:
            return (
                float(self._data.ank_ang * self.MOTOR_CONSTANTS.RAD_PER_COUNT)
                - self.joint_zero_position
                - self.joint_position_offset
            ) * self.joint_direction
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def joint_velocity(self) -> float:
        """
        Joint velocity in rad/s.
        """
        if self._data is not None:
            return float(self._data.ank_vel * RAD_PER_DEG) * self.joint_direction
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def joint_torque(self) -> float:
        """
        Torque at the joint output in Nm.
        This is calculated using motor current, k_t, and the gear ratio.
        """
        return self.motor_torque * self.gear_ratio

    @property
    def case_temperature(self) -> float:
        """
        Case temperature of the actuator in celsius.
        """
        if self._data is not None:
            return float(self._data.temperature)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def winding_temperature(self) -> float:
        """
        ESTIMATED temperature of the windings in celsius.
        This is calculated based on the thermal model using motor current.
        """
        if self._data is not None:
            return float(self._thermal_model.T_w)
        else:
            return 0.0

    @property
    def genvars(self):
        """Dephy's 'genvars' object."""
        if self._data is not None:
            return np.array(
                object=[
                    self._data.genvar_0,
                    self._data.genvar_1,
                    self._data.genvar_2,
                    self._data.genvar_3,
                    self._data.genvar_4,
                    self._data.genvar_5,
                ]
            )
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning zeros"
            )
            return np.zeros(shape=6)

    @property
    def accelx(self) -> float:
        """
        Acceleration in x direction in m/s^2.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data.accelx * M_PER_SEC_SQUARED_ACCLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def accely(self) -> float:
        """
        Acceleration in y direction in m/s^2.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data.accely * M_PER_SEC_SQUARED_ACCLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def accelz(self) -> float:
        """
        Acceleration in z direction in m/s^2.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data.accelz * M_PER_SEC_SQUARED_ACCLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def gyrox(self) -> float:
        """
        Angular velocity in x direction in rad/s.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data.gyrox * RAD_PER_SEC_GYROLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def gyroy(self) -> float:
        """
        Angular velocity in y direction in rad/s.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data.gyroy * RAD_PER_SEC_GYROLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

    @property
    def is_streaming(self):
        return self._is_streaming

    @is_streaming.setter
    def is_streaming(self, value: bool):
        self._is_streaming = value

    @property
    def is_open(self):
        return self._is_open

    @is_open.setter
    def is_open(self, value: bool):
        self._is_open = value

    @property
    def gyroz(self) -> float:
        """
        Angular velocity in z direction in rad/s.
        Measured using actpack's onboard IMU.
        """
        if self._data is not None:
            return float(self._data.gyroz * RAD_PER_SEC_GYROLSB)
        else:
            LOGGER.debug(
                msg="Actuator data is none, please ensure that the actuator is connected and streaming. Returning 0.0."
            )
            return 0.0

accelx: float property

Acceleration in x direction in m/s^2. Measured using actpack's onboard IMU.

accely: float property

Acceleration in y direction in m/s^2. Measured using actpack's onboard IMU.

accelz: float property

Acceleration in z direction in m/s^2. Measured using actpack's onboard IMU.

battery_current: float property

Battery current in mA.

battery_voltage: float property

Battery voltage in mV.

case_temperature: float property

Case temperature of the actuator in celsius.

genvars property

Dephy's 'genvars' object.

gyrox: float property

Angular velocity in x direction in rad/s. Measured using actpack's onboard IMU.

gyroy: float property

Angular velocity in y direction in rad/s. Measured using actpack's onboard IMU.

gyroz: float property

Angular velocity in z direction in rad/s. Measured using actpack's onboard IMU.

joint_encoder_counts: int property

Raw reading from joint encoder in counts.

joint_position: float property

Joint position in radians.

joint_torque: float property

Torque at the joint output in Nm. This is calculated using motor current, k_t, and the gear ratio.

joint_velocity: float property

Joint velocity in rad/s.

motor_acceleration: float property

Motor acceleration in rad/s^2.

motor_encoder_counts: int property

Raw reading from motor encoder in counts.

motor_position: float property

Motor position in radians.

motor_torque: float property

Torque at the motor output in Nm.

motor_velocity: float property

Motor velocity in rad/s.

motor_voltage: float property

Q-axis motor voltage in mV.

winding_temperature: float property

ESTIMATED temperature of the windings in celsius. This is calculated based on the thermal model using motor current.

set_motor_current(value)

Sets the motor current in mA.

Parameters:

Name	Type	Description	Default
value	float	The current to set in mA.	required

Source code in opensourceleg/actuators/dephy.py

def set_motor_current(
    self,
    value: float,
):
    """
    Sets the motor current in mA.

    Args:
        value (float): The current to set in mA.
    """
    self.send_motor_command(ctrl_mode=c_int(self.mode.value), value=int(value))

set_motor_position(value)

Sets the motor position in radians. If in impedance mode, this sets the equilibrium angle in radians.

Parameters:

Name	Type	Description	Default
value	float	The position to set	required

Source code in opensourceleg/actuators/dephy.py

def set_motor_position(self, value: float) -> None:
    """
    Sets the motor position in radians.
    If in impedance mode, this sets the equilibrium angle in radians.

    Args:
        value (float): The position to set
    """
    self.send_motor_command(
        ctrl_mode=c_int(self.mode.value),
        value=int(
            (value + self.motor_zero_position + self.motor_position_offset) / self.MOTOR_CONSTANTS.RAD_PER_COUNT
        ),
    )

set_motor_voltage(value)

Sets the motor voltage in mV.

Parameters:

Name	Type	Description	Default
value	float	The voltage to set in mV.	required

Source code in opensourceleg/actuators/dephy.py

def set_motor_voltage(self, value: float) -> None:
    """
    Sets the motor voltage in mV.

    Args:
        value (float): The voltage to set in mV.
    """
    self.send_motor_command(ctrl_mode=c_int(self.mode.value), value=int(value))

ThermalModel

Thermal model of a motor developed by Jianping Lin and Gray C. Thomas @U-M Locomotion Lab, directed by Dr. Robert Gregg

Assumptions

1: The motor is a lumped system with two thermal nodes: the winding and the case. 2: The winding and the case are assumed to be in thermal equilibrium with the ambient. 3: The winding and the case are assumed to be in thermal equilibrium with each other.

Equations

1: C_w * dT_w/dt = (I^2)R + (T_c-T_w)/R_WC 2: C_c * dT_c/dt = (T_w-T_c)/R_WC + (T_w-T_a)/R_CA

where

C_w: Thermal capacitance of the winding C_c: Thermal capacitance of the case R_WC: Thermal resistance between the winding and the case R_CA: Thermal resistance between the case and the ambient T_w: Temperature of the winding T_c: Temperature of the case T_a: Temperature of the ambient I: Current R: Resistance

Implementation

1: The model is updated at every time step with the current and the ambient temperature. 2: The model can be used to predict the temperature of the winding and the case at any time step. 3: The model can also be used to scale the torque based on the temperature of the winding and the case.

Parameters:

Name	Type	Description	Default
ambient	float	Ambient temperature in Celsius. Defaults to 21.	21
params	dict	Dictionary of parameters. Defaults to dict().	None
temp_limit_windings	float	Maximum temperature of the windings in Celsius. Defaults to 115.	115
soft_border_C_windings	float	Soft border of the windings in Celsius. Defaults to 15.	15
temp_limit_case	float	Maximum temperature of the case in Celsius. Defaults to 80.	80
soft_border_C_case	float	Soft border of the case in Celsius. Defaults to 5.	5

Source code in opensourceleg/math/math.py

class ThermalModel:
    """
    Thermal model of a motor developed by Jianping Lin and Gray C. Thomas
    @U-M Locomotion Lab, directed by Dr. Robert Gregg

    Assumptions:
        1: The motor is a lumped system with two thermal nodes: the winding and the case.
        2: The winding and the case are assumed to be in thermal equilibrium with the ambient.
        3: The winding and the case are assumed to be in thermal equilibrium with each other.

    Equations:
        1: C_w * dT_w/dt = (I^2)R + (T_c-T_w)/R_WC
        2: C_c * dT_c/dt = (T_w-T_c)/R_WC + (T_w-T_a)/R_CA

    where:
        C_w: Thermal capacitance of the winding
        C_c: Thermal capacitance of the case
        R_WC: Thermal resistance between the winding and the case
        R_CA: Thermal resistance between the case and the ambient
        T_w: Temperature of the winding
        T_c: Temperature of the case
        T_a: Temperature of the ambient
        I: Current
        R: Resistance

    Implementation:
        1: The model is updated at every time step with the current and the ambient temperature.
        2: The model can be used to predict the temperature of the winding and the case at any time step.
        3: The model can also be used to scale the torque based on the temperature of the winding and the case.

    Args:
        ambient (float): Ambient temperature in Celsius. Defaults to 21.
        params (dict): Dictionary of parameters. Defaults to dict().
        temp_limit_windings (float): Maximum temperature of the windings in Celsius. Defaults to 115.
        soft_border_C_windings (float): Soft border of the windings in Celsius. Defaults to 15.
        temp_limit_case (float): Maximum temperature of the case in Celsius. Defaults to 80.
        soft_border_C_case (float): Soft border of the case in Celsius. Defaults to 5.


    """

    def __init__(
        self,
        ambient: float = 21,
        params: Optional[dict[Any, Any]] = None,
        temp_limit_windings: float = 115,
        soft_border_C_windings: float = 15,
        temp_limit_case: float = 80,
        soft_border_C_case: float = 5,
    ) -> None:
        # The following parameters result from Jack Schuchmann's test with no fans
        if params is None:
            params = {}
        self.C_w: float = 0.20 * 81.46202695970649
        self.R_WC = 1.0702867186480716
        self.C_c = 512.249065845453
        self.R_CA = 1.9406620046327363
        self.α: float = 0.393 * 1 / 100  # Pure copper. Taken from thermalmodel3.py
        self.R_T_0 = 65  # temp at which resistance was measured
        self.R_ϕ_0 = 0.376  # emirical, from the computed resistance (q-axis voltage/ q-axis current). Ohms

        self.__dict__.update(params)
        self.T_w: float = ambient
        self.T_c: float = ambient
        self.T_a: float = ambient
        self.soft_max_temp_windings: float = temp_limit_windings - soft_border_C_windings
        self.abs_max_temp_windings: float = temp_limit_windings
        self.soft_border_windings: float = soft_border_C_windings

        self.soft_max_temp_case: float = temp_limit_case - soft_border_C_case
        self.abs_max_temp_case: float = temp_limit_case
        self.soft_border_case: float = soft_border_C_case

    def __repr__(self) -> str:
        return "ThermalModel"

    def update(self, dt: float = 1 / 200, motor_current: float = 0) -> None:
        """
        Updates the temperature of the winding and the case based on the current and the ambient temperature.

        Args:
            dt (float): Time step in seconds. Defaults to 1/200.
            motor_current (float): Motor current in mA. Defaults to 0.

        Dynamics:
            1: self.C_w * d self.T_w /dt = (I^2)R + (self.T_c-self.T_w)/self.R_WC
            2: self.C_c * d self.T_c /dt = (self.T_w-self.T_c)/self.R_WC + (self.T_w-self.T_a)/self.R_CA
        """

        I_q_des: float = motor_current * 1e-3

        I2R = (
            I_q_des**2 * self.R_ϕ_0 * (1 + self.α * (self.T_w - self.R_T_0))
        )  # accounts for resistance change due to temp.

        dTw_dt = (I2R + (self.T_c - self.T_w) / self.R_WC) / self.C_w
        dTc_dt: float = ((self.T_w - self.T_c) / self.R_WC + (self.T_a - self.T_c) / self.R_CA) / self.C_c
        self.T_w += dt * dTw_dt
        self.T_c += dt * dTc_dt

    def update_and_get_scale(self, dt, motor_current: float = 0, FOS: float = 1.0):
        """
        Updates the temperature of the winding and the case based on the current and the ambient temperature and returns the scale factor for the torque.

        Args:
            dt (float): Time step in seconds.
            motor_current (float): Motor current in mA. Defaults to 0.
            FOS (float): Factor of safety. Defaults to 3.0.

        Returns:
            float: Scale factor for the torque.

        Dynamics:
            1: self.C_w * d self.T_w /dt = (I^2)R + (self.T_c-self.T_w)/self.R_WC
            2: self.C_c * d self.T_c /dt = (self.T_w-self.T_c)/self.R_WC + (self.T_w-self.T_a)/self.R_CA
        """

        I_q_des: float = motor_current * 1e-3

        I2R_des = (
            FOS * I_q_des**2 * self.R_ϕ_0 * (1 + self.α * (self.T_w - self.R_T_0))
        )  # accounts for resistance change due to temp.
        scale = 1.0
        if self.T_w > self.abs_max_temp_windings:
            scale = 0.0
        elif self.T_w > self.soft_max_temp_windings:
            scale *= (self.abs_max_temp_windings - self.T_w) / (
                self.abs_max_temp_windings - self.soft_max_temp_windings
            )

        if self.T_c > self.abs_max_temp_case:
            scale = 0.0
        elif self.T_c > self.soft_max_temp_case:
            scale *= (self.abs_max_temp_case - self.T_w) / (self.abs_max_temp_case - self.soft_max_temp_case)

        I2R = I2R_des * scale

        dTw_dt = (I2R + (self.T_c - self.T_w) / self.R_WC) / self.C_w
        dTc_dt: float = ((self.T_w - self.T_c) / self.R_WC + (self.T_a - self.T_c) / self.R_CA) / self.C_c
        self.T_w += dt * dTw_dt
        self.T_c += dt * dTc_dt

        if scale <= 0.0:
            return 0.0
        if scale >= 1.0:
            return 1.0

        return np.sqrt(scale)  # this is how much the torque should be scaled

update(dt=1 / 200, motor_current=0)

Updates the temperature of the winding and the case based on the current and the ambient temperature.

Parameters:

Name	Type	Description	Default
dt	float	Time step in seconds. Defaults to 1/200.	1 / 200
motor_current	float	Motor current in mA. Defaults to 0.	0

Dynamics

1: self.C_w * d self.T_w /dt = (I^2)R + (self.T_c-self.T_w)/self.R_WC 2: self.C_c * d self.T_c /dt = (self.T_w-self.T_c)/self.R_WC + (self.T_w-self.T_a)/self.R_CA

Source code in opensourceleg/math/math.py

def update(self, dt: float = 1 / 200, motor_current: float = 0) -> None:
    """
    Updates the temperature of the winding and the case based on the current and the ambient temperature.

    Args:
        dt (float): Time step in seconds. Defaults to 1/200.
        motor_current (float): Motor current in mA. Defaults to 0.

    Dynamics:
        1: self.C_w * d self.T_w /dt = (I^2)R + (self.T_c-self.T_w)/self.R_WC
        2: self.C_c * d self.T_c /dt = (self.T_w-self.T_c)/self.R_WC + (self.T_w-self.T_a)/self.R_CA
    """

    I_q_des: float = motor_current * 1e-3

    I2R = (
        I_q_des**2 * self.R_ϕ_0 * (1 + self.α * (self.T_w - self.R_T_0))
    )  # accounts for resistance change due to temp.

    dTw_dt = (I2R + (self.T_c - self.T_w) / self.R_WC) / self.C_w
    dTc_dt: float = ((self.T_w - self.T_c) / self.R_WC + (self.T_a - self.T_c) / self.R_CA) / self.C_c
    self.T_w += dt * dTw_dt
    self.T_c += dt * dTc_dt

update_and_get_scale(dt, motor_current=0, FOS=1.0)

Updates the temperature of the winding and the case based on the current and the ambient temperature and returns the scale factor for the torque.

Parameters:

Name	Type	Description	Default
dt	float	Time step in seconds.	required
motor_current	float	Motor current in mA. Defaults to 0.	0
FOS	float	Factor of safety. Defaults to 3.0.	1.0

Returns:

Name	Type	Description
float		Scale factor for the torque.

Dynamics

1: self.C_w * d self.T_w /dt = (I^2)R + (self.T_c-self.T_w)/self.R_WC 2: self.C_c * d self.T_c /dt = (self.T_w-self.T_c)/self.R_WC + (self.T_w-self.T_a)/self.R_CA

Source code in opensourceleg/math/math.py

def update_and_get_scale(self, dt, motor_current: float = 0, FOS: float = 1.0):
    """
    Updates the temperature of the winding and the case based on the current and the ambient temperature and returns the scale factor for the torque.

    Args:
        dt (float): Time step in seconds.
        motor_current (float): Motor current in mA. Defaults to 0.
        FOS (float): Factor of safety. Defaults to 3.0.

    Returns:
        float: Scale factor for the torque.

    Dynamics:
        1: self.C_w * d self.T_w /dt = (I^2)R + (self.T_c-self.T_w)/self.R_WC
        2: self.C_c * d self.T_c /dt = (self.T_w-self.T_c)/self.R_WC + (self.T_w-self.T_a)/self.R_CA
    """

    I_q_des: float = motor_current * 1e-3

    I2R_des = (
        FOS * I_q_des**2 * self.R_ϕ_0 * (1 + self.α * (self.T_w - self.R_T_0))
    )  # accounts for resistance change due to temp.
    scale = 1.0
    if self.T_w > self.abs_max_temp_windings:
        scale = 0.0
    elif self.T_w > self.soft_max_temp_windings:
        scale *= (self.abs_max_temp_windings - self.T_w) / (
            self.abs_max_temp_windings - self.soft_max_temp_windings
        )

    if self.T_c > self.abs_max_temp_case:
        scale = 0.0
    elif self.T_c > self.soft_max_temp_case:
        scale *= (self.abs_max_temp_case - self.T_w) / (self.abs_max_temp_case - self.soft_max_temp_case)

    I2R = I2R_des * scale

    dTw_dt = (I2R + (self.T_c - self.T_w) / self.R_WC) / self.C_w
    dTc_dt: float = ((self.T_w - self.T_c) / self.R_WC + (self.T_a - self.T_c) / self.R_CA) / self.C_c
    self.T_w += dt * dTw_dt
    self.T_c += dt * dTc_dt

    if scale <= 0.0:
        return 0.0
    if scale >= 1.0:
        return 1.0

    return np.sqrt(scale)  # this is how much the torque should be scaled

deprecated_with_routing(alternative_func)

Decorator to provide an alternative function for a deprecated function. The alternative function will be called instead of the deprecated function.

Source code in opensourceleg/logging/decorators.py

def deprecated_with_routing(alternative_func):
    """
    Decorator to provide an alternative function for a deprecated function. The alternative function will be called
    instead of the deprecated function.
    """

    def decorator(func):
        @wraps(func)
        def wrapper(*args, **kwargs):
            LOGGER.warning(
                f"Function `{func.__name__}` is deprecated. Please use `{alternative_func.__name__}` instead, which will be called automatically now."
            )
            return alternative_func(*args, **kwargs)

        return wrapper

    return decorator