1use rand::{rngs::StdRng, SeedableRng};
35use rand_distr::{Distribution, Normal};
36use rust_robotics_core::normalize_angle;
37use rust_robotics_core::{RoboticsError, RoboticsResult};
38
39#[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#[derive(Debug, Clone, Copy, PartialEq, Eq)]
65pub enum ContactMode {
66 Separated,
68 Stick,
70 SlideUp,
72 SlideDown,
74}
75
76#[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 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 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#[derive(Debug, Clone, Copy, PartialEq)]
115pub struct PusherSliderParams {
116 pub half_extent: f64,
118 pub char_len: f64,
120 pub pusher_friction: f64,
122}
123
124impl Default for PusherSliderParams {
125 fn default() -> Self {
126 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 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 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 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 let fn_ = fx * d[0] + fy * d[1];
201 let ft = fx * t[0] + fy * t[1];
202 if fn_ <= 0.0 {
203 return ([0.0; 3], ContactMode::Separated);
205 }
206
207 let mu = self.pusher_friction;
208 if ft.abs() <= mu * fn_ + 1e-12 {
209 let omega = (px * fy - py * fx) / c2;
211 ([fx, fy, omega], ContactMode::Stick)
212 } else {
213 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 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 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 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 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 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 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 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 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 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 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 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#[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 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 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
443fn 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#[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 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#[derive(Debug, Clone, PartialEq)]
500pub struct PusherMppiConfig {
501 pub horizon: usize,
502 pub samples: usize,
503 pub dt: f64,
504 pub contact_sigma: f64,
506 pub tangent_sigma: f64,
508 pub push_sigma: f64,
510 pub push_speed: f64,
512 pub max_push_speed: f64,
514 pub position_weight: f64,
515 pub heading_weight: f64,
516 pub obstacle_weight: f64,
518 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 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#[derive(Debug, Clone, PartialEq)]
595pub struct PusherMppiPlan {
596 pub command: PusherCommand,
597 pub best_cost: f64,
598}
599
600struct PusherNoise {
602 contact: Normal<f64>,
603 tangent: Normal<f64>,
604 push: Normal<f64>,
605}
606
607#[derive(Debug, Clone)]
615pub struct PusherSliderMppiController {
616 config: PusherMppiConfig,
617 params: PusherSliderParams,
618 nominals: Vec<Vec<PusherCommand>>,
620 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 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 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 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 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 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 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#[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
818pub 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
829pub 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#[derive(Debug, Clone, PartialEq)]
888pub struct MultiPushReport {
889 pub objects: Vec<PushReport>,
891 pub final_poses: Vec<[f64; 3]>,
893 pub max_position_error: f64,
894 pub max_heading_error: f64,
895}
896
897pub 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 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 let (single, _) = params.twist(pushing);
969 for (a, b) in twist.iter().zip(single) {
970 assert!((a - b).abs() < 1e-12);
971 }
972 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 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 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 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 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 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 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 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}