feat: add PD and PID controller implementations (#804)

* feat: add a PID controller implementation

* feat: add small rigid-body utilities + test interpolation test

* fix: make scrolling weaker on macos

* feat: add the option to use the PID controller in the character controller demo.

* feat: add a stateless PD controller

* feat(rapier_testbed): cleanup & support PidController in 2D too

* chore: add comments for the PD and PID controllers

* chore: update changelog

* feat: rename PidErrors to PdErrors which is more accurate

* fix cargo doc

* chore: remove dead code

* chore: make test module non-pub

examples2d/all_examples2.rs

@@ -43,6 +43,8 @@ mod s2d_pyramid;
 mod sensor2;
 mod trimesh2;
 
+mod utils;
+
 #[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))]
 pub fn main() {
     let mut builders: Vec<(_, fn(&mut Testbed))> = vec![

examples2d/character_controller2.rs

@@ -1,3 +1,6 @@
+use crate::utils::character;
+use crate::utils::character::CharacterControlMode;
+use rapier2d::control::{KinematicCharacterController, PidController};
 use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
 use std::f32::consts::PI;
@@ -25,7 +28,12 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Character we will control manually.
      */
-    let rigid_body = RigidBodyBuilder::kinematic_position_based().translation(vector![-3.0, 5.0]);
+    let rigid_body = RigidBodyBuilder::kinematic_position_based()
+        .translation(vector![-3.0, 5.0])
+        // The two config below makes the character
+        // nicer to control with the PID control enabled.
+        .gravity_scale(10.0)
+        .soft_ccd_prediction(10.0);
     let character_handle = bodies.insert(rigid_body);
     let collider = ColliderBuilder::capsule_y(0.3, 0.15);
     colliders.insert_with_parent(collider, character_handle, &mut bodies);
@@ -110,7 +118,7 @@ pub fn init_world(testbed: &mut Testbed) {
     /*
      * Create a moving platform.
      */
-    let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5]);
+    let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 0.0]);
     // .rotation(-0.3);
     let platform_handle = bodies.insert(body);
     let collider = ColliderBuilder::cuboid(2.0, ground_height);
@@ -160,10 +168,34 @@ pub fn init_world(testbed: &mut Testbed) {
         }
     });
 
+    /*
+     * Callback to update the character based on user inputs.
+     */
+    let mut control_mode = CharacterControlMode::Kinematic;
+    let mut controller = KinematicCharacterController {
+        max_slope_climb_angle: impossible_slope_angle - 0.02,
+        min_slope_slide_angle: impossible_slope_angle - 0.02,
+        slide: true,
+        ..Default::default()
+    };
+    let mut pid = PidController::default();
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        if let Some(graphics) = graphics {
+            character::update_character(
+                graphics,
+                physics,
+                &mut control_mode,
+                &mut controller,
+                &mut pid,
+                character_handle,
+            );
+        }
+    });
+
     /*
      * Set up the testbed.
      */
     testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
-    testbed.set_character_body(character_handle);
     testbed.look_at(point![0.0, 1.0], 100.0);
 }

examples2d/rope_joints2.rs

@@ -1,3 +1,6 @@
+use crate::utils::character;
+use crate::utils::character::CharacterControlMode;
+use rapier2d::control::{KinematicCharacterController, PidController};
 use rapier2d::prelude::*;
 use rapier_testbed2d::Testbed;
 
@@ -54,10 +57,29 @@ pub fn init_world(testbed: &mut Testbed) {
     let joint = RopeJointBuilder::new(2.0).local_anchor2(point![0.0, 0.0]);
     impulse_joints.insert(character_handle, child_handle, joint, true);
 
+    /*
+     * Callback to update the character based on user inputs.
+     */
+    let mut control_mode = CharacterControlMode::Kinematic;
+    let mut controller = KinematicCharacterController::default();
+    let mut pid = PidController::default();
+
+    testbed.add_callback(move |graphics, physics, _, _| {
+        if let Some(graphics) = graphics {
+            character::update_character(
+                graphics,
+                physics,
+                &mut control_mode,
+                &mut controller,
+                &mut pid,
+                character_handle,
+            );
+        }
+    });
+
     /*
      * Set up the testbed.
      */
     testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
-    testbed.set_character_body(character_handle);
     testbed.look_at(point![0.0, 1.0], 100.0);
 }

examples2d/utils/character.rs

