rapier2d/control/
pid_controller.rs

1use crate::dynamics::{AxesMask, RigidBody, RigidBodyPosition, RigidBodyVelocity};
2use crate::math::{Isometry, Point, Real, Rotation, Vector};
3use parry::math::AngVector;
4
5/// A Proportional-Derivative (PD) controller.
6///
7/// This is useful for controlling a rigid-body at the velocity level so it matches a target
8/// pose.
9///
10/// This is a [PID controller](https://en.wikipedia.org/wiki/Proportional%E2%80%93integral%E2%80%93derivative_controller)
11/// without the Integral part to keep the API immutable, while having a behaviour generally
12/// sufficient for games.
13#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
14#[derive(Debug, Copy, Clone, PartialEq)]
15pub struct PdController {
16    /// The Proportional gain applied to the instantaneous linear position errors.
17    ///
18    /// This is usually set to a multiple of the inverse of simulation step time
19    /// (e.g. `60` if the delta-time is `1.0 / 60.0`).
20    pub lin_kp: Vector<Real>,
21    /// The Derivative gain applied to the instantaneous linear velocity errors.
22    ///
23    /// This is usually set to a value in `[0.0, 1.0]` where `0.0` implies no damping
24    /// (no correction of velocity errors) and `1.0` implies complete damping (velocity errors
25    /// are corrected in a single simulation step).
26    pub lin_kd: Vector<Real>,
27    /// The Proportional gain applied to the instantaneous angular position errors.
28    ///
29    /// This is usually set to a multiple of the inverse of simulation step time
30    /// (e.g. `60` if the delta-time is `1.0 / 60.0`).
31    pub ang_kp: AngVector<Real>,
32    /// The Derivative gain applied to the instantaneous angular velocity errors.
33    ///
34    /// This is usually set to a value in `[0.0, 1.0]` where `0.0` implies no damping
35    /// (no correction of velocity errors) and `1.0` implies complete damping (velocity errors
36    /// are corrected in a single simulation step).
37    pub ang_kd: AngVector<Real>,
38    /// The axes affected by this controller.
39    ///
40    /// Only coordinate axes with a bit flags set to `true` will be taken into
41    /// account when calculating the errors and corrections.
42    pub axes: AxesMask,
43}
44
45impl Default for PdController {
46    fn default() -> Self {
47        Self::new(60.0, 0.8, AxesMask::all())
48    }
49}
50
51/// A Proportional-Integral-Derivative (PID) controller.
52///
53/// For video games, the Proportional-Derivative [`PdController`] is generally sufficient and
54/// offers an immutable API.
55#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
56#[derive(Debug, Copy, Clone, PartialEq)]
57pub struct PidController {
58    /// The Proportional-Derivative (PD) part of this PID controller.
59    pub pd: PdController,
60    /// The translational error accumulated through time for the Integral part of the PID controller.
61    pub lin_integral: Vector<Real>,
62    /// The angular error accumulated through time for the Integral part of the PID controller.
63    pub ang_integral: AngVector<Real>,
64    /// The linear gain applied to the Integral part of the PID controller.
65    pub lin_ki: Vector<Real>,
66    /// The angular gain applied to the Integral part of the PID controller.
67    pub ang_ki: AngVector<Real>,
68}
69
70impl Default for PidController {
71    fn default() -> Self {
72        Self::new(60.0, 1.0, 0.8, AxesMask::all())
73    }
74}
75
76/// Position or velocity errors measured for PID control.
77pub struct PdErrors {
78    /// The linear (translational) part of the error.
79    pub linear: Vector<Real>,
80    /// The angular (rotational) part of the error.
81    pub angular: AngVector<Real>,
82}
83
84impl From<RigidBodyVelocity<Real>> for PdErrors {
85    fn from(vels: RigidBodyVelocity<Real>) -> Self {
86        Self {
87            linear: vels.linvel,
88            angular: vels.angvel,
89        }
90    }
91}
92
93impl PdController {
94    /// Initialized the PD controller with uniform gain.
95    ///
96    /// The same gain are applied on all axes. To configure per-axes gains, construct
97    /// the [`PdController`] by setting its fields explicitly instead.
98    ///
99    /// Only the axes specified in `axes` will be enabled (but the gain values are set
100    /// on all axes regardless).
101    pub fn new(kp: Real, kd: Real, axes: AxesMask) -> PdController {
102        #[cfg(feature = "dim2")]
103        return Self {
104            lin_kp: Vector::repeat(kp),
105            lin_kd: Vector::repeat(kd),
106            ang_kp: kp,
107            ang_kd: kd,
108            axes,
109        };
110
111        #[cfg(feature = "dim3")]
112        return Self {
113            lin_kp: Vector::repeat(kp),
114            lin_kd: Vector::repeat(kd),
115            ang_kp: AngVector::repeat(kp),
116            ang_kd: AngVector::repeat(kd),
117            axes,
118        };
119    }
120
121    /// Calculates the linear correction from positional and velocity errors calculated automatically
122    /// from a rigid-body and the desired positions/velocities.
123    ///
124    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
125    /// the inverse of the simulation step so the returned value is a linear rigid-body velocity
126    /// change.
127    pub fn linear_rigid_body_correction(
128        &self,
129        rb: &RigidBody,
130        target_pos: Point<Real>,
131        target_linvel: Vector<Real>,
132    ) -> Vector<Real> {
133        self.rigid_body_correction(
134            rb,
135            Isometry::from(target_pos),
136            RigidBodyVelocity {
137                linvel: target_linvel,
138                #[allow(clippy::clone_on_copy)]
139                angvel: rb.angvel().clone(),
140            },
141        )
142        .linvel
143    }
144
145    /// Calculates the angular correction from positional and velocity errors calculated automatically
146    /// from a rigid-body and the desired positions/velocities.
147    ///
148    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
149    /// the inverse of the simulation step so the returned value is an angular rigid-body velocity
150    /// change.
151    pub fn angular_rigid_body_correction(
152        &self,
153        rb: &RigidBody,
154        target_rot: Rotation<Real>,
155        target_angvel: AngVector<Real>,
156    ) -> AngVector<Real> {
157        self.rigid_body_correction(
158            rb,
159            Isometry::from_parts(na::one(), target_rot),
160            RigidBodyVelocity {
161                linvel: *rb.linvel(),
162                angvel: target_angvel,
163            },
164        )
165        .angvel
166    }
167
168    /// Calculates the linear and angular  correction from positional and velocity errors calculated
169    /// automatically from a rigid-body and the desired poses/velocities.
170    ///
171    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
172    /// the inverse of the simulation step so the returned value is a rigid-body velocity
173    /// change.
174    pub fn rigid_body_correction(
175        &self,
176        rb: &RigidBody,
177        target_pose: Isometry<Real>,
178        target_vels: RigidBodyVelocity<Real>,
179    ) -> RigidBodyVelocity<Real> {
180        let pose_errors = RigidBodyPosition {
181            position: rb.pos.position,
182            next_position: target_pose,
183        }
184        .pose_errors(rb.local_center_of_mass());
185        let vels_errors = target_vels - rb.vels;
186        self.correction(&pose_errors, &vels_errors.into())
187    }
188
189    /// Mask where each component is 1.0 or 0.0 depending on whether
190    /// the corresponding linear axis is enabled.
191    fn lin_mask(&self) -> Vector<Real> {
192        #[cfg(feature = "dim2")]
193        return Vector::new(
194            self.axes.contains(AxesMask::LIN_X) as u32 as Real,
195            self.axes.contains(AxesMask::LIN_Y) as u32 as Real,
196        );
197        #[cfg(feature = "dim3")]
198        return Vector::new(
199            self.axes.contains(AxesMask::LIN_X) as u32 as Real,
200            self.axes.contains(AxesMask::LIN_Y) as u32 as Real,
201            self.axes.contains(AxesMask::LIN_Z) as u32 as Real,
202        );
203    }
204
205    /// Mask where each component is 1.0 or 0.0 depending on whether
206    /// the corresponding angular axis is enabled.
207    fn ang_mask(&self) -> AngVector<Real> {
208        #[cfg(feature = "dim2")]
209        return self.axes.contains(AxesMask::ANG_Z) as u32 as Real;
210        #[cfg(feature = "dim3")]
211        return Vector::new(
212            self.axes.contains(AxesMask::ANG_X) as u32 as Real,
213            self.axes.contains(AxesMask::ANG_Y) as u32 as Real,
214            self.axes.contains(AxesMask::ANG_Z) as u32 as Real,
215        );
216    }
217
218    /// Calculates the linear and angular correction from the given positional and velocity errors.
219    ///
220    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
221    /// the inverse of the simulation step so the returned value is a rigid-body velocity
222    /// change.
223    pub fn correction(
224        &self,
225        pose_errors: &PdErrors,
226        vel_errors: &PdErrors,
227    ) -> RigidBodyVelocity<Real> {
228        let lin_mask = self.lin_mask();
229        let ang_mask = self.ang_mask();
230
231        RigidBodyVelocity {
232            linvel: (pose_errors.linear.component_mul(&self.lin_kp)
233                + vel_errors.linear.component_mul(&self.lin_kd))
234            .component_mul(&lin_mask),
235            #[cfg(feature = "dim2")]
236            angvel: (pose_errors.angular * self.ang_kp + vel_errors.angular * self.ang_kd)
237                * ang_mask,
238            #[cfg(feature = "dim3")]
239            angvel: (pose_errors.angular.component_mul(&self.ang_kp)
240                + vel_errors.angular.component_mul(&self.ang_kd))
241            .component_mul(&ang_mask),
242        }
243    }
244}
245
246impl PidController {
247    /// Initialized the PDI controller with uniform gain.
248    ///
249    /// The same gain are applied on all axes. To configure per-axes gains, construct
250    /// the [`PidController`] by setting its fields explicitly instead.
251    ///
252    /// Only the axes specified in `axes` will be enabled (but the gain values are set
253    /// on all axes regardless).
254    pub fn new(kp: Real, ki: Real, kd: Real, axes: AxesMask) -> PidController {
255        #[cfg(feature = "dim2")]
256        return Self {
257            pd: PdController::new(kp, kd, axes),
258            lin_integral: na::zero(),
259            ang_integral: na::zero(),
260            lin_ki: Vector::repeat(ki),
261            ang_ki: ki,
262        };
263
264        #[cfg(feature = "dim3")]
265        return Self {
266            pd: PdController::new(kp, kd, axes),
267            lin_integral: na::zero(),
268            ang_integral: na::zero(),
269            lin_ki: Vector::repeat(ki),
270            ang_ki: AngVector::repeat(ki),
271        };
272    }
273
274    /// Set the axes errors and corrections are computed for.
275    ///
276    /// This doesn’t modify any of the gains.
277    pub fn set_axes(&mut self, axes: AxesMask) {
278        self.pd.axes = axes;
279    }
280
281    /// Get the axes errors and corrections are computed for.
282    pub fn axes(&self) -> AxesMask {
283        self.pd.axes
284    }
285
286    /// Resets to zero the accumulated linear and angular errors used by
287    /// the Integral part of the controller.
288    pub fn reset_integrals(&mut self) {
289        self.lin_integral = na::zero();
290        self.ang_integral = na::zero();
291    }
292
293    /// Calculates the linear correction from positional and velocity errors calculated automatically
294    /// from a rigid-body and the desired positions/velocities.
295    ///
296    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
297    /// the inverse of the simulation step so the returned value is a linear rigid-body velocity
298    /// change.
299    ///
300    /// This method is mutable because of the need to update the accumulated positional
301    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
302    /// an immutable API is needed.
303    pub fn linear_rigid_body_correction(
304        &mut self,
305        dt: Real,
306        rb: &RigidBody,
307        target_pos: Point<Real>,
308        target_linvel: Vector<Real>,
309    ) -> Vector<Real> {
310        self.rigid_body_correction(
311            dt,
312            rb,
313            Isometry::from(target_pos),
314            RigidBodyVelocity {
315                linvel: target_linvel,
316                #[allow(clippy::clone_on_copy)]
317                angvel: rb.angvel().clone(),
318            },
319        )
320        .linvel
321    }
322
323    /// Calculates the angular correction from positional and velocity errors calculated automatically
324    /// from a rigid-body and the desired positions/velocities.
325    ///
326    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
327    /// the inverse of the simulation step so the returned value is an angular rigid-body velocity
328    /// change.
329    ///
330    /// This method is mutable because of the need to update the accumulated positional
331    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
332    /// an immutable API is needed.
333    pub fn angular_rigid_body_correction(
334        &mut self,
335        dt: Real,
336        rb: &RigidBody,
337        target_rot: Rotation<Real>,
338        target_angvel: AngVector<Real>,
339    ) -> AngVector<Real> {
340        self.rigid_body_correction(
341            dt,
342            rb,
343            Isometry::from_parts(na::one(), target_rot),
344            RigidBodyVelocity {
345                linvel: *rb.linvel(),
346                #[allow(clippy::clone_on_copy)]
347                angvel: target_angvel.clone(),
348            },
349        )
350        .angvel
351    }
352
353    /// Calculates the linear and angular  correction from positional and velocity errors calculated
354    /// automatically from a rigid-body and the desired poses/velocities.
355    ///
356    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
357    /// the inverse of the simulation step so the returned value is a rigid-body velocity
358    /// change.
359    ///
360    /// This method is mutable because of the need to update the accumulated positional
361    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
362    /// an immutable API is needed.
363    pub fn rigid_body_correction(
364        &mut self,
365        dt: Real,
366        rb: &RigidBody,
367        target_pose: Isometry<Real>,
368        target_vels: RigidBodyVelocity<Real>,
369    ) -> RigidBodyVelocity<Real> {
370        let pose_errors = RigidBodyPosition {
371            position: rb.pos.position,
372            next_position: target_pose,
373        }
374        .pose_errors(rb.local_center_of_mass());
375        let vels_errors = target_vels - rb.vels;
376        self.correction(dt, &pose_errors, &vels_errors.into())
377    }
378
379    /// Calculates the linear and angular correction from the given positional and velocity errors.
380    ///
381    /// The unit of the returned value depends on the gain values. In general, `kd` is proportional to
382    /// the inverse of the simulation step so the returned value is a rigid-body velocity
383    /// change.
384    ///
385    /// This method is mutable because of the need to update the accumulated positional
386    /// errors for the Integral part of this controller. Prefer the [`PdController`] instead if
387    /// an immutable API is needed.
388    pub fn correction(
389        &mut self,
390        dt: Real,
391        pose_errors: &PdErrors,
392        vel_errors: &PdErrors,
393    ) -> RigidBodyVelocity<Real> {
394        self.lin_integral += pose_errors.linear * dt;
395        self.ang_integral += pose_errors.angular * dt;
396
397        let lin_mask = self.pd.lin_mask();
398        let ang_mask = self.pd.ang_mask();
399
400        RigidBodyVelocity {
401            linvel: (pose_errors.linear.component_mul(&self.pd.lin_kp)
402                + vel_errors.linear.component_mul(&self.pd.lin_kd)
403                + self.lin_integral.component_mul(&self.lin_ki))
404            .component_mul(&lin_mask),
405            #[cfg(feature = "dim2")]
406            angvel: (pose_errors.angular * self.pd.ang_kp
407                + vel_errors.angular * self.pd.ang_kd
408                + self.ang_integral * self.ang_ki)
409                * ang_mask,
410            #[cfg(feature = "dim3")]
411            angvel: (pose_errors.angular.component_mul(&self.pd.ang_kp)
412                + vel_errors.angular.component_mul(&self.pd.ang_kd)
413                + self.ang_integral.component_mul(&self.ang_ki))
414            .component_mul(&ang_mask),
415        }
416    }
417}
rapier2d/control/pid_controller.rs

rapier2d/control/
pid_controller.rs
