nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add simple inverse-kinematics solver for multibodies (#632)

Browse Source 
* feat: add a simple jacobian-based inverse-kinematics implementation for multibodies

* feat: add 2d inverse kinematics example

* feat: make forward_kinematics auto-fix the root’s degrees of freedom

* feat: add 3d inverse kinematics example

* chore: update changelog

* chore: clippy fixes

* chore: more clippy fixes

* fix tests
This commit is contained in:
Sébastien Crozet
2024-05-25 10:36:34 +02:00
committed by GitHub
parent af1ac9baa2
commit 62379de9ec
22 changed files with 755 additions and 212 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
CHANGELOG.md
View File

@@ -1,3 +1,26 @@
## Unreleased
### Added
- Add `Multibody::inverse_kinematics`, `Multibody::inverse_kinematics_delta`,
and `::inverse_kinematics_delta_with_jacobian`
for running inverse kinematics on a multibody to align one its links pose to the given prescribed pose.
- Add `InverseKinematicsOption` to customize some behaviors of the inverse-kinematics solver.
- Add `Multibody::body_jacobian` to get the jacobian of a specific link.
- Add `Multibody::update_rigid_bodies` to update rigid-bodies based on the multibody links poses.
- Add `Multibody::forward_kinematics_single_link` to run forward-kinematics to compute the new pose and jacobian of a
single link without mutating the multibody. This can take an optional displacement on generalized coordinates that are
taken into account during transform propagation.
### Modified
- The `Multibody::forward_kinematics` method will no longer automatically update the poses of the `RigidBody` associated
to each joint. Instead `Multibody::update_rigid_bodies` has to be called explicitly.
- The `Multibody::forward_kinematics` method will automatically adjust the multibody’s degrees of freedom if the root
rigid-body changed type (between dynamic and non-dynamic). It can also optionally apply the root’s rigid-body pose
instead of the root link’s pose (useful for example if you modified the root rigid-body pose externally and wanted
to propagate it to the multibody).
## v0.19.0 (05 May 2024)
### Fix

2
crates/rapier_testbed2d-f64/Cargo.toml
View File

@@ -28,7 +28,7 @@ other-backends = ["wrapped2d"]
features = ["parallel", "other-backends"]
[dependencies]
nalgebra = { version = "0.32", features = ["rand"] }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }

2
crates/rapier_testbed2d/Cargo.toml
View File

@@ -28,7 +28,7 @@ other-backends = ["wrapped2d"]
features = ["parallel", "other-backends"]
[dependencies]
nalgebra = { version = "0.32", features = ["rand"] }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }

2
crates/rapier_testbed3d-f64/Cargo.toml
View File

@@ -27,7 +27,7 @@ parallel = ["rapier/parallel", "num_cpus"]
features = ["parallel"]
[dependencies]
nalgebra = { version = "0.32", features = ["rand"] }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }

2
crates/rapier_testbed3d/Cargo.toml
View File

@@ -28,7 +28,7 @@ other-backends = ["physx", "physx-sys", "glam"]
features = ["parallel", "other-backends"]
[dependencies]
nalgebra = { version = "0.32", features = ["rand"] }
nalgebra = { version = "0.32", features = ["rand", "glam025"] }
rand = "0.8"
rand_pcg = "0.3"
instant = { version = "0.1", features = ["web-sys", "now"] }

2
examples2d/all_examples2.rs
View File

@@ -20,6 +20,7 @@ mod debug_total_overlap2;
mod debug_vertical_column2;
mod drum2;
mod heightfield2;
mod inverse_kinematics2;
mod joint_motor_position2;
mod joints2;
mod locked_rotations2;
@@ -84,6 +85,7 @@ pub fn main() {
("Damping", damping2::init_world),
("Drum", drum2::init_world),
("Heightfield", heightfield2::init_world),
("Inverse kinematics", inverse_kinematics2::init_world),
("Joints", joints2::init_world),
("Locked rotations", locked_rotations2::init_world),
("One-way platforms", one_way_platforms2::init_world),

96
examples2d/inverse_kinematics2.rs Normal file
View File

@@ -0,0 +1,96 @@
use rapier2d::prelude::*;
use rapier_testbed2d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground_size = 1.0;
let ground_height = 0.01;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height]);
let floor_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height);
colliders.insert_with_parent(collider, floor_handle, &mut bodies);
/*
* Setup groups
*/
let num_segments = 10;
let body = RigidBodyBuilder::fixed();
let mut last_body = bodies.insert(body);
let mut last_link = MultibodyJointHandle::invalid();
for i in 0..num_segments {
let size = 1.0 / num_segments as f32;
let body = RigidBodyBuilder::dynamic().can_sleep(false);
let new_body = bodies.insert(body);
// NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
// the demo look nicer. IK could be used without cuboid.
let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0)
.density(0.0)
.sensor(true);
colliders.insert_with_parent(collider, new_body, &mut bodies);
let link_ab = RevoluteJointBuilder::new()
.local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32])
.local_anchor2(point![0.0, -size / 2.0])
.build()
.data;
last_link = multibody_joints
.insert(last_body, new_body, link_ab, true)
.unwrap();
last_body = new_body;
}
let mut displacements = DVector::zeros(0);
testbed.add_callback(move |graphics, physics, _, _| {
let Some(graphics) = graphics else { return };
if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
// Ensure our displacement vector has the right number of elements.
if displacements.nrows() < multibody.ndofs() {
displacements = DVector::zeros(multibody.ndofs());
} else {
displacements.fill(0.0);
}
let Some(mouse_point) = graphics.mouse().point else {
return;
};
// We will have the endpoint track the mouse position.
let target_point = mouse_point.coords;
let options = InverseKinematicsOption {
constrained_axes: JointAxesMask::LIN_AXES,
..Default::default()
};
multibody.inverse_kinematics(
&physics.bodies,
link_id,
&options,
&Isometry::from(target_point),
&mut displacements,
);
multibody.apply_displacements(displacements.as_slice());
}
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 0.0], 300.0);
}

