How to add damped constraint force to constrained dynamics simulation?

Question

I have implemented a constraint dynamics physics simulation as proposed by Andrew Witkin et al 1990, but I cannot get the initial constraint "snapping" correctly.

I implemented

$$ JWJ^{T} \lambda = −\dot{J} \dot{q} − J W Q − k_s C − k_d \dot{C}$$

(from here, where $J$ is the jacobian of $C$ with respect with particle positions, $W$ is the weight of each particle, $q$ is the velocity of the particles, $Q$ is the particles accelerations, $C$ is the constraint function value for each constraint, and $\dot{C}$ is the derivative of $C$ with respect to particle positions, $k_s$ and $k_d$ are constants to prevent drift) as:

# f = dJ @ dq + J @ W @ Q + ks * C + kd * dC
# g = J @ W @ J.T
f, g, J, c = SimulationFunctions.matrices(self.ks, self.kd, self.particles, self.constraints)

# Solve for λ in g λ = -f, minimizing ||g λ + f||, where f = dJ dq + J W Q + ks C + kd dC and g = J W J.T

r: scipy.optimize.OptimizeResult = scipy.optimize.least_squares(lambda l: g @ l + f, np.zeros_like(f),
                                                                jac=lambda _: g, method='trf')
l: np.ndarray = r.x
self.error = f"constraint {c} solve {np.linalg.norm(g * l + f)}"

I want to replace the $k_s C + k_d \dot{C}$ term with a better cost that allows the system to smoothly transition into satisfying the constraint, like a "critically damped system". I also get cases where the system simply cannot reach the constraint and explodes or cases where the constraint is satisfied but acts as a spring, causing the system to accelerate.

Here is an example with timestep 0.00001, $k_s = 0.0001$, $k_d = 0.01$ where the particle should approach, then become stationary on a small constraint:

Here is another example with timestep 0.00001, $k_s = 0.0001$, $k_d = 0.01$ where the particle overshoots and then goes back to the constraint, also I think the acceleration curve is wrong, having low acceleration initially and then accelerating and overshooting:

Here is an example with timestep 0.00001, $k_s = 0.001$, $k_d = 0.1$ where a distance constraint causes problems by adding velocity to the system (sorry the quality is bad, I had trouble uploading bigger gifs):

How should I go about getting a "critically damped system" behaviour when the constraint is not satisfied?

My code is here https://github.com/EmmanuelMess/Interactive-Dynamics-Physics-Simulations/tree/master.

Note: I have tried to use the harmonic oscillator model as proposed here but this doesn't work, as the constraint function doesn't have properties that force the system to always behave as a harmonic oscillator. To do this I changed the cost term to: $JWJ^{T} \lambda = −\dot{J} \dot{q} − J W Q − k_s C − \sqrt{4 m k_s} \dot{C} $ (I set $m = 1$ and also set all masses for all particles to 1 to simplify for the test).

Here is the first test with timestep 0.00001, $k_s = 0.001$, $k_d = \sqrt{4*k_s} = 0.0632$:

Second test (timestep 0.00001, $k_s = 0.001$, $k_d = \sqrt{4*k_s} = 0.0632$):

But for the third test it fails catastrophically (timestep 0.00001, $k_s = 0.001$, $k_d = \sqrt{4*k_s} = 0.0632$):

So this didn't work, I think it is because the acceleration of the system (with or without damping) is not taken into account when calculating the damping force. In other words, this is just setting $\ddot{C} = − k_s C − \sqrt{4 m k_s} \dot{C}$ which doesn't really make sense.

Answer 1

It seems there is no easy answer to your question, and I think what you are trying to do is not possible unless you restrict somehow the set of constraints that you want to consider.

As a simple example to demostrante that it is not possible to (or at least there is no known way to) solve the problem in general, we can consider the 1-D case.

Let's imagine that you were able to define and implement an algorithm that can simulate a particle that moves along a line until it quitely falls onto a point that is a zero of a generic function $C$. Then you can just use that algorithm to find zeros of any kind of function of a real parameter. This would be worth much more than a Fields medal.

Moreover, I would like to share some intuitions about this problem, even though I could not sketch out formal statements, nor formal proofs.

Sticking to the 1-D case, if you restrict your algorithm to constraint functions $C$ that are monotonic, twice differentiable, amd apply to one particle each, the game could be feasible (gradient descent on $C^2$ could definitely be an option, or maybe the scheme that you were trying to use could work already).

If it works, you could try to scale up to more dimensions, but you have to apply other notions rather than the monotonic one, which would not make sense when you have greater dimensionality (maybe the function should have no local minima/maxima except for those that are also zeros?).