rust_robotics_planning/
bipedal_planner.rs

1//! Bipedal walking planner with modified designated footsteps.
2//!
3//! Ports the linear inverted-pendulum footstep adjustment demo from
4//! PythonRobotics.
5//!
6//! Reference:
7//! - PythonRobotics `Bipedal/bipedal_planner/bipedal_planner.py`
8
9use rust_robotics_core::{Point2D, RoboticsError, RoboticsResult};
10
11/// One foot placement in world or body-relative coordinates.
12#[derive(Debug, Clone, Copy, PartialEq)]
13pub struct Footstep {
14    pub x: f64,
15    pub y: f64,
16    pub theta: f64,
17}
18
19impl Footstep {
20    pub fn new(x: f64, y: f64, theta: f64) -> Self {
21        Self { x, y, theta }
22    }
23}
24
25/// Configuration for the bipedal planner.
26#[derive(Debug, Clone, Copy)]
27pub struct BipedalPlannerConfig {
28    /// Support phase duration \[s\].
29    pub t_sup: f64,
30    /// Center-of-mass height \[m\].
31    pub z_c: f64,
32    /// Footstep modification weight on COM position error.
33    pub a: f64,
34    /// Footstep modification weight on COM velocity error.
35    pub b: f64,
36    /// Number of Euler integration steps per support phase.
37    pub time_split: usize,
38    /// Keep one COM sample every `trajectory_stride` integration steps.
39    pub trajectory_stride: usize,
40    /// Gravitational acceleration [m/s^2].
41    pub gravity: f64,
42}
43
44impl Default for BipedalPlannerConfig {
45    fn default() -> Self {
46        Self {
47            t_sup: 0.8,
48            z_c: 0.8,
49            a: 10.0,
50            b: 1.0,
51            time_split: 100,
52            trajectory_stride: 10,
53            gravity: 9.8,
54        }
55    }
56}
57
58impl BipedalPlannerConfig {
59    fn validate(&self) -> RoboticsResult<()> {
60        if self.t_sup <= 0.0 || !self.t_sup.is_finite() {
61            return Err(RoboticsError::InvalidParameter(
62                "BipedalPlanner: t_sup must be positive".to_string(),
63            ));
64        }
65        if self.z_c <= 0.0 || !self.z_c.is_finite() {
66            return Err(RoboticsError::InvalidParameter(
67                "BipedalPlanner: z_c must be positive".to_string(),
68            ));
69        }
70        if self.a <= 0.0 || self.b <= 0.0 {
71            return Err(RoboticsError::InvalidParameter(
72                "BipedalPlanner: a and b must be positive".to_string(),
73            ));
74        }
75        if self.time_split == 0 || self.trajectory_stride == 0 {
76            return Err(RoboticsError::InvalidParameter(
77                "BipedalPlanner: time_split and trajectory_stride must be greater than zero"
78                    .to_string(),
79            ));
80        }
81        if self.gravity <= 0.0 || !self.gravity.is_finite() {
82            return Err(RoboticsError::InvalidParameter(
83                "BipedalPlanner: gravity must be positive".to_string(),
84            ));
85        }
86        Ok(())
87    }
88}
89
90/// Walking plan output.
91#[derive(Debug, Clone)]
92pub struct BipedalPlan {
93    pub reference_footsteps: Vec<Footstep>,
94    pub modified_footsteps: Vec<Footstep>,
95    pub com_trajectory: Vec<Point2D>,
96}
97
98#[derive(Debug, Clone, Copy)]
99struct PendulumState {
100    x: f64,
101    x_dot: f64,
102    y: f64,
103    y_dot: f64,
104}
105
106/// Planner for the PythonRobotics-style bipedal footstep adjustment demo.
107pub struct BipedalPlanner {
108    config: BipedalPlannerConfig,
109}
110
111impl BipedalPlanner {
112    pub fn new(config: BipedalPlannerConfig) -> Self {
113        Self { config }
114    }
115
116    pub fn config(&self) -> BipedalPlannerConfig {
117        self.config
118    }
119
120    /// Compute modified footsteps and a coarse COM trajectory.
121    pub fn plan(&self, reference_footsteps: &[Footstep]) -> RoboticsResult<BipedalPlan> {
122        self.config.validate()?;
123        if reference_footsteps.is_empty() {
124            return Err(RoboticsError::InvalidParameter(
125                "BipedalPlanner: reference footsteps must not be empty".to_string(),
126            ));
127        }
128
129        let mut plan = BipedalPlan {
130            reference_footsteps: vec![Footstep::new(0.0, 0.0, 0.0)],
131            modified_footsteps: vec![Footstep::new(0.0, 0.0, 0.0)],
132            com_trajectory: Vec::new(),
133        };
134
135        let mut px = 0.0;
136        let mut py = 0.0;
137        let mut px_star = 0.0;
138        let mut py_star = 0.0;
139        let mut state = PendulumState {
140            x: 0.0,
141            x_dot: 0.0,
142            y: 0.01,
143            y_dot: 0.0,
144        };
145
146        for (index, current_step) in reference_footsteps.iter().copied().enumerate() {
147            state =
148                self.integrate_inverted_pendulum(state, px_star, py_star, &mut plan.com_trajectory);
149
150            let step_number = index + 1;
151            let sign = alternating_sign(step_number);
152            let (dx, dy) =
153                rotate_vector(current_step.theta, current_step.x, -sign * current_step.y);
154            px += dx;
155            py += dy;
156
157            let next_step = reference_footsteps
158                .get(index + 1)
159                .copied()
160                .unwrap_or_else(|| Footstep::new(0.0, 0.0, 0.0));
161            let tc = (self.config.z_c / self.config.gravity).sqrt();
162            let c = (self.config.t_sup / tc).cosh();
163            let s = (self.config.t_sup / tc).sinh();
164
165            let (x_ref, y_ref) =
166                rotate_vector(next_step.theta, next_step.x / 2.0, sign * next_step.y / 2.0);
167            let (vx_ref, vy_ref) = rotate_vector(
168                next_step.theta,
169                (1.0 + c) / (tc * s) * x_ref,
170                (c - 1.0) / (tc * s) * y_ref,
171            );
172
173            plan.reference_footsteps
174                .push(Footstep::new(px, py, current_step.theta));
175
176            let xd = px + x_ref;
177            let yd = py + y_ref;
178            let xd_dot = vx_ref;
179            let yd_dot = vy_ref;
180
181            let d = self.config.a * (c - 1.0).powi(2) + self.config.b * (s / tc).powi(2);
182            px_star = -self.config.a * (c - 1.0) / d * (xd - c * state.x - tc * s * state.x_dot)
183                - self.config.b * s / (tc * d) * (xd_dot - s / tc * state.x - c * state.x_dot);
184            py_star = -self.config.a * (c - 1.0) / d * (yd - c * state.y - tc * s * state.y_dot)
185                - self.config.b * s / (tc * d) * (yd_dot - s / tc * state.y - c * state.y_dot);
186
187            plan.modified_footsteps
188                .push(Footstep::new(px_star, py_star, current_step.theta));
189        }
190
191        Ok(plan)
192    }
193
194    fn integrate_inverted_pendulum(
195        &self,
196        mut state: PendulumState,
197        px_star: f64,
198        py_star: f64,
199        com_trajectory: &mut Vec<Point2D>,
200    ) -> PendulumState {
201        let delta_time = self.config.t_sup / self.config.time_split as f64;
202
203        for step in 0..self.config.time_split {
204            let x_ddot = self.config.gravity / self.config.z_c * (state.x - px_star);
205            state.x += state.x_dot * delta_time;
206            state.x_dot += x_ddot * delta_time;
207
208            let y_ddot = self.config.gravity / self.config.z_c * (state.y - py_star);
209            state.y += state.y_dot * delta_time;
210            state.y_dot += y_ddot * delta_time;
211
212            if step % self.config.trajectory_stride == 0 {
213                com_trajectory.push(Point2D::new(state.x, state.y));
214            }
215        }
216
217        state
218    }
219}
220
221fn alternating_sign(n: usize) -> f64 {
222    if n % 2 == 0 {
223        1.0
224    } else {
225        -1.0
226    }
227}
228
229fn rotate_vector(theta: f64, x: f64, y: f64) -> (f64, f64) {
230    let cos_theta = theta.cos();
231    let sin_theta = theta.sin();
232    (cos_theta * x - sin_theta * y, sin_theta * x + cos_theta * y)
233}
234
235#[cfg(test)]
236mod tests {
237    use super::*;
238
239    fn assert_close(actual: f64, expected: f64, tolerance: f64) {
240        assert!(
241            (actual - expected).abs() <= tolerance,
242            "expected {expected:.12}, got {actual:.12}, tolerance {tolerance:.3e}"
243        );
244    }
245
246    fn default_pythonrobotics_footsteps() -> Vec<Footstep> {
247        vec![
248            Footstep::new(0.0, 0.2, 0.0),
249            Footstep::new(0.3, 0.2, 0.0),
250            Footstep::new(0.3, 0.2, 0.2),
251            Footstep::new(0.3, 0.2, 0.2),
252            Footstep::new(0.0, 0.2, 0.2),
253        ]
254    }
255
256    #[test]
257    fn test_bipedal_planner_config_defaults() {
258        let config = BipedalPlannerConfig::default();
259        assert_eq!(config.t_sup, 0.8);
260        assert_eq!(config.z_c, 0.8);
261        assert_eq!(config.a, 10.0);
262        assert_eq!(config.b, 1.0);
263        assert_eq!(config.time_split, 100);
264        assert_eq!(config.trajectory_stride, 10);
265        assert_eq!(config.gravity, 9.8);
266    }
267
268    #[test]
269    fn test_bipedal_planner_matches_pythonrobotics_default_walk() {
270        let planner = BipedalPlanner::new(BipedalPlannerConfig::default());
271        let plan = planner
272            .plan(&default_pythonrobotics_footsteps())
273            .expect("planner should succeed");
274
275        assert_eq!(plan.reference_footsteps.len(), 6);
276        assert_eq!(plan.modified_footsteps.len(), 6);
277        assert_eq!(plan.com_trajectory.len(), 50);
278
279        let expected_reference = [
280            Footstep::new(0.0, 0.0, 0.0),
281            Footstep::new(0.0, 0.2, 0.0),
282            Footstep::new(0.3, 0.0, 0.0),
283            Footstep::new(0.5542861071933602, 0.2556141148067667, 0.2),
284            Footstep::new(0.888039946704745, 0.11920159847703674, 0.2),
285            Footstep::new(0.8483060805457328, 0.31521491404528507, 0.2),
286        ];
287        let expected_modified = [
288            Footstep::new(0.0, 0.0, 0.0),
289            Footstep::new(-0.02068187189406957, 0.16806092102834097, 0.0),
290            Footstep::new(0.2914994382902407, -0.004038373768573439, 0.0),
291            Footstep::new(0.5054452070274589, 0.2653647289918646, 0.2),
292            Footstep::new(0.9019274635077843, 0.16741541638077787, 0.2),
293            Footstep::new(0.8258088100265661, 0.29759216443331915, 0.2),
294        ];
295
296        for (actual, expected) in plan
297            .reference_footsteps
298            .iter()
299            .zip(expected_reference.iter())
300        {
301            assert_close(actual.x, expected.x, 1.0e-12);
302            assert_close(actual.y, expected.y, 1.0e-12);
303            assert_close(actual.theta, expected.theta, 1.0e-12);
304        }
305        for (actual, expected) in plan.modified_footsteps.iter().zip(expected_modified.iter()) {
306            assert_close(actual.x, expected.x, 1.0e-12);
307            assert_close(actual.y, expected.y, 1.0e-12);
308            assert_close(actual.theta, expected.theta, 1.0e-12);
309        }
310
311        let first = plan.com_trajectory.first().expect("trajectory");
312        let middle = &plan.com_trajectory[25];
313        let last = plan.com_trajectory.last().expect("trajectory");
314        assert_close(first.x, 0.0, 1.0e-12);
315        assert_close(first.y, 0.01, 1.0e-12);
316        assert_close(middle.x, 0.2858111071163949, 1.0e-12);
317        assert_close(middle.y, 0.04910787389588683, 1.0e-12);
318        assert_close(last.x, 0.8572352745281623, 1.0e-12);
319        assert_close(last.y, 0.22162409933209326, 1.0e-12);
320    }
321
322    #[test]
323    fn test_bipedal_planner_rejects_empty_footsteps() {
324        let planner = BipedalPlanner::new(BipedalPlannerConfig::default());
325        let error = planner.plan(&[]).expect_err("empty footsteps must fail");
326        let message = error.to_string();
327        assert!(message.contains("reference footsteps"));
328    }
329}
rust_robotics_planning/bipedal_planner.rs

rust_robotics_planning/
bipedal_planner.rs
