From 87ec0ced4031633e9084ee2f60d47c4cd57f7f7b Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 8 Jan 2022 21:09:11 +0100 Subject: Address issues with the genral-case for multibody joints --- src/dynamics/solver/island_solver.rs | 4 ++-- src/dynamics/solver/velocity_solver.rs | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index a862480..aeb2698 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -132,8 +132,8 @@ impl IslandSolver { if link.id == 0 || link.id == 1 && !multibody.root_is_dynamic { let accels = &multibody.accelerations; multibody.velocities.axpy(params.dt, accels, 1.0); - multibody.integrate_next(params.dt); - multibody.forward_kinematics_next(bodies, false); + multibody.integrate(params.dt); + multibody.forward_kinematics(bodies, false); } } else { // Since we didn't run the velocity solver we need to integrate the accelerations here diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index 9ecdbfb..ed1fe3a 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -145,8 +145,8 @@ impl VelocitySolver { .rows(multibody.solver_id, multibody.ndofs()); let prev_vels = multibody.velocities.clone(); // FIXME: avoid allocations. multibody.velocities += mj_lambdas; - multibody.integrate_next(params.dt); - multibody.forward_kinematics_next(bodies, false); + multibody.integrate(params.dt); + multibody.forward_kinematics(bodies, false); if params.max_stabilization_iterations > 0 { multibody.velocities = prev_vels; -- cgit