Optional violation correction in velocity solver by emilk · Pull Request #116 · dimforge/rapier

emilk · 2021-02-16T11:11:00Z

This PR introduces a restorative impulse in the velocity solver that aims to resolve the constraint violation. The default strength of this is zero, i.e. it is disabled.

This PR also introduces a multiplier for the velocity correction of the velocity solver. The default is 1 (keeping the old behavior).

Other changes / improvements

I fixed a bug in the prismatic limit joint that used to cause violations of the conservation of momentum.

I also added support for having the prismatic joints range be zero or negative (i.e. activating both sides of the prismatic limits).

Benefits

Some of the benefits of having the violation resolution in the velocity solver include:

No need for the positional solver
- Saves some CPU time
- Saves us from potential energy violations
Faster violation resolution
- Since we generally run the velocity solver for more iterations we have more iterations to converge on a solution
Better coupling between velocity and position violations

Downsides

Initial overlaps between colliders introduces a separating velocity, which makes the objects shoot away from each other rather than slowly separate.

Progress

There are five constraints in Rapier (contact, ball, fixed, prismatic and revolute). Each has a scalar and simd (wide) version. Each has a dynamic-dynamic and dynamic-static ("ground") version. Each has a 2d and 3d version. So there are 5x2x2x2 = 40 things to update in this PR.

Open questions

For contacts: How should we handle allowed_linear_error? It is currently pretty large (5 mm), and we also have prediction_distance (at 2mm).

Testing

I've found these parameters work well:

max_position_iterations: 0,
warmstart_coeff: 0.5,
velocity_solve_fraction: 0.95,
velocity_based_erp: 0.20,

(and the rest left at default)

It would be nice to be able to test this in some automated fashion. That is, run the test barrage with these params and see that the tests works approximately the same as they do with the default parameters.

...though, is there even a test barrage? There is src_testbed, but I haven't found any docs or explanation for it. Seems to be only for manual testing?

Results

In the subsequent test I compare the default parameters with the one described above.

Contacts

When stirring around in a bowl the position solver will often damp out the movement, and the objects will tunnel though each other:

rapier-contact-position.mp4

With the new velocity-based solver objects are properly displaced and move to make way for the object being dragged:

rapier-contact-velocity.mp4

Fixjoint resolution

In this test I have created a chain of bodies attached by fixjoints, with huge initial violations.

Before

The position solver is slow to converge towards equilibrium, and the bodies falls asleep before reaching it (I need to poke the root body to wake up the chain):

After

We see a much faster convergence. An analogy to machine learning comes to mind: while the position solver does gradient decent (with lr=0.2), the new solver does gradient decent with momentum.

Revolute resolution

In this test a series of boxes are connected with revolute joints (the root is connected to the world).

Before

The position solver does not seem to at all converge on a solution. I'm not sure if this is a bug or not:

After

The new velocity solver converges after some swaying:

increasing the number of velocity iteration makes it converge faster (as expected).

emilk · 2021-02-23T17:35:22Z

         * Setup limit constraint.
         */
-        let mut limits_forcedirs = None;
+        let limits_forcedir2 = axis2.into_inner(); // hopefully axis1 is colinear with axis2


The old code used two different axes here, which means the applied impulses were not opposite and equal, thus violating conservation of momentum.

When the angle constraint is close to satisfied axis1≈axis2, then either axis1 or axis2 works (or maybe we need to flip the sign of one of them; this seems to be undocumented).

If the angle constraint is very violated, then there is no single vector that makes sense here, but one could consider taking the mean of axis1 and axis2, just so as not to favor one constraint over the other. For now I think this is an improvement over what already existed, since at least we conserve momentum.

emilk · 2021-02-23T17:39:50Z

-                limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1);
-                limits_impulse = joint.limits_impulse;
-            } else if dist > joint.limits[1] {
-                limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner()));


this was the momentum-violating bug mentioned in the PR description

emilk · 2021-02-23T18:09:18Z

+        let anchor_world1 = rb1.position * joint.local_anchor1;
+        let anchor_world2 = rb2.position * joint.local_anchor2;
+        let anchor1 = anchor_world1 - rb1.world_com;
+        let anchor2 = anchor_world2 - rb2.world_com;


The names here are wildly inconsistent between files. It seems more common that let anchor1 = position1 * self.local_anchor1; (e.g. in ball_position_constraint.rs). Also sometimes the words local and world are part of the names (yay!) but sometimes not (😢 ). My approach is to try to make the smallest change possible to minimize conflicts.

It seems what is here called anchor1 is often called r1.

You are right, the names are very inconsistent. I think the naming I would prefer are:

anchor1/anchor2 to identify the world-space anchors.

r1/r2 to indentify the vector between the world-space anchor and the world-space center-of-mass.

But there is no need to unify this in this PR.

emilk · 2021-02-23T18:43:23Z

+            let lin_err = anchor2 - anchor1;
+
+            let local_axis1 =
+                Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);


maybe a UnitVector type alias could be useful (similar as for UnitQuaternion)

I agree, I will create an issue to add this to nalgebra.

