From 62379de9ecc81fb42b7c2a0d2b8e3e1b02d63f38 Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sat, 25 May 2024 10:36:34 +0200 Subject: feat: add simple inverse-kinematics solver for multibodies (#632) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 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 --- src/dynamics/solver/velocity_solver.rs | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/dynamics/solver') diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs index b0bafb8..9d5c279 100644 --- a/src/dynamics/solver/velocity_solver.rs +++ b/src/dynamics/solver/velocity_solver.rs @@ -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 -- cgit