Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 23 additions & 1 deletion src/dynamics/joint/multibody_joint/multibody.rs
Original file line number Diff line number Diff line change
Expand Up @@ -1270,11 +1270,33 @@ impl Multibody {
}

#[inline]
pub(crate) fn integrate(&mut self, dt: Real) {
pub(crate) fn integrate(&mut self, dt: Real, bodies: &RigidBodySet) {
// The linear velocity of a dynamic root is the velocity of its center-of-mass
// (see `Self::update_velocities`), but its free joint stores the pose of the
// rigid-body origin. Rotate around the center-of-mass, like regular rigid-bodies
// do, otherwise a root with a non-zero local center-of-mass drifts.
let root_com = (self.root_is_dynamic && self.links[0].joint.data.locked_axes.is_empty())
.then(|| bodies.get(self.links[0].rigid_body))
.flatten()
.map(|rb| {
(
rb.mprops.local_mprops.local_com,
self.links[0].joint.joint_rot(),
)
});

for rb in self.links.iter_mut() {
rb.joint
.integrate(dt, &self.velocities.as_slice()[rb.assembly_id..])
}

if let Some((local_com, rot_before)) = root_com {
let joint = &mut self.links[0].joint;
let shift = rot_before * local_com - joint.joint_rot() * local_com;
for i in 0..DIM {
joint.coords[i] += shift[i];
}
}
}

/// Apply displacements, in generalized coordinates, to this multibody.
Expand Down
75 changes: 75 additions & 0 deletions src/dynamics/joint/multibody_joint/multibody_regression_tests.rs
Original file line number Diff line number Diff line change
Expand Up @@ -648,3 +648,78 @@ fn joint_friction_bounds_the_joint_force() {
"an oversized friction bound must stay inert, not chatter: {extreme}"
);
}

/// Total center-of-mass of `bodies`, weighted by their masses.
#[cfg(feature = "dim3")]
fn assembly_com(world: &PhysicsWorld, bodies: &[RigidBodyHandle]) -> Vector {
let mut weighted = Vector::ZERO;
let mut mass = 0.0;
for handle in bodies {
let rb = &world.bodies[*handle];
weighted += rb.center_of_mass() * rb.mass();
mass += rb.mass();
}
weighted / mass
}

/// The free joint of a dynamic multibody root stores the pose of the rigid-body origin, while
/// its linear velocity is the velocity of the root's center-of-mass. Its rotation must therefore
/// be integrated around the center-of-mass: otherwise, a pure torque applied to a root whose
/// local center-of-mass isn't at its origin translates the whole assembly.
#[test]
#[cfg(feature = "dim3")]
fn multibody_root_with_offset_center_of_mass_rotates_around_it() {
for root_com in [
Vector::ZERO,
Vector::new(0.2, 0.0, 0.0),
Vector::new(0.1, -0.3, 0.2),
] {
let mut world = PhysicsWorld::new();
world.gravity = Vector::ZERO;

let root = world.insert_body(
RigidBodyBuilder::dynamic()
.can_sleep(false)
.additional_mass_properties(MassProperties::new(
root_com,
2.0,
Vector::new(0.1, 0.2, 0.3),
)),
);
let link = world.insert_body(
RigidBodyBuilder::dynamic()
.translation(Vector::new(0.0, 1.0, 0.0))
.can_sleep(false)
.additional_mass_properties(MassProperties::new(
Vector::ZERO,
1.0,
Vector::splat(0.1),
)),
);
world.insert_multibody_joint(
root,
link,
FixedJointBuilder::new().local_anchor1(Vector::new(0.0, 1.0, 0.0)),
);

world.step();
let start = assembly_com(&world, &[root, link]);
for _ in 0..60 {
let rb = world.bodies.get_mut(root).unwrap();
rb.reset_torques(true);
rb.add_torque(Vector::new(0.0, 0.0, 1.0), true);
world.step();
}
let angle = world.bodies[root].rotation().to_scaled_axis().length();
let drift = (assembly_com(&world, &[root, link]) - start).length();

assert!(
angle > 0.3,
"the torque should rotate the assembly: {angle}"
);
assert!(
drift < 1.0e-3,
"a pure torque must not translate the center-of-mass (root com {root_com}): {drift}"
);
}
}
2 changes: 1 addition & 1 deletion src/dynamics/solver/staged_island_solver/helpers.rs
Original file line number Diff line number Diff line change
Expand Up @@ -138,7 +138,7 @@ impl VelocitySolver {
.generic_solver_vels
.rows(multibody.solver_id as usize, multibody.ndofs());
multibody.velocities.copy_from(&solver_vels);
multibody.integrate(params.dt);
multibody.integrate(params.dt, bodies);
multibody.forward_kinematics(bodies, false);
multibody.update_rigid_bodies_internal(bodies, !is_last_substep, true, false);

Expand Down
Loading