MPPI And Motion Control On My Diff Drive

Motor Control

So, tested on carpet, I think it’s about not having (+/-) on the spot that makes it

• motor controller - aware that the robot turns better using something like:

left = ±u right = 0

rather than opposite-wheel spinning. So we want

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
Global planner
    |
    v
Path / waypoints
    |
    v
Local planner / MPPI
    - samples feasible motor commands
    - predicts robot motion
    - scores path tracking, obstacle clearance, heading, smoothness
    |
    v
Motor command filter / safety layer
    - clamps commands
    - forbids bad command pairs
    - rate limits changes
    - applies deadband compensation
    |
    v
Serial motor driver

MPPI TODO

Need to test with feasible motion.

That is command: (+-0.8, 0). I noticed that the right wheel was rolling.

MPPI samples commands closer to what the robot actually accepts:

1
2
3
4
5
6
7
8
9
10
11
u = [left_motor_cmd, right_motor_cmd]

ACTIONS = [
    # forward / backward
    (+0.2, +0.2),
 ... 

    # maybe forbid these if bad
    # (+0.4, -0.4),
    # (-0.4, +0.4),
]

Then MPPI samples from these commands and predicts motion using your learned or measured dynamics model. Your controller no longer says:

I want omega = 1.5 rad/s, so I command (+u, -u).

Instead it says:

I want to reduce heading error, and among the feasible actions, (left=+0.45, right=0.0) produces the best progress.

Step 1 - Build Feasibility Velocity Set

1

1. Collect command-response data per surface.
2. For each surface: carpet, wood, heavy_carpet, left_cmd ∈ [-0.5, 0.5]right_cmd ∈ [-0.5, 0.5], with stride=0.1. That gives121 commands.
3. For each pair:
4. Stop robot
5. Wait 0.5 s
6. Apply left_cmd, right_cmd for 4 s
7. Record odom/IMU/encoder data in CSV
8. Stop robot
9. Wait 1 s
10. Repeat 3–5 times

1
2
3
4
5
6
7
8
9
10
timestamp  
surface_label  
left_cmd  
right_cmd  
battery_voltage  
x  
y  
yaw  
v_measured  
w_measured

1. Aggregate the info

1
2
3
4
5
6
7
8
surface  
left_cmd  
right_cmd  
mean_v  
mean_w  
std_v  
std_w
num_trials

1. Build inverse map: surface, desired_v, desired_w → best_left_cmd, best_right_cmd

1
2
3
4
5
6
7
surface = heavy_carpet
desired_v = 0.0
desired_w = 0.4

best command:
left_cmd = +0.6
right_cmd = 0.0

1. Build forward map:

1
2
3
4
5
6
7
8
surface = heavy_carpet
left_cmd = +0.5
right_cmd = -0.5

predicted:
v = 0.01 m/s
w = 0.04 rad/s
success_score = 0.2

1. Runtime surface estimator: commanded wheels + observed motion → surface probability
2. for the past 1s, predict what surface
3. Then output a probability score: P(surface) = surface_score / sum(surface_scores)
4. Feedforward: desired_v,w + surface probability → better wheel command
5. MPPI
6. soft switching: predicted_motion = 0.7 * carpet_model(cmd) + 0.2 * wood_model(cmd) + 0.1 * heavy_carpet_model(cmd)
7. Then during run time, given v and w, and wheel command, the controller infers which surface it’s on (and this will help with the feedforward term too). controller Publishes surface on a topic, mppi reads it, then switches feasibility set.

Surface Switching

Use a Ros2 Topic for fast changing runtime state

• Previous
  AI Agent Setup
• Next
  [BEV] BEV Introduction