2
examples3d/all_examples3.rs
View File

@@ -42,6 +42,7 @@ mod debug_cube_high_mass_ratio3;
mod debug_internal_edges3;
mod debug_long_chain3;
mod debug_multibody_ang_motor_pos3;
mod inverse_kinematics3;
mod joint_motor_position3;
mod keva3;
mod locked_rotations3;
@@ -108,6 +109,7 @@ pub fn main() {
("Dynamic trimeshes", dynamic_trimesh3::init_world),
("Heightfield", heightfield3::init_world),
("Impulse Joints", joints3::init_world_with_joints),
("Inverse kinematics", inverse_kinematics3::init_world),
("Joint Motor Position", joint_motor_position3::init_world),
("Locked rotations", locked_rotations3::init_world),
("One-way platforms", one_way_platforms3::init_world),

103
examples3d/inverse_kinematics3.rs Normal file
View File

@@ -0,0 +1,103 @@
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
pub fn init_world(testbed: &mut Testbed) {
/*
* World
*/
let mut bodies = RigidBodySet::new();
let mut colliders = ColliderSet::new();
let impulse_joints = ImpulseJointSet::new();
let mut multibody_joints = MultibodyJointSet::new();
/*
* Ground
*/
let ground_size = 0.2;
let ground_height = 0.01;
let rigid_body = RigidBodyBuilder::fixed().translation(vector![0.0, -ground_height, 0.0]);
let floor_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, floor_handle, &mut bodies);
/*
* Setup groups
*/
let num_segments = 10;
let body = RigidBodyBuilder::fixed();
let mut last_body = bodies.insert(body);
let mut last_link = MultibodyJointHandle::invalid();
for i in 0..num_segments {
let size = 1.0 / num_segments as f32;
let body = RigidBodyBuilder::dynamic().can_sleep(false);
let new_body = bodies.insert(body);
// NOTE: we add a sensor collider just to make the destbed draw a rectangle to make
// the demo look nicer. IK could be used without cuboid.
let collider = ColliderBuilder::cuboid(size / 8.0, size / 2.0, size / 8.0)
.density(0.0)
.sensor(true);
colliders.insert_with_parent(collider, new_body, &mut bodies);
let link_ab = SphericalJointBuilder::new()
.local_anchor1(point![0.0, size / 2.0 * (i != 0) as usize as f32, 0.0])
.local_anchor2(point![0.0, -size / 2.0, 0.0])
.build()
.data;
last_link = multibody_joints
.insert(last_body, new_body, link_ab, true)
.unwrap();
last_body = new_body;
}
let mut displacements = DVector::zeros(0);
testbed.add_callback(move |graphics, physics, _, _| {
let Some(graphics) = graphics else { return };
if let Some((multibody, link_id)) = physics.multibody_joints.get_mut(last_link) {
// Ensure our displacement vector has the right number of elements.
if displacements.nrows() < multibody.ndofs() {
displacements = DVector::zeros(multibody.ndofs());
} else {
displacements.fill(0.0);
}
let Some(mouse_ray) = graphics.mouse().ray else {
return;
};
// Cast a ray on a plane aligned with the camera passing through the origin.
let halfspace = HalfSpace {
normal: -UnitVector::new_normalize(graphics.camera_fwd_dir()),
};
let mouse_ray = Ray::new(mouse_ray.0, mouse_ray.1);
let Some(hit) = halfspace.cast_local_ray(&mouse_ray, f32::MAX, false) else {
return;
};
let target_point = mouse_ray.point_at(hit);
let options = InverseKinematicsOption {
constrained_axes: JointAxesMask::LIN_AXES,
..Default::default()
};
multibody.inverse_kinematics(
&physics.bodies,
link_id,
&options,
&Isometry::from(target_point),
&mut displacements,
);
multibody.apply_displacements(displacements.as_slice());
}
});
/*
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
testbed.look_at(point![0.0, 0.5, 2.5], point![0.0, 0.5, 0.0]);
}

2
src/dynamics/joint/multibody_joint/mod.rs
View File

@@ -1,6 +1,7 @@
//! MultibodyJoints using the reduced-coordinates formalism or using constraints.
pub use self::multibody::Multibody;
pub use self::multibody_ik::InverseKinematicsOption;
pub use self::multibody_joint::MultibodyJoint;
pub use self::multibody_joint_set::{
MultibodyIndex, MultibodyJointHandle, MultibodyJointSet, MultibodyLinkId,
@@ -13,5 +14,6 @@ mod multibody_joint_set;
mod multibody_link;
mod multibody_workspace;
mod multibody_ik;
mod multibody_joint;
mod unit_multibody_joint;

217
src/dynamics/joint/multibody_joint/multibody.rs
View File

@@ -89,6 +89,7 @@ impl Default for Multibody {
Multibody::new()
}
}
impl Multibody {
/// Creates a new multibody with no link.
pub fn new() -> Self {
@@ -115,6 +116,8 @@ impl Multibody {
pub(crate) fn with_root(handle: RigidBodyHandle) -> Self {
let mut mb = Multibody::new();
// NOTE: we have no way of knowing if the root in fixed at this point, so
// we mark it as dynamic and will fixe later with `Self::update_root_type`.
mb.root_is_dynamic = true;
let joint = MultibodyJoint::free(Isometry::identity());
mb.add_link(None, joint, handle);
@@ -747,6 +750,12 @@ impl Multibody {
self.velocities.rows(0, self.ndofs)
}
/// The body jacobian for link `link_id` calculated by the last call to [`Multibody::forward_kinematics`].
#[inline]
pub fn body_jacobian(&self, link_id: usize) -> &Jacobian<Real> {
&self.body_jacobians[link_id]
}
/// The mutable generalized velocities of this multibodies.
#[inline]
pub fn generalized_velocity_mut(&mut self) -> DVectorViewMut<Real> {
@@ -762,17 +771,27 @@ impl Multibody {
}
/// Apply displacements, in generalized coordinates, to this multibody.
///
/// Note this does **not** updates the link poses, only their generalized coordinates.
/// To update the link poses and associated rigid-bodies, call [`Self::forward_kinematics`]
/// or [`Self::finalize`].
pub fn apply_displacements(&mut self, disp: &[Real]) {
for link in self.links.iter_mut() {
link.joint.apply_displacement(&disp[link.assembly_id..])
}
}
pub(crate) fn update_root_type(&mut self, bodies: &mut RigidBodySet) {
pub(crate) fn update_root_type(&mut self, bodies: &RigidBodySet, take_body_pose: bool) {
if let Some(rb) = bodies.get(self.links[0].rigid_body) {
if rb.is_dynamic() != self.root_is_dynamic {
let root_pose = if take_body_pose {
*rb.position()
} else {
self.links[0].local_to_world
};
if rb.is_dynamic() {
let free_joint = MultibodyJoint::free(*rb.position());
let free_joint = MultibodyJoint::free(root_pose);
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = free_joint;
self.links[0].assembly_id = 0;
@@ -791,7 +810,7 @@ impl Multibody {
assert!(self.damping.len() >= SPATIAL_DIM);
assert!(self.accelerations.len() >= SPATIAL_DIM);
let fixed_joint = MultibodyJoint::fixed(*rb.position());
let fixed_joint = MultibodyJoint::fixed(root_pose);
let prev_root_ndofs = self.links[0].joint().ndofs();
self.links[0].joint = fixed_joint;
self.links[0].assembly_id = 0;
@@ -820,30 +839,86 @@ impl Multibody {
}
// Make sure the positions are properly set to match the rigid-body’s.
if self.links[0].joint.data.locked_axes.is_empty() {
self.links[0].joint.set_free_pos(*rb.position());
if take_body_pose {
if self.links[0].joint.data.locked_axes.is_empty() {
self.links[0].joint.set_free_pos(*rb.position());
} else {
self.links[0].joint.data.local_frame1 = *rb.position();
}
}
}
}
/// Update the rigid-body poses based on this multibody joint poses.
///
/// This is typically called after [`Self::forward_kinematics`] to apply the new joint poses
/// to the rigid-bodies.
pub fn update_rigid_bodies(&self, bodies: &mut RigidBodySet, update_mass_properties: bool) {
self.update_rigid_bodies_internal(bodies, update_mass_properties, false, true)
}
pub(crate) fn update_rigid_bodies_internal(
&self,
bodies: &mut RigidBodySet,
update_mass_properties: bool,
update_next_positions_only: bool,
change_tracking: bool,
) {
// Handle the children. They all have a parent within this multibody.
for link in self.links.iter() {
let rb = if change_tracking {
bodies.get_mut_internal_with_modification_tracking(link.rigid_body)
} else {
self.links[0].joint.data.local_frame1 = *rb.position();
bodies.get_mut_internal(link.rigid_body)
};
if let Some(rb) = rb {
rb.pos.next_position = link.local_to_world;
if !update_next_positions_only {
rb.pos.position = link.local_to_world;
}
if update_mass_properties {
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
}
}
// TODO: make a version that doesn’t write back to bodies and doesn’t update the jacobians
// (i.e. just something used by the velocity solver’s small steps).
/// Apply forward-kinematics to this multibody and its related rigid-bodies.
pub fn forward_kinematics(&mut self, bodies: &mut RigidBodySet, update_rb_mass_props: bool) {
/// Apply forward-kinematics to this multibody.
///
/// This will update the [`MultibodyLink`] pose information as wall as the body jacobians.
/// This will also ensure that the multibody has the proper number of degrees of freedom if
/// its root node changed between dynamic and non-dynamic.
///
/// Note that this does **not** update the poses of the [`RigidBody`] attached to the joints.
/// Run [`Self::update_rigid_bodies`] to trigger that update.
///
/// This method updates `self` with the result of the forward-kinematics operation.
/// For a non-mutable version running forward kinematics on a single link, see
/// [`Self::forward_kinematics_single_link`].
///
/// ## Parameters
/// - `bodies`: the set of rigid-bodies.
/// - `read_root_pose_from_rigid_body`: if set to `true`, the root joint (either a fixed joint,
/// or a free joint) will have its pose set to its associated-rigid-body pose. Set this to `true`
/// when the root rigid-body pose has been modified and needs to affect the multibody.
pub fn forward_kinematics(
&mut self,
bodies: &RigidBodySet,
read_root_pose_from_rigid_body: bool,
) {
// Be sure the degrees of freedom match and take the root position if needed.
self.update_root_type(bodies, read_root_pose_from_rigid_body);
// Special case for the root, which has no parent.
{
let link = &mut self.links[0];
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = link.local_to_parent;
if let Some(rb) = bodies.get_mut_internal(link.rigid_body) {
rb.pos.next_position = link.local_to_world;
if update_rb_mass_props {
rb.mprops.update_world_mass_properties(&link.local_to_world);
}
}
}
// Handle the children. They all have a parent within this multibody.
@@ -865,20 +940,11 @@ impl Multibody {
link.shift23 = c3 - c2;
}
let link_rb = bodies.index_mut_internal(link.rigid_body);
link_rb.pos.next_position = link.local_to_world;
assert_eq!(
link_rb.body_type,
bodies[link.rigid_body].body_type,
RigidBodyType::Dynamic,
"A rigid-body that is not at the root of a multibody must be dynamic."
);
if update_rb_mass_props {
link_rb
.mprops
.update_world_mass_properties(&link.local_to_world);
}
}
/*
@@ -887,6 +953,107 @@ impl Multibody {
self.update_body_jacobians();
}
/// Apply forward-kinematics to compute the position of a single link of this multibody.
///
/// If `out_jacobian` is `Some`, this will simultaneously compute the new jacobian of this link.
/// If `displacement` is `Some`, the generalized position considered during transform propagation
/// is the sum of the current position of `self` and this `displacement`.
// TODO: this shares a lot of code with `forward_kinematics` and `update_body_jacobians`, except
// that we are only traversing a single kinematic chain. Could this be refactored?
pub fn forward_kinematics_single_link(
&self,
bodies: &RigidBodySet,
link_id: usize,
displacement: Option<&[Real]>,
mut out_jacobian: Option<&mut Jacobian<Real>>,
) -> Isometry<Real> {
let mut branch = vec![]; // Perf: avoid allocation.
let mut curr_id = Some(link_id);
while let Some(id) = curr_id {
branch.push(id);
curr_id = self.links[id].parent_id();
}
branch.reverse();
if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
if out_jacobian.ncols() != self.ndofs {
*out_jacobian = Jacobian::zeros(self.ndofs);
} else {
out_jacobian.fill(0.0);
}
}
let mut parent_link: Option<MultibodyLink> = None;
for i in branch {
let mut link = self.links[i];
if let Some(displacement) = displacement {
link.joint
.apply_displacement(&displacement[link.assembly_id..]);
}
let parent_to_world;
if let Some(parent_link) = parent_link {
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = parent_link.local_to_world * link.local_to_parent;
{
let parent_rb = &bodies[parent_link.rigid_body];
let link_rb = &bodies[link.rigid_body];
let c0 = parent_link.local_to_world * parent_rb.mprops.local_mprops.local_com;
let c2 = link.local_to_world
* Point::from(link.joint.data.local_frame2.translation.vector);
let c3 = link.local_to_world * link_rb.mprops.local_mprops.local_com;
link.shift02 = c2 - c0;
link.shift23 = c3 - c2;
}
parent_to_world = parent_link.local_to_world;
if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
let (mut link_j_v, parent_j_w) =
out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
let shift_tr = (link.shift02).gcross_matrix_tr();
link_j_v.gemm(1.0, &shift_tr, &parent_j_w, 1.0);
}
} else {
link.local_to_parent = link.joint.body_to_parent();
link.local_to_world = link.local_to_parent;
parent_to_world = Isometry::identity();
}
if let Some(out_jacobian) = out_jacobian.as_deref_mut() {
let ndofs = link.joint.ndofs();
let mut tmp = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::zeros();
let mut link_joint_j = tmp.columns_mut(0, ndofs);
let mut link_j_part = out_jacobian.columns_mut(link.assembly_id, ndofs);
link.joint.jacobian(
&(parent_to_world.rotation * link.joint.data.local_frame1.rotation),
&mut link_joint_j,
);
link_j_part += link_joint_j;
{
let (mut link_j_v, link_j_w) =
out_jacobian.rows_range_pair_mut(0..DIM, DIM..DIM + ANG_DIM);
let shift_tr = link.shift23.gcross_matrix_tr();
link_j_v.gemm(1.0, &shift_tr, &link_j_w, 1.0);
}
}
parent_link = Some(link);
}
parent_link
.map(|link| link.local_to_world)
.unwrap_or_else(Isometry::identity)
}
/// The total number of freedoms of this multibody.
#[inline]
pub fn ndofs(&self) -> usize {

238
src/dynamics/joint/multibody_joint/multibody_ik.rs Normal file
View File

@@ -0,0 +1,238 @@
use crate::dynamics::{JointAxesMask, Multibody, RigidBodySet};
use crate::math::{Isometry, Jacobian, Real, ANG_DIM, DIM, SPATIAL_DIM};
use na::{self, DVector, SMatrix};
use parry::math::SpacialVector;
#[derive(Copy, Clone, Debug, PartialEq)]
/// Options for the jacobian-based Inverse Kinematics solver for multibodies.
pub struct InverseKinematicsOption {
/// A damping coefficient.
///
/// Small value can lead to overshooting preventing convergence. Large
/// values can slown down convergence, requiring more iterations to converge.
pub damping: Real,
/// The maximum number of iterations the iterative IK solver can take.
pub max_iters: usize,
/// The axes the IK solver will solve for.
pub constrained_axes: JointAxesMask,
/// The error threshold on the linear error.
///
/// If errors on both linear and angular parts fall below this
/// threshold, the iterative resolution will stop.
pub epsilon_linear: Real,
/// The error threshold on the angular error.
///
/// If errors on both linear and angular parts fall below this
/// threshold, the iterative resolution will stop.
pub epsilon_angular: Real,
}
impl Default for InverseKinematicsOption {
fn default() -> Self {
Self {
damping: 1.0,
max_iters: 10,
constrained_axes: JointAxesMask::all(),
epsilon_linear: 1.0e-3,
epsilon_angular: 1.0e-3,
}
}
}
impl Multibody {
/// Computes the displacement needed to have the link identified by `link_id` move by the
/// desired transform.
///
/// The displacement calculated by this function is added to the `displacement` vector.
pub fn inverse_kinematics_delta(
&self,
link_id: usize,
desired_movement: &SpacialVector<Real>,
damping: Real,
displacements: &mut DVector<Real>,
) {
let body_jacobian = self.body_jacobian(link_id);
Self::inverse_kinematics_delta_with_jacobian(
body_jacobian,
desired_movement,
damping,
displacements,
);
}
/// Computes the displacement needed to have a link with the given jacobian move by the
/// desired transform.
///
/// The displacement calculated by this function is added to the `displacement` vector.
pub fn inverse_kinematics_delta_with_jacobian(
jacobian: &Jacobian<Real>,
desired_movement: &SpacialVector<Real>,
damping: Real,
displacements: &mut DVector<Real>,
) {
let identity = SMatrix::<Real, SPATIAL_DIM, SPATIAL_DIM>::identity();
let jj = jacobian * &jacobian.transpose() + identity * (damping * damping);
let inv_jj = jj.pseudo_inverse(1.0e-5).unwrap_or(identity);
displacements.gemv_tr(1.0, jacobian, &(inv_jj * desired_movement), 1.0);
}
/// Computes the displacement needed to have the link identified by `link_id` have a pose
/// equal (or as close as possible) to `target_pose`.
///
/// If `displacement` is given non-zero, the current pose of the rigid-body is considered to be
/// obtained from its current generalized coordinates summed with the `displacement` vector.
///
/// The `displacements` vector is overwritten with the new displacement.
pub fn inverse_kinematics(
&self,
bodies: &RigidBodySet,
link_id: usize,
options: &InverseKinematicsOption,
target_pose: &Isometry<Real>,
displacements: &mut DVector<Real>,
) {
let mut jacobian = Jacobian::zeros(0);
for _ in 0..options.max_iters {
let pose = self.forward_kinematics_single_link(
bodies,
link_id,
Some(displacements.as_slice()),
Some(&mut jacobian),
);
let delta_lin = target_pose.translation.vector - pose.translation.vector;
let delta_ang = (target_pose.rotation * pose.rotation.inverse()).scaled_axis();
#[cfg(feature = "dim2")]
let mut delta = na::vector![delta_lin.x, delta_lin.y, delta_ang.x];
#[cfg(feature = "dim3")]
let mut delta = na::vector![
delta_lin.x,
delta_lin.y,
delta_lin.z,
delta_ang.x,
delta_ang.y,
delta_ang.z
];
if !options.constrained_axes.contains(JointAxesMask::X) {
delta[0] = 0.0;
}
if !options.constrained_axes.contains(JointAxesMask::Y) {
delta[1] = 0.0;
}
#[cfg(feature = "dim3")]
if !options.constrained_axes.contains(JointAxesMask::Z) {
delta[2] = 0.0;
}
if !options.constrained_axes.contains(JointAxesMask::ANG_X) {
delta[DIM] = 0.0;
}
#[cfg(feature = "dim3")]
if !options.constrained_axes.contains(JointAxesMask::ANG_Y) {
delta[DIM + 1] = 0.0;
}
#[cfg(feature = "dim3")]
if !options.constrained_axes.contains(JointAxesMask::ANG_Z) {
delta[DIM + 2] = 0.0;
}
// TODO: measure convergence on the error variation instead?
if delta.rows(0, DIM).norm() <= options.epsilon_linear
&& delta.rows(DIM, ANG_DIM).norm() <= options.epsilon_angular
{
break;
}
Self::inverse_kinematics_delta_with_jacobian(
&jacobian,
&delta,
options.damping,
displacements,
);
}
}
}
#[cfg(test)]
mod test {
use crate::dynamics::{
MultibodyJointHandle, MultibodyJointSet, RevoluteJointBuilder, RigidBodyBuilder,
RigidBodySet,
};
use crate::math::{Jacobian, Real, Vector};
use approx::assert_relative_eq;
#[test]
fn one_link_fwd_kinematics() {
let mut bodies = RigidBodySet::new();
let mut multibodies = MultibodyJointSet::new();
let num_segments = 10;
let body = RigidBodyBuilder::fixed();
let mut last_body = bodies.insert(body);
let mut last_link = MultibodyJointHandle::invalid();
for _ in 0..num_segments {
let body = RigidBodyBuilder::dynamic().can_sleep(false);
let new_body = bodies.insert(body);
#[cfg(feature = "dim2")]
let builder = RevoluteJointBuilder::new();
#[cfg(feature = "dim3")]
let builder = RevoluteJointBuilder::new(Vector::z_axis());
let link_ab = builder
.local_anchor1((Vector::y() * (0.5 / num_segments as Real)).into())
.local_anchor2((Vector::y() * (-0.5 / num_segments as Real)).into());
last_link = multibodies
.insert(last_body, new_body, link_ab, true)
.unwrap();
last_body = new_body;
}
let (multibody, last_id) = multibodies.get_mut(last_link).unwrap();
multibody.forward_kinematics(&bodies, true); // Be sure all the dofs are up to date.
assert_eq!(multibody.ndofs(), num_segments);
/*
* No displacement.
*/
let mut jacobian2 = Jacobian::zeros(0);
let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
let jacobian1 = multibody.body_jacobian(last_id);
let link_pose2 =
multibody.forward_kinematics_single_link(&bodies, last_id, None, Some(&mut jacobian2));
assert_eq!(link_pose1, link_pose2);
assert_eq!(jacobian1, &jacobian2);
/*
* Arbitrary displacement.
*/
let niter = 100;
let displacement_part: Vec<_> = (0..multibody.ndofs())
.map(|i| i as Real * -0.1 / niter as Real)
.collect();
let displacement_total: Vec<_> = displacement_part
.iter()
.map(|d| *d * niter as Real)
.collect();
let link_pose2 = multibody.forward_kinematics_single_link(
&bodies,
last_id,
Some(&displacement_total),
Some(&mut jacobian2),
);
for _ in 0..niter {
multibody.apply_displacements(&displacement_part);
multibody.forward_kinematics(&bodies, false);
}
let link_pose1 = *multibody.link(last_id).unwrap().local_to_world();
let jacobian1 = multibody.body_jacobian(last_id);
assert_relative_eq!(link_pose1, link_pose2, epsilon = 1.0e-5);
assert_relative_eq!(jacobian1, &jacobian2, epsilon = 1.0e-5);
}
}

