I would like to create a constraint for holding the angle between two connecting rods to a certain angle. The constraint force will only act on one of the objects. The constraint is intended to create a motor on one of the connected objects as well as a rigid connection between the two when the motor is not activated that can be violated if enough external force is applied. The constraint does not need to keep the objects physically connected -- the objects are kept physically connected by a separate constraint connecting them at end of the rods that I already have working. The angle constraint will create a motor by varying the target angle in the constraint equation. I am using sequential impulses.

I am having trouble coming up with the correct constraint equation. I thought about having C = dot(normalize(P_1_center - P_1_end), normalize(P_2_center - P_2_end)) - 1 where the P_2 terms would be treated as constants when the derivative is taken due to the constraint force only applying on the first object as the first object is the object with the motor. However I am unsure if this will work, or where to go next. I believe that the velocity bias result for this constraint will be what ultimately rotates the object, but I am unsure if it will work as a function of dot product, or if I will be able to cap the motor force and still achieve correct results. As well, the issue with using the dot product to find the angle is that it does not discriminate on the axis of the angle -- I would like to restrict the axises with which the objects can rotate with respect to each other.

I store the rotation as a quaternion about each object's center of mass, so another option instead of dot product is to look at the quaternions directly and use C = R1-R2, but calculating the quaternion between two objects as the constraint function involves quaternion multiplication and quaternion inverse to give another quaternion, of which I am unsure how to use in the velocity bias term because my angular velocities are in euler angles. I can't figure out how to convert the quaternion result from C to a change in impulse as a quaternion can't be applied directly to an angular velocity.

Does anyone have insight on how to solve this?

You can convert a quaternion to axis-angle representation by doing something like this:

axis.angle = 2 * acos(quat.w) * (3.14159 / 180);    // angle of rotation
axis.x = quat.x;
axis.y = quat.y;
axis.z = quat.z;

normalize(axis);

Maybe that will help.

I need the angles for each degree of freedom separately. I found a function to get the euler angles from a quaternion, but I'm not sure if it works for all quadrants. As well, I'm not sure if this approach is the right way to go about this in the first place. I was trying to follow this: http://www.dyn4j.org/2010/12/angle-constraint/ but it doesn't say anything about how to actually apply the bias. My stem now appears to mostly kind of work, but it's very unstable and can take a very long time to converge to a correct state, even with 1000 iterations of sequential impulses.

This is what I have so far. It is still incredibly unstable:

I need the angles for each degree of freedom separately. I found a function to get the euler angles from a quaternion, but I'm not sure if it works for all quadrants.

Euler angles are order dependent and won't work for physics simulation this way.

What worked for me is taking the dot product of the 'rotation vector' with the axis of the coordinate frame, where rotation vector means representation like we use vor angular velocities or torques.

E.g.:

quat relativeError = rotation from first constraint space orientation to the second

float angle; vec3 axis; relativeError.ToAxisAndAngle(&angle, vec3 &axis); // like deftware said

vec3 rotationVector = axis * angle;

float xAngle = vec3(1,0,0).Dot(rotationVector);

float yAngle = vec3(0,1,0).Dot(rotationVector);

float zAngle = vec3(0,0,1).Dot(rotationVector); // yes, you could use rotationVector.z instead but just to show this works for any direction using the dot product :)

You see the 3 angles are order independent and it makes sense, but i'm uncertain if there is some kind of error involved.

However, I made a working powered ragdoll joint out of this so it's worth a try.

Edit:

You could download Newton http://newtondynamics.com/forum/index.php to see how it works in practice. The test setup is very similar to your video.

Probably has commented out the demo for testing purposes.

Open DemosSandbox project, edit newton-dynamics-master\applications\demosSandbox\sdkDemos\demos\StandardJoints.cpp

Uncomment at the very bottom:

