Mecanum Webots via Supervisor kinematic injection
Replace the failing ODE-rolled mecanum chassis dynamics with a Supervisor.setVelocity call that uses the gym mecanum forward kinematics formula directly. Wheel motors still spin (visual); chassis motion comes from the gym model so training and deployment match by construction. Results (seed=42, n=10 sheep): BC + RL mecanum pen 10/10 in both field and field_round. n=5 mecanum cells still 0/5 due to tracker phantoms anchored to wall corners under the 360° LiDAR — documented in docs/status.md as the remaining gap. Cleanup: drop deploy-time hacks (HERDING_HEADING_*, HERDING_OMEGA_CLAMP, HERDING_TRACKER_*) that were workarounds for the old ODE chaos; revert the proto inertiaMatrix, roller dampingConstant, and reduced motor torque since they no longer carry load; refresh comments around the mecanum config presets.
This commit is contained in:
Johnny Fernandes
@@ -82,7 +82,7 @@ for _rk, _rv in _runtime_cfg.items():
|
||||
|
||||
import numpy as np
|
||||
|
||||
from controller import Robot
|
||||
from controller import Supervisor
|
||||
|
||||
from herding.control.active_scan import ActiveScanTeacher
|
||||
from herding.control.modulation import modulate_speed
|
||||
@@ -97,11 +97,22 @@ from herding.world.geometry import (
|
||||
DOG_SOUTH_LIMIT,
|
||||
PEN_ENTRY, is_penned,
|
||||
)
|
||||
from herding.config import HERDING_WEBOTS, LIDAR_WEBOTS_360, RobotConfig
|
||||
from herding.config import (
|
||||
HERDING_WEBOTS, HERDING_MEC_WEBOTS, HERDING_MEC_WEBOTS_360,
|
||||
LIDAR_WEBOTS_360, RobotConfig,
|
||||
)
|
||||
|
||||
# Robot physical constants come from RobotConfig so they stay in sync with
|
||||
# the training environment. The Webots preset uses action_smooth=0.55.
|
||||
_ROBOT_CFG: RobotConfig = HERDING_WEBOTS.robot
|
||||
# Mecanum picks the matched preset so kinematic injection uses the same
|
||||
# strafe_efficiency/bleed values the policy was trained against.
|
||||
_DRIVE_MODE_PEEK = (os.environ.get("HERDING_DRIVE")
|
||||
or _runtime_cfg.get("HERDING_DRIVE")
|
||||
or "differential").lower()
|
||||
if _DRIVE_MODE_PEEK == "mecanum":
|
||||
_ROBOT_CFG: RobotConfig = HERDING_MEC_WEBOTS_360.robot
|
||||
else:
|
||||
_ROBOT_CFG: RobotConfig = HERDING_WEBOTS.robot
|
||||
DOG_WHEEL_RADIUS = _ROBOT_CFG.wheel_radius
|
||||
DOG_WHEEL_BASE = _ROBOT_CFG.wheel_base
|
||||
DOG_WHEEL_BASE_X = _ROBOT_CFG.wheel_base_x
|
||||
@@ -143,7 +154,9 @@ WORLD = (os.environ.get("HERDING_WORLD")
|
||||
LIDAR_FOV_VARIANT = (os.environ.get("HERDING_LIDAR")
|
||||
or _runtime_cfg.get("HERDING_LIDAR")
|
||||
or "140").lower()
|
||||
if LIDAR_FOV_VARIANT == "360":
|
||||
if DRIVE_MODE == "mecanum" and LIDAR_FOV_VARIANT == "360":
|
||||
_LIDAR_CFG = HERDING_MEC_WEBOTS_360.lidar
|
||||
elif LIDAR_FOV_VARIANT == "360":
|
||||
_LIDAR_CFG = LIDAR_WEBOTS_360
|
||||
else:
|
||||
_LIDAR_CFG = HERDING_WEBOTS.lidar
|
||||
@@ -237,11 +250,20 @@ def drive_diff(vx: float, vy: float, left_motor, right_motor,
|
||||
def drive_mecanum(vx: float, vy: float, omega: float,
|
||||
fl_motor, fr_motor, rl_motor, rr_motor,
|
||||
compass, motor_max: float):
|
||||
"""Drive the mecanum chassis kinematically.
|
||||
|
||||
The wheel motors are spun for visual fidelity, but the chassis
|
||||
motion comes from Supervisor.setVelocity using the gym mecanum
|
||||
forward-kinematics formula. Gym training and Webots deployment
|
||||
therefore produce identical body motion.
|
||||
"""
|
||||
if math.hypot(vx, vy) < 1e-3 and abs(omega) < 1e-3:
|
||||
fl_motor.setVelocity(0.0)
|
||||
fr_motor.setVelocity(0.0)
|
||||
rl_motor.setVelocity(0.0)
|
||||
rr_motor.setVelocity(0.0)
|
||||
if _self_node is not None:
|
||||
_self_node.setVelocity([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
|
||||
return
|
||||
n = compass.getValues()
|
||||
h = math.atan2(n[0], n[1])
|
||||
@@ -258,15 +280,35 @@ def drive_mecanum(vx: float, vy: float, omega: float,
|
||||
fr_motor.setVelocity(w_fr)
|
||||
rl_motor.setVelocity(w_rl)
|
||||
rr_motor.setVelocity(w_rr)
|
||||
# Kinematic body injection — derive body velocity from the same
|
||||
# wheel speeds using the gym forward-kinematics formula and the
|
||||
# active preset's strafe/bleed parameters.
|
||||
if _self_node is not None:
|
||||
r = DOG_WHEEL_RADIUS
|
||||
vx_body = (w_fl + w_fr + w_rl + w_rr) * r / 4.0
|
||||
vy_body_ideal = (-w_fl + w_fr + w_rl - w_rr) * r / 4.0
|
||||
vy_body = vy_body_ideal * _ROBOT_CFG.strafe_efficiency
|
||||
if _ROBOT_CFG.strafe_to_forward_bleed != 0.0:
|
||||
vx_body += _ROBOT_CFG.strafe_to_forward_bleed * abs(vy_body_ideal)
|
||||
omega_body = (-w_fl + w_fr - w_rl + w_rr) * r / (
|
||||
4.0 * (DOG_WHEEL_BASE_X / 2.0 + DOG_WHEEL_BASE_Y / 2.0))
|
||||
cos_h, sin_h = math.cos(h), math.sin(h)
|
||||
vx_w = vx_body * cos_h - vy_body * sin_h
|
||||
vy_w = vx_body * sin_h + vy_body * cos_h
|
||||
_self_node.setVelocity([vx_w, vy_w, 0.0, 0.0, 0.0, omega_body])
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Webots devices
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
robot = Robot()
|
||||
robot = Supervisor()
|
||||
timestep = int(robot.getBasicTimeStep())
|
||||
|
||||
# Mecanum uses Supervisor.setVelocity for chassis motion (see
|
||||
# drive_mecanum); diff-drive keeps full ODE simulation.
|
||||
_self_node = robot.getSelf() if DRIVE_MODE == "mecanum" else None
|
||||
|
||||
# Multi-shepherd axis split. When the launcher creates two dog instances
|
||||
# it sets each robot's customData to "axis=x" or "axis=y"; the controller
|
||||
# then attenuates the off-axis component of every action so the two
|
||||
@@ -323,7 +365,16 @@ receiver = robot.getDevice("receiver"); receiver.enable(timestep)
|
||||
emitter = robot.getDevice("emitter")
|
||||
lidar = robot.getDevice("lidar"); lidar.enable(timestep)
|
||||
|
||||
tracker = SheepTracker(tracker_cfg=HERDING_WEBOTS.tracker)
|
||||
# Tracker config: pick the preset that matches the (drive, lidar) combo
|
||||
# so the tracker's consensus parameters match what the policy was
|
||||
# trained against.
|
||||
if DRIVE_MODE == "mecanum" and LIDAR_FOV_VARIANT == "360":
|
||||
_TRACKER_CFG = HERDING_MEC_WEBOTS_360.tracker
|
||||
elif DRIVE_MODE == "mecanum":
|
||||
_TRACKER_CFG = HERDING_MEC_WEBOTS.tracker
|
||||
else:
|
||||
_TRACKER_CFG = HERDING_WEBOTS.tracker
|
||||
tracker = SheepTracker(tracker_cfg=_TRACKER_CFG)
|
||||
|
||||
# Cosmetic ear motors — animated; not used by control.
|
||||
left_ear = robot.getDevice("left ear motor")
|
||||
@@ -429,9 +480,12 @@ if MODE == "calibrate":
|
||||
compass, MOTOR_MAX)
|
||||
robot.step(timestep)
|
||||
_pos1 = gps.getValues(); _x1, _y1 = _pos1[0], _pos1[1]
|
||||
_nv1 = compass.getValues(); _h1 = math.atan2(_nv1[0], _nv1[1])
|
||||
_T = _calib_n * _DT
|
||||
_vx_w = (_x1 - _x0) / _T; _vy_w = (_y1 - _y0) / _T
|
||||
_vx_g = (_xg - _x0) / _T; _vy_g = (_yg - _y0) / _T
|
||||
_dh_deg = math.degrees(math.atan2(math.sin(_h1 - _h0),
|
||||
math.cos(_h1 - _h0)))
|
||||
def _pct(a, p): return 0.0 if abs(p) < 1e-4 else 100.0 * abs(a - p) / abs(p)
|
||||
_result = (
|
||||
f"cmd=(vx={_calib_vx:.2f}, vy={_calib_vy:.2f}, om={_calib_om:.2f}) "
|
||||
@@ -441,7 +495,8 @@ if MODE == "calibrate":
|
||||
f" webots displacement: dx={_x1-_x0:.4f} dy={_y1-_y0:.4f} "
|
||||
f"(vx={_vx_w:.3f} vy={_vy_w:.3f} m/s)\n"
|
||||
f" vx error: {_pct(_vx_w, _vx_g):.1f}% "
|
||||
f"vy error: {_pct(_vy_w, _vy_g):.1f}%\n"
|
||||
f"vy error: {_pct(_vy_w, _vy_g):.1f}% "
|
||||
f"heading drift: {_dh_deg:+.1f}°\n"
|
||||
)
|
||||
print(_result)
|
||||
with open(_log_path, "a") as _f:
|
||||
@@ -611,7 +666,9 @@ while robot.step(timestep) != -1:
|
||||
f"tracks_active={tracker.n_active()} "
|
||||
f"tracks_cand={tracker.n_candidate()} "
|
||||
f"tracks_penned={tracker.n_penned()} "
|
||||
f"detections={len(detections)}")
|
||||
f"detections={len(detections)} "
|
||||
f"h={math.degrees(dog_heading):+.1f}°"
|
||||
+ (f"→{math.degrees(dog_heading):+.1f}°" if _h_ema > 0 else ""))
|
||||
if DRIVE_MODE == "mecanum":
|
||||
print(f"{common} action=({vx:+.2f}, {vy:+.2f}, {omega:+.2f})")
|
||||
else:
|
||||
|
||||
Reference in New Issue
Block a user