update param

bam/actuator.py

@@ -252,9 +252,115 @@ def compute_torque(self, control: float | None, q: float, dq: float) -> float:
         return torque
 
 
+class Sts3250(Actuator):
+    """
+    Represents an STS3250 position-controlled servo motor actuator
+    """
+
+    def __init__(self, testbench_class: Testbench):
+        super().__init__(testbench_class)
+
+        # Position control gain
+        self.kp: float = 32.0  # Initial gain value, adjust as needed
+
+        # Maximum torque output [Nm]
+        self.max_torque: float = 4.9  # Adjust based on STS3250 specifications
+
+        # Maximum speed [rad/s]
+        self.max_speed: float = 7.853  # Adjust based on STS3250 specifications
+
+
+        # pfb30: debug this
+        # This gain, if multiplied by a position error and firmware KP, gives duty cycle
+        # It was determined using an oscilloscope and MX actuators
+        self.error_gain: float = 0.158
+        # Maximum allowable duty cycle, also determined with oscilloscope
+        self.max_pwm: float = 1.0
+
+        self.vin: float = 5.1
+
+    def load_log(self, log: dict):
+        super().load_log(log)
+
+        if "kp" in log:
+            self.kp = log["kp"]
+
+    def initialize(self):
+        # We don't have access to internal motor parameters, so we'll model overall behavior
+        # Torque constant [Nm/A] or [V/(rad/s)]
+        self.model.kt = Parameter(1.0, 0.1, 3.0)
+
+        # Motor armature / apparent inertia [kg m^2]
+        self.model.armature = Parameter(0.005, 0.001, 0.05)
+
+        # Motor resistance [Ohm]
+        self.model.R = Parameter(2.0, 1.0, 8.0)
+
+
+    def get_extra_inertia(self) -> float:
+        return self.model.armature.value
+
+    def control_unit(self) -> str:
+        return "volts"
+
+    def compute_control(
+        self, position_error: float, q: float, dq: float
+    ) -> float | None:
+        duty_cycle = position_error * self.kp * self.error_gain
+        duty_cycle = np.clip(duty_cycle, -self.max_pwm, self.max_pwm)
+
+        return self.vin * duty_cycle
+
+    def compute_torque(self, control: float | None, q: float, dq: float) -> float:
+        # Volts to None means that the motor is disconnected
+        if control is None:
+            return 0.0
+
+        volts = control
+
+        # Torque produced
+        torque = self.model.kt.value * volts / self.model.R.value
+
+        # Back EMF
+        torque -= (self.model.kt.value**2) * dq / self.model.R.value
+
+        return torque
+
+    """
+    def control_unit(self) -> str:
+        return "position"
+
+
+    def compute_control(
+        self, position_error: float, q: float, dq: float
+    ) -> float | None:
+        # For a position-controlled servo, we return the target position
+        return q + position_error
+
+    def compute_torque(self, control: float | None, q: float, dq: float) -> float:
+        if control is None:
+            return 0.0
+
+        target_position = control
+
+        # Compute torque based on position error and velocity
+        position_error = target_position - q
+        torque = self.kp * position_error - self.model.kt.value * dq
+
+        # Limit torque based on maximum torque specification
+        torque = np.clip(torque, -self.max_torque, self.max_torque)
+
+        # Limit torque based on maximum speed
+        if abs(dq) > self.max_speed:
+            torque = 0.0
+
+        return torque
+    """
+
 actuators = {
     "mx64": lambda: MXActuator(Pendulum),
     "mx106": lambda: MXActuator(Pendulum),
     "erob80_100": lambda: Erob(Pendulum, damping=2.0),
     "erob80_50": lambda: Erob(Pendulum, damping=1.0),
+    "sts3250": lambda: Sts3250(Pendulum),
 }

bam/export_values.py

