NewtonUserJointAddAngularRow : what does it do ?

[blackbird_dream]

Hi all friends,
pls help me understanding the NewtonUserJointAddAngularRow function.
I mean what is it supposed to do in mathematical terms.
Here below, the example of a hinge joint (this example works, I tested it).
I undersand the 3 lines with NewtonUserJointAddLinearRow, ok.
the 2 other constraints are said to

enforce the constraint condition at pin direction considered fixed to both bodies

. Ma che, cosa significa ?

Code: Select all

 //compute the pin matrix of each body in global syst
  SetGlobalMatrixOne(matrix0, matrix1);

  // Restrict the movement on the pivot point along all tree orthonormal direction i.e. lock the pinpoint
  NewtonUserJointAddLinearRow(Joint, @matrix0.V[3].V[0], @matrix1.V[3].V[0], @matrix1.V[3].V[0]);
  NewtonUserJointSetRowStiffness(Joint, Stiffness);
  NewtonUserJointAddLinearRow(Joint, @matrix0.V[3].V[0], @matrix1.V[3].V[0], @matrix1.V[1].V[0]);
  NewtonUserJointSetRowStiffness(Joint, Stiffness);
  NewtonUserJointAddLinearRow(Joint, @matrix0.V[3].V[0], @matrix1.V[3].V[0], @matrix1.V[2].V[0]);
  NewtonUserJointSetRowStiffness(Joint, Stiffness);

  // two rows to restrict rotation around the parent coordinate system
  NewtonUserJointAddAngularRow(Joint, CalculateAngle(matrix1.V[0], matrix1.V[0], matrix1.V[1]), @matrix1.V[1].V[0]);
  NewtonUserJointSetRowStiffness(Joint, Stiffness);

  NewtonUserJointAddAngularRow(Joint, CalculateAngle(matrix1.V[0], matrix1.V[0], matrix1.V[2]), @matrix1.V[2].V[0]);
  NewtonUserJointSetRowStiffness(Joint, Stiffness);         

[Julio Jerez]

umm that's a tall explanation but here we go again.

from the newton-Euler equation of motion. the rotation of a rigid body in free form is govern by equation. (the symbols are metrices and vector, I assume you can apply dimensional analysis.

I = moment of inertia
w = angular velocity
L = angular momentum.
T = torque

I * W = L

this equation is valid in all reference frames.
to get the equation on motion we take the derivative of that equation with respect to time
so.

d(I * w) / dt = d(L) / dt

d(I) / dt * w + I * dw / dt = T (torque acting on the body)

we now most figure out what each term means and how to calculate them.

for d(I) * dt =
I is the inertia matrix and can be writen R * Il * Rt
the is the rotation matrix time the local inertia time the transpose of the rotation matrix
you can expand that derivative using the expression of the derivative of a rotating item is the angular velocity cross product the item, this is
d(R) = ~w x R (where ~w is the angular velocity expressed in matrix notation for combinience because since the matrix is three orthonormal vectors, we can get the derivative with one matrix multiplication. The rest is the same, Il is constant, and the transpose if the same produce transpose

the derivation of that expression is a textbook exercise, and you can look that on in wikipedia
that was Euler demonstration.

however, once you understand the concept it comes down to the definition of the derivative of a rotating item, basically it if the angular momenton is a quantity them its time derivative is the cross product of the angular velocity of the angular momentum time the angular momentum. so the left side of the equation becomes.

d(I * w) / dt = d (L)/dt() = T

I * d(w) /dt + w x L = T

now the expression w x L is what we know as gyroscope torque and it is the expression that enforces conservation of angular momentum. you can think of it as a constration that inforces consevation of angular momentum.

so the final equation for a free spinning body is

I * d(w) /dt = T - w x L

here you can see that on the left side the values are function of the angular acceleration, where on
the right size, the acceleration is not part of the equation.

this is the equation of motion for any spinning rigid body, and to get a body change its motion
the only accion we can take is to change the external torque T.

since torque are additive, we can break the external torque into a sum of partial torques, so

T = Text + T1 + T2 + ...... Tn

the Tn are constraints contributions, and they can be of any kind, for example
you can apply a damping T(i) = -K * w
a spring damper T(i) = k * w - k(angle)
a motor, T a const motor.
and so on.
basically, anything that can be expressed as a torque

one such constraint is the rigid body constraint.
that is, you enforce the body to spins around an axis shared between two bodies.
The value of tgat constraint is governed by Newton's third law of motion, for each action and equal and opposite reaction.

if you are with me until here, in the next post we derive how we apply a rigid body constraint which is what you are asking for, but to explain the math it needs this background.

[Julio Jerez]

ok now let us see how we make so that the body spins around an arbitrary axis fixed on the body.
from the Torque equation

T = Text + T(1) + T(2) ... T(n)

we can decompose it into the torque that we know and the reaction torques around that pin.
we are going the define that reaction toque as

T(i) = g * J

that is torque will be a magnitude g and direction vector J
so the equation reduces

T = Text + g * J

if there are more pins, them torque is just the sum of all constraints, then the equation becomes.

T(i) = Text(i) + sum (g(i, k) * J(i,k))

in the equation J (the pin direction is known by definition. it is a pin fixed to the body. meaning when the body moves, the ping goes with it.
where i in the index of the body, and k the index to the other body sharing tge ping

but we also said that the constraint is applied between two bodies, so the same restrictions apply to the secund body as well, meaning the pin most also be fixed on the secund body.

therefore, now for the secund body the Torque equation is

T(k) = Text(k) + sum (g(k, i) * J(k,i))

so now we have two equations, one for each bodies, let us copy them here

T(i) = Text(i) + sum (g(i, k) * J(i,k))
T(k) = Text(k) + sum (g(k, i) * J(k,i))

at this point if we have those two forces and we apply them to the bodies, we expect the body to spin around the pin. but we still do not know how to calculate g

in the two equations above, the only unknown is g (the magnitud of the constraint torque.

j(i,k) is the diriection which is a parameter
and j(k,i) is determined but the third law of newton.
that is for a hinge for example

j(i,k) = -j(k,i)

but for some other joint it can be something differents.

we are now going simplify the notation, instead of using 3 x 1 vectors we are going to use 6 x 1 vectors

that is say a vector has components

v0 (x0, y0, z0)
v1 (x1, y1, z1)

we are placing the component is one dimentional vector

v (x0, y0, z0, x1, y1, z1)

so by doing this, the two equation above becomes one like

T(x0, y0, z0, x1, y1, z1) = Text(x0, y0, z0, x1, y1, z1) + g * J(x0, y0, z0, x1, y1, z1)

so the torque between the two body is made from the torque on body I and the torque on body k

and now we can just drop the component and simply call it

T = Text + g * J

this can also be done to express the two bodies

we now reduced the problem to simple figuring out what should be the value of g that will force body i and body k to spin around direction vector J

next post will derive that equation.

[Julio Jerez]

ok, need to figure out the value of g.

we can now make a one-dimensional geometrical constraint equation on the angular velocity of the body

that is the angular velocity of between the two boding alone the pin is given by

j(i,k) Dot (w(i)) + j(k,i) Dot (w(k)) = r

since these are dot products, this mean r is a scalar value.
we can use the 6-dimensional notation and write

j dot (w) = r

and using. for dot product

we can get a concise equations

J . W = r

J and W are 6 x 1 vectors

we now have one equation equation for the torque, and a set of one dimensional constraion equation for each direction that we want to restrict the movement. we can wriet teh here

1) I * d(W)/dt = Text - w x L + g * J
2) J . W = r

but we still have a problem because while equation one is
expressed for the derivative of W, equation 2 is expressed on W

there are two ways to solve the problem
1- you can discretize equation one by a dt,
and you get

1) I * w - I * W0 = (Text - w x L + g * J) * dt
2) J . W = r

now you have a set of linear equation on w and g
this is known as impulsive methods.

this happens to be very fast but it is not really physically based, because integration equation 1 lead to a path that is unconstrained, and the quantities involved are nonlinear.
beside while the arithmetic of linearizing equation 1 is valid, the physics is just wrong, simply because there is no such thing the law of impulses for bilateral joints.
there is the empirical law of restitution, which empirically say when two bodies collide the momentum after the collision is equal to the momentum before multiplying by a lost.
so for rigid body collision the impulsive method does well, provided the time step is small.
the reason for that is that, on the right side of equation 1.

integral of (Text - w x L + g * J) * dt

as dt is small, them the nonlinear term banish, that is

(Text + w x L) * dt ~= 0

while the integral of the torque is just an impulse, that is

g Jt × dt = Q
the impulse applied so that leave equation one as

1) I * w = I * W0 + g Jt * dt


The secund method, which is the one Newton used
is, we calculate the time derivative of equation 2, that is

d (J . W) / dt =d (r) / dt

d (J)/dt . W + J . d(W)/dt) = d(r)/dt

and here is where the concept that vector J is fixed to the two bodies play a roll.
since the bodies moves on an inertial frame and the value of J is fixed in the frame, the J derivative is zero.

that is d(j)/d(t) = 0;

if for example the frame was non inertial like a spinning earth or been on a ship at sea and the ship is the frame of reference, them d(j) / d(t) inherit that Change of rate and become gyro, coruolis and so on.
these details are deal with per joint type. for normal joints like hinges, slider and so on d(j)/dt = 0;

so now equation 2 becomes

J . d(W)/dt) = d(r)/dt

and if we copy the two equations again, we get

1) I * d(W)/dt = Text - w x L + g * J
2) J . d(W)/dt) = d(r)/dt

two equations with two unknown.
let us call d(w)/dt the angular acceleration a
them the equations becomes

1) I * a = Text - w x L + g * J
2) J . a = d(r)/dt

but we still do not know what the value d(r)/dt is.
but that's quite easy, since the value is just a relative acceleration along the pin.
this value is specified by the Joint type. for example

if the joint is rigid
d(r)/dt = 0, r = 0

is was a spring damper
d(r)/dt = spring * angle - damper * r

if it was a motor,
dr / dt = some ramp until r it reached some value them in become zero.

and you can try different combinations based on the joint type.

the next post is how to calculate g from equation 1 and 2.

[Julio Jerez]

ok, last post how to get g

we now have these two equations

1) I * a = Text - w x L + g * J
2) J . a = d(r)/dt

let us call d(r)/d(t) = b
where b is the joint feature, a rigid joint, a motor, a spring damper and limit and so on.
the system becomes

1) I * a = Text - w x L + g * J
2) J . a = b

from here is just plain linear algebra.

from equation 1 we can factor a to get

a = I^1 * (Text - w x L) + I^1 * J * g (g is a scalar so it commute)

now from substituting in equation 2

J . I^1 * (Text - w x L) + I^1 * J * g = b

and moving terms that do not depend on g to the right side

3) J . I^1 * J * g = b - J . I^1 * (Text - w x L)

now the equation is still expressed in dot vector product, but we know that we can express a dot product as a product of two metrices as:

a . b = a * bt
or
a . b = b * at

where t is the transpose of a n x 1 vector, so using this identity in equation 3 we get
and let us call I^1 = invI we get

4) J * invI * Jt * g = b - invI * (Text - w x L) * Jt

and that's the linear system of equation that the engine solves in every step

J * invI * Jt

is a positive semidefinite matrix and g is a scale column vector with one entry for each constraint.
once you solve for g

you go back to the newton-euler equation and now you have all the reaction torques acting on the body. Therefore you can integrate each body indendently as if it was a free body

this is, the torque equation is now complete

I * a = Text - w x L + g * J

that is

w1 = w0 + dw = w0 + invI * (Text - w x L + g * J) * dt
q1 = q0 + dq = 1/2 w1 * q0 * dt

and that all there is to it.

[Julio Jerez]

final thought.

notice that the newton integration method in no way breaks Newtonian laws of physics.

to summanize we simple calculate the free force, we solve using that all faction free body diagram
https://en.wikipedia.org/wiki/Free_body_diagram

get the forces, add them for each body we integrate the acting force and torque.

now where we invent brand new laws of physics, stuff like:
shock propagation waves,
adaptive forces,
position based physics,
velocity and position passes.
the clip impulse,
long distance constraint,
and goes on and on.

and if you what to know how insidious and pernicious this is among the self-appointed experts
here is an example.
https://classic.gazebosim.org/tutorials?tut=parallel&cat=physics#PositionErrorCorrectionThread
and they are not the only one doing that, that's the way all of the so popular libraries actually work.
a tail tale clue is when the ask you to set position passes, or velocity passes, or Adaptiv forces, and some kind of parameter like that that you will not find in any textbook.

these people for more than 15 years prefer to use a method that clearly violated of the law of classical physics (conservation of momentum) rather than try an engine like Newton, that at least claim and try to do it right, no withstanding bugs) simply because they have a prejudice against my persona.
and when you call fault on that, is simply makes it worse with fan base piling on.

what is scarier, is the dunning Kruger arrogant of some of these people.
they are so confident that they are rewriting the narrative, they are writing books and papers about these fast bran new law of physics, simple because they are fast.
As if it become true because it was on a book or a paper.

[blackbird_dream]

ok thk you very much.
I took the time to read your msgs carefully and for me that's all clear (I stumbled across the product derivative of the momentum but I see you made the correction). I know better now how you handle the constraints dof.
However, my question is still pending. I mean, I understand that to build a hinge you need to lock the 2 others axis with 2 newtonuserjointaddangularrow constraints. Is that true ?
If so, geometrically it's not intuitive. It's difficult to imagine the torque actions in space.
Additional question: what does the relativeAngleError mean precisely ?
Thks.

[Julio Jerez]

my question is still pending. I mean, I understand that to build a hinge you need to lock the 2 others axis with 2 newtonuserjointaddangularrow constraints. Is that true ?

There are two methods for doing constraint dynamics.
Generalized coordinates, and reduced coordinated.
In general there are few generalized coodinate methods:
Lagragian, virtual displacement, A very popular special case of lagragian is the Featherstone.
But these methds are quite hard to implement as a tool building blocks objects. The reason is that they do not play well with under and over determined systems, this is called holonomizity. Which basically means, the number of degree of freedom most be equal to the number of geometrical constraint.
These methods are also preferred by software packages that do symbolic dynamic. They can generate differential equation from energy function and geometrical constraints.
This method also deal with non lineal constraint better.
At the high level, you can think of these method as a top down tree, where each node has one parent, and each child can only move on the local scapce of it parent.
So the math is derived on a subset of degree of freedom.
This lend to a recursive evaluation of the system.

The problem with these system is that ther are very limited, the moment a body interact with anything that constitute a loop in the tree, and the recursion is not longer possible.

The second method is reduced condinates.
This is the method that comes out straight of newton equation of motion.
The movement of all bodies is on the same space, inertial or non inertial.
Each body is described by six dof, and can only change its state of motion by a Generalized force.
This is the method taught in most of high school and under graduate classes in college, and is its not until you take graduate classes on mechanic that you learn Generalized systems.

In reduced system, you apply a force to change the motion, you can consider that preventing a relative motion is a change of motion.
So what you do is that you add a constraint forces to all the dof you want to freeze, and let the one you want the motion free or partially free.
So a hinge will add 3 linear constraint to prevent realative motion, and two angular constraint to prevent rotation around any axis perpendicular to the ping.
Since you can described any ping perpendicular to the hinge axis with two mutualy perpendicular unit ping, you only need two constraints.
So yes, a free hinge will has 5 degrees of restricted degrees of freedoms in a reduced system. As opposed to a hinge is say virtual force that can specify with 1 dof.

Following the 6 x 1 and 6 x 6 vector and matrix notation.
functions newtonuserjointaddangularrow and newtonuserjointaddlinearrow
are the implementation of the vector components of equation

J * invM ×Jt = c

that form part of a larger sparce matrix for the entire system of interconnected bodies.

The functions populates C, J, Jt. alone with some precomputed values based on the joint type. for example, say you want to define a hinge.
A hinge is defined by one free degree of freedom, using reduced coordinate system.
it means we restrict 5 degrees of freedom and leave one free, we can also allow for the six dof to have some control kind of motion.
in code this translate to 5 or 6 constraint equation into the general force equation
and here is where the 6 x 1 notation help because we can say that a for pair is a vector of 6 x 1 dimension where three are force and three are torque. so instead of calling it torque we call it F

that is F = text + g0 * J0 + g1 * J1 + g2 * J2 + + g3 * J3 + g4 * J4

if we derive this, We will quickly realize that there is a pattern to do this that generalize for any arbitrary arrangement of rigid body contrarians.

for a hinge:
three rows are three non colinear points to enforce zero relative acceleration and velocity and two mutually perpendicular angular axis to the desired ping of rotation to enforce zero relative angular acceleration and angular drift.

optionally it can emit the six dof in the case the hinge is something like an actuator or whatever angular behavior you want.

once all those vectors are set, the rest in the linear algebraic solution of that larger sparce matrix.
In the larger sparse matrix, in which each joint is a block diagonal with two off diagonals off block matrices.

[Julio Jerez]

Relative angular error are heuristic tweak to solver the matrix.

For example say you set a joint limit, if you wait untill the joint hit the limit to apply the correction, you now has a variable penetration error.
So you need to add a penalty that convert the penetration error into a recovering force in one timestep.
that's fine for that timestep, but because conservation of momentum what comes next is that the relative motion cause the two object to bounce back in a unpredictable way because the penetation and the impact speed are also kind of random.

But if you monitor the error angle, the moment the angular error is within that range, you can attenuate the reaction by Applying a correction proportional to the distance to the limit before it hit the limit.
An example of this in real life is elevators doors.
They usually have a switch before the last stop, that when triggered, the motor reduces the speed, so that when it hit the stop, does not bounce back too violently.

BTW, for newton 4 most of those thing has changed since now joints can have more than 6 dof, and the complementary solver determine which dof are the essential.

If so, geometrically it's not intuitive. It's difficult to imagine the torque actions in space.
Additional question: what does the relativeAngleError mean precisely ?

yes that is correct, and yes, it is very difficult to imagine torques in space.
in fact, it is totally counter intuitive, except for the most trivial but uninteresting cases.
if you have two angular velocities acting of a body with a non uniform inertia on two perpendicular axis,
they are like a magnetic wave, they generate self-induced torque on the third axis.
this is because the inertia of a body changes with the orientation, so the product

I0 *W0 = I1 * W1

that is, momentum before a step equal momentum after the step is a function of a set of variables for each roads.
We know that in general, the inertia changes with the orientation so the only way the identity can be maintained is if the angular velocity also changes with the change of orientation in such way that the product with the inertia remain constant, we also know, by the secund law, the only way to change the velocity is by adding a torque, so the question is where that torque comes from?

It turns out that it is that mysterious torque -w x L
that has not physical explanation but comes from the mathematics.
This is what makes it not intuitive.

what I do is that I test with simple cases that I can predict the expected results and I trust that it continues working for the cases that are not so intuitive.

that's how I get results like the video below, and for me, if it can comes close to that, I take it as good enought acceptable.
https://www.youtube.com/watch?v=UlErvZoU7Q0

but to give you an idea how counter intuitive it is, look at this
https://www.youtube.com/watch?v=BCVQFoPO5qQ

if you get that behavior in some simulation, and you do not know that's a real behavior.
you will think is a bug, but it is not.

[blackbird_dream]

thks for your explanations. Great !!
Your simulations of inertial effects are the evidence your engine is exact.
coming back to the hinge. Is it to say that a double hinge can be modelled by one angular constraint only ?

[Julio Jerez]

Yes that is correct.
A double hinge will remove four degrees of freedom from two connected bodies. Three linear and on angular.
The linear are intuitive to visualize geometrically.
The angular is more difficult

For a double hinge the geometrical interpretation is.
The geometrical constraint is:
You fix one axis to one body, and you fix another axis to the second body, and you inforce those two axis to be perpendicular to each other.
Once you write the code to establish that geometric condition, the third axis is the cross product of the two axis fixed to their respective bodies.
So the constraint simply cancel the angular error and angular velocity between that axis and the ideal plane formed by these two axis.
The job of the joint them, is to calculate that axis, the angular error and angular velocity error from the geometry of the system and the code calculated, the acceleration, and mass matrix elements, of the block 6x6 and 6x1 matrix.

Noticed that exact is not really a correct description.
What is exact is the solution of the block matrices to get the reaction forces and torques needed to be added to each free body diagram of each rigid body.
The integration is still numerical an as such, accumulates numerical integration errors.
What is correct is the solution of construction of bilateral joints, because it uses direct method, not just for the simple block submatrices, but for the complete articulation, unless the app decides to use the iterative solver.

In newton construction of joints, and their immediate contacts are solved exactly via direct complementarity solvers, the contacts are solved iterative.
So the errors are push to the contact joints.

Regarding the numerical integration.
For free body, linear motion the integration in semi implicit and angular motion is implicit.

For contacts, intermediate steps is a variance of Runge Kuta 4, the variance is that we use weight coefficient
0.25, 0.25, 0.25, 0.25
as opposed to the optimal
1/3, 1/6, 1/3, 1/6
This makes a very big difference on intermediate partial vectors, making the solver many time over slower and memory hungry for a very marginal game on stability that is almost ummeasurable.

I recommend that you try newton 4.0
Over the years, there has been lot of refinement on those things, and in this last version I decided for not compromises and couching of the end user.
The engine exposes everything, and the shortcuts are places on the user side.
So many things that were previously part of the engine are now done on the demos side and it is the end app that decide the level of accuracy and stability that wants to model.

[blackbird_dream]

I wrote 'exact' in the sense numerical methods serving TRUE laws of Mech, bearing in mind necessary processing of 'singularities' and not impaired physix to accomodate simpler or faster numerical methods. If not clear, forget it, anyway was'nt criticism but rather flattering.


Last but not least. Yes i'd be glad using NDv4. That was also Dave's advice. My concern is I moved back to free pascal for sake of simplicity with opengl stuffs. And I understood there is no wrapper available yet. Am I correct ?
Thks so much for your support.

[Julio Jerez]

I know, I know.
but I have to make those clarification.
There are people with agendas in many forums dedicated to recommending people what to use, that use those quotes out of context to make the engine untrustworthy.
The goal is that a person seeking advice to not even look at the library.

Anyway. On the pascal, I use Swig to make automatic wrappers.
unfortunately, swig can only translate from cpp to languages that can be described with LARS parsers.
Pascal, Fortran, Ada, and few other are the old guard languages that do not fix that model.
so no, there is not Pascal wrapper.

[blackbird_dream]

Haven't you tested http://www.fmxexpress.com/create-wrapper-interfaces-for-c-and-c-libraries-using-swig-with-delphi-support/? I'm sorry it's just a quick search, I'm not expert at all in computing.
How did you do with NDv3 I'm currently using ?

[blackbird_dream]

some clarifications I need for 100% understanding:
you wrote:

in the equation J (the pin direction is known by definition. it is a pin fixed to the body. meaning when the body moves, the ping goes with it.

Is it the same pin of the function below ?

Code: Select all

void NewtonUserJointAddAngularRow( const NewtonJoint* joint, dFloat relativeAngleError, const dFloat *pin)

if the pin moves as the bodies then how can it be:

since the bodies moves on an inertial frame and the value of J is fixed in the frame, the J derivative is zero.

J is fixed to a moving body and fixed in the frame at the same time. Moreover as result the pin axis can move in space if the 2 bodies are free to move. Certainly it's only the way you formulated this that confused me, but I'm confused.
Globally I understand you compute the acceleration so that the component of the relative angle between the 2 bodies is constant along one given pin.
I tested the double hinge, it's ok with 1Rdof constraint.