Skip to main content

rust_robotics_control/
pusher_slider.rs

1//! Quasi-static planar pushing (pusher-slider) with contact modes and MPPI.
2//!
3//! A pure-Rust reproduction slice for contact-rich planar manipulation in the
4//! spirit of "Push Anything": a point pusher that may contact any of the
5//! slider's four faces shoves a rigid square slider across a table to a goal
6//! pose. The slider obeys the classic quasi-static ellipsoidal limit-surface
7//! model (Goyal/Howe/Mason; Lynch; Hogan-Rodriguez): motion is determined by the
8//! contact, not by inertia, and the contact can *stick* or *slide* depending on
9//! whether the required tangential force stays inside the pusher friction cone.
10//!
11//! - [`PusherSliderParams`] holds the slider half-extent, the limit-surface
12//!   characteristic length, and the pusher friction coefficient.
13//! - [`PusherCommand`] is a body-frame pusher motion on a chosen face: the face
14//!   index, a contact offset along it, a normal push speed, and a tangential
15//!   slip speed.
16//! - [`PusherSliderParams::step`] advances the slider one quasi-static step and
17//!   reports the realized [`ContactMode`].
18//! - [`PusherSliderMppiController`] runs MPPI per face and executes the command
19//!   from the lowest-cost face, so it can switch faces to reach goals (such as a
20//!   pure rotation) that a single face cannot; [`simulate_push`] runs the closed
21//!   loop and returns a [`PushReport`].
22//! - [`simulate_multi_push`] arranges several sliders into goal slots one at a
23//!   time, treating the other objects as keep-out discs so the active slider
24//!   routes around them.
25//! - [`PusherSliderParams::two_contact_twist`] / `two_contact_step` solve two
26//!   simultaneous contacts contact-implicitly (per-contact stick/slide mode
27//!   enumeration with a 4x4 force solve), so a couple can spin the slider in
28//!   place — motion a single contact cannot produce.
29//!
30//! The model is exact for the single-contact stick/slide regimes; the two-contact
31//! solver resolves the rigid-redundant (both-stick) case onto a cone edge, and a
32//! contact-implicit controller over the contacts is left as an extension.
33
34use rand::{rngs::StdRng, SeedableRng};
35use rand_distr::{Distribution, Normal};
36use rust_robotics_core::normalize_angle;
37use rust_robotics_core::{RoboticsError, RoboticsResult};
38
39/// Planar pose `[x, y, theta]` of the slider in the world frame.
40#[derive(Debug, Clone, Copy, PartialEq)]
41pub struct SliderState {
42    pub pose: [f64; 3],
43}
44
45impl SliderState {
46    pub fn new(x: f64, y: f64, theta: f64) -> Self {
47        Self {
48            pose: [x, y, theta],
49        }
50    }
51
52    pub fn x(self) -> f64 {
53        self.pose[0]
54    }
55    pub fn y(self) -> f64 {
56        self.pose[1]
57    }
58    pub fn theta(self) -> f64 {
59        self.pose[2]
60    }
61}
62
63/// The realized contact regime for a step.
64#[derive(Debug, Clone, Copy, PartialEq, Eq)]
65pub enum ContactMode {
66    /// No (or negative) normal force: the pusher is not pushing.
67    Separated,
68    /// Contact sticks: the contact point moves with the pusher.
69    Stick,
70    /// Contact slides along the face toward +tangent (friction cone saturated).
71    SlideUp,
72    /// Contact slides along the face toward -tangent.
73    SlideDown,
74}
75
76/// Body-frame pusher command on one of the slider's four faces.
77///
78/// `face` selects the contact face (`0` = back `-x`, `1` = `+y`, `2` = front
79/// `+x`, `3` = `-y`). The pusher pushes along the inward normal of that face;
80/// `contact` is the offset along the face, `push_speed` is the normal speed
81/// (clamped to be non-negative), and `tangent_speed` is the commanded tangential
82/// slip.
83#[derive(Debug, Clone, Copy, PartialEq)]
84pub struct PusherCommand {
85    pub face: usize,
86    pub contact: f64,
87    pub push_speed: f64,
88    pub tangent_speed: f64,
89}
90
91impl PusherCommand {
92    /// A command on the back face (`face = 0`).
93    pub fn new(contact: f64, push_speed: f64, tangent_speed: f64) -> Self {
94        Self {
95            face: 0,
96            contact,
97            push_speed,
98            tangent_speed,
99        }
100    }
101
102    /// A command on an explicit face (wrapped into `0..4`).
103    pub fn on_face(face: usize, contact: f64, push_speed: f64, tangent_speed: f64) -> Self {
104        Self {
105            face: face % 4,
106            contact,
107            push_speed,
108            tangent_speed,
109        }
110    }
111}
112
113/// Quasi-static pusher-slider parameters.
114#[derive(Debug, Clone, Copy, PartialEq)]
115pub struct PusherSliderParams {
116    /// Half side length of the square slider \[m\].
117    pub half_extent: f64,
118    /// Limit-surface characteristic length `c` \[m\] (couples force to torque).
119    pub char_len: f64,
120    /// Coulomb friction coefficient between pusher and slider.
121    pub pusher_friction: f64,
122}
123
124impl Default for PusherSliderParams {
125    fn default() -> Self {
126        // c ~ 0.6 * half-extent is the usual uniform-pressure square estimate.
127        Self {
128            half_extent: 0.05,
129            char_len: 0.03,
130            pusher_friction: 0.3,
131        }
132    }
133}
134
135impl PusherSliderParams {
136    pub fn new(half_extent: f64, char_len: f64, pusher_friction: f64) -> RoboticsResult<Self> {
137        let positive = |v: f64| v.is_finite() && v > 0.0;
138        if !positive(half_extent) || !positive(char_len) {
139            return Err(RoboticsError::InvalidParameter(
140                "pusher-slider half_extent and char_len must be finite and positive".to_string(),
141            ));
142        }
143        if !pusher_friction.is_finite() || pusher_friction < 0.0 {
144            return Err(RoboticsError::InvalidParameter(
145                "pusher-slider pusher_friction must be finite and non-negative".to_string(),
146            ));
147        }
148        Ok(Self {
149            half_extent,
150            char_len,
151            pusher_friction,
152        })
153    }
154
155    /// Body-frame contact point `p`, inward normal `d`, and tangent `t` for a
156    /// face index (`0..4`) and offset along the face.
157    pub fn contact_frame(self, face: usize, contact: f64) -> ([f64; 2], [f64; 2], [f64; 2]) {
158        let b = self.half_extent;
159        let s = contact.clamp(-b, b);
160        match face % 4 {
161            0 => ([-b, s], [1.0, 0.0], [0.0, 1.0]),
162            1 => ([s, b], [0.0, -1.0], [1.0, 0.0]),
163            2 => ([b, s], [-1.0, 0.0], [0.0, -1.0]),
164            _ => ([s, -b], [0.0, 1.0], [-1.0, 0.0]),
165        }
166    }
167
168    /// Body-frame slider twist `[vx, vy, omega]` and contact mode for a command.
169    ///
170    /// The limit-surface solve is identical for every face; only the contact
171    /// frame `(p, d, t)` rotates, so the friction cone is evaluated along the
172    /// face's own normal/tangent rather than the body axes.
173    pub fn twist(self, command: PusherCommand) -> ([f64; 3], ContactMode) {
174        let c2 = self.char_len * self.char_len;
175        let (p, d, t) = self.contact_frame(command.face, command.contact);
176        let [px, py] = p;
177        let vn = command.push_speed.max(0.0);
178        let vt = command.tangent_speed;
179
180        if vn <= 1e-12 {
181            return ([0.0; 3], ContactMode::Separated);
182        }
183
184        // Pusher velocity in the body frame and the limit-surface solve for the
185        // contact force whose motion matches it (same M for every face):
186        //   [wx, wy]^T = (1/c^2) [[c^2+py^2, -px*py], [-px*py, c^2+px^2]] [fx, fy]^T.
187        let wx = vn * d[0] + vt * t[0];
188        let wy = vn * d[1] + vt * t[1];
189        let m11 = (c2 + py * py) / c2;
190        let m12 = -(px * py) / c2;
191        let m22 = (c2 + px * px) / c2;
192        let det = m11 * m22 - m12 * m12;
193        let (fx, fy) = if det.abs() > 1e-15 {
194            ((m22 * wx - m12 * wy) / det, (-m12 * wx + m11 * wy) / det)
195        } else {
196            (wx, wy)
197        };
198
199        // Resolve the force into the face's normal/tangent for the friction cone.
200        let fn_ = fx * d[0] + fy * d[1];
201        let ft = fx * t[0] + fy * t[1];
202        if fn_ <= 0.0 {
203            // Would require pulling on the object: no contact motion.
204            return ([0.0; 3], ContactMode::Separated);
205        }
206
207        let mu = self.pusher_friction;
208        if ft.abs() <= mu * fn_ + 1e-12 {
209            // Stick: the slider twist is the limit-surface image of the wrench.
210            let omega = (px * fy - py * fx) / c2;
211            ([fx, fy, omega], ContactMode::Stick)
212        } else {
213            // Slide: the tangential force saturates on the friction-cone edge and
214            // the contact slides along the face. Scale the cone-edge wrench so the
215            // normal contact-point speed still matches the commanded push.
216            let sign = if ft > 0.0 { 1.0 } else { -1.0 };
217            let fe = [d[0] + sign * mu * t[0], d[1] + sign * mu * t[1]];
218            let omega1 = (px * fe[1] - py * fe[0]) / c2;
219            // Contact-point velocity per unit scale, projected on the normal d.
220            let cv = [fe[0] - omega1 * py, fe[1] + omega1 * px];
221            let proj = cv[0] * d[0] + cv[1] * d[1];
222            let k = if proj.abs() > 1e-12 { vn / proj } else { vn };
223            let k = k.max(0.0);
224            let mode = if sign > 0.0 {
225                ContactMode::SlideUp
226            } else {
227                ContactMode::SlideDown
228            };
229            ([k * fe[0], k * fe[1], k * omega1], mode)
230        }
231    }
232
233    /// Advance the slider one quasi-static step, returning the new state and the
234    /// realized contact mode.
235    pub fn step(
236        self,
237        state: SliderState,
238        command: PusherCommand,
239        dt: f64,
240    ) -> (SliderState, ContactMode) {
241        let ([vx_b, vy_b, omega], mode) = self.twist(command);
242        if mode == ContactMode::Separated {
243            return (state, mode);
244        }
245        // Rotate the body-frame CoM velocity into the world and integrate.
246        let theta = state.theta();
247        let (s, co) = theta.sin_cos();
248        let vx_w = co * vx_b - s * vy_b;
249        let vy_w = s * vx_b + co * vy_b;
250        let next = SliderState::new(
251            state.x() + vx_w * dt,
252            state.y() + vy_w * dt,
253            theta + omega * dt,
254        );
255        (next, mode)
256    }
257
258    /// World-frame contact point for a command (useful for rendering).
259    pub fn contact_point(self, state: SliderState, command: PusherCommand) -> [f64; 2] {
260        let ([px, py], _, _) = self.contact_frame(command.face, command.contact);
261        let (s, co) = state.theta().sin_cos();
262        [state.x() + co * px - s * py, state.y() + s * px + co * py]
263    }
264
265    /// Body-frame slider twist `[vx, vy, omega]` and per-contact modes for *two*
266    /// simultaneous point contacts, solved contact-implicitly.
267    ///
268    /// Each contact maintains its commanded normal speed; its tangential degree
269    /// of freedom is either sticking (tangential contact-point velocity matches
270    /// the pusher) or sliding (the tangential force saturates the friction cone).
271    /// The realized regime is found by enumerating the per-contact stick/slide
272    /// modes, solving the resulting 4x4 contact-force system, and keeping the
273    /// first combination whose forces are pushing (`fn >= 0`), respect the
274    /// friction cone (stick) or sit on the correct cone edge with a consistent
275    /// slip direction (slide).
276    pub fn two_contact_twist(
277        self,
278        c1: PusherCommand,
279        c2: PusherCommand,
280    ) -> ([f64; 3], [ContactMode; 2]) {
281        let c2c = self.char_len * self.char_len;
282        let (p1, d1, t1) = self.contact_frame(c1.face, c1.contact);
283        let (p2, d2, t2) = self.contact_frame(c2.face, c2.contact);
284        let mu = self.pusher_friction;
285
286        // omega = g . f, with f = [f1x, f1y, f2x, f2y].
287        let g = [-p1[1] / c2c, p1[0] / c2c, -p2[1] / c2c, p2[0] / c2c];
288        let wx_row = [1.0, 0.0, 1.0, 0.0];
289        let wy_row = [0.0, 1.0, 0.0, 1.0];
290        let add = |a: [f64; 4], b: [f64; 4], k: f64| {
291            [
292                a[0] + k * b[0],
293                a[1] + k * b[1],
294                a[2] + k * b[2],
295                a[3] + k * b[3],
296            ]
297        };
298        // Contact-point velocity rows (as linear forms in f).
299        let vc1x = add(wx_row, g, -p1[1]);
300        let vc1y = add(wy_row, g, p1[0]);
301        let vc2x = add(wx_row, g, -p2[1]);
302        let vc2y = add(wy_row, g, p2[0]);
303        let dot = |a: [f64; 4], b: [f64; 4]| a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3];
304
305        // Normal equations (always): contact velocity along the normal = vn.
306        let row_normal = |vcx: [f64; 4], vcy: [f64; 4], d: [f64; 2]| {
307            [
308                d[0] * vcx[0] + d[1] * vcy[0],
309                d[0] * vcx[1] + d[1] * vcy[1],
310                d[0] * vcx[2] + d[1] * vcy[2],
311                d[0] * vcx[3] + d[1] * vcy[3],
312            ]
313        };
314        let n1 = row_normal(vc1x, vc1y, d1);
315        let n2 = row_normal(vc2x, vc2y, d2);
316
317        // Candidate tangential modes: stick, slide+ (ft = +mu fn), slide- .
318        let modes = [0i32, 1, -1];
319        let mut chosen: Option<([f64; 3], [ContactMode; 2])> = None;
320        for &m1 in &modes {
321            for &m2 in &modes {
322                let (tang1, rhs1) =
323                    tangential_row(vc1x, vc1y, t1, d1, mu, m1, c1.tangent_speed, true);
324                let (tang2, rhs2) =
325                    tangential_row(vc2x, vc2y, t2, d2, mu, m2, c2.tangent_speed, false);
326                let a = [n1, tang1, n2, tang2];
327                let b = [c1.push_speed.max(0.0), rhs1, c2.push_speed.max(0.0), rhs2];
328                let Some(f) = solve4(a, b) else { continue };
329
330                let f1 = [f[0], f[1]];
331                let f2 = [f[2], f[3]];
332                let fn1 = f1[0] * d1[0] + f1[1] * d1[1];
333                let fn2 = f2[0] * d2[0] + f2[1] * d2[1];
334                let ft1 = f1[0] * t1[0] + f1[1] * t1[1];
335                let ft2 = f2[0] * t2[0] + f2[1] * t2[1];
336                if fn1 < -1e-9 || fn2 < -1e-9 {
337                    continue;
338                }
339                let slip1 = dot(vc1x, f) * t1[0] + dot(vc1y, f) * t1[1] - c1.tangent_speed;
340                let slip2 = dot(vc2x, f) * t2[0] + dot(vc2y, f) * t2[1] - c2.tangent_speed;
341                if !mode_valid(m1, fn1, ft1, slip1, mu) || !mode_valid(m2, fn2, ft2, slip2, mu) {
342                    continue;
343                }
344
345                let wx = dot(wx_row, f);
346                let wy = dot(wy_row, f);
347                let omega = dot(g, f);
348                chosen = Some(([wx, wy, omega], [mode_tag(m1), mode_tag(m2)]));
349                break;
350            }
351            if chosen.is_some() {
352                break;
353            }
354        }
355
356        if let Some(found) = chosen {
357            return found;
358        }
359
360        // No combination keeps both contacts: one pusher may lose contact
361        // (the slider moves away from it faster than it pushes). Try each
362        // contact alone and keep it if the other one really separates.
363        let alone = |(push, other): (PusherCommand, PusherCommand)| {
364            let (twist, mode) = self.twist(push);
365            if mode == ContactMode::Separated {
366                return None;
367            }
368            let (p, d, _) = self.contact_frame(other.face, other.contact);
369            let [vx, vy, omega] = twist;
370            let contact_velocity = [vx - omega * p[1], vy + omega * p[0]];
371            let away = contact_velocity[0] * d[0] + contact_velocity[1] * d[1];
372            (away >= other.push_speed.max(0.0) - 1e-12).then_some((twist, mode))
373        };
374        if let Some((twist, mode)) = alone((c1, c2)) {
375            return (twist, [mode, ContactMode::Separated]);
376        }
377        if let Some((twist, mode)) = alone((c2, c1)) {
378            return (twist, [ContactMode::Separated, mode]);
379        }
380        ([0.0; 3], [ContactMode::Separated, ContactMode::Separated])
381    }
382
383    /// Advance the slider one quasi-static step under two simultaneous contacts.
384    pub fn two_contact_step(
385        self,
386        state: SliderState,
387        c1: PusherCommand,
388        c2: PusherCommand,
389        dt: f64,
390    ) -> (SliderState, [ContactMode; 2]) {
391        let ([vx_b, vy_b, omega], modes) = self.two_contact_twist(c1, c2);
392        let theta = state.theta();
393        let (s, co) = theta.sin_cos();
394        let vx_w = co * vx_b - s * vy_b;
395        let vy_w = s * vx_b + co * vy_b;
396        let next = SliderState::new(
397            state.x() + vx_w * dt,
398            state.y() + vy_w * dt,
399            theta + omega * dt,
400        );
401        (next, modes)
402    }
403}
404
405/// Build a tangential equation row and rhs for contact mode `m`
406/// (`0` = stick, `+1`/`-1` = slide on the upper/lower cone edge).
407#[allow(clippy::too_many_arguments)]
408fn tangential_row(
409    vcx: [f64; 4],
410    vcy: [f64; 4],
411    t: [f64; 2],
412    d: [f64; 2],
413    mu: f64,
414    m: i32,
415    vt: f64,
416    first_contact: bool,
417) -> ([f64; 4], f64) {
418    if m == 0 {
419        // Stick: contact velocity along the tangent equals the pusher tangent.
420        let row = [
421            t[0] * vcx[0] + t[1] * vcy[0],
422            t[0] * vcx[1] + t[1] * vcy[1],
423            t[0] * vcx[2] + t[1] * vcy[2],
424            t[0] * vcx[3] + t[1] * vcy[3],
425        ];
426        (row, vt)
427    } else {
428        // Slide: f . t = m * mu * (f . d), a pure force constraint on this
429        // contact's own two force components (slots 0,1 for contact 1, 2,3 for
430        // contact 2).
431        let sgn = m as f64;
432        let cx = t[0] - sgn * mu * d[0];
433        let cy = t[1] - sgn * mu * d[1];
434        let row = if first_contact {
435            [cx, cy, 0.0, 0.0]
436        } else {
437            [0.0, 0.0, cx, cy]
438        };
439        (row, 0.0)
440    }
441}
442
443/// Validate a contact mode against the solved forces and slip.
444fn mode_valid(m: i32, fn_: f64, ft: f64, slip: f64, mu: f64) -> bool {
445    match m {
446        0 => ft.abs() <= mu * fn_ + 1e-9,
447        1 => fn_ >= -1e-9 && slip <= 1e-9,
448        _ => fn_ >= -1e-9 && slip >= -1e-9,
449    }
450}
451
452fn mode_tag(m: i32) -> ContactMode {
453    match m {
454        0 => ContactMode::Stick,
455        1 => ContactMode::SlideUp,
456        _ => ContactMode::SlideDown,
457    }
458}
459
460/// Solve a 4x4 linear system by Gaussian elimination with partial pivoting.
461#[allow(clippy::needless_range_loop)]
462fn solve4(mut a: [[f64; 4]; 4], mut b: [f64; 4]) -> Option<[f64; 4]> {
463    for col in 0..4 {
464        // Partial pivot.
465        let mut piv = col;
466        let mut best = a[col][col].abs();
467        for (r, row) in a.iter().enumerate().skip(col + 1) {
468            if row[col].abs() > best {
469                best = row[col].abs();
470                piv = r;
471            }
472        }
473        if best < 1e-12 {
474            return None;
475        }
476        a.swap(col, piv);
477        b.swap(col, piv);
478        let pivot = a[col][col];
479        for r in (col + 1)..4 {
480            let factor = a[r][col] / pivot;
481            for k in col..4 {
482                a[r][k] -= factor * a[col][k];
483            }
484            b[r] -= factor * b[col];
485        }
486    }
487    let mut x = [0.0; 4];
488    for i in (0..4).rev() {
489        let mut sum = b[i];
490        for k in (i + 1)..4 {
491            sum -= a[i][k] * x[k];
492        }
493        x[i] = sum / a[i][i];
494    }
495    Some(x)
496}
497
498/// Configuration for the sampling-based pushing controller.
499#[derive(Debug, Clone, PartialEq)]
500pub struct PusherMppiConfig {
501    pub horizon: usize,
502    pub samples: usize,
503    pub dt: f64,
504    /// Std-dev of the contact-offset perturbation \[m\].
505    pub contact_sigma: f64,
506    /// Std-dev of the tangential-slip perturbation \[m/s\].
507    pub tangent_sigma: f64,
508    /// Std-dev of the normal-push perturbation \[m/s\].
509    pub push_sigma: f64,
510    /// Initial (nominal) normal push speed \[m/s\].
511    pub push_speed: f64,
512    /// Maximum normal push speed \[m/s\] (the controller may slow toward 0).
513    pub max_push_speed: f64,
514    pub position_weight: f64,
515    pub heading_weight: f64,
516    /// Penalty weight for the slider center entering an obstacle keep-out disc.
517    pub obstacle_weight: f64,
518    /// Keep-out radius around each obstacle center \[m\] (`0` disables it).
519    pub obstacle_radius: f64,
520    pub lambda: f64,
521    pub seed: u64,
522}
523
524impl Default for PusherMppiConfig {
525    fn default() -> Self {
526        Self {
527            horizon: 18,
528            samples: 400,
529            dt: 0.1,
530            contact_sigma: 0.03,
531            tangent_sigma: 0.05,
532            push_sigma: 0.04,
533            push_speed: 0.06,
534            max_push_speed: 0.12,
535            // Pose weights are large because positions are O(0.1 m): squared
536            // errors are tiny, so the softmax temperature needs cost values of
537            // order 1-100 to discriminate rollouts (and let the pusher brake).
538            position_weight: 250.0,
539            heading_weight: 6.0,
540            obstacle_weight: 0.0,
541            obstacle_radius: 0.0,
542            lambda: 1.0,
543            seed: 7,
544        }
545    }
546}
547
548fn validate_config(config: &PusherMppiConfig) -> RoboticsResult<()> {
549    if config.horizon == 0 || config.samples == 0 {
550        return Err(RoboticsError::InvalidParameter(
551            "pusher MPPI horizon and samples must be positive".to_string(),
552        ));
553    }
554    if !config.dt.is_finite() || config.dt <= 0.0 {
555        return Err(RoboticsError::InvalidParameter(
556            "pusher MPPI dt must be finite and positive".to_string(),
557        ));
558    }
559    if !config.contact_sigma.is_finite()
560        || config.contact_sigma <= 0.0
561        || !config.tangent_sigma.is_finite()
562        || config.tangent_sigma <= 0.0
563        || !config.push_sigma.is_finite()
564        || config.push_sigma <= 0.0
565    {
566        return Err(RoboticsError::InvalidParameter(
567            "pusher MPPI sigmas must be finite and positive".to_string(),
568        ));
569    }
570    if !config.max_push_speed.is_finite() || config.max_push_speed <= 0.0 {
571        return Err(RoboticsError::InvalidParameter(
572            "pusher MPPI max_push_speed must be finite and positive".to_string(),
573        ));
574    }
575    if !config.lambda.is_finite() || config.lambda <= 0.0 {
576        return Err(RoboticsError::InvalidParameter(
577            "pusher MPPI lambda must be finite and positive".to_string(),
578        ));
579    }
580    if config.position_weight < 0.0
581        || config.heading_weight < 0.0
582        || config.push_speed < 0.0
583        || config.obstacle_weight < 0.0
584        || config.obstacle_radius < 0.0
585    {
586        return Err(RoboticsError::InvalidParameter(
587            "pusher MPPI weights, push_speed, and obstacle terms must be non-negative".to_string(),
588        ));
589    }
590    Ok(())
591}
592
593/// Result of one `plan` call.
594#[derive(Debug, Clone, PartialEq)]
595pub struct PusherMppiPlan {
596    pub command: PusherCommand,
597    pub best_cost: f64,
598}
599
600/// The three per-control Gaussian perturbations used while sampling.
601struct PusherNoise {
602    contact: Normal<f64>,
603    tangent: Normal<f64>,
604    push: Normal<f64>,
605}
606
607/// Deterministic, seeded sampling controller for planar pushing.
608///
609/// The controller is face-aware: it keeps a warm-started nominal plan for each of
610/// the slider's four faces, runs MPPI within each face, and executes the command
611/// from whichever face yields the lowest-cost rollout. Switching faces is what
612/// makes rotation-without-lateral-drift goals reachable (e.g. push the back face
613/// to spin one way, the front face to spin back and cancel the translation).
614#[derive(Debug, Clone)]
615pub struct PusherSliderMppiController {
616    config: PusherMppiConfig,
617    params: PusherSliderParams,
618    /// One warm-started nominal sequence per face (`0..4`).
619    nominals: Vec<Vec<PusherCommand>>,
620    /// Obstacle centers (e.g. other objects) the slider should keep clear of.
621    obstacles: Vec<[f64; 2]>,
622    rng: StdRng,
623}
624
625impl PusherSliderMppiController {
626    pub fn new(config: PusherMppiConfig, params: PusherSliderParams) -> RoboticsResult<Self> {
627        validate_config(&config)?;
628        let nominals = (0..4)
629            .map(|face| {
630                vec![PusherCommand::on_face(face, 0.0, config.push_speed, 0.0); config.horizon]
631            })
632            .collect();
633        let rng = StdRng::seed_from_u64(config.seed);
634        Ok(Self {
635            config,
636            params,
637            nominals,
638            obstacles: Vec::new(),
639            rng,
640        })
641    }
642
643    /// Attach obstacle centers (keep-out discs of `config.obstacle_radius`) that
644    /// the slider should avoid — used for multi-object pushing.
645    pub fn with_obstacles(mut self, obstacles: Vec<[f64; 2]>) -> Self {
646        self.obstacles = obstacles;
647        self
648    }
649
650    pub fn config(&self) -> &PusherMppiConfig {
651        &self.config
652    }
653
654    fn rollout_cost(
655        &self,
656        start: SliderState,
657        goal: SliderState,
658        controls: &[PusherCommand],
659    ) -> f64 {
660        let mut state = start;
661        let mut cost = 0.0;
662        for (i, &command) in controls.iter().enumerate() {
663            let (next, _) = self.params.step(state, command, self.config.dt);
664            state = next;
665            // Weight later steps more heavily (terminal emphasis).
666            let w = (i + 1) as f64 / controls.len() as f64;
667            cost += w * self.pose_cost(state, goal);
668        }
669        cost
670    }
671
672    fn pose_cost(&self, state: SliderState, goal: SliderState) -> f64 {
673        let dx = state.x() - goal.x();
674        let dy = state.y() - goal.y();
675        let dtheta = normalize_angle(state.theta() - goal.theta());
676        let mut cost = self.config.position_weight * (dx * dx + dy * dy)
677            + self.config.heading_weight * dtheta * dtheta;
678        // Keep-out penalty for overlapping other objects (multi-object pushing).
679        let r = self.config.obstacle_radius;
680        if r > 0.0 && self.config.obstacle_weight > 0.0 {
681            for o in &self.obstacles {
682                let ox = state.x() - o[0];
683                let oy = state.y() - o[1];
684                let d = (ox * ox + oy * oy).sqrt();
685                if d < r {
686                    let overlap = r - d;
687                    cost += self.config.obstacle_weight * overlap * overlap;
688                }
689            }
690        }
691        cost
692    }
693
694    /// Run MPPI for a single face, returning its warm-start-shifted nominal and
695    /// the best (minimum) rollout cost seen for that face.
696    fn plan_face(
697        &mut self,
698        face: usize,
699        start: SliderState,
700        goal: SliderState,
701        per_face_samples: usize,
702        noise: &PusherNoise,
703    ) -> (Vec<PusherCommand>, f64) {
704        let contact_noise = &noise.contact;
705        let tangent_noise = &noise.tangent;
706        let push_noise = &noise.push;
707        let b = self.params.half_extent;
708        let max_push = self.config.max_push_speed;
709        let horizon = self.config.horizon;
710        let base_seq = self.nominals[face].clone();
711
712        let mut costs = Vec::with_capacity(per_face_samples);
713        let mut sequences = Vec::with_capacity(per_face_samples);
714        let mut best_cost = f64::INFINITY;
715
716        for _ in 0..per_face_samples {
717            let mut controls = Vec::with_capacity(horizon);
718            for &bcmd in &base_seq {
719                let contact = (bcmd.contact + contact_noise.sample(&mut self.rng)).clamp(-b, b);
720                let tangent = bcmd.tangent_speed + tangent_noise.sample(&mut self.rng);
721                let push =
722                    (bcmd.push_speed + push_noise.sample(&mut self.rng)).clamp(0.0, max_push);
723                controls.push(PusherCommand::on_face(face, contact, push, tangent));
724            }
725            let cost = self.rollout_cost(start, goal, &controls);
726            best_cost = best_cost.min(cost);
727            costs.push(cost);
728            sequences.push(controls);
729        }
730
731        let min_cost = costs.iter().copied().fold(f64::INFINITY, f64::min);
732        let mut weight_sum = 0.0;
733        let mut weights = Vec::with_capacity(costs.len());
734        for &cost in &costs {
735            let w = (-(cost - min_cost) / self.config.lambda).exp();
736            weight_sum += w;
737            weights.push(w);
738        }
739
740        let updated = if weight_sum <= 0.0 || !weight_sum.is_finite() {
741            let best_index = costs
742                .iter()
743                .enumerate()
744                .min_by(|a, b| a.1.partial_cmp(b.1).unwrap_or(std::cmp::Ordering::Equal))
745                .map(|(i, _)| i)
746                .unwrap_or(0);
747            sequences[best_index].clone()
748        } else {
749            let mut acc_seq = vec![PusherCommand::on_face(face, 0.0, 0.0, 0.0); horizon];
750            for (w, controls) in weights.iter().zip(&sequences) {
751                let n = w / weight_sum;
752                for (acc, command) in acc_seq.iter_mut().zip(controls) {
753                    acc.contact += n * command.contact;
754                    acc.push_speed += n * command.push_speed;
755                    acc.tangent_speed += n * command.tangent_speed;
756                }
757            }
758            for command in &mut acc_seq {
759                command.contact = command.contact.clamp(-b, b);
760                command.push_speed = command.push_speed.clamp(0.0, max_push);
761            }
762            acc_seq
763        };
764
765        (updated, best_cost)
766    }
767
768    /// Plan the next pusher command toward `goal`, choosing the best contact face.
769    pub fn plan(
770        &mut self,
771        start: SliderState,
772        goal: SliderState,
773    ) -> RoboticsResult<PusherMppiPlan> {
774        let make = |sigma: f64, what: &str| {
775            Normal::new(0.0, sigma).map_err(|_| {
776                RoboticsError::InvalidParameter(format!("invalid pusher {what} distribution"))
777            })
778        };
779        let noise = PusherNoise {
780            contact: make(self.config.contact_sigma, "contact")?,
781            tangent: make(self.config.tangent_sigma, "tangent")?,
782            push: make(self.config.push_sigma, "push")?,
783        };
784        let per_face = (self.config.samples / 4).max(1);
785
786        let mut best: Option<(PusherCommand, f64)> = None;
787        for face in 0..4 {
788            let (updated, cost) = self.plan_face(face, start, goal, per_face, &noise);
789            let first = updated[0];
790            // Warm-start: shift this face's nominal by one step.
791            let mut shifted = updated[1..].to_vec();
792            shifted.push(*updated.last().unwrap());
793            self.nominals[face] = shifted;
794
795            if best.map(|(_, c)| cost < c).unwrap_or(true) {
796                best = Some((first, cost));
797            }
798        }
799
800        let (command, best_cost) = best.unwrap();
801        Ok(PusherMppiPlan { command, best_cost })
802    }
803}
804
805/// Metrics for a closed-loop push.
806#[derive(Debug, Clone, PartialEq)]
807pub struct PushReport {
808    pub steps: usize,
809    pub final_pose: [f64; 3],
810    pub position_error: f64,
811    pub heading_error: f64,
812    pub stick_fraction: f64,
813    pub slide_fraction: f64,
814    pub path: Vec<[f64; 3]>,
815    pub modes: Vec<ContactMode>,
816}
817
818/// Drive the pushing controller toward a goal pose and report the result.
819pub fn simulate_push(
820    config: PusherMppiConfig,
821    params: PusherSliderParams,
822    start: SliderState,
823    goal: SliderState,
824    max_steps: usize,
825) -> RoboticsResult<PushReport> {
826    simulate_push_with_obstacles(config, params, start, goal, &[], max_steps)
827}
828
829/// Like [`simulate_push`] but the controller keeps the slider clear of the given
830/// obstacle centers (keep-out discs of `config.obstacle_radius`).
831pub fn simulate_push_with_obstacles(
832    config: PusherMppiConfig,
833    params: PusherSliderParams,
834    start: SliderState,
835    goal: SliderState,
836    obstacles: &[[f64; 2]],
837    max_steps: usize,
838) -> RoboticsResult<PushReport> {
839    let dt = config.dt;
840    let mut controller =
841        PusherSliderMppiController::new(config, params)?.with_obstacles(obstacles.to_vec());
842    let mut state = start;
843    let mut path = vec![state.pose];
844    let mut modes = Vec::new();
845    let mut stick = 0usize;
846    let mut slide = 0usize;
847    let mut executed = 0usize;
848
849    for _ in 0..max_steps {
850        let plan = controller.plan(state, goal)?;
851        let (next, mode) = params.step(state, plan.command, dt);
852        match mode {
853            ContactMode::Stick => stick += 1,
854            ContactMode::SlideUp | ContactMode::SlideDown => slide += 1,
855            ContactMode::Separated => {}
856        }
857        state = next;
858        path.push(state.pose);
859        modes.push(mode);
860        executed += 1;
861
862        let dx = state.x() - goal.x();
863        let dy = state.y() - goal.y();
864        if (dx * dx + dy * dy).sqrt() < 0.2 * params.half_extent
865            && normalize_angle(state.theta() - goal.theta()).abs() < 0.05
866        {
867            break;
868        }
869    }
870
871    let dx = state.x() - goal.x();
872    let dy = state.y() - goal.y();
873    let denom = executed.max(1) as f64;
874    Ok(PushReport {
875        steps: executed,
876        final_pose: state.pose,
877        position_error: (dx * dx + dy * dy).sqrt(),
878        heading_error: normalize_angle(state.theta() - goal.theta()).abs(),
879        stick_fraction: stick as f64 / denom,
880        slide_fraction: slide as f64 / denom,
881        path,
882        modes,
883    })
884}
885
886/// Metrics for a multi-object push.
887#[derive(Debug, Clone, PartialEq)]
888pub struct MultiPushReport {
889    /// Per-object push reports, in the order the objects were pushed.
890    pub objects: Vec<PushReport>,
891    /// Final pose of every object (same indexing as the inputs).
892    pub final_poses: Vec<[f64; 3]>,
893    pub max_position_error: f64,
894    pub max_heading_error: f64,
895}
896
897/// Push several sliders to their goals one at a time, treating the *other*
898/// objects (at their current poses) as keep-out obstacles for the active one.
899///
900/// Objects are pushed in index order; each object's resting pose then becomes an
901/// obstacle for the objects pushed after it. This reproduces the multi-object
902/// arrangement setting of "Push Anything" without a simultaneous multi-contact
903/// solve.
904pub fn simulate_multi_push(
905    config: PusherMppiConfig,
906    params: PusherSliderParams,
907    starts: &[SliderState],
908    goals: &[SliderState],
909    max_steps: usize,
910) -> RoboticsResult<MultiPushReport> {
911    if starts.len() != goals.len() {
912        return Err(RoboticsError::InvalidParameter(
913            "multi-push starts and goals must have equal length".to_string(),
914        ));
915    }
916    let n = starts.len();
917    let mut poses: Vec<SliderState> = starts.to_vec();
918    let mut objects = Vec::with_capacity(n);
919    let mut max_position_error = 0.0_f64;
920    let mut max_heading_error = 0.0_f64;
921
922    for i in 0..n {
923        let obstacles: Vec<[f64; 2]> = (0..n)
924            .filter(|&j| j != i)
925            .map(|j| [poses[j].x(), poses[j].y()])
926            .collect();
927        let report = simulate_push_with_obstacles(
928            config.clone(),
929            params,
930            poses[i],
931            goals[i],
932            &obstacles,
933            max_steps,
934        )?;
935        poses[i] = SliderState {
936            pose: report.final_pose,
937        };
938        max_position_error = max_position_error.max(report.position_error);
939        max_heading_error = max_heading_error.max(report.heading_error);
940        objects.push(report);
941    }
942
943    Ok(MultiPushReport {
944        final_poses: poses.iter().map(|p| p.pose).collect(),
945        objects,
946        max_position_error,
947        max_heading_error,
948    })
949}
950
951#[cfg(test)]
952mod tests {
953    use super::*;
954
955    #[test]
956    fn a_contact_the_slider_pulls_away_from_separates() {
957        // Two pushers on the back face; only one pushes. The pushed contact
958        // drives the slider and the idle one loses contact.
959        let params = PusherSliderParams::new(0.05, 0.05 / 3.0_f64.sqrt(), 0.3).unwrap();
960        let b = params.half_extent;
961        let pushing = PusherCommand::on_face(0, 0.6 * b, 0.05, 0.0);
962        let idle = PusherCommand::on_face(0, -0.6 * b, 0.0, 0.0);
963        let (twist, modes) = params.two_contact_twist(pushing, idle);
964        assert_ne!(twist, [0.0; 3], "nothing moved");
965        assert_ne!(modes[0], ContactMode::Separated);
966        assert_eq!(modes[1], ContactMode::Separated);
967        // Same as pushing with the first contact alone.
968        let (single, _) = params.twist(pushing);
969        for (a, b) in twist.iter().zip(single) {
970            assert!((a - b).abs() < 1e-12);
971        }
972        // Swapped order gives the mirrored assignment.
973        let (_, modes) = params.two_contact_twist(idle, pushing);
974        assert_eq!(modes[0], ContactMode::Separated);
975        assert_ne!(modes[1], ContactMode::Separated);
976    }
977
978    #[test]
979    fn straight_centered_push_translates_without_rotation() {
980        let params = PusherSliderParams::default();
981        let mut state = SliderState::new(0.0, 0.0, 0.0);
982        for _ in 0..20 {
983            let (next, mode) = params.step(state, PusherCommand::new(0.0, 0.05, 0.0), 0.05);
984            assert_eq!(mode, ContactMode::Stick);
985            state = next;
986        }
987        assert!(state.x() > 0.0, "should move +x: {}", state.x());
988        assert!(state.y().abs() < 1e-9, "no lateral drift: {}", state.y());
989        assert!(state.theta().abs() < 1e-9, "no rotation: {}", state.theta());
990    }
991
992    #[test]
993    fn offcenter_push_induces_rotation() {
994        let params = PusherSliderParams::default();
995        let mut state = SliderState::new(0.0, 0.0, 0.0);
996        let cmd = PusherCommand::new(0.7 * params.half_extent, 0.05, 0.0);
997        for _ in 0..20 {
998            let (next, _) = params.step(state, cmd, 0.05);
999            state = next;
1000        }
1001        assert!(
1002            state.theta().abs() > 1e-3,
1003            "should rotate: {}",
1004            state.theta()
1005        );
1006    }
1007
1008    #[test]
1009    fn large_tangent_slip_slides() {
1010        let params = PusherSliderParams::default();
1011        let state = SliderState::new(0.0, 0.0, 0.0);
1012        let (_, mode) = params.step(state, PusherCommand::new(0.0, 0.05, 1.0), 0.05);
1013        assert!(matches!(
1014            mode,
1015            ContactMode::SlideUp | ContactMode::SlideDown
1016        ));
1017    }
1018
1019    #[test]
1020    fn no_push_keeps_state() {
1021        let params = PusherSliderParams::default();
1022        let state = SliderState::new(0.3, -0.2, 0.5);
1023        let (next, mode) = params.step(state, PusherCommand::new(0.0, 0.0, 0.2), 0.05);
1024        assert_eq!(mode, ContactMode::Separated);
1025        assert_eq!(next, state);
1026    }
1027
1028    #[test]
1029    fn rejects_invalid_params() {
1030        assert!(PusherSliderParams::new(0.0, 0.03, 0.3).is_err());
1031        assert!(PusherSliderParams::new(0.05, -0.01, 0.3).is_err());
1032        assert!(PusherSliderParams::new(0.05, 0.03, -0.1).is_err());
1033    }
1034
1035    #[test]
1036    fn controller_pushes_toward_goal() {
1037        let params = PusherSliderParams::default();
1038        let start = SliderState::new(0.0, 0.0, 0.0);
1039        // On-axis reach: a single back-face pusher drives the slider forward.
1040        let goal = SliderState::new(0.3, 0.0, 0.0);
1041        let report = simulate_push(PusherMppiConfig::default(), params, start, goal, 150).unwrap();
1042        let start_err = ((start.x() - goal.x()).powi(2) + (start.y() - goal.y()).powi(2)).sqrt();
1043        assert!(
1044            report.position_error < 0.3 * start_err,
1045            "should approach goal: {} vs start {}",
1046            report.position_error,
1047            start_err
1048        );
1049    }
1050
1051    #[test]
1052    fn simulation_is_deterministic() {
1053        let params = PusherSliderParams::default();
1054        let start = SliderState::new(0.0, 0.0, 0.0);
1055        let goal = SliderState::new(0.3, 0.0, 0.0);
1056        let run = || simulate_push(PusherMppiConfig::default(), params, start, goal, 80).unwrap();
1057        let a = run();
1058        let b = run();
1059        assert_eq!(a.path, b.path);
1060        assert_eq!(a.position_error, b.position_error);
1061    }
1062
1063    #[test]
1064    fn pushing_on_a_chosen_face_reverses_translation() {
1065        // The front face (face 2) pushes the slider in -x, the back face in +x.
1066        let params = PusherSliderParams::default();
1067        let state = SliderState::new(0.0, 0.0, 0.0);
1068        let (back, _) = params.step(state, PusherCommand::on_face(0, 0.0, 0.05, 0.0), 0.1);
1069        let (front, _) = params.step(state, PusherCommand::on_face(2, 0.0, 0.05, 0.0), 0.1);
1070        assert!(back.x() > 0.0, "back face pushes +x: {}", back.x());
1071        assert!(front.x() < 0.0, "front face pushes -x: {}", front.x());
1072    }
1073
1074    #[test]
1075    fn multi_push_places_every_object() {
1076        let params = PusherSliderParams::default();
1077        let config = PusherMppiConfig {
1078            obstacle_weight: 400.0,
1079            obstacle_radius: 2.4 * params.half_extent,
1080            ..PusherMppiConfig::default()
1081        };
1082        let starts = [
1083            SliderState::new(0.0, 0.10, 0.0),
1084            SliderState::new(0.0, 0.0, 0.0),
1085            SliderState::new(0.0, -0.10, 0.0),
1086        ];
1087        let goals = [
1088            SliderState::new(0.28, 0.10, 0.0),
1089            SliderState::new(0.28, 0.0, 0.0),
1090            SliderState::new(0.28, -0.10, 0.0),
1091        ];
1092        let report = simulate_multi_push(config, params, &starts, &goals, 200).unwrap();
1093        assert_eq!(report.objects.len(), 3);
1094        assert!(
1095            report.max_position_error < 3.0 * params.half_extent,
1096            "all objects should reach: max err {}",
1097            report.max_position_error
1098        );
1099    }
1100
1101    #[test]
1102    fn two_contact_symmetric_push_is_pure_translation() {
1103        // Two contacts on the back face at +/-h, both pushing forward, cancel the
1104        // off-center rotation a single contact would cause.
1105        let params = PusherSliderParams::default();
1106        let h = 0.6 * params.half_extent;
1107        let c1 = PusherCommand::on_face(0, h, 0.05, 0.0);
1108        let c2 = PusherCommand::on_face(0, -h, 0.05, 0.0);
1109        let ([vx, vy, omega], _modes) = params.two_contact_twist(c1, c2);
1110        assert!(vx > 0.0, "should translate +x: {vx}");
1111        assert!(omega.abs() < 1e-9, "no net rotation: {omega}");
1112        assert!(vy.abs() < 1e-9, "no lateral: {vy}");
1113        // A single off-center contact at +h does rotate.
1114        let ([_, _, omega_single], _) = params.twist(PusherCommand::on_face(0, h, 0.05, 0.0));
1115        assert!(
1116            omega_single.abs() > 1e-6,
1117            "single contact rotates: {omega_single}"
1118        );
1119    }
1120
1121    #[test]
1122    fn two_contact_antipodal_couple_rotates_in_place() {
1123        // Back-face contact high pushing +x and front-face contact low pushing -x
1124        // form a couple: rotation with little net translation.
1125        let params = PusherSliderParams::default();
1126        let h = 0.7 * params.half_extent;
1127        let c1 = PusherCommand::on_face(0, h, 0.05, 0.0);
1128        let c2 = PusherCommand::on_face(2, -h, 0.05, 0.0);
1129        let ([vx, vy, omega], _) = params.two_contact_twist(c1, c2);
1130        let speed = (vx * vx + vy * vy).sqrt();
1131        assert!(omega.abs() > 1e-3, "should rotate: {omega}");
1132        // The rotation dominates the (couple-cancelled) translation.
1133        assert!(
1134            speed < 0.5 * omega.abs() * params.half_extent + 1e-6,
1135            "translation {speed} should be small vs rotation {omega}"
1136        );
1137    }
1138
1139    #[test]
1140    fn multi_push_rejects_mismatched_lengths() {
1141        let params = PusherSliderParams::default();
1142        let starts = [SliderState::new(0.0, 0.0, 0.0)];
1143        let goals = [
1144            SliderState::new(0.2, 0.0, 0.0),
1145            SliderState::new(0.2, 0.1, 0.0),
1146        ];
1147        assert!(
1148            simulate_multi_push(PusherMppiConfig::default(), params, &starts, &goals, 50).is_err()
1149        );
1150    }
1151
1152    #[test]
1153    fn multiface_rotates_without_net_translation() {
1154        // A pure rotation goal (theta only) is unreachable by a single back-face
1155        // pusher but reachable once the controller may switch faces.
1156        let params = PusherSliderParams::default();
1157        let start = SliderState::new(0.0, 0.0, 0.0);
1158        let goal = SliderState::new(0.0, 0.0, 0.5);
1159        let report = simulate_push(PusherMppiConfig::default(), params, start, goal, 300).unwrap();
1160        assert!(
1161            report.heading_error < 0.1,
1162            "should reach the heading: err {}",
1163            report.heading_error
1164        );
1165        assert!(
1166            report.position_error < 2.0 * params.half_extent,
1167            "should keep position near origin: err {}",
1168            report.position_error
1169        );
1170    }
1171}