@@ -0,0 +1,46 @@
+"""
+Export values from a model to a JSON file.
+
+Usage:
+python -m bam.fit \
+    --actuator sts3250 \
+    --model m6 \
+    --logdir bam/data_processed_feetech \
+    --method cmaes \
+    --output params/feetech/m6.json \
+    --trials 1000
+
+python -m bam.export_values \
+    --params params/feetech/m6.json \
+    --vin 5.1 \
+    --kp_firmware 32
+"""
+
+from .model import models, load_model
+from .actuator import actuators
+import argparse
+
+parser = argparse.ArgumentParser()
+parser.add_argument("--vin", type=float, default=5.1, help="VIN value")
+parser.add_argument("--kp_firmware", type=int, default=32, help="KP_FIRMWARE value")
+parser.add_argument("--params", type=str, default="params/feetech/m6.json")
+args = parser.parse_args()
+
+model = load_model(args.params)
+
+viscous = model.friction_viscous.value
+frictionloss = model.friction_base.value
+kt = model.kt.value
+R = model.R.value
+armature = model.armature.value
+
+forcerange = args.vin * kt / R
+kp = model.actuator.error_gain * args.kp_firmware * args.vin * kt / R
+damping = viscous + kt**2 / R
+
+
+print(f"Armature: {armature}")
+print(f"Force range: {forcerange}")
+print(f"Kp: {kp}")
+print(f"Friction loss: {frictionloss}")
+print(f"Damping: {damping}")

bam/feetech/all_record.py

@@ -0,0 +1,37 @@
+import argparse
+import os
+import time
+
+
+arg_parser = argparse.ArgumentParser()
+arg_parser.add_argument("--mass", type=float, required=True)
+arg_parser.add_argument("--length", type=float, required=True)
+arg_parser.add_argument("--motor", type=str, required=True)
+arg_parser.add_argument("--ip", type=str, default="192.168.42.1")
+arg_parser.add_argument("--logdir", type=str, required=True)
+arg_parser.add_argument("--speak", action="store_true")
+args = arg_parser.parse_args()
+
+kps = [32]
+trajectories = ["sin_sin", "lift_and_drop", "up_and_down", "sin_time_square"]
+
+command_base = f"python3 feetech/record.py --mass {args.mass} --length {args.length}"
+command_base += f" --ip {args.ip} --logdir {args.logdir} --motor {args.motor}"
+
+
+for kp in kps:
+    for trajectory in trajectories:
+        sentence = f"Kp {kp}, trajectory {trajectory.replace('_', ' ')}"
+        print(sentence)
+
+        if args.speak:
+            from gtts import gTTS 
+            myobj = gTTS(text=sentence, lang='en', slow=False) 
+            myobj.save("/tmp/message.mp3")
+            os.system("mpg321 /tmp/message.mp3")
+
+        command = f"{command_base} --kp {kp} --trajectory {trajectory}"
+        os.system(command)
+
+        if trajectory == "sin_time_square":
+            time.sleep(3)

bam/feetech/feetech.py

@@ -0,0 +1,77 @@
+import os
+import numpy as np
+from openlch import hal as olch_hal
+
+# Torque enable
+ADDR_TORQUE_ENABLE = 24
+# P Gain
+ADDR_P_GAIN = 28
+# Goal position
+ADDR_GOAL_POSITION = 30
+# Present position (2 bytes)
+ADDR_PRESENT_POSITION = 36
+# Present speed (2 bytes)
+ADDR_PRESENT_SPEED = 38
+# Present load (2 bytes)
+ADDR_PRESENT_LOAD = 40
+# Present voltage (1 byte)
+ADDR_PRESENT_VOLTAGE = 42
+# Present temperature (1 byte)
+ADDR_PRESENT_TEMPERATURE = 43
+
+
+class FeetechSTS3250:
+    def __init__(self, ip: str, id: int = 1):
+        self.id = id
+        self.hal = olch_hal.HAL(ip)
+        self.servo = self.hal.servo
+
+    def set_p_gain(self, gain: int):
+        pass
+        # Set P gain
+
+        ## unimplemented
+
+    def set_torque(self, enable: bool):
+        self.servo.set_torque_enable([(self.id, enable)])
+
+    def set_goal_position(self, position: float):
+        # convert to degrees
+        position = position * 180.0 / np.pi
+        self.servo.set_positions([(self.id, position)])
+
+    def read_data(self):
+        # Reading position, speed, load, voltage and temperature
+
+        max_retries = 10
+        retry_count = 0
+        while retry_count < max_retries:
+            data = self.servo.get_servo_info(self.id)
+            if data['temperature'] != 0.0:
+                break
+            retry_count += 1
+        if retry_count == max_retries:
+            print(f"Warning: Failed to get non-zero data after {max_retries} attempts")
+
+        # Position is in degrees (convert to radians)
+        position = data["current_position"] / 180.0 * np.pi
+
+        # Speed is degrees per second (convert to rad/s)
+        speed = data["speed"] / 180.0 * np.pi 
+
+        # Applied "load"
+        load = 0  # unimplemented
+
+        # Voltage is a byte value, units are 0.1 V
+        volts = data["voltage"]
+
+        # Temperature are °C
+        temp = data["temperature"]
+
+        return {
+            "position": position,
+            "speed": speed,
+            "load": load,
+            "input_volts": volts,
+            "temp": temp,
+        }