@@ -0,0 +1,250 @@
+use rapier2d::{
+    control::{CharacterLength, KinematicCharacterController, PidController},
+    prelude::*,
+};
+use rapier_testbed2d::ui::egui::Align2;
+use rapier_testbed2d::{
+    ui::egui::{ComboBox, Slider, Ui, Window},
+    KeyCode, PhysicsState, TestbedGraphics,
+};
+
+#[derive(PartialEq, Eq, Clone, Copy, Debug)]
+pub enum CharacterControlMode {
+    Kinematic,
+    Pid,
+}
+
+pub fn update_character(
+    graphics: &mut TestbedGraphics,
+    physics: &mut PhysicsState,
+    control_mode: &mut CharacterControlMode,
+    controller: &mut KinematicCharacterController,
+    pid: &mut PidController,
+    character_handle: RigidBodyHandle,
+) {
+    let prev_control_mode = *control_mode;
+    character_control_ui(graphics, controller, pid, control_mode);
+
+    if *control_mode != prev_control_mode {
+        match control_mode {
+            CharacterControlMode::Kinematic => physics.bodies[character_handle]
+                .set_body_type(RigidBodyType::KinematicPositionBased, false),
+            CharacterControlMode::Pid => {
+                physics.bodies[character_handle].set_body_type(RigidBodyType::Dynamic, true)
+            }
+        }
+    }
+
+    match *control_mode {
+        CharacterControlMode::Kinematic => {
+            update_kinematic_controller(graphics, physics, character_handle, controller)
+        }
+        CharacterControlMode::Pid => {
+            update_pid_controller(graphics, physics, character_handle, pid)
+        }
+    }
+}
+
+fn character_movement_from_inputs(
+    gfx: &TestbedGraphics,
+    mut speed: Real,
+    artificial_gravity: bool,
+) -> Vector<Real> {
+    let mut desired_movement = Vector::zeros();
+
+    for key in gfx.keys().get_pressed() {
+        match *key {
+            KeyCode::ArrowRight => {
+                desired_movement += Vector::x();
+            }
+            KeyCode::ArrowLeft => {
+                desired_movement -= Vector::x();
+            }
+            KeyCode::Space => {
+                desired_movement += Vector::y() * 2.0;
+            }
+            KeyCode::ControlRight => {
+                desired_movement -= Vector::y();
+            }
+            KeyCode::ShiftRight => {
+                speed /= 10.0;
+            }
+            _ => {}
+        }
+    }
+
+    desired_movement *= speed;
+
+    if artificial_gravity {
+        desired_movement -= Vector::y() * speed;
+    }
+
+    desired_movement
+}
+
+fn update_pid_controller(
+    gfx: &mut TestbedGraphics,
+    phx: &mut PhysicsState,
+    character_handle: RigidBodyHandle,
+    pid: &mut PidController,
+) {
+    let desired_movement = character_movement_from_inputs(gfx, 0.1, false);
+    let character_body = &mut phx.bodies[character_handle];
+
+    // Adjust the controlled axis depending on the keys pressed by the user.
+    // - If the user is jumping, enable control over Y.
+    // - If the user isn’t pressing any key, disable all linear controls to let
+    //   gravity/collision do their thing freely.
+    let mut axes = AxisMask::ANG_Z;
+
+    if desired_movement.norm() != 0.0 {
+        axes |= if desired_movement.y == 0.0 {
+            AxisMask::LIN_X
+        } else {
+            AxisMask::LIN_X | AxisMask::LIN_Y
+        }
+    };
+
+    pid.set_axes(axes);
+
+    let corrective_vel = pid.rigid_body_correction(
+        phx.integration_parameters.dt,
+        character_body,
+        (character_body.translation() + desired_movement).into(),
+        RigidBodyVelocity::zero(),
+    );
+    let new_vel = *character_body.vels() + corrective_vel;
+
+    character_body.set_vels(new_vel, true);
+}
+
+fn update_kinematic_controller(
+    gfx: &mut TestbedGraphics,
+    phx: &mut PhysicsState,
+    character_handle: RigidBodyHandle,
+    controller: &KinematicCharacterController,
+) {
+    let speed = 0.1;
+    let desired_movement = character_movement_from_inputs(gfx, speed, true);
+
+    let character_body = &phx.bodies[character_handle];
+    let character_collider = &phx.colliders[character_body.colliders()[0]];
+    let character_mass = character_body.mass();
+
+    let mut collisions = vec![];
+    let mvt = controller.move_shape(
+        phx.integration_parameters.dt,
+        &phx.bodies,
+        &phx.colliders,
+        &phx.query_pipeline,
+        character_collider.shape(),
+        character_collider.position(),
+        desired_movement.cast::<Real>(),
+        QueryFilter::new().exclude_rigid_body(character_handle),
+        |c| collisions.push(c),
+    );
+
+    if mvt.grounded {
+        gfx.set_body_color(character_handle, [0.1, 0.8, 0.1]);
+    } else {
+        gfx.set_body_color(character_handle, [0.8, 0.1, 0.1]);
+    }
+
+    controller.solve_character_collision_impulses(
+        phx.integration_parameters.dt,
+        &mut phx.bodies,
+        &phx.colliders,
+        &phx.query_pipeline,
+        character_collider.shape(),
+        character_mass,
+        &*collisions,
+        QueryFilter::new().exclude_rigid_body(character_handle),
+    );
+
+    let character_body = &mut phx.bodies[character_handle];
+    let pose = character_body.position();
+    character_body.set_next_kinematic_translation(pose.translation.vector + mvt.translation);
+}
+
+fn character_control_ui(
+    gfx: &mut TestbedGraphics,
+    character_controller: &mut KinematicCharacterController,
+    pid_controller: &mut PidController,
+    control_mode: &mut CharacterControlMode,
+) {
+    Window::new("Character Control")
+        .anchor(Align2::RIGHT_TOP, [-15.0, 15.0])
+        .show(gfx.ui_context_mut().ctx_mut(), |ui| {
+            ComboBox::from_label("control mode")
+                .selected_text(format!("{:?}", *control_mode))
+                .show_ui(ui, |ui| {
+                    ui.selectable_value(control_mode, CharacterControlMode::Kinematic, "Kinematic");
+                    ui.selectable_value(control_mode, CharacterControlMode::Pid, "Pid");
+                });
+
+            match control_mode {
+                CharacterControlMode::Kinematic => {
+                    kinematic_control_ui(ui, character_controller);
+                }
+                CharacterControlMode::Pid => {
+                    pid_control_ui(ui, pid_controller);
+                }
+            }
+        });
+}
+
+fn pid_control_ui(ui: &mut Ui, pid_controller: &mut PidController) {
+    let mut lin_kp = pid_controller.pd.lin_kp.x;
+    let mut lin_ki = pid_controller.lin_ki.x;
+    let mut lin_kd = pid_controller.pd.lin_kd.x;
+    let mut ang_kp = pid_controller.pd.ang_kp;
+    let mut ang_ki = pid_controller.ang_ki;
+    let mut ang_kd = pid_controller.pd.ang_kd;
+
+    ui.add(Slider::new(&mut lin_kp, 0.0..=100.0).text("linear Kp"));
+    ui.add(Slider::new(&mut lin_ki, 0.0..=10.0).text("linear Ki"));
+    ui.add(Slider::new(&mut lin_kd, 0.0..=1.0).text("linear Kd"));
+    ui.add(Slider::new(&mut ang_kp, 0.0..=100.0).text("angular Kp"));
+    ui.add(Slider::new(&mut ang_ki, 0.0..=10.0).text("angular Ki"));
+    ui.add(Slider::new(&mut ang_kd, 0.0..=1.0).text("angular Kd"));
+
+    pid_controller.pd.lin_kp.fill(lin_kp);
+    pid_controller.lin_ki.fill(lin_ki);
+    pid_controller.pd.lin_kd.fill(lin_kd);
+    pid_controller.pd.ang_kp = ang_kp;
+    pid_controller.ang_ki = ang_ki;
+    pid_controller.pd.ang_kd = ang_kd;
+}
+
+fn kinematic_control_ui(ui: &mut Ui, character_controller: &mut KinematicCharacterController) {
+    ui.checkbox(&mut character_controller.slide, "slide")
+        .on_hover_text("Should the character try to slide against the floor if it hits it?");
+    #[allow(clippy::useless_conversion)]
+    {
+        ui.add(Slider::new(&mut character_controller.max_slope_climb_angle, 0.0..=std::f32::consts::TAU.into()).text("max_slope_climb_angle"))
+            .on_hover_text("The maximum angle (radians) between the floor’s normal and the `up` vector that the character is able to climb.");
+        ui.add(Slider::new(&mut character_controller.min_slope_slide_angle, 0.0..=std::f32::consts::FRAC_PI_2.into()).text("min_slope_slide_angle"))
+            .on_hover_text("The minimum angle (radians) between the floor’s normal and the `up` vector before the character starts to slide down automatically.");
+    }
+    let mut is_snapped = character_controller.snap_to_ground.is_some();
+    if ui.checkbox(&mut is_snapped, "snap_to_ground").changed {
+        match is_snapped {
+            true => {
+                character_controller.snap_to_ground = Some(CharacterLength::Relative(0.1));
+            }
+            false => {
+                character_controller.snap_to_ground = None;
+            }
+        }
+    }
+    if let Some(snapped) = &mut character_controller.snap_to_ground {
+        match snapped {
+            CharacterLength::Relative(val) => {
+                ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Relative Character Length"));
+            }
+            CharacterLength::Absolute(val) => {
+                ui.add(Slider::new(val, 0.0..=10.0).text("Snapped Absolute Character Length"));
+            }
+        }
+    }
+}

examples2d/utils/mod.rs

@@ -0,0 +1 @@
+pub mod character;