Implementing hinge constraint
Implementing hinge constraint
Hi!
I'm trying to understand joints implementation based on Lagnange multipliers. Now I'm thinking about a hinge joint between two rigid bodies. Here's how I think it should be done. Please tell me if I'm missing something or if something could be done better.
Let us talk about just angular movement for a moment. I want to constraint relative angular movement of a bodyB relative to bodyA in a way that bodyB can only rotate about a vector V tighly bounded with bodyA. I.e. lets (Vx, Vy, Vz) be a constant coordinates in a bodyA frame.
Quaternion describing relative orientation of bodyB against bodyA is qR = qB * qAconj, where qA, qB are quaternions describing orientations of bodies, and qAconj is conjugation.
If V would point in Z axis direction, e.g. (0, 0, 1), then the constatints would be
qR.x = 0,
qR.y = 0,
sin(minAngle / 2) <= qR.z <= sin(maxAngle / 2).
If is not the case I'd find (a constant) quaternion qC which rotates V to (0, 0, V) to reduce the problem to that case. So finally, my constraints would be
(qC * qB * qAconj).x = 0,
(qC * qB * qAconj).y = 0,
sin(minAngle / 2) <= (qC * qB * qAconj).z <= sin(maxAngle / 2).
Then I'd differentiare equations, express derivatives of qB and qAconj in terms of wA and wB and get the Jacobian. And then run velocities corrections iterations and position correction iterations in a NGS way.
I'm trying to understand joints implementation based on Lagnange multipliers. Now I'm thinking about a hinge joint between two rigid bodies. Here's how I think it should be done. Please tell me if I'm missing something or if something could be done better.
Let us talk about just angular movement for a moment. I want to constraint relative angular movement of a bodyB relative to bodyA in a way that bodyB can only rotate about a vector V tighly bounded with bodyA. I.e. lets (Vx, Vy, Vz) be a constant coordinates in a bodyA frame.
Quaternion describing relative orientation of bodyB against bodyA is qR = qB * qAconj, where qA, qB are quaternions describing orientations of bodies, and qAconj is conjugation.
If V would point in Z axis direction, e.g. (0, 0, 1), then the constatints would be
qR.x = 0,
qR.y = 0,
sin(minAngle / 2) <= qR.z <= sin(maxAngle / 2).
If is not the case I'd find (a constant) quaternion qC which rotates V to (0, 0, V) to reduce the problem to that case. So finally, my constraints would be
(qC * qB * qAconj).x = 0,
(qC * qB * qAconj).y = 0,
sin(minAngle / 2) <= (qC * qB * qAconj).z <= sin(maxAngle / 2).
Then I'd differentiare equations, express derivatives of qB and qAconj in terms of wA and wB and get the Jacobian. And then run velocities corrections iterations and position correction iterations in a NGS way.
Re: Implementing hinge constraint
I'm stuck. Even for just one body which I want to constraint in rotation about one axis it doesn't work, though the problem seems obvious.
Let V = (Vx, Vy, Vz) be a vector coordinates in a world frame, and q is a unit orientation quaternion of a body (so that if u is a column of coordinates in a body frame (q u qConj) is a column with vector coordinates in a world frame). Let us complete V to a right handed orthonormal basis {a, b, V}. q must desribe rotation about V. So its imaginary part must be colinear to V. Which it equivalent to orthogonality to a and b. So the constraints are
a * q.im = 0,
b * q.im = 0,
(where * is a dot product, q.im  is a vectorial part of a quaternion).
The velocity constraints are
0.5 * a * (q * w).im = 0,
0.5 * b * (q * w).im = 0.
But if I use that velocity constraints I get wrong behaviour if a and b vectors are not along coordinate axes. I have no idea what it wrong here. Completion to basis, calculating of jacobians and inverse mass seems OK: as an independent test, I changed velocity constraints to obvious correct ones:
a * (q * w * qConj).im,
a * (q * w * qConj).im
(angular velocity vector must be colinear to the rotation axis)
and it worked fine. But I doubt that antiderivative of (\dot q * \bar q) exists, so I haven't a position constraint.
Let V = (Vx, Vy, Vz) be a vector coordinates in a world frame, and q is a unit orientation quaternion of a body (so that if u is a column of coordinates in a body frame (q u qConj) is a column with vector coordinates in a world frame). Let us complete V to a right handed orthonormal basis {a, b, V}. q must desribe rotation about V. So its imaginary part must be colinear to V. Which it equivalent to orthogonality to a and b. So the constraints are
a * q.im = 0,
b * q.im = 0,
(where * is a dot product, q.im  is a vectorial part of a quaternion).
The velocity constraints are
0.5 * a * (q * w).im = 0,
0.5 * b * (q * w).im = 0.
But if I use that velocity constraints I get wrong behaviour if a and b vectors are not along coordinate axes. I have no idea what it wrong here. Completion to basis, calculating of jacobians and inverse mass seems OK: as an independent test, I changed velocity constraints to obvious correct ones:
a * (q * w * qConj).im,
a * (q * w * qConj).im
(angular velocity vector must be colinear to the rotation axis)
and it worked fine. But I doubt that antiderivative of (\dot q * \bar q) exists, so I haven't a position constraint.
Re: Implementing hinge constraint
The Jacobians are simply a and b for the first body (and, of course, a, and b for the second body). Is this the result you are getting?
Re: Implementing hinge constraint
Nope. Jacobians are coordinate expressions of differentials of maps from "coordinate space" to "constraint space". In simple words, if we have n constraints C_i and m coordinates x_j, ijth matrix element J_{ij} is dC_i/dt _{v_j = 1}, where v_j = dx_j/dt. It's written in the previous post. For example, J_{12} = 0.5 * a * (q * (0, 0, 1, 0)).im, where first * is just scalars multiplication, second  dot product, third  quaternions multiplication. (For convenience while experimenting I just write short functions getC, getCdot by which Jacobian is calculated.) Is there some possibility that in the general case (a and b not coordinate system orts) semiimplicit Euler method does not converge? I didn't have time to analyse it on paper, but, if I didn't do some stupid mistake in the code, it doesn't (sounds oddly).
It seems that I found the correct constraints (fixed version from the first post). Let us concentrate just on unlimited hinge rotation of two rigid bodies about some common axis. So we need to get rid of two degrees of freedom. Let
q = rAconj qAconj qB rB,
where qA and qB are usual orientation quaternions (s.t. if a vector V has a coordinates column V' in a frame of a body B (lets name this frame B') then (qB*(0, V')*qBconj).im is its coordinates column in a world frame (lets call this frame W)), rB is a quaternion which carry out rotation from frame where the axis, bodies rotate about, direction vector has coordinates (0, 0, 1) (lets call this frame B'') to frame B', rA defined by analogy. So we have a mapThe hinge constraint would not be violated iff vectorial part of q is collinear with Z axis. I.e. the (coordinate) constraints are just
q.x = 0,
q.y = 0.
Then differentiating this equations, minding Leibniz rule, unitarity of quaternions and dq/dt = 0.5 q (0, w) identity we get
qdot = 0.5 rAconj (qAconj qB (0, wB)  (0, wA) qAconj qB) rB
and the velocity constraints:
qdot.x = 0,
qdot.y = 0.
Velocities are "decoupled" so to get Jacobian is a matter of simple quaternion multiplication. This one converges (hooray!), but there's still an issue (oh...). Everything works fine if one of the bodies has an infinite inertia (kinematic body) or (if both bodies are dynamic) if angular velocities are orthogonal or collinear to rotation axis. If angular velocities are in general case simulation blows up. And this is an uptodate WTF. This is not because of quadratic term (Coriolis', gyroscopic, ...) in the Euler equations  I ignore this term for now for the sake of stability.
It seems that I found the correct constraints (fixed version from the first post). Let us concentrate just on unlimited hinge rotation of two rigid bodies about some common axis. So we need to get rid of two degrees of freedom. Let
q = rAconj qAconj qB rB,
where qA and qB are usual orientation quaternions (s.t. if a vector V has a coordinates column V' in a frame of a body B (lets name this frame B') then (qB*(0, V')*qBconj).im is its coordinates column in a world frame (lets call this frame W)), rB is a quaternion which carry out rotation from frame where the axis, bodies rotate about, direction vector has coordinates (0, 0, 1) (lets call this frame B'') to frame B', rA defined by analogy. So we have a map
Code: Select all
q rB qB qAconj rAconj
B'' > A'': B'' > B' > W > A' > A''
q.x = 0,
q.y = 0.
Then differentiating this equations, minding Leibniz rule, unitarity of quaternions and dq/dt = 0.5 q (0, w) identity we get
qdot = 0.5 rAconj (qAconj qB (0, wB)  (0, wA) qAconj qB) rB
and the velocity constraints:
qdot.x = 0,
qdot.y = 0.
Velocities are "decoupled" so to get Jacobian is a matter of simple quaternion multiplication. This one converges (hooray!), but there's still an issue (oh...). Everything works fine if one of the bodies has an infinite inertia (kinematic body) or (if both bodies are dynamic) if angular velocities are orthogonal or collinear to rotation axis. If angular velocities are in general case simulation blows up. And this is an uptodate WTF. This is not because of quadratic term (Coriolis', gyroscopic, ...) in the Euler equations  I ignore this term for now for the sake of stability.
Re: Implementing hinge constraint
I have a fully working constraint model and that's what I have for the Jacobian for a hinge. I don't know if quaternions would make any of my calculations easier, but I don't use any. It just seems they need to be converted to a matrix so often that they are more trouble than they're worth.
Anyway, maybe you'll have better luck with the following thread. See the post by Dirk Gregorius in particular  he computes the Jacobians as the cross product of the other two vectors, but the result is essentially the same (it will be slightly different if there is already an error in the hinge constraint, but I couldn't tell you offhand if his method is better or worse than mine ... I just know that mine works stably).
http://www.bulletphysics.org/Bullet/php ... f=4&t=8686
Anyway, maybe you'll have better luck with the following thread. See the post by Dirk Gregorius in particular  he computes the Jacobians as the cross product of the other two vectors, but the result is essentially the same (it will be slightly different if there is already an error in the hinge constraint, but I couldn't tell you offhand if his method is better or worse than mine ... I just know that mine works stably).
http://www.bulletphysics.org/Bullet/php ... f=4&t=8686
Re: Implementing hinge constraint
That's because you use another constraints. Of course it's possible to use kind of (for the first body)bone wrote:I have a fully working constraint model and that's what I have for the Jacobian for a hinge.
a * e3A = 0,
b * e3A = 0,
where e3A is the unit vector rigidly connected with body A. In a body A frame it can rotate only so
d/dt e3A = wA x e3A,
where x is a cross product. So the velocity constraints are
a * (wA x e3A) = wA * (e3A x a) = 0,
b * (wA x e3A) = wA * (e3A x b) = 0.
So we have the Jacobian, etc. Lagrange multipliers would talk us how e3A must be changed (rotated), from which we'd derive how angular velocities and quaternions (in NGS) must be changed. This should work. I used the velocity constraint based on the similar idea in the second post, to make myself sure that code for computing jacobians and correcting velocities is OK. I tested it now for a two dynamic bodied joined by a hinge constraint (velocity constraint only)  seems OK. But my sence of beauty objects againt such sollution It's unnatural to mess with implicit representation of orientation quaternions (e3A or something like it) against explicit: quaternions itself. Plus, as Dirk mentions in that thread, that constraints are bistable. Which is bad by itself, plus, by continuity, it should have worse convergence (because the more constraint violated, the closer it to the "saddle point" (90 degree angle) where, by symmetry and the fact that a continuous function with different signs on the sides of a segment has zero, seems to have zero restoring force (Cdot)).
Last edited by vanger on Fri Mar 11, 2016 5:05 pm, edited 1 time in total.
Re: Implementing hinge constraint
Ahh, now I see where you're coming from. Well I'm afraid my math isn't good enough to help you. Good luck!
Re: Implementing hinge constraint
I figured out what the problem was. It was... an error in the basic matrix class from which everything linear algebra stuff like vectors inherited. The error was in access to a matrix element function. But it did not show because almost always I worked with whether vectors (matrices with 1 width) or with linear operators (square matrices). It's a miracle how such trivial error in such important place didn't appear for quite a long time. Related to the topic I suspected an error in quaternion gymnastics, sudden divergence of numerical method and so on. But not THIS. I'm in a superposition of
So finally I have a working hinge constraint. A different versions of it, actually: quaternion based described upwards and the one based directly on vectors orthogonality (similar to mentioned upwards too). I'm too lazy to compare their convergence speed experimentally (especially, if the result seems obvious), but I verified that the former manages angles violation more that 90 degrees fine, unlike the latter, as expected. Also, it's much more convenient to manage limit rotation about a common axis in the quaternion based approach. And that's what I want to talk about It seems the question deserves its own topic: http://www.bulletphysics.org/Bullet/php ... =4&t=11095
So finally I have a working hinge constraint. A different versions of it, actually: quaternion based described upwards and the one based directly on vectors orthogonality (similar to mentioned upwards too). I'm too lazy to compare their convergence speed experimentally (especially, if the result seems obvious), but I verified that the former manages angles violation more that 90 degrees fine, unlike the latter, as expected. Also, it's much more convenient to manage limit rotation about a common axis in the quaternion based approach. And that's what I want to talk about It seems the question deserves its own topic: http://www.bulletphysics.org/Bullet/php ... =4&t=11095
Last edited by vanger on Sat Mar 26, 2016 1:30 pm, edited 2 times in total.
Re: Implementing hinge constraint
Congrats! And yes, I have also encountered some of my own fundamental math code with a mistake. Usually it was either never used before, or just had a marginal impact on the end result.
Anyway, your hinge constraint sounds robust. Can it even handle rotations more than 180 degrees (like it is being wound up like a spring?)
Anyway, your hinge constraint sounds robust. Can it even handle rotations more than 180 degrees (like it is being wound up like a spring?)
Re: Implementing hinge constraint
Thanks!
No. That would require some history of a mutual movement, not just body states. So it would be not so natural. Though, it's definitely doable, but there would the same issues both in quaternion based approach and local frames based one.
No. That would require some history of a mutual movement, not just body states. So it would be not so natural. Though, it's definitely doable, but there would the same issues both in quaternion based approach and local frames based one.

 Posts: 9
 Joined: Sun May 01, 2016 12:45 am
