Skip to content

Commit b0903e9

Browse files
authored
feat: add PID controller implementations (#311)
* feat: update to rapier 0.23 * feat: add bindings and examples for rapier’s new PidController * chore: switch to rapier 0.23.1 * chore: cargo fmt
1 parent a49ecf0 commit b0903e9

18 files changed

+789
-23
lines changed

CHANGELOG.md

+8-4
Original file line numberDiff line numberDiff line change
@@ -1,16 +1,20 @@
11
### Unreleased
22

3-
### Modified
3+
#### Added
4+
5+
- Added `PidController`, `World.createPidController`, `World.removePidController` to create a Position-Integral-Derivative
6+
controller; a building block for building velocity-based dynamic character controllers.
7+
8+
#### Modified
49

10+
- Update to Rapier 0.24.
511
- Package tree shaking has been disabled to avoid a crash on certain build configuration (#289).
612

713
### 0.14.0 (20 July 2024)
814

915
#### Modified
1016

11-
- Update from the rust library of rapier 0.19 to 0.22,
12-
see [rapier's changelog](https://github.com/dimforge/rapier/blob/master/CHANGELOG.md#v0210-23-june-2024) for more
13-
context.
17+
- Update from the rust library of rapier 0.19 to 0.22, see [rapier's changelog](https://github.com/dimforge/rapier/blob/master/CHANGELOG.md#v0210-23-june-2024) for more context.
1418

1519
#### Added
1620

Cargo.toml

+1
Original file line numberDiff line numberDiff line change
@@ -5,6 +5,7 @@ members = ["rapier2d", "rapier3d"]
55
debug = false
66
codegen-units = 1
77
#lto = true
8+
strip = true # Workaround for https://github.com/bevyengine/bevy/issues/16030 (the bug only happens in 2D)
89

910
[patch.crates-io]
1011
#rapier2d = { path = "../rapier/crates/rapier2d" }

rapier2d/Cargo.toml

+2-6
Original file line numberDiff line numberDiff line change
@@ -27,22 +27,18 @@ rust.unexpected_cfgs = { level = "warn", check-cfg = [
2727
] }
2828

2929
[dependencies]
30-
rapier2d = { version = "0.22", features = [
31-
"wasm-bindgen",
30+
rapier2d = { version = "0.23.1", features = [
3231
"serde-serialize",
3332
"enhanced-determinism",
3433
"debug-render",
3534
] }
3635
ref-cast = "1"
37-
wasm-bindgen = "0.2.90"
36+
wasm-bindgen = "0.2.100"
3837
js-sys = "0.3"
3938
nalgebra = "0.33"
4039
serde = { version = "1", features = ["derive", "rc"] }
4140
bincode = "1"
42-
crossbeam-channel = "0.5"
4341
palette = "0.7"
44-
libm = "0.2"
45-
parry2d = "0.17" # Use at least this version to fix a regression in 0.15.0
4642

4743
[package.metadata.wasm-pack.profile.release]
4844
# add -g to keep debug symbols

rapier3d/Cargo.toml

+2-5
Original file line numberDiff line numberDiff line change
@@ -27,21 +27,18 @@ rust.unexpected_cfgs = { level = "warn", check-cfg = [
2727
] }
2828

2929
[dependencies]
30-
rapier3d = { version = "0.22.0", features = [
31-
"wasm-bindgen",
30+
rapier3d = { version = "0.23.1", features = [
3231
"serde-serialize",
3332
"enhanced-determinism",
3433
"debug-render",
3534
] }
3635
ref-cast = "1"
37-
wasm-bindgen = "^0.2.90"
36+
wasm-bindgen = "0.2.100"
3837
js-sys = "0.3"
3938
nalgebra = "0.33"
4039
serde = { version = "1", features = ["derive", "rc"] }
4140
bincode = "1"
42-
crossbeam-channel = "0.5"
4341
palette = "0.7"
44-
parry2d = "0.17" # Use at least this version to fix a regression in 0.15.0
4542

4643
[package.metadata.wasm-pack.profile.release]
4744
# add -g to keep debug symbols

src.ts/control/index.ts

+1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,5 @@
11
export * from "./character_controller";
2+
export * from "./pid_controller";
23

34
// #if DIM3
45
export * from "./ray_cast_vehicle_controller";

src.ts/control/pid_controller.ts

+207
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,207 @@
1+
import {RawPidController} from "../raw";
2+
import {Rotation, RotationOps, Vector, VectorOps} from "../math";
3+
import {Collider, ColliderSet, InteractionGroups, Shape} from "../geometry";
4+
import {QueryFilterFlags, QueryPipeline, World} from "../pipeline";
5+
import {IntegrationParameters, RigidBody, RigidBodySet} from "../dynamics";
6+
7+
// TODO: unify with the JointAxesMask
8+
/**
9+
* An enum representing the possible joint axes controlled by a PidController.
10+
* They can be ORed together, like:
11+
* PidAxesMask.LinX || PidAxesMask.LinY
12+
* to get a pid controller that only constraints the translational X and Y axes.
13+
*
14+
* Possible axes are:
15+
*
16+
* - `X`: X translation axis
17+
* - `Y`: Y translation axis
18+
* - `Z`: Z translation axis
19+
* - `AngX`: X angular rotation axis (3D only)
20+
* - `AngY`: Y angular rotation axis (3D only)
21+
* - `AngZ`: Z angular rotation axis
22+
*/
23+
export enum PidAxesMask {
24+
None = 0,
25+
LinX = 1 << 0,
26+
LinY = 1 << 1,
27+
LinZ = 1 << 2,
28+
// #if DIM3
29+
AngX = 1 << 3,
30+
AngY = 1 << 4,
31+
// #endif
32+
AngZ = 1 << 5,
33+
// #if DIM2
34+
AllLin = PidAxesMask.LinX | PidAxesMask.LinY,
35+
AllAng = PidAxesMask.AngZ,
36+
// #endif
37+
// #if DIM3
38+
AllLin = PidAxesMask.LinX | PidAxesMask.LinY | PidAxesMask.LinZ,
39+
AllAng = PidAxesMask.AngX | PidAxesMask.AngY | PidAxesMask.AngZ,
40+
// #endif
41+
All = PidAxesMask.AllLin | PidAxesMask.AllAng,
42+
}
43+
44+
/**
45+
* A controller for controlling dynamic bodies using the
46+
* Proportional-Integral-Derivative correction model.
47+
*/
48+
export class PidController {
49+
private raw: RawPidController;
50+
51+
private params: IntegrationParameters;
52+
private bodies: RigidBodySet;
53+
54+
constructor(
55+
params: IntegrationParameters,
56+
bodies: RigidBodySet,
57+
kp: number,
58+
ki: number,
59+
kd: number,
60+
axes: PidAxesMask,
61+
) {
62+
this.params = params;
63+
this.bodies = bodies;
64+
this.raw = new RawPidController(kp, ki, kd, axes);
65+
}
66+
67+
/** @internal */
68+
public free() {
69+
if (!!this.raw) {
70+
this.raw.free();
71+
}
72+
73+
this.raw = undefined;
74+
}
75+
76+
public setKp(kp: number, axes: PidAxesMask) {
77+
this.raw.set_kp(kp, axes);
78+
}
79+
80+
public setKi(ki: number, axes: PidAxesMask) {
81+
this.raw.set_kp(ki, axes);
82+
}
83+
84+
public setKd(kd: number, axes: PidAxesMask) {
85+
this.raw.set_kp(kd, axes);
86+
}
87+
88+
public setAxes(axes: PidAxesMask) {
89+
this.raw.set_axes_mask(axes);
90+
}
91+
92+
public resetIntegrals() {
93+
this.raw.reset_integrals();
94+
}
95+
96+
public applyLinearCorrection(
97+
body: RigidBody,
98+
targetPosition: Vector,
99+
targetLinvel: Vector,
100+
) {
101+
let rawPos = VectorOps.intoRaw(targetPosition);
102+
let rawVel = VectorOps.intoRaw(targetLinvel);
103+
this.raw.apply_linear_correction(
104+
this.params.dt,
105+
this.bodies.raw,
106+
body.handle,
107+
rawPos,
108+
rawVel,
109+
);
110+
rawPos.free();
111+
rawVel.free();
112+
}
113+
114+
// #if DIM2
115+
public applyAngularCorrection(
116+
body: RigidBody,
117+
targetRotation: number,
118+
targetAngVel: number,
119+
) {
120+
this.raw.apply_angular_correction(
121+
this.params.dt,
122+
this.bodies.raw,
123+
body.handle,
124+
targetRotation,
125+
targetAngVel,
126+
);
127+
}
128+
// #endif
129+
130+
// #if DIM3
131+
public applyAngularCorrection(
132+
body: RigidBody,
133+
targetRotation: Rotation,
134+
targetAngVel: Vector,
135+
) {
136+
let rawPos = RotationOps.intoRaw(targetRotation);
137+
let rawVel = VectorOps.intoRaw(targetAngVel);
138+
this.raw.apply_angular_correction(
139+
this.params.dt,
140+
this.bodies.raw,
141+
body.handle,
142+
rawPos,
143+
rawVel,
144+
);
145+
rawPos.free();
146+
rawVel.free();
147+
}
148+
// #endif
149+
150+
public linearCorrection(
151+
body: RigidBody,
152+
targetPosition: Vector,
153+
targetLinvel: Vector,
154+
): Vector {
155+
let rawPos = VectorOps.intoRaw(targetPosition);
156+
let rawVel = VectorOps.intoRaw(targetLinvel);
157+
let correction = this.raw.linear_correction(
158+
this.params.dt,
159+
this.bodies.raw,
160+
body.handle,
161+
rawPos,
162+
rawVel,
163+
);
164+
rawPos.free();
165+
rawVel.free();
166+
167+
return VectorOps.fromRaw(correction);
168+
}
169+
170+
// #if DIM2
171+
public angularCorrection(
172+
body: RigidBody,
173+
targetRotation: number,
174+
targetAngVel: number,
175+
): number {
176+
return this.raw.angular_correction(
177+
this.params.dt,
178+
this.bodies.raw,
179+
body.handle,
180+
targetRotation,
181+
targetAngVel,
182+
);
183+
}
184+
// #endif
185+
186+
// #if DIM3
187+
public angularCorrection(
188+
body: RigidBody,
189+
targetRotation: Rotation,
190+
targetAngVel: Vector,
191+
): Vector {
192+
let rawPos = RotationOps.intoRaw(targetRotation);
193+
let rawVel = VectorOps.intoRaw(targetAngVel);
194+
let correction = this.raw.angular_correction(
195+
this.params.dt,
196+
this.bodies.raw,
197+
body.handle,
198+
rawPos,
199+
rawVel,
200+
);
201+
rawPos.free();
202+
rawVel.free();
203+
204+
return VectorOps.fromRaw(correction);
205+
}
206+
// #endif
207+
}

src.ts/pipeline/world.ts

+52-1
Original file line numberDiff line numberDiff line change
@@ -54,7 +54,11 @@ import {SerializationPipeline} from "./serialization_pipeline";
5454
import {EventQueue} from "./event_queue";
5555
import {PhysicsHooks} from "./physics_hooks";
5656
import {DebugRenderBuffers, DebugRenderPipeline} from "./debug_render_pipeline";
57-
import {KinematicCharacterController} from "../control";
57+
import {
58+
KinematicCharacterController,
59+
PidAxesMask,
60+
PidController,
61+
} from "../control";
5862
import {Coarena} from "../coarena";
5963

6064
// #if DIM3
@@ -84,6 +88,7 @@ export class World {
8488
serializationPipeline: SerializationPipeline;
8589
debugRenderPipeline: DebugRenderPipeline;
8690
characterControllers: Set<KinematicCharacterController>;
91+
pidControllers: Set<PidController>;
8792

8893
// #if DIM3
8994
vehicleControllers: Set<DynamicRayCastVehicleController>;
@@ -111,6 +116,7 @@ export class World {
111116
this.serializationPipeline.free();
112117
this.debugRenderPipeline.free();
113118
this.characterControllers.forEach((controller) => controller.free());
119+
this.pidControllers.forEach((controller) => controller.free());
114120

115121
// #if DIM3
116122
this.vehicleControllers.forEach((controller) => controller.free());
@@ -130,6 +136,7 @@ export class World {
130136
this.serializationPipeline = undefined;
131137
this.debugRenderPipeline = undefined;
132138
this.characterControllers = undefined;
139+
this.pidControllers = undefined;
133140

134141
// #if DIM3
135142
this.vehicleControllers = undefined;
@@ -173,6 +180,7 @@ export class World {
173180
rawDebugRenderPipeline,
174181
);
175182
this.characterControllers = new Set<KinematicCharacterController>();
183+
this.pidControllers = new Set<PidController>();
176184

177185
// #if DIM3
178186
this.vehicleControllers = new Set<DynamicRayCastVehicleController>();
@@ -482,6 +490,49 @@ export class World {
482490
controller.free();
483491
}
484492

493+
/**
494+
* Creates a new PID (Proportional-Integral-Derivative) controller.
495+
*
496+
* @param kp - The Proportional gain applied to the instantaneous linear position errors.
497+
* This is usually set to a multiple of the inverse of simulation step time
498+
* (e.g. `60` if the delta-time is `1.0 / 60.0`).
499+
* @param ki - The linear gain applied to the Integral part of the PID controller.
500+
* @param kd - The Derivative gain applied to the instantaneous linear velocity errors.
501+
* This is usually set to a value in `[0.0, 1.0]` where `0.0` implies no damping
502+
* (no correction of velocity errors) and `1.0` implies complete damping (velocity errors
503+
* are corrected in a single simulation step).
504+
* @param axes - The axes affected by this controller.
505+
* Only coordinate axes with a bit flags set to `true` will be taken into
506+
* account when calculating the errors and corrections.
507+
*/
508+
public createPidController(
509+
kp: number,
510+
ki: number,
511+
kd: number,
512+
axes: PidAxesMask,
513+
): PidController {
514+
let controller = new PidController(
515+
this.integrationParameters,
516+
this.bodies,
517+
kp,
518+
ki,
519+
kd,
520+
axes,
521+
);
522+
this.pidControllers.add(controller);
523+
return controller;
524+
}
525+
526+
/**
527+
* Removes a PID controller from this world.
528+
*
529+
* @param controller - The PID controller to remove.
530+
*/
531+
public removePidController(controller: PidController) {
532+
this.pidControllers.delete(controller);
533+
controller.free();
534+
}
535+
485536
// #if DIM3
486537
/**
487538
* Creates a new vehicle controller.

0 commit comments

Comments
 (0)