Allow to control the motor in pos/vel with PD controller

furuta/controls/controllers.py

@@ -4,7 +4,6 @@
 import crocoddyl
 import numpy as np
 import pinocchio as pin
-from simple_pid import PID
 
 
 class Controller(ABC):
@@ -16,23 +15,30 @@ def compute_command(self, state):
 class PIDController(Controller):
     def __init__(
         self,
-        dt: float = None,
+        dt: float,
         Kp: float = 0.0,
         Ki: float = 0.0,
         Kd: float = 0.0,
         setpoint: float = 0.0,
     ):
+        self.dt = dt
+        self.Kp = Kp
+        self.Ki = Ki
+        self.Kd = Kd
+        self.setpoint = setpoint
 
-        self.pid = PID(
-            Kp=Kp,
-            Ki=Ki,
-            Kd=Kd,
-            setpoint=setpoint,
-            sample_time=dt,
-        )
+        self.integral = 0.0
+        self.prev_error = 0.0
+
+    def compute_command(self, state: float) -> float:
+        error = self.setpoint - state
+        self.integral += error * self.dt
+        derivative = (error - self.prev_error) / self.dt
+
+        command = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
 
-    def compute_command(self, position: float):
-        return self.pid(position)
+        self.prev_error = error
+        return command
 
 
 class SwingUpController(Controller):

furuta/plotter.py

@@ -64,12 +64,13 @@ def plot(self):
             / "layout.yaml"
         )
         layout = load_layout(layout_path)
+        ax = None
         for k, tab in enumerate(layout.tabs):
             plt.figure(k + 1)
             plt.suptitle(tab.name)
 
             for idx, plot in enumerate(tab.plots):
-                ax = plt.subplot(*tab.shape, idx + 1)
+                ax = plt.subplot(*tab.shape, idx + 1, sharex=ax)
                 ax.set_title(plot.name)
 
                 for var in plot.variables:

furuta/rl/envs/furuta_real.py

@@ -3,7 +3,6 @@
 from typing import Optional
 
 import numpy as np
-from simple_pid import PID
 
 from furuta.rl.envs.furuta_base import FurutaBase
 from furuta.robot import Robot
@@ -55,18 +54,9 @@ def reset(
 
         if self._state is not None:  # if not first reset
             logging.debug("Stopping motor")
-            # motor_pid = PID(
-            #     self.motor_stop_pid[0],
-            #     self.motor_stop_pid[1],
-            #     self.motor_stop_pid[2],
-            #     setpoint=0.0,
-            #     output_limits=(-1, 1),
-            # )
 
             reset_time = 0
             while abs(self._state[THETA_DOT]) > 0.5 and reset_time < MAX_MOTOR_RESET_TIME:
-                # act = motor_pid(self._state[THETA_DOT])
-                # self._update_state(act)
                 reset_time += self.timing.dt
                 sleep(self.timing.dt)
 
@@ -86,7 +76,7 @@ def reset(
                 logging.info(f"Reset timeout, alpha: {np.rad2deg(self._state[ALPHA])}")
 
         # reset both encoder, motor back to pos=0
-        self.robot.reset_encoders()
+        self.robot.reset()
 
         logging.info("Reset done")
         # else the first computed velocity will take into account previous episode

furuta/robot.py

@@ -1,4 +1,5 @@
 import struct
+import time
 from dataclasses import dataclass
 from pathlib import Path
 
@@ -28,30 +29,81 @@ def __init__(
         self.ser = serial.Serial(device, baudrate)
         self.motor_encoder_cpr = motor_encoder_cpr
         self.pendulum_encoder_cpr = pendulum_encoder_cpr
+        self.write_packet_size = 11
+        self.read_packet_size = 23
 
-    def step(self, motor_command: float):
-        direction = motor_command < 0
-        # convert motor command to 16 bit unsigned int
-        int_motor_command = int(np.abs(motor_command) * (2**16 - 1))
-        tx = b"\x10\x02"  # start sequence
-        tx += b"\x01"  # command type = STEP = 0x01
-        tx += struct.pack("<?H", direction, int_motor_command)
-        self.ser.write(tx)
-        resp = self.ser.read(12)
-        raw_motor_angle, raw_pendulum_angle, raw_timestamp = struct.unpack("<iiL", resp)
-        motor_angle = 2 * np.pi * raw_motor_angle / self.motor_encoder_cpr
-        pendulum_angle = 2 * np.pi * raw_pendulum_angle / self.pendulum_encoder_cpr
-        timestamp = raw_timestamp / 1e6
-        return motor_angle, pendulum_angle, timestamp
-
-    def reset_encoders(self):
+    def _send_command(
+        self,
+        command_type: str,
+        motor_command: float = 0.0,
+        desired_position: float = 0.0,
+        desired_velocity: float = 0.0,
+    ):
         tx = b"\x10\x02"  # start sequence
-        tx += b"\x00"  # command type = RESET = 0x00
-        tx += b"\x00\x00\x00"  # three empty bytes to have fixed len packets
+        bytes_sent = 2
+        if command_type == "RESET":
+            tx += b"\x00"  # command type = RESET = 0x00
+            bytes_sent += 1
+        elif command_type == "STEP":
+            tx += b"\x01"  # command type = STEP = 0x01
+            # convert the motor command in uint16 (0 - 65535) + sign
+            motor_command = np.clip(motor_command, -1, 1)
+            direction = motor_command < 0
+            motor_command_int = int(np.abs(motor_command) * (2**16 - 1))
+            tx += struct.pack("<?H", direction, motor_command_int)
+            bytes_sent += 4
+        elif command_type == "STEP_PID":
+            tx += b"\x02"  # command type = STEP_PID = 0x02
+            tx += struct.pack("<ff", desired_position, desired_velocity)
+            bytes_sent += 9
+        # complete packet with empty bytes to have a fixed packet size
+        for _ in range(self.write_packet_size - bytes_sent):
+            tx += b"\x00"
         self.ser.write(tx)
 
+    def _read_measures(self):
+        resp = self.ser.read(self.read_packet_size)
+        (
+            motor_position,
+            pendulum_position,
+            motor_velocity,
+            pendulum_velocity,
+            timestamp_micros,
+            motor_direction,
+            motor_command_int,
+        ) = struct.unpack("<ffffL?H", resp)
+        timestamp = timestamp_micros / 1e6
+        motor_command = motor_command_int / (2**16 - 1)
+        if motor_direction:
+            motor_command *= -1
+        return (
+            motor_position,
+            pendulum_position,
+            motor_velocity,
+            pendulum_velocity,
+            timestamp,
+            motor_command,
+        )
+
+    def step(self, motor_command: float):
+        self._send_command(command_type="STEP", motor_command=motor_command)
+        return self._read_measures()
+
+    def step_PID(self, desired_position: float, desired_velocity: float):
+        self._send_command(
+            command_type="STEP_PID",
+            desired_position=desired_position,
+            desired_velocity=desired_velocity,
+        )
+        return self._read_measures()
+
+    def reset(self):
+        self._send_command(command_type="RESET")
+        time.sleep(0.5)
+        self.ser.read_all()
+
     def close(self):
-        self.step(0)
+        self.reset()
         self.ser.close()
 
 

pyproject.toml

@@ -9,7 +9,6 @@ requires-python = ">=3.11"
 dependencies = [
     "gymnasium>=0.29.1",
     "pyserial>=3.5",
-    "simple-pid>=2.0.0",
     "mcap-protobuf-support>=0.4.1",
     "scipy>=1.11.4",
     "pygame>=2.5.2",