Re: Implementing hinge constraint
Would you mind explaining how you isolated wA and wB so as to identify the Jacobian by inspection? I've tried rearranging qdot but I don't quite understand how construct the Jacobian such that qdot = Jv.vanger wrote: qdot = 0.5 rAconj (qAconj qB (0, wB)  (0, wA) qAconj qB) rB
and the velocity constraints:
qdot.x = 0,
qdot.y = 0.
Velocities are "decoupled" so to get Jacobian is a matter of simple quaternion multiplication.
Thanks!

 Posts: 9
 Joined: Sun May 01, 2016 12:45 am
Re: Implementing hinge constraint
Answering my own question here, as I've managed to come to a better understanding of the mathematics involved:
Quaternion multiplication can be represented in matrix form, such that
q * p = Q( q ) * p
Furthermore, quaternion multiplication with a pure quaternion (e.g. omega in this case) can be written as
q * (0, w) = P( q ) * w
where w is a standard 3vector. Definitions of these matrices are located here under chapter 14 and in the notation appendix, in Game Physics Pearls under chapter 9 (which is an excellent, book I might add), or one can simply derive them by hand by expanding the standard quaternion multiplication, i.e.
q * p = ( qs * ps  qv • pv, qs * pv + ps * qv + qv x pv )
where '•' is the standard dotproduct and 'x' is the standard crossproduct. Note that because quaternion multiplication is noncommutative, the above matrices will have left and rightproduct forms corresponding to the multiplication order.
Therefore, the equation
qdot = 0.5 rAconj (qAconj qB (0, wB)  (0, wA) qAconj qB) rB
can be written in matrix form as
qdot = 0.5 * Qr( rAconj * qAconj * qb ) * Pl( rB ) * wB  0.5 * Qr( rAconj ) * Pl( qAconj * qB * rB ) * wA
where the 'r' and 'l' subscripts on Q and P denote right and leftproducts, respectively. In this form, wA and wB are now isolated and we can easily identify our jacobian by inspection. Hope this helps someone!
Quaternion multiplication can be represented in matrix form, such that
q * p = Q( q ) * p
Furthermore, quaternion multiplication with a pure quaternion (e.g. omega in this case) can be written as
q * (0, w) = P( q ) * w
where w is a standard 3vector. Definitions of these matrices are located here under chapter 14 and in the notation appendix, in Game Physics Pearls under chapter 9 (which is an excellent, book I might add), or one can simply derive them by hand by expanding the standard quaternion multiplication, i.e.
q * p = ( qs * ps  qv • pv, qs * pv + ps * qv + qv x pv )
where '•' is the standard dotproduct and 'x' is the standard crossproduct. Note that because quaternion multiplication is noncommutative, the above matrices will have left and rightproduct forms corresponding to the multiplication order.
Therefore, the equation
qdot = 0.5 rAconj (qAconj qB (0, wB)  (0, wA) qAconj qB) rB
can be written in matrix form as
qdot = 0.5 * Qr( rAconj * qAconj * qb ) * Pl( rB ) * wB  0.5 * Qr( rAconj ) * Pl( qAconj * qB * rB ) * wA
where the 'r' and 'l' subscripts on Q and P denote right and leftproducts, respectively. In this form, wA and wB are now isolated and we can easily identify our jacobian by inspection. Hope this helps someone!