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

Address issues with the genral-case for multibody joints

Browse Source
This commit is contained in:
Sébastien Crozet
2022-01-08 21:09:11 +01:00
parent 9726738cd2
commit 87ec0ced40
7 changed files with 261 additions and 587 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/dynamics/solver/island_solver.rs
View File

@@ -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

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

@@ -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;