PIDmod

[brainwavecoder9]

Motor PID Configuration Tool

A utility script for reading and modifying PID gains on Reachy Mini robot motors to address motor oscillation issues.

Problem

This tool addresses motor oscillation by allowing adjustment of PID (Proportional-Integral-Derivative) control parameters. I was able to solve the problem by modifying the D (derivative/damping) gain on the base motor.

Quick Start

Read current values (no changes):

python modify_motor_pid.py

Add damping to reduce oscillation:

python modify_motor_pid.py -d 250

Set multiple gains:

python modify_motor_pid.py -p 400 -i 0 -d 250

Default Values

Initial values that I observed on my base motor (believed to be factory defaults):

• P: 400
• I: 0
• D: 0

Features

• Auto-detects serial ports
• Validates PID values before writing
• Shows before/after comparison
• Automatically disables/re-enables torque during changes
• Supports configuring different motor IDs

Recommendation

Use this tool to identify optimal PID values during testing, then pre-configure motors before shipping. End users should not need to adjust these values except in rare cases, as arbitrary changes could cause issues.

Usage Examples

See python modify_motor_pid.py --help for complete documentation including:

• Valid parameter ranges
• Register addresses
• Common motor IDs
• Safety notes

[brainwavecoder9]

Bonus: Adding PID D Gain Configuration to Motor Setup Script

Seeing #325 made me realize that setup_motors.py might be what you use to configure the motors initially. If so, here are the instructions to also configure the dampening:

1. Add to MotorConfig dataclass (around line 18):

@dataclass
class MotorConfig:
    """Motor configuration."""

    id: int
    offset: int
    angle_limit_min: int
    angle_limit_max: int
    return_delay_time: int
    shutdown_error: int
    pid_d: int = 0  # Add this line - D gain for damping

2. Update parse_yaml_config to read the PID value (around line 44):

motor_ids[name] = MotorConfig(
    id=params["id"],
    offset=params["offset"],
    angle_limit_min=params["lower_limit"],
    angle_limit_max=params["upper_limit"],
    return_delay_time=params["return_delay_time"],
    shutdown_error=params["shutdown_error"],
    pid_d=params.get("pid_d", 0),  # Add this line - defaults to 0 if not in config
)

3. Add new function to change PID D gain (around line 200):

def change_pid_d_gain(
    serial_port: str, id: int, d_gain: int, baudrate: int
):
    """Change the PID D gain of the motor with the given ID on the specified serial port."""
    print(
        f"Changing PID D gain for motor with ID {id} to {d_gain}...",
        end="",
        flush=True,
    )
    import struct
    c = Xl330PyController(serial_port, baudrate=baudrate, timeout=SERIAL_TIMEOUT)
    # Register 80 is the D gain register (Kpd)
    c.write(id, 80, list(struct.pack('<H', d_gain)))
    print("✅")

4. Call it in setup_motor function (after change_return_delay_time, around line 138):

        change_return_delay_time(
            serial_port,
            id=motor_config.id,
            return_delay_time=motor_config.return_delay_time,
            baudrate=target_baudrate,
        )

        time.sleep(MOTOR_SETUP_DELAY)

        # Add these lines:
        if motor_config.pid_d > 0:
            change_pid_d_gain(
                serial_port,
                id=motor_config.id,
                d_gain=motor_config.pid_d,
                baudrate=target_baudrate,
            )
            time.sleep(MOTOR_SETUP_DELAY)

5. Add to your hardware_config.yaml:

motors:
  - body_yaw:
      id: 10
      offset: 0
      lower_limit: 0
      upper_limit: 4095
      return_delay_time: 0
      shutdown_error: 36
      pid_d: 250  # Add this line

This would configure D gain during motor setup, or factory reset.

[brainwavecoder9]

The algorithm is listening...

I was served this neat visualization for PID control today. It's a pretty cool way of describing this that I hadn't seen before, so I thought I would share (instead of twisting knobs, we're changing values through the modify_motor_pid script):
https://www.youtube.com/watch?v=qKy98Cbcltw

[pierre-rouanet]

Thanks a lot @brainwavecoder9! That's a really nice contribution!
Yes I think it would be really nice to integrate it into the hardware_config and setup_motors tools directly!

[brainwavecoder9]

It seems that something is unexpectedly overwriting PID values back to factory defaults... I wouldn't expect this to happen during a "software only" install.

###==================================================
###CURRENT PID VALUES:
###==================================================
P = 400 (Kpp = 3.1250)
I = 0 (Kpi = 0.000000)
D = 0 (Kpd = 0.0000)

I've run the sever and the reachy_mini_conversation_app, and reinstalled dependencies, and some other things... not sure what to blame, but was surprised to see this. I wouldn't necessarily say this is a 'bug' since we're in beta, but mentioning as an FYI in case it's not on your radar.

I can track it down if helpful to isolate the cause.

[pierre-rouanet]

Not sure to exactly understand. Indeed, we should not write PIDs anywhere in the software at the moment.

It seems that something is unexpectedly overwriting PID values back to factory defaults...
Just to make sure, the PID registers are in the RAM area so they are indeed reset each time your motors are turned off and on again.

I was thinking of maybe running the setup-motors tools at each daemon launch. But I'm still not sure if it's a good idea or not.

[brainwavecoder9]

Ah yes, this is ram-only so it resets on power cycle.

I don't have a depth of experience with this so approach needs to be confirmed, but it sounds like the Dynamixel servos have an option to Backup current configuration (0x20) and then there's a Startup Configuration bit (address 60, bit 1 = 1) which can be set to restore from that backup.

So, after flipping the bit for restore from config on startup the process would be "write the desired PID/other params to ram" (like we do with this script) and then just add the backup which would be something like packetHandler.writeTxRx(portHandler, id, 0x20, [0x01,0x43,0x54,0x52,0x4C])

That way when the motors power on the motor's own backup would be restored. Doing it this way would mean set once within the servo and persist indefinitely, whether at the factory or a later re-tune.