# How to add damped constraint force to constrained dynamics simulation?

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.

$$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?

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.

• Can you type your problem using MathJax so we can understand what your objective function and what your constraints are? Commented Jul 6 at 23:51
• @nicoguaro I have typed the central equation in MathJax, I can give constraint examples, but the program is meant to be modular as to be able to implement any constraint function. I don't know how much info I should give on the simulation dynamics. Commented Jul 7 at 0:12
• The MIT OCW notes that you reference show the conventional model of a spring-mass system with viscous damping that is found in many calculus and physics texts. If you say that it "doesn't work" you need to explain in some detail why the damping model you want is not represented by this equation. Commented Jul 7 at 13:44
• @BillGreene "The MIT OCW notes that you reference show the conventional model of a spring-mass system with viscous damping that is found in many calculus and physics texts." I don't think that's correct, as critically damped system evaluate the position velocity and acceleration to generate the $k,b$ values, but the constraint term doesn't actually take velocity or acceleration into account. I have tried setting $k_d = 4 * W * k_s$ but it doesn't always work. I can add visual examples to the question if you want. Commented Jul 7 at 16:18
• @BillGreene I tried to add more info to the question, sorry if its difficult to understand, thanks for your time. Commented Jul 7 at 16:27