// AddJoesPoweredRagDoll(scene, dVector(0.0f, 0.0f, -25.0f), 0.0f, 20);
// AddJoesPoweredRagDoll(scene, dVector(0.0f, 0.0f, 5.0f), 1.5f, 4);
// AddJoesPoweredRagDoll(scene, dVector(0.0f, 0.0f, 15.0f), 0.0f, 4);

My joint is in the same file. Using the engine i don't need to know how constraint equations are made or solved from my given rows, but i guess you do or can figure it out from there.

You can do a swing-twist composition. First you need to compute the orientations of each rod. This really depends how you what you are simulating. Then for each pair of rods your build the relative quaternion. The relative quaternion is the rotation of the child as seen by the parent. Let q1 be the parent and q2 be the child. Then the relative quaternion simply becomes:

qr = conjugate( q1 ) * q2

Now you can decompose this into swing and twist rotations. I spare you the math, but the formulas are very simple:

qr = qs * qt

qt = normalize( Quaternion( 0, 0, qr.z, qr.w );

qs = qr * conjugate( qr )

From there you can extract angles.

HTH,

-Dirk

You can do a swing-twist composition. First you need to compute the orientations of each rod. This really depends how you what you are simulating. Then for each pair of rods your build the relative quaternion. The relative quaternion is the rotation of the child as seen by the parent. Let q1 be the parent and q2 be the child. Then the relative quaternion simply becomes: qr = conjugate( q1 ) * q2 Now you can decompose this into swing and twist rotations. I spare you the math, but the formulas are very simple: qr = qs * qt qt = normalize( Quaternion( 0, 0, qr.z, qr.w ); qs = qr * conjugate( qr ) From there you can extract angles. HTH, -Dirk

Interesting you say that, because this was my first approach but no luck. Twist / swing worked perfectly for joint limits, but not for motors (unstable and jitter).

I concluded the reason is: Twist swing is also order dependent, although it's only two angles.

I remember now for the 3 axis method i have posted it works best to align the constraint space with the error axis, so one row do all the work and the other two just keep it stable.

Of course those things may depend on the solver as well.

Have you used the twist swing approach successfully for a motorized joint?

Maybe i have made a mistake, will try again...

Yes, all my joints use a quaternion based framework. Angular constraints, limits and motors. It is basically a derivation of Claude Larcousieres work from his PhD thesis. You need the correct corresponding Jacobian though which are not trivial to compute. If you drive the velocities into the wrong direction it will obviously not work. Here are some joint derivations, but I am not sure if they are correct. I use a different notation, so I didn't verify.

http://www.mft-spirit.nl/portfolio/publications.php

http://www.mft-spirit.nl/files/MTamis_Constraints.pdf

I am ashamed to admit that I am not fluent enough in math to follow the notation in that person's papers. Basically, if it's written in LaTeX, chances are, I won't understand it.

What is the jacobian you used? I am essentially using a two-row jacobian for the angular constraint: I combine both a positional and angular constraint, so I have

J_angle = [0 -1 0 1]

J_position = [-1 skew(leverArm_1) 1 skew(leverArm_2])]

These two constraints are run as separate constraints but I am under the belief that they should work the same as a 2-row Jacobian when running with sequential impulses. The motor is achieved by varying the constant in C so that the bias factor pushes it around its motor.

I believe that the constraint is working better now, but becomes unstable past a certain limit:

Edit: I actually just tested again, and I realize that it only becomes unstable if I rotate on more than one axis (in the above video, it rotates on segment about the x axis, and the other segment about y) -- if I use just the x axis, it does not become unstable at all, as far as I can tell. I am unsure why rotating on the y axis and x axis together is stable up to a point, but then eventually is unstable.

Here is one more video demonstrating how it is stable in the x, and stable in the y at first:

@JoeJ, am I correct in understanding that you are using quaternions for calculating the impulse using the Jacobian (in that the derivative of the quaternion is simply the euler angular velocity, without anything special done to it), but for calculating the error to use for the bias correction factor, you use the dot product instead?