11
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -286,6 +286,13 @@ impl MultibodyJointSet {
self.multibodies.get(index.0)
}
/// Gets a mutable reference to a multibody, based on its temporary index.
/// `MultibodyJointSet`.
pub fn get_multibody_mut(&mut self, index: MultibodyIndex) -> Option<&mut Multibody> {
// TODO: modification tracking.
self.multibodies.get_mut(index.0)
}
/// Gets a mutable reference to a multibody, based on its temporary index.
///
/// This method will bypass any modification-detection automatically done by the
@@ -363,13 +370,13 @@ impl MultibodyJointSet {
let parent1 = link1.parent_id();
if parent1 == Some(id2.id) {
Some((MultibodyJointHandle(rb1.0), mb, &link1))
Some((MultibodyJointHandle(rb1.0), mb, link1))
} else {
let link2 = mb.link(id2.id)?;
let parent2 = link2.parent_id();
if parent2 == Some(id1.id) {
Some((MultibodyJointHandle(rb2.0), mb, &link2))
Some((MultibodyJointHandle(rb2.0), mb, link2))
} else {
None
}

2
src/dynamics/joint/multibody_joint/multibody_link.rs
View File

@@ -6,7 +6,7 @@ use crate::prelude::RigidBodyVelocity;
/// One link of a multibody.
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Clone)]
#[derive(Copy, Clone)]
pub struct MultibodyLink {
// FIXME: make all those private.
pub(crate) internal_id: usize,

6
src/dynamics/rigid_body.rs
View File

@@ -292,9 +292,9 @@ impl RigidBody {
allow_rotations_z: bool,
wake_up: bool,
) {
if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) != !allow_rotations_x
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) != !allow_rotations_y
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) != !allow_rotations_z
if self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_X) == allow_rotations_x
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Y) == allow_rotations_y
|| self.mprops.flags.contains(LockedAxes::ROTATION_LOCKED_Z) == allow_rotations_z
{
if self.is_dynamic() && wake_up {
self.wake_up(true);

5
src/dynamics/solver/velocity_solver.rs
View File

@@ -251,8 +251,9 @@ impl VelocitySolver {
.rows(multibody.solver_id, multibody.ndofs());
multibody.velocities.copy_from(&solver_vels);
multibody.integrate(params.dt);
// PERF: we could have a mode where it doesn’t write back to the `bodies` yet.
multibody.forward_kinematics(bodies, !is_last_substep);
// PERF: don’t write back to the rigid-body poses `bodies` before the last step?
multibody.forward_kinematics(bodies, false);
multibody.update_rigid_bodies_internal(bodies, !is_last_substep, true, false);
if !is_last_substep {
// These are very expensive and not needed if we don’t

6
src/geometry/contact_pair.rs
View File

@@ -359,16 +359,10 @@ impl ContactManifoldData {
pub trait ContactManifoldExt {
/// Computes the sum of all the impulses applied by contacts from this contact manifold.
fn total_impulse(&self) -> Real;
/// Computes the maximum impulse applied by contacts from this contact manifold.
fn max_impulse(&self) -> Real;
}
impl ContactManifoldExt for ContactManifold {
fn total_impulse(&self) -> Real {
self.points.iter().map(|pt| pt.data.impulse).sum()
}
fn max_impulse(&self) -> Real {
self.points.iter().fold(0.0, |a, pt| a.max(pt.data.impulse))
}
}

6
src/pipeline/physics_pipeline.rs
View File

@@ -469,10 +469,10 @@ impl PhysicsPipeline {
// TODO: do this only on user-change.
// TODO: do we want some kind of automatic inverse kinematics?
for multibody in &mut multibody_joints.multibodies {
multibody.1.update_root_type(bodies);
// FIXME: what should we do here? We should not
// rely on the next state here.
multibody.1.forward_kinematics(bodies, true);
multibody
.1
.update_rigid_bodies_internal(bodies, true, false, false);
}
self.detect_collisions(

163
src/utils.rs
View File

@@ -1,8 +1,8 @@
//! Miscellaneous utilities.
use na::{
Matrix1, Matrix2, Matrix3, Point2, Point3, RowVector2, Scalar, SimdRealField, UnitComplex,
UnitQuaternion, Vector1, Vector2, Vector3,
Matrix1, Matrix2, Matrix3, RowVector2, Scalar, SimdRealField, UnitComplex, UnitQuaternion,
Vector1, Vector2, Vector3,
};
use num::Zero;
use simba::simd::SimdValue;
@@ -90,35 +90,6 @@ impl SimdSign<SimdReal> for SimdReal {
}
}
pub(crate) trait SimdComponent: Sized {
type Element;
fn min_component(self) -> Self::Element;
fn max_component(self) -> Self::Element;
}
impl SimdComponent for Real {
type Element = Real;
fn min_component(self) -> Self::Element {
self
}
fn max_component(self) -> Self::Element {
self
}
}
impl SimdComponent for SimdReal {
type Element = Real;
fn min_component(self) -> Self::Element {
self.simd_horizontal_min()
}
fn max_component(self) -> Self::Element {
self.simd_horizontal_max()
}
}
/// Trait to compute the orthonormal basis of a vector.
pub trait SimdBasis: Sized {
/// The type of the array of orthonormal vectors.
@@ -166,89 +137,6 @@ impl<N: SimdRealCopy + SimdSign<N>> SimdBasis for Vector3<N> {
}
}
pub(crate) trait SimdVec: Sized {
type Element;
fn horizontal_inf(&self) -> Self::Element;
fn horizontal_sup(&self) -> Self::Element;
}
impl<N: Scalar + Copy + SimdComponent> SimdVec for Vector2<N>
where
N::Element: Scalar,
{
type Element = Vector2<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Vector2::new(self.x.min_component(), self.y.min_component())
}
fn horizontal_sup(&self) -> Self::Element {
Vector2::new(self.x.max_component(), self.y.max_component())
}
}
impl<N: Scalar + Copy + SimdComponent> SimdVec for Point2<N>
where
N::Element: Scalar,
{
type Element = Point2<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Point2::new(self.x.min_component(), self.y.min_component())
}
fn horizontal_sup(&self) -> Self::Element {
Point2::new(self.x.max_component(), self.y.max_component())
}
}
impl<N: Scalar + Copy + SimdComponent> SimdVec for Vector3<N>
where
N::Element: Scalar,
{
type Element = Vector3<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Vector3::new(
self.x.min_component(),
self.y.min_component(),
self.z.min_component(),
)
}
fn horizontal_sup(&self) -> Self::Element {
Vector3::new(
self.x.max_component(),
self.y.max_component(),
self.z.max_component(),
)
}
}
impl<N: Scalar + Copy + SimdComponent> SimdVec for Point3<N>
where
N::Element: Scalar,
{
type Element = Point3<N::Element>;
fn horizontal_inf(&self) -> Self::Element {
Point3::new(
self.x.min_component(),
self.y.min_component(),
self.z.min_component(),
)
}
fn horizontal_sup(&self) -> Self::Element {
Point3::new(
self.x.max_component(),
self.y.max_component(),
self.z.max_component(),
)
}
}
pub(crate) trait SimdCrossMatrix: Sized {
type CrossMat;
type CrossMatTr;
@@ -463,28 +351,21 @@ impl<N: SimdRealCopy> SimdQuat<N> for UnitQuaternion<N> {
pub(crate) trait SimdAngularInertia<N> {
type AngVector;
type LinVector;
type AngMatrix;
fn inverse(&self) -> Self;
fn transform_lin_vector(&self, pt: Self::LinVector) -> Self::LinVector;
fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector;
fn squared(&self) -> Self;
fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix;
fn into_matrix(self) -> Self::AngMatrix;
}
impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
type AngVector = N;
type LinVector = Vector2<N>;
type AngMatrix = N;
fn inverse(&self) -> Self {
simd_inv(*self)
}
fn transform_lin_vector(&self, pt: Vector2<N>) -> Vector2<N> {
pt * *self
}
fn transform_vector(&self, pt: N) -> N {
pt * *self
}
@@ -493,10 +374,6 @@ impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
*self * *self
}
fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix {
*mat * *self
}
fn into_matrix(self) -> Self::AngMatrix {
self
}
@@ -504,7 +381,6 @@ impl<N: SimdRealCopy> SimdAngularInertia<N> for N {
impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
type AngVector = Vector3<Real>;
type LinVector = Vector3<Real>;
type AngMatrix = Matrix3<Real>;
fn inverse(&self) -> Self {
@@ -540,10 +416,6 @@ impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
}
}
fn transform_lin_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
self.transform_vector(v)
}
fn transform_vector(&self, v: Vector3<Real>) -> Vector3<Real> {
let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
@@ -559,16 +431,10 @@ impl SimdAngularInertia<Real> for SdpMatrix3<Real> {
self.m13, self.m23, self.m33,
)
}
#[rustfmt::skip]
fn transform_matrix(&self, m: &Matrix3<Real>) -> Matrix3<Real> {
*self * *m
}
}
impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
type AngVector = Vector3<SimdReal>;
type LinVector = Vector3<SimdReal>;
type AngMatrix = Matrix3<SimdReal>;
fn inverse(&self) -> Self {
@@ -593,10 +459,6 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
}
}
fn transform_lin_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
self.transform_vector(v)
}
fn transform_vector(&self, v: Vector3<SimdReal>) -> Vector3<SimdReal> {
let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z;
let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z;
@@ -623,27 +485,6 @@ impl SimdAngularInertia<SimdReal> for SdpMatrix3<SimdReal> {
self.m13, self.m23, self.m33,
)
}
#[rustfmt::skip]
fn transform_matrix(&self, m: &Matrix3<SimdReal>) -> Matrix3<SimdReal> {
let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31;
let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31;
let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31;
let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32;
let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32;
let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32;
let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33;
let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33;
let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33;
Matrix3::new(
x0, x1, x2,
y0, y1, y2,
z0, z1, z2,
)
}
}
// This is an RAII structure that enables flushing denormal numbers