bam/feetech/record.py

@@ -0,0 +1,83 @@
+import json
+import datetime
+import os
+import numpy as np
+import argparse
+import time
+from bam.feetech import feetech
+from bam.trajectory import *
+
+arg_parser = argparse.ArgumentParser()
+arg_parser.add_argument("--mass", type=float, required=True)
+arg_parser.add_argument("--length", type=float, required=True)
+arg_parser.add_argument("--ip", type=str, default="192.168.42.1")
+arg_parser.add_argument("--logdir", type=str, required=True)
+arg_parser.add_argument("--trajectory", type=str, default="lift_and_drop")
+arg_parser.add_argument("--motor", type=str, required=True)
+arg_parser.add_argument("--kp", type=int, default=32)
+arg_parser.add_argument("--vin", type=float, default=12.0)
+arg_parser.add_argument("--id", type=int, default=1)
+args = arg_parser.parse_args()
+
+if args.trajectory not in trajectories:
+    raise ValueError(f"Unknown trajectory: {args.trajectory}")
+
+servo = feetech.FeetechSTS3250(args.ip, id=args.id)
+trajectory = trajectories[args.trajectory]
+
+start = time.time()
+while time.time() - start < 1.0:
+    goal_position, torque_enable = trajectory(0)
+    if torque_enable:
+        servo.set_goal_position(goal_position)
+    servo.set_torque(torque_enable)
+    servo.set_p_gain(args.kp)
+
+start = time.time()
+data = {
+    "mass": args.mass,
+    "length": args.length,
+    "kp": args.kp,
+    "vin": args.vin,
+    "motor": args.motor,
+    "trajectory": args.trajectory,
+    "entries": []
+}
+
+while time.time() - start < trajectory.duration:
+    t = time.time() - start
+    goal_position, new_torque_enable = trajectory(t)
+    if new_torque_enable != torque_enable:
+        servo.set_torque(new_torque_enable)
+        torque_enable = new_torque_enable
+        time.sleep(0.001)
+    if torque_enable:
+        servo.set_goal_position(goal_position)
+        time.sleep(0.001)
+
+    t0 = time.time() - start
+    entry = servo.read_data()
+    t1 = time.time() - start
+
+    entry["timestamp"] = (t0 + t1) / 2.0
+    entry["goal_position"] = goal_position
+    entry["torque_enable"] = torque_enable
+    data["entries"].append(entry)
+
+goal_position = data["entries"][-1]["position"]
+return_dt = 0.01
+max_variation = return_dt * 1.0
+while abs(goal_position) > 0:
+    if goal_position > 0:
+        goal_position = max(0, goal_position - max_variation)
+    else:
+        goal_position = min(0, goal_position + max_variation)
+    servo.set_goal_position(goal_position)
+    time.sleep(return_dt)
+servo.set_torque(False)
+
+#Format YYYY-MM-DD_HH:mm:ss"
+date = datetime.datetime.now().strftime("%Y-%m-%d_%Hh%Mm%S")
+
+filename = f"{args.logdir}/{date}.json"
+json.dump(data, open(filename, "w"))

params/feetech/m6.json

@@ -0,0 +1 @@
+{"kt": 0.7007020801847325, "armature": 0.008793405204572328, "R": 1.0218624869613808, "friction_base": 0.013343597773929877, "friction_stribeck": 0.015979030965067358, "load_friction_motor": 0.043595874959984376, "load_friction_external": 0.11605609187444317, "load_friction_motor_stribeck": 0.1869529966374398, "load_friction_external_stribeck": 0.15456184624490737, "load_friction_motor_quad": 0.009421827480947312, "load_friction_external_quad": 0.005073033111788205, "dtheta_stribeck": 0.3263311104179638, "alpha": 2.2834117819170268, "friction_viscous": 0.054986676303726924, "model": "m6", "actuator": "sts3250"}

requirements_bam.txt

@@ -2,9 +2,10 @@ zmq
 protobuf==3.20
 numpy
 dynamixel_sdk
+openlch
 optuna
 cmaes
 wandb
 matplotlib
 pygame
-colorama
+colorama