2d_robot

MPPI · Safe Motion Planning · Planar Robot 7-dof

Human Safety Cost — J_human

Drag the joint sliders to pose the planar robot. The cost function evaluates all 14 control points (2 per link) against each human sphere and sums the maximum exponential proximity cost per sphere.

Robot configuration

Control points (2/link) Human spheres Min-distance line

q₁ 20°

q₂ -40°

q₃ 15°

q₄ 60°

α 5

w_h 5

Cost breakdown — J_human

J_human = w_h · Σ_i max_j[exp(−α·ReLU(d_ij))]

—

ReLU(d_ij) = max(d_ij, 0)

Inside sphere: d < 0 → ReLU = 0 → exp(0) = 1.0 (saturated)

At surface: d = 0 → cost = 1.0

Outside: d > 0 → exp(−αd) ∈ (0,1), decays smoothly

max_j over 14 CPs → worst link drives cost

Σ_i over all spheres → no body segment ignored

d_ij(q) = ‖x_j(q) − p_i‖₂ − r_i   →   signed proximity: positive outside, zero at surface, negative inside sphere

J_human(x_t) = w_h · Σ_i=1^N_h max_j[ exp(−α · ReLU(d_ij(q))) ]

robot: 7 links × 2 CPs = 14 control points ·