1
src_testbed/lib.rs
View File

@@ -25,6 +25,7 @@ mod camera3d;
mod debug_render;
mod graphics;
pub mod harness;
mod mouse;
pub mod objects;
pub mod physics;
#[cfg(all(feature = "dim3", feature = "other-backends"))]

46
src_testbed/mouse.rs Normal file
View File

@@ -0,0 +1,46 @@
use crate::math::Point;
use bevy::prelude::*;
use bevy::window::PrimaryWindow;
#[derive(Component)]
pub struct MainCamera;
#[derive(Default, Copy, Clone, Debug, Resource)]
pub struct SceneMouse {
#[cfg(feature = "dim2")]
pub point: Option<Point<f32>>,
#[cfg(feature = "dim3")]
pub ray: Option<(Point<f32>, crate::math::Vector<f32>)>,
}
pub fn track_mouse_state(
mut scene_mouse: ResMut<SceneMouse>,
windows: Query<&Window, With<PrimaryWindow>>,
camera: Query<(&GlobalTransform, &Camera), With<MainCamera>>,
) {
if let Ok(window) = windows.get_single() {
for (camera_transform, camera) in camera.iter() {
if let Some(cursor) = window.cursor_position() {
let ndc_cursor = ((cursor / Vec2::new(window.width(), window.height()) * 2.0)
- Vec2::ONE)
* Vec2::new(1.0, -1.0);
let ndc_to_world =
camera_transform.compute_matrix() * camera.projection_matrix().inverse();
let ray_pt1 =
ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, -1.0));
#[cfg(feature = "dim2")]
{
scene_mouse.point = Some(Point::new(ray_pt1.x, ray_pt1.y));
}
#[cfg(feature = "dim3")]
{
let ray_pt2 =
ndc_to_world.project_point3(Vec3::new(ndc_cursor.x, ndc_cursor.y, 1.0));
let ray_dir = ray_pt2 - ray_pt1;
scene_mouse.ray = Some((na::Vector3::from(ray_pt1).into(), ray_dir.into()));
}
}
}
}
}

30
src_testbed/testbed.rs
View File

@@ -9,8 +9,8 @@ use bevy::prelude::*;
use crate::debug_render::{DebugRenderPipelineResource, RapierDebugRenderPlugin};
use crate::physics::{DeserializedPhysicsSnapshot, PhysicsEvents, PhysicsSnapshot, PhysicsState};
use crate::plugin::TestbedPlugin;
use crate::ui;
use crate::{graphics::GraphicsManager, harness::RunState};
use crate::{mouse, ui};
use na::{self, Point2, Point3, Vector3};
#[cfg(feature = "dim3")]
@@ -135,7 +135,7 @@ pub struct TestbedState {
pub solver_type: RapierSolverType,
pub physx_use_two_friction_directions: bool,
pub snapshot: Option<PhysicsSnapshot>,
nsteps: usize,
pub nsteps: usize,
camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button.
}
@@ -161,6 +161,7 @@ pub struct TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
camera_transform: GlobalTransform,
camera: &'a mut OrbitCamera,
keys: &'a ButtonInput<KeyCode>,
mouse: &'a SceneMouse,
}
pub struct Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -400,6 +401,7 @@ impl TestbedApp {
brightness: 0.3,
..Default::default()
})
.init_resource::<mouse::SceneMouse>()
.add_plugins(DefaultPlugins.set(window_plugin))
.add_plugins(OrbitCameraPlugin)
.add_plugins(WireframePlugin)
@@ -419,7 +421,9 @@ impl TestbedApp {
.insert_resource(self.builders)
.insert_non_send_resource(self.plugins)
.add_systems(Update, update_testbed)
.add_systems(Update, egui_focus);
.add_systems(Update, egui_focus)
.add_systems(Update, track_mouse_state);
init(&mut app);
app.run();
}
@@ -472,6 +476,15 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> TestbedGraphics<'a, 'b, 'c, 'd, 'e, 'f> {
pub fn keys(&self) -> &ButtonInput<KeyCode> {
self.keys
}
pub fn mouse(&self) -> &SceneMouse {
self.mouse
}
#[cfg(feature = "dim3")]
pub fn camera_fwd_dir(&self) -> Vector<f32> {
(self.camera_transform * -Vec3::Z).normalize().into()
}
}
impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
@@ -1047,7 +1060,8 @@ fn setup_graphics_environment(mut commands: Commands) {
.insert(OrbitCamera {
rotate_sensitivity: 0.05,
..OrbitCamera::default()
});
})
.insert(MainCamera);
}
#[cfg(feature = "dim2")]
@@ -1078,7 +1092,8 @@ fn setup_graphics_environment(mut commands: Commands) {
zoom: 100.0,
pan_sensitivity: 0.02,
..OrbitCamera::default()
});
})
.insert(MainCamera);
}
fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera>) {
@@ -1091,12 +1106,14 @@ fn egui_focus(mut ui_context: EguiContexts, mut cameras: Query<&mut OrbitCamera>
}
}
use crate::mouse::{track_mouse_state, MainCamera, SceneMouse};
use bevy::window::PrimaryWindow;
fn update_testbed(
mut commands: Commands,
windows: Query<&Window, With<PrimaryWindow>>,
// mut pipelines: ResMut<Assets<RenderPipelineDescriptor>>,
mouse: Res<SceneMouse>,
mut meshes: ResMut<Assets<Mesh>>,
mut materials: ResMut<Assets<BevyMaterial>>,
builders: ResMut<SceneBuilders>,
@@ -1126,6 +1143,7 @@ fn update_testbed(
camera_transform: *cameras.single().1,
camera: &mut cameras.single_mut().2,
keys: &keys,
mouse: &mouse,
};
let mut testbed = Testbed {
@@ -1216,6 +1234,7 @@ fn update_testbed(
camera_transform: *cameras.single().1,
camera: &mut cameras.single_mut().2,
keys: &keys,
mouse: &mouse,
};
let mut testbed = Testbed {
@@ -1389,6 +1408,7 @@ fn update_testbed(
camera_transform: *cameras.single().1,
camera: &mut cameras.single_mut().2,
keys: &keys,
mouse: &mouse,
};
harness.step_with_graphics(Some(&mut testbed_graphics));