emilk · 2021-02-25T09:16:31Z

Ok, I think I finally got all the kinks worked out now

sebcrozet

Thank you for this very complete PR! Everything looks OK.

How should we handle allowed_linear_error?

I dragged that parameter from what I did in nphysics years ago (which used GJK/EPA for almost all its collision-detection), so it's been a while since I revisited its usefulness and its default value.

The main reason for this parameter is to avoid the situation where the distance between the two objects is exactly (or close zero). This zero-distance configuration can cause numerical issues in the GJK/EPA collision-detection algorithms which reduces stability when you have a large pile of objects.

Let's not do anything about it in this PR as it it out of its scope. but here are various ways to handle this:

See if a smaller value continues to work properly for, e.g, piles of convex polyhedron.
Enable this threshold only for contacts involving problematic pairs of shapes (cones, cylinders, rounded shapes, and convex polyhedron).
The best way to handle this would be to get rid of GJK/EPA to use something more robust. I have a couple of ideas of what we could use instead, but we are not there yet.

...though, is there even a test barrage? There is src_testbed, but I haven't found any docs or explanation for it. Seems to be only for manual testing?

Right now there is only manual testing by running for example cargo run --release --bin all_examples3.

sebcrozet · 2021-03-02T13:56:27Z

+        let anchor_world1 = rb1.position * joint.local_anchor1;
+        let anchor_world2 = rb2.position * joint.local_anchor2;
+        let anchor1 = anchor_world1 - rb1.world_com;
+        let anchor2 = anchor_world2 - rb2.world_com;


You are right, the names are very inconsistent. I think the naming I would prefer are:

anchor1/anchor2 to identify the world-space anchors.

r1/r2 to indentify the vector between the world-space anchor and the world-space center-of-mass.

But there is no need to unify this in this PR.

sebcrozet · 2021-03-02T14:12:07Z

+            let lin_err = anchor2 - anchor1;
+
+            let local_axis1 =
+                Unit::<Vector<_>>::from(array![|ii| joints[ii].local_axis1; SIMD_WIDTH]);


I agree, I will create an issue to add this to nalgebra.

emilk force-pushed the corrective-velocity-solve branch 4 times, most recently from bd97c01 to 13e49e9 Compare February 19, 2021 08:44

emilk commented Feb 19, 2021

View reviewed changes

Comment thread src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs

emilk commented Feb 19, 2021

View reviewed changes

Comment thread src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs

emilk force-pushed the corrective-velocity-solve branch from 49f4ea1 to 1282b9a Compare February 23, 2021 13:57

emilk commented Feb 23, 2021

View reviewed changes

emilk marked this pull request as ready for review February 23, 2021 18:48

emilk mentioned this pull request Feb 23, 2021

Embark's pull requests EmbarkStudios/rust-ecosystem#20

Open

26 tasks

emilk force-pushed the corrective-velocity-solve branch from a215555 to 18d5781 Compare February 24, 2021 16:10

emilk commented Feb 24, 2021

View reviewed changes

Comment thread src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs Outdated

emilk marked this pull request as draft February 24, 2021 16:30

emilk marked this pull request as ready for review February 25, 2021 09:16

emilk mentioned this pull request Feb 25, 2021

Joint motor stability #125

Closed

emilk added 11 commits February 26, 2021 11:06

Add restorative impulse in velocity solver

21247a1

Add params.velocity_based_erp_inv_dt() helper

4162aed

Implement ball wide

3f26b46

cleanup

ede4f0f

Implemented prismatic narrow. Needs testing and close review

d3f39a9

Implement revolute narrow

48708d9

Implement fixed wide

27366e2

Implement prismatic wide

89de690

Implement revolute wide

48afbac

Implement ground wide

b94cdfa

fix

59796e4

emilk added 7 commits February 26, 2021 11:06

Fix the narrow pismatic velocity constraint

4ee09a8

fix narrow revolute velocity

9bbb081

fix the revolute wide

a082477

Fix primatic wide

4ef7b1c

simplify prismatic limits

54eae9b

Final cleanup

f517601

fix the body-body revolute angle velocity erp

115bae1

emilk force-pushed the corrective-velocity-solve branch from 07b01d1 to 115bae1 Compare February 26, 2021 10:06

sebcrozet approved these changes Mar 2, 2021

View reviewed changes

sebcrozet merged commit a74097b into dimforge:master Mar 2, 2021

sebcrozet mentioned this pull request Mar 2, 2021

Add some IntegrationParameter presets #131

Closed

hrydgard deleted the corrective-velocity-solve branch March 2, 2021 15:22

Uh oh!

Uh oh!

Conversation

emilk commented Feb 16, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Other changes / improvements

Benefits

Downsides

Progress

Open questions

Testing

Results

Contacts

Fixjoint resolution

Before

After

Revolute resolution

Before

After

Uh oh!

Uh oh!

Uh oh!

emilk Feb 23, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Uh oh!

emilk commented Feb 25, 2021

Uh oh!

sebcrozet left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

emilk commented Feb 16, 2021 •

edited

Loading

emilk Feb 23, 2021 •

edited

Loading