# Constraint for angle between connected rods

## Recommended Posts

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?

Edited by Vexal

##### Share on other sites

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.

##### Share on other sites

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.

Edited by Vexal

##### Share on other sites

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

https://youtu.be/8nay2RbWenk

Using angle constraints and sequential impulses to achieve motors connecting rods.

The bottom two segments are connected by positional constraints only; the rest of the segments are connected as rigid motors with a set target angle using angle constraints, as well as connected by positional constraints.  The middle segment moves its motor by varying the target angle of the constraint equation over time for either the x or y angle when I press a key on the keyboard. The rest of the segments connected by angle constraints are set to stay at x rotation = .5 radians.  I also apply force to the bottom two segments in this video as well as apply force to the rest when shooting it with a gun.

The angle constraint is C = Rot_2 - Rot_1
The position constraint is C = P_2 - P_1
Edited by Vexal

##### Share on other sites
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.

Edited by JoeJ

##### Share on other sites
I don't think the order dependency of euler angles is the immediate issue. In most of the video, it is only applying force and rotating about the x axis, which should make ordering not applicable.

##### Share on other sites

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

##### Share on other sites

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...

##### Share on other sites

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

##### Share on other sites
Posted (edited)

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:

https://youtu.be/tvKEm9TST4M

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:

https://youtu.be/h7hzwfgceeY

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

Edited by Vexal

##### Share on other sites

I'm a bit more ashamed than you not knowing how a physics solver actually works :)

Newton engine builds the jacobians, what i give as input for a motor is angular acceleration about an axis.

In theory we would need only need one axis and angle to get any rotation we want, but newton likes to do this for 3 orthonormal axis, so everything is defined and stable. I guess something similar applies to you.

I refer to the orientation build from this 3 axis as 'constraint space' (not sure it's the proper term), and the primary axis where the moter acts as 'error axis' (axis from Dirks 'qr' quat).

The problem is: Newton wants constraints order independent, which makes sense because we want them to be solved at the same time.

But it is impossible to decompose a rotation to multiple rotations so you can combine (multiply) them together in any order and get the initial result. Whatever order you take, you get a different result.

So my trick here limits the problem, because at least the measurement of angles is guaranteed to be order independent, and by aligning constraint space with error axis any error becomes negligible.

However, if your (or Dirks) solver solves the constraints in given order, then you don't have this problem.

The problem in your video looks like a jump over 180 degrees or similar gimbal lock issue coming from eulers. I guess not your jacobians are wrong but more likely the angle measurement.

But i also can see some unnatural twisting, also a typical euler issue. Euler angles == evil. Both Dirks or mine suggestion should fix those things.

Other than that it looks good - seems you're not far off.

I tried to work on twist / swing again, but Newton seems somehow broken at the moment due to heavy changes for new softbody feature. I need to clarify this...

##### Share on other sites
Posted (edited)

@JoeJ

I changed my angle computation to figure out the error using your suggested method.  It seemed to make it better -- the method in which it fails now appears to be consistent -- it will correctly rotate unless the target angle for any degree of freedom crosses the 180 degree mark.  When moving the target back inside the working quadrant, it becomes stable again.  Aside from that, it appears to be stable, even when it's shot with the gun.

https://youtu.be/RdfKQvJOzmg

Edited by Vexal

##### Share on other sites
Posted (edited)

I remember that when you do an Euler decomposition the middle angle can vary a lot. So you need to be careful with this. Personally I have never used Euler angles for the above reasons.

Edited by Dirk Gregorius

##### Share on other sites
I realized that I might be using axis angle, not euler angles for angular velocity. I didn't realize they're not the same thing.

##### Share on other sites
Posted (edited)

This is what I am doing to compute the velocity bias:

                const XMVECTOR targetAngles = XMVectorSet(xRot, yRot, 0, 0);
const XMVECTOR targetQuaternion = XMQuaternionRotationRollPitchYawFromVector(targetAngles);
const XMVECTOR quaternionBetweenObjects = XMQuaternionMultiply(this->object2->qOrientation, XMQuaternionInverse(this->object1->qOrientation));
const XMVECTOR quaternionError = XMQuaternionMultiply(quaternionBetweenObjects, XMQuaternionInverse(targetQuaternion));

XMVECTOR axisBetweenObjects;
float angleBetweenObjects;
XMQuaternionToAxisAngle(&axisBetweenObjects, &angleBetweenObjects, quaternionError);

const XMVECTOR targetAngularVelocity = XMVectorMultiply(XMVector3Normalize(axisBetweenObjects), XMVectorReplicate(angleBetweenObjects));
this->velocityBias = XMVectorMultiply(targetAngularVelocity, XMVectorReplicate(-FractalTreeResources::angleBB));


Whenever yRot in targetAngles is between the radian equivalent of -90 degrees and +90 degrees, it works correctly with any value for xRot.  If xRot is 0, any value for yRot works.  If xRot is non-zero, then as soon as yRot is no longer set to be in the range of -90 and +90, it explodes.  Moving yRot back into the working range un-explodes it and it becomes stable again, even after being unstable for a long time.  After this works, the values of targetAngles will come from my inverse kinematics solver, so it needs 3 degrees of freedom because the robot arm segments can have the ability to move on any axis the player sets.

Edited by Vexal

##### Share on other sites
Posted (edited)
Dirk at one point helped me go through derivation of some joints. Maybe you could open a new topic to figure out the calculus for deriving the joint (or just reuse this topic). It's not too complicated and could be a great learning experience. I would be happy to try and help with the derivation. I understand the mathematics can seem very farfetched, but the problem here is truly a math problem.

Edit: Oh and I couldn't help with quaternion based derivations, but I could help with dot product based ones. Dot product does not give a full range of motion, but can be good enough to work with. To get started, you need to remove 2 DOF (two axes) and leave the third axis unrestricted. This adds two rows to jacobian. Then we add in limit on last axis for another row (when it is active), and finally the motor as a non-holonomic constraint (constraints velocity, no position level equation). When all constraints are active (including the 3 linear constraints) in total the joint would have 3 + 2 + 2 constraints. Once a good design is found to remove these freedoms, the rest is calculus. Edited by Randy Gaul

## Create an account

Register a new account

• ### Forum Statistics

• Total Topics
628676
• Total Posts
2984178

• 13
• 12
• 9
• 10
• 10