From 27b11b9d61f52125c78aaabf78e3c1196462fb15 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Fri, 17 Oct 2025 12:22:23 +0200 Subject: [PATCH] fix regressions with sleeping behavior of kinematic bodies (#885) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * fix kinematic bodies ignoring the wake_up flag when setting velocities * fix: don’t allow kinematic bodies to fall asleep unless they velocities are at zero exactly. * feat: add debug example for kinematic bodies sleep * chore: update changelog * chore: typo --- CHANGELOG.md | 8 +++ examples3d/all_examples3.rs | 5 ++ examples3d/debug_sleeping_kinematic3.rs | 88 +++++++++++++++++++++++++ src/dynamics/island_manager.rs | 31 +++++++-- src/dynamics/rigid_body.rs | 15 +---- 5 files changed, 129 insertions(+), 18 deletions(-) create mode 100644 examples3d/debug_sleeping_kinematic3.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index c77f2ee..fcca700 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,11 @@ +## Unreleased + +- Kinematic rigid-bodies will no longer fall asleep if they have a nonzero velocity, even if that velocity is very + small. The rationale is that, since that velocity is chosen by the user, they really want the platform to move even + if the speed is low. +- Fix bug where kinematic bodies would ignore the `wake_up` flag passed to `set_linear_velocity` and + `set_angular_velocity`. + ## v0.30.0 (03 Oct. 2025) - Update to parry 0.25 (which involves breaking changes in the `Voxels` API but improves its internal storage to support diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index ae272dd..4054346 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -44,6 +44,7 @@ mod debug_cube_high_mass_ratio3; mod debug_internal_edges3; mod debug_long_chain3; mod debug_multibody_ang_motor_pos3; +mod debug_sleeping_kinematic3; mod gyroscopic3; mod inverse_kinematics3; mod joint_motor_position3; @@ -132,6 +133,10 @@ pub fn main() { "(Debug) shape modification", debug_shape_modification3::init_world, ), + ( + "(Debug) sleeping kinematics", + debug_sleeping_kinematic3::init_world, + ), ("(Debug) deserialize", debug_deserialize3::init_world), ( "(Debug) multibody ang. motor pos.", diff --git a/examples3d/debug_sleeping_kinematic3.rs b/examples3d/debug_sleeping_kinematic3.rs new file mode 100644 index 0000000..b224f76 --- /dev/null +++ b/examples3d/debug_sleeping_kinematic3.rs @@ -0,0 +1,88 @@ +use rapier_testbed3d::Testbed; +use rapier3d::prelude::*; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Setup a velocity-based kinematic rigid body. + */ + let platform_body = + RigidBodyBuilder::kinematic_velocity_based().translation(vector![0.0, 1.5 + 0.8, 0.0]); + let platform_handle = bodies.insert(platform_body); + let collider = ColliderBuilder::cuboid(5.0, 0.5, 5.0); + colliders.insert_with_parent(collider, platform_handle, &mut bodies); + + // A second velocity-based platform but this one will move super slow. + let slow_platform_body = + RigidBodyBuilder::kinematic_velocity_based().translation(vector![0.0, 0.0, 0.0]); + let slow_platform_handle = bodies.insert(slow_platform_body); + let collider = ColliderBuilder::cuboid(5.0, 0.5, 5.0); + colliders.insert_with_parent(collider, slow_platform_handle, &mut bodies); + + /* + * Setup a callback to control the platform. + */ + let start_tick = 500; + let stop_tick = 1000; + + testbed.add_callback(move |_, physics, _, run_state| { + if run_state.timestep_id == stop_tick { + println!("Both platforms should stop moving now and eventually fall asleep."); + // The platforms moved until this time. They must not be sleeping. + assert!(!physics.bodies[platform_handle].is_sleeping()); + assert!(!physics.bodies[slow_platform_handle].is_sleeping()); + + if let Some(slow_platform) = physics.bodies.get_mut(slow_platform_handle) { + slow_platform.set_linvel(Vector::zeros(), true); + } + if let Some(platform) = physics.bodies.get_mut(platform_handle) { + platform.set_linvel(Vector::zeros(), true); + } + } + + if run_state.timestep_id > stop_tick + 500 { + // Platforms should fall asleep shortly after we stopped moving them. + assert!(physics.bodies[platform_handle].is_sleeping()); + assert!(physics.bodies[slow_platform_handle].is_sleeping()); + } + + if run_state.timestep_id < start_tick || run_state.timestep_id >= stop_tick { + return; + } else if run_state.timestep_id == start_tick { + println!("Platforms should start moving now and never stop."); + println!("The slow platform should move up and not sleep."); + // Platforms should have had plenty of time to fall asleep before we start moving them. + assert!(physics.bodies[platform_handle].is_sleeping()); + assert!(physics.bodies[slow_platform_handle].is_sleeping()); + + let slow_velocity = vector![0.0, 0.01, 0.0]; + if let Some(slow_platform) = physics.bodies.get_mut(slow_platform_handle) { + slow_platform.set_linvel(slow_velocity, true); + } + } + + let velocity = vector![ + 0.0, + (run_state.time * 2.0).cos(), + run_state.time.sin() * 2.0 + ]; + + // Update the velocity-based kinematic body by setting its velocity. + if let Some(platform) = physics.bodies.get_mut(platform_handle) { + platform.set_linvel(velocity, true); + } + }); + + /* + * Run the simulation. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![-10.0, 5.0, -10.0], Point::origin()); +} diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 9f89d9b..d444d79 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -159,7 +159,14 @@ impl IslandManager { let sq_linvel = rb.vels.linvel.norm_squared(); let sq_angvel = rb.vels.angvel.gdot(rb.vels.angvel); - update_energy(length_unit, &mut rb.activation, sq_linvel, sq_angvel, dt); + update_energy( + &mut rb.activation, + rb.body_type, + length_unit, + sq_linvel, + sq_angvel, + dt, + ); if rb.activation.time_since_can_sleep >= rb.activation.time_until_sleep { // Mark them as sleeping for now. This will @@ -291,16 +298,28 @@ impl IslandManager { } fn update_energy( - length_unit: Real, activation: &mut RigidBodyActivation, + body_type: RigidBodyType, + length_unit: Real, sq_linvel: Real, sq_angvel: Real, dt: Real, ) { - let linear_threshold = activation.normalized_linear_threshold * length_unit; - if sq_linvel < linear_threshold * linear_threshold.abs() - && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs() - { + let can_sleep = match body_type { + RigidBodyType::Dynamic => { + let linear_threshold = activation.normalized_linear_threshold * length_unit; + sq_linvel < linear_threshold * linear_threshold.abs() + && sq_angvel < activation.angular_threshold * activation.angular_threshold.abs() + } + RigidBodyType::KinematicPositionBased | RigidBodyType::KinematicVelocityBased => { + // Platforms only sleep if both velocities are exactly zero. If it’s not exactly + // zero, then the user really wants them to move. + sq_linvel == 0.0 && sq_angvel == 0.0 + } + RigidBodyType::Fixed => true, + }; + + if can_sleep { activation.time_since_can_sleep += dt; } else { activation.time_since_can_sleep = 0.0; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index 026a416..2137e1b 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -769,15 +769,12 @@ impl RigidBody { pub fn set_linvel(&mut self, linvel: Vector, wake_up: bool) { if self.vels.linvel != linvel { match self.body_type { - RigidBodyType::Dynamic => { + RigidBodyType::Dynamic | RigidBodyType::KinematicVelocityBased => { self.vels.linvel = linvel; if wake_up { self.wake_up(true) } } - RigidBodyType::KinematicVelocityBased => { - self.vels.linvel = linvel; - } RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} } } @@ -791,15 +788,12 @@ impl RigidBody { pub fn set_angvel(&mut self, angvel: Real, wake_up: bool) { if self.vels.angvel != angvel { match self.body_type { - RigidBodyType::Dynamic => { + RigidBodyType::Dynamic | RigidBodyType::KinematicVelocityBased => { self.vels.angvel = angvel; if wake_up { self.wake_up(true) } } - RigidBodyType::KinematicVelocityBased => { - self.vels.angvel = angvel; - } RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} } } @@ -813,15 +807,12 @@ impl RigidBody { pub fn set_angvel(&mut self, angvel: Vector, wake_up: bool) { if self.vels.angvel != angvel { match self.body_type { - RigidBodyType::Dynamic => { + RigidBodyType::Dynamic | RigidBodyType::KinematicVelocityBased => { self.vels.angvel = angvel; if wake_up { self.wake_up(true) } } - RigidBodyType::KinematicVelocityBased => { - self.vels.angvel = angvel; - } RigidBodyType::Fixed | RigidBodyType::KinematicPositionBased => {} } }