Motor testing, movement calibration and odometry verification for Rob Box robot. Use when testing motor response, calibrating gear_ratio or min_duty, verifying odometry accuracy, or diagnosing movement issues (speed amplification, turning problems, deadzone compensation).
Procedures for testing motor response, calibrating speed parameters, and verifying odometry accuracy on Rob Box.
zenoh-dev-setup skill)# Source ROS 2
source /opt/ros/humble/setup.bash
# Zenoh env (adjust paths)
export RMW_IMPLEMENTATION=rmw_zenoh_cpp
export ZENOH_SESSION_CONFIG_URI=$(pwd)/local_test/zenoh_local_session.json5
export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
# Verify topics
ros2 topic list | grep -E "odom|cmd_vel"
# Must see: /odom, /cmd_vel_web (among others)
# Usage: python3 local_test/move_test.py [speed_m/s] [duration_s]
# 10 cm test (default): 0.1 m/s × 1s
python3 local_test/move_test.py
# 50 cm test: 0.1 m/s × 5s
python3 local_test/move_test.py 0.1 5.0
# 1 meter test: 0.2 m/s × 5s
python3 local_test/move_test.py 0.2 5.0
# 2 meter test: 0.2 m/s × 10s
python3 local_test/move_test.py 0.2 10.0
The script reports:
Compare physical distance (ruler) with ODOM DISTANCE and EXPECTED.
All in src/rob_box_description/urdf/rob_box_ros2_control.xacro:
| Parameter | Current Value | Effect |
|---|---|---|
gear_ratio | 2.46 | Motor-to-wheel speed reduction. Higher = slower wheel speed |
min_duty | 0.04 | Deadzone compensation. Minimum PWM sent to VESC when speed > 0 |
wheel_radius | 0.1143 m | Physical wheel radius |
max_rps | 6.5 | Maximum motor revolutions per second |
poles | 30 | VESC motor pole count |
max_speed = 2π × (max_rps / gear_ratio) × wheel_radius
With current values: 2π × (6.5 / 2.46) × 0.1143 = 1.897 m/s
When: Odometry consistently over- or undershoots physical distance at moderate speeds (0.1-0.2 m/s) with min_duty=0.0.
Formula:
gear_ratio_new = gear_ratio_old / ratio
where ratio = physical_distance / expected_distance
Procedure:
min_duty to 0.0 temporarily (eliminates deadzone distortion)python3 local_test/move_test.py 0.2 5.0gear_ratio in xacro, rebuild and restart robot-state-publisher container on robotHistorical calibration data (gear_ratio iterations):
| gear_ratio | min_duty | Test | Odom | Ruler | Ratio | Notes |
|---|---|---|---|---|---|---|
| 1.0 (implicit) | 0.04 | 10cm | 30cm | ~30cm | 3.0 | Way too fast, min_duty amplifies |
| 1.0 (implicit) | 0.0 | 50cm | 18.4cm | — | 0.37 | Too slow without deadzone comp |
| 5.0 | 0.0 | 50cm | 108cm | — | 2.17 | Overshoot |
| 2.3 | 0.0 | 50cm | 47cm | — | 0.94 | Close |
| 2.16 | 0.0 | 100cm | 87cm | — | 0.87 | Still under |
| 2.46 | 0.0 | 100cm | 98.3cm | 99cm | 0.983 | ✅ Calibrated |
| 2.46 | 0.0 | 200cm | 206cm | 206cm | 1.03 | ✅ Confirmed |
| 2.46 | 0.04 | 10cm | 17.9cm | — | 1.79 | min_duty distorts short/slow |
When: Robot fails to turn or move at low duty cycles (nav2 sends very small angular velocities).
Tradeoff: min_duty > 0 helps motors overcome friction/deadzone but amplifies low-speed movements. Effect is proportionally larger at shorter distances / lower speeds.
Current decision: min_duty=0.04 — acceptable for nav2 turning, distorts short low-speed tests.
Testing procedure:
min_duty in xacropython3 local_test/move_test.py 0.2 5.0 (1m)Commands go through twist_mux. Only one source controls at a time:
| Source | Topic | Priority | Timeout |
|---|---|---|---|
| Emergency | — | 255 | — |
| Joystick | /cmd_vel_joy | 100 | 0.5s |
| Web | /cmd_vel_web | 50 | 1.0s |
| Voice | /cmd_vel_voice | 25 | — |
| Nav2 | /cmd_vel | 10 | — |
move_test.py publishes to /cmd_vel_web (priority 50). Ensure joystick is disarmed (not publishing), otherwise it blocks at priority 100.
ros2 topic hz /cmd_vel_joy — should show no messagesros2 topic echo /cmd_vel_out --oncesshpass -p 'open' ssh [email protected] 'docker ps | grep control'min_duty (current: 0.04) or increase nav2's angular velocity limitsdocker/main/config/controllers/controller_manager.yaml| File | Purpose |
|---|---|
local_test/move_test.py | Movement test script |
src/rob_box_description/urdf/rob_box_ros2_control.xacro | gear_ratio, min_duty, wheel params |
src/vesc_nexus/vesc_ackermann/src/vesc_handler.cpp | Speed calculation, deadzone compensation |
docker/main/config/controllers/controller_manager.yaml | diff_drive_controller params |
docker/main/config/twist_mux/twist_mux.yaml | Input priorities and timeouts |