NewtonUserJointAddLinearRow sanity check

A place to discuss everything related to Newton Dynamics.

Moderators: Sascha Willems, walaber

NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Sun Dec 28, 2008 9:04 pm

Sorry but I'm starting to lose the plot.

Say you've got a point on the parent: [x1,y1,z2] and a point on the child: [x2,y2,z2]. Additionally, have 2 axes [a,0,b] and [-b,0,a]

Then do two NewtonUserJointAddLinearRow with the two points on each body, with each of the axes.

OK - am I right in thinking that this procedure will not move the bodies in any way along the y axis (i.e. y1 and y2 will remain unchanged)? Or have I gone mad?

Thanks, Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: NewtonUserJointAddLinearRow sanity check

Postby Julio Jerez » Sun Dec 28, 2008 11:59 pm

can you rephrase the question?
Julio Jerez
Moderator
Moderator
 
Posts: 11154
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Mon Dec 29, 2008 7:18 am

I'll try. I'm trying to restrict movement in a plane when a reference vector from the pivot has rotated too far (past a max_angle). I know I'm getting my angles correctly but something's wrong in the data I'm giving to AddLinearRow.

Basically I have a point of where the ref vector is (past max_angle) and where it should be (at max_angle) and I want the error to be driven to zero within the plane of rotation. My problem seems to be the pin vectors are sending to 2 AddLinearRow calls.

I'm starting with the basic case of rotation happening in the xz plane. If I feed [1,0,0] and [0,0,1] as the pin vectors to AddLinearRow then it works. However, because the joint will not be constrained so that this rotation has to be in the xz plane, I need to give different pin vectors depending on the rotation of the joint's pivot/pin matrix. I am using the two vectors that are in the plane of rotation (and orthogonal). These do rotate round with the rotation but to my mind still represent an orthogonal pair in the plane. Therefore I don't see why using them is not working for me.

In the basic case, instead of feeding [1,0,0] and [0,0,1] I'm feeding something like [a,0,b] and [-b,0,a]. Strangely, this is giving incorrect results including movement in the y-axis.

I hope this makes sense.

Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Mon Dec 29, 2008 7:38 am

OK, I've just tested it by having an orthogonal pair that's primarily generated from the axis of rotation (i.e. two fixed axes perpendicular to the axis). This pair does not rotate round in the plane. And it works, in the general case as well. I guess I can do that but I'm puzzled as to why my other method did not work.
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: NewtonUserJointAddLinearRow sanity check

Postby Julio Jerez » Mon Dec 29, 2008 10:31 am

In general when the geometry of the problem is a faithful representation of the joint, the function will work.
The linear row will drive the projected distance of two points over a direction vector to zero.
Basically is does this
Let p0 be a point on body 0
Let p1 be a point in body 1
Let dir be a pint vector
Linear row do the following operations

Error = p1 % dir – p0 % dir

Then it will calculate an acceleration that will cancel the relative velocity and the relative distance between those two points.

Reasons for the function to fail are:
-Passing more than one linear row with pin vectors that ate no perpendicular,
-permutation of p1 and p0 leading to a positive error.
-picking the coordinate system in the wrong body, for example dir can p1, and p1 can be correct but the correction do not reduce the error between p0, and p1.

I myself ussually have to try more than oen to get a joint right, because what seen logical some time is not.
can you post the code? mayeb I can see why it does no work.
Julio Jerez
Moderator
Moderator
 
Posts: 11154
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Mon Dec 29, 2008 12:12 pm

What I'm trying to create is a generalised ball and socket joint where both bodies rotate around the 3 axes of the pivot. They don't have to be axis aligned in any way. Each rotation should have a min and max angle. Here is the code that limits the max angle around the pivot's m_front axis:

Code: Select all
   y = global_pin_pivot1.m_right % global_pin_pivot0.m_up;
   x = global_pin_pivot1.m_right % global_pin_pivot0.m_right;
 
   angle = atan2(y,x);
   min_angle=mMinAngle.X*PI/180.0f;
   max_angle=mMaxAngle.X*PI/180.0f;

   if (angle > max_angle + 0.001f)
   {
      dVector point_on_main_body(global_pin_pivot1.m_posit +
                           global_pin_pivot1.m_up.Scale(sin(-angle+max_angle)*MIN_JOINT_PIN_LENGTH) +
                           global_pin_pivot1.m_right.Scale(cos(-angle+max_angle)*MIN_JOINT_PIN_LENGTH)
                           );
      dVector point_on_child_body(global_pin_pivot1.m_posit +
                           global_pin_pivot1.m_right.Scale(MIN_JOINT_PIN_LENGTH)
                           );

      dVector reference(0.0f,1.0f,0.0f);
      if (fabs(global_pin_pivot1.m_front % reference)>0.5f) // remember that both are normalised
         reference=dVector(0.0f,0.0f,1.0f);
      dVector pin1(global_pin_pivot1.m_front * reference);
      dVector pin2(global_pin_pivot1.m_front * pin1);

      NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_main_body[0],&point_on_child_body[0],&pin1[0]);
      NewtonUserJointAddLinearRow(mpNewtonJoint,&point_on_main_body[0],&point_on_child_body[0],&pin2[0]);
   }


The first part of the code determines the current relative angle. I have tested that this works for all angles (-pi to pi). The limit code works for angles less than 88 degrees. However, above this it goes crazy. This is the same situation as what I began this thread except in that instance I was setting pin1 = global_pin_pivot1.m_up and pin2 = global_pin_pivot1.m_right and it was going crazy no matter what max_angle was. This time I'm creating orthogonal pin1 and pin2 relative to global_pin_pivot1.m_front.

As I said, this does work up to 88 degrees. I've tried different pins (e.g. [0, sqrt(2), sqrt(2)] but it makes no difference. When it blows up, there is movement in the m_front axis (as well as the other two) which I find strange.

Here is some data at a blow up point:
angle = 1.5720401568444156
max_angle = 1.5707963705062866
point_on_main_body = {m_x=0.00000000 m_y=50.800018 m_z=-7.9538673e-005}
point_on_child_body = {m_x=0.00000000 m_y=50.799976 m_z=-0.062268838}
pin1 = {m_x=0.00000000 m_y=0.00000000 m_z=1.0000000}
pin2 = {m_x=0.00000000 m_y=50.799976 m_z=-0.062268838}

Any help greatly appreciated. Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: NewtonUserJointAddLinearRow sanity check

Postby Julio Jerez » Mon Dec 29, 2008 2:45 pm

JoeWright wrote:What I'm trying to create is a generalized ball and socket joint where both bodies rotate around the 3 axes of the pivot. They don't have to be axis aligned in any way. Each rotation should have a min and max angle. Here is the code that limits the max angle around the pivot's m_front axis:

The geometrical interpretation of a joint like that is much harder than you think.
I will tell you tah bad new first then I will tell you the good new.

The reason is that a geometrical representation you need a frame of reference to make measurement.
For example a hinge is easy because you can fix a pin either on body1 and the angle the relative angle is the body2 rotates around that pin is and independent variable.

For a double hinge of universal you can fix a pin because you can fix a pin on body 1 and a pin on body2 the it is like two hinges, when the angle body1 rotates around the pin fix on body 0 is independent of the angle that body0 rotates around a pin fix on body1, as long as pteh tow refence ping are perpendicular.

For a general ball and socket you have three pins but only two bodies. So you it is difficult to fix any pint on anything.

You will have to pick one frame as the reference frame and then measure the relative angle other body rotate around that frame.
That brink the problems that to calculate the relative angles of a frame relative to another frame depend on the convention other of rotation and it will always run into the citation that you will loss a degree of freedom.
This is what the axis you select to project over the other axis will be aligned with another axis preventing form extraction the angles.
That is in regard for extraction the relative angle from the matrices, but there is a much bigger fundamental problem.

This comes for the fact that independent angular rotation around fix axis do not commute. This is very, very important.

Say you manage to successfully decompose you matrix into three angles and you summit you row so that the toques around that angle are calculated.
Here is the very big problem, the engine will calculate three torques and add then together alone with any other torque that was applied to the body to produce a final net torque. That is what the law of physic say. The problems I that the three angle you calculate will only produce the desired rotation when they are applied to the frame of reference in the order in which you calculated them.
So the only way the that method will work was if the engine applied a torque around the first axis and rotate the body, then a torque around the second body and rotate it, the a torque around the third axis and rotate it, but that is very different that adding the three torque and applying just one final torque to the body to rotate the relative frame.
I hope this was clear to you.

The reason is that when you apply a torque to a body the body to no rotates following any order of Eulers angles the body rotate following the shorter angular path. Fortunately the Rodriguez parameter allows determining the shortest angular path very easy. But the visualization of the joint is different and you will.
Here is what you do to get a control ball and socket.
You need to read the matrices in quaternion form (you can use the newton to get the matrix in quaternion from)
NewtonBodyGetRotation(body, rotation);

You need to establish a relationship between the two matrices of each frame that you want to be enforced. Say you picj body1 as you reference frame, then

Q1 = RelQ * Q0
Q0 = quaternion rotation of body0
Q1 = quaternion rotation of body1
RelQ = relative quaternion rotation that body0 have to rotate to align to body1

Now that you have your relative rotation, you can use the definition of the quaternion to determine how to apply the constraints rows to the joint.
The definition of a quaternion is given by four values
Q (q0, q1, q2, q3)
Where
q0 = cos (angle/2)
(q1, q2, q3) = sin (angle/2) * v(x, y, z)
That is a rotation by angle around the unit vector (x, y, z)

To help with this operation you can use the dMath and quaternion Class in the SDK

You can make a power joint, and angular limited joint or a combination of the two limited and powered.

Let us make the easiest, the powered jopint.
That means you want that to enforce and arbitrary relative rotation RelQ, meaning RelQ is a parameter
Let us call it ReaQp


With that definition and RelQ and you ne to do is figure out the unit vector and the angle

// calculate RelQ

Q0 = NetwonBodyGetRotation(body0);
Q1 = NetwonBodyGetRotation(body1);

// from relation Q1 = RelQ * RelQp * Q0 we get
RealQ = Q1 * (RelQp * Q0).Inverse();

// get the plane of rotation
dVector planeOfRotation (RealQ.q1, RealQ.q2, RealQ.q3)
If (fabs (ReqQ.q0 – 1.0f) < small value) {
planeOfRotation (1, 0, 0);
}

planeOfRotation = planeOfRotation.Normalize();
angle = 2.0f * Acos (RealQ.q0)

// create a coordinate system using planeOfRotation as the fron pin
Matrix rotationFrame (dgGrammSchmidt (planeOfRotation));

// now we need to the tow point to keep to drive to angle to zero
// this is like craeteion a hinge tah spin the body around the plane of rotation
Matrix0 = RelQ * RelQp * bodyMatrix0;
dVector p1 (bodymatrix1.Tranform (Vector(0, somedist, 0) )
dVector p0 (Matrix0.Tranform (Vector(0, somedist, 0) )
AddLenearRow (p0, p1, rotationFrame.m_right);

// add find the two lateral row to heep stability
dVector q1 (bodymatrix1.Tranform (Vector( somedist, 0, 0) )
dVector q0 (Matrix0.Tranform (Vector(0, somedist, 0) )
AddLenearRow (q0, q1, rotationFrame.m_up);
AddLenearRow (q0, q1, rotationFrame.m_right);


see if you undertand teh consept, if you do you soudl be able to make the joing even if my pseudo code is no right,
if you do no teh I can make the joint on teh SDK a post it.
Julio Jerez
Moderator
Moderator
 
Posts: 11154
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Mon Dec 29, 2008 7:10 pm

Thanks for the post Julio.

That explains quite a lot. I'll start getting up to speed on quarternions. What isn't so obvious to be is how a specify and interpret the 3 pairs of min/max limits I want, in terms of quarternions. Anyway, I'll start to look into it.

Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: NewtonUserJointAddLinearRow sanity check

Postby Julio Jerez » Mon Dec 29, 2008 8:27 pm

I will make teh joint for you, and you can get going form there.

There is one more thing I have to tell you about this method, and that is that it is an all or nothing joint, you can not implement joint with fir degree of freadom.
It is graet for power joint but it is not good for making forf example a hinge that rotate free.
anyway I will make it and you see how it works.
Julio Jerez
Moderator
Moderator
 
Posts: 11154
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Mon Dec 29, 2008 8:31 pm

Thanks
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: NewtonUserJointAddLinearRow sanity check

Postby Julio Jerez » Fri Jan 02, 2009 1:35 pm

In case you are wondering, I still have no done it, I was working on oeh feature but now I am ready.
I will do tomorrow.
Julio Jerez
Moderator
Moderator
 
Posts: 11154
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Fri Jan 02, 2009 3:43 pm

No problem Julio. I wasn't expecting it because of New Year's.

Just to clarify - what I am wanting is a joint to construct a basic model of a human body. I good example of how it would be used is the hip joint. This allows rotations around 3 axes (albeit 1 of them has very limited movement). I would want to specify minimum and maximum angles of rotation around each of the 3 axes. I'm not too bothered about having a powered joint as I'll probably apply external forces but I suppose for completeness it wouldn't hurt.

Also, to make things as general as possible I would have it so that the bodies (in the initial position) do not have to align along any of the axes.

Thanks for your help, Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Re: NewtonUserJointAddLinearRow sanity check

Postby Julio Jerez » Fri Jan 02, 2009 7:04 pm

JoeWright wrote:This allows rotations around 3 axes (albeit 1 of them has very limited movement).

Ha you do not mention that, you said you want a general ball joint, and I this no possible because there are more unknown variables than independent equations to extract the angles from a matrix.
I am not making it up, this is an arithmetic fact.
If you imposed but definition the constraint that at least one of the angles is restated to live in some quadrants, then that becomes the extra equation necessary to solve the problem.
I will write the analytical formulation so that you are clear about.
Let us specify that we want a child body orientation to be specified by three angles relative to the parent frame of reference.
Let us say the three angles are x, y, z
So we can write the equation fl the chil matrix as:
Rotc = Rot(x) * Rot(y) * Rot (z)

Now we can write the raltion form the paret matrix to the chuidl matrix as

Rotp = Rot(x + x0) * Rot (y + y0) * Rot (z + z0)

That is we need to find the angles x0, y0, z0, that will convert the rotation Rotx into to Rotp
Let us write and example to that you can see what I mean.
the joint is a hinge that can rotate free around x axis,
this meants that

z0 = z and y0 = z0, and x0 can be anything

for example is the child frame rotate 30 degree round x , by making x0 = -30 the above equation is satisfied.
The problem come when we try to extract x, y, and z from the child frame as you will see there are many possible solutions, which means there are ambiguities that problem, a general solution.

Let us expand the matrix that results from equation
Rotc = Rot(x) * Rot(y) * Rot (z)

This matrix is:


Code: Select all
       cos(y) * cos(z)                                cos(y) * coz(z)      -sin(y)
rotc = cos(x) * sin(z) - coz(z) * sin(x) * sin(y)     cos(x) * coz(z)      sin(x)cos(z)
       sin(x) * sin(z) - coz(z) * coz(x) * sin(y)     sin(x) * coz(z)      cos(x)cos(z)



let us say that

a02 = -sin (y)

is is cleat that we can get y form

y = asin(-a02)

by here is the problem

since sin (y) = sin (180 - y)

now we have the problem that we have two possible choices for each angle wish means that for each teh oeth tow axis we also have two other choices.
altotoght it is more difficult to have the thore two solutions.
That is there are 2 x 2 x 2 = 8 possible solutions ways to decompose the matrix by projection.
That is what explain so many diffrent ways to make joint by gemetrical projections.

next teh soution by enforceion one contraint.
Julio Jerez
Moderator
Moderator
 
Posts: 11154
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: NewtonUserJointAddLinearRow sanity check

Postby Julio Jerez » Fri Jan 02, 2009 7:04 pm

Now let us say that we will impose the condition that the rotation around the x axis is limited to the first and third quadrant.
For example say you are making a neck joint, if to make the head to be the child joint and the x axis of the of the frame is align with the neck since we said that the rotation is limited to less than +-90 degrees around the x axis the other two angles can be calculate without problems.
He is how, Let us say that

a12 = sin(x) * cos(z)
a22 = cos(x) * cos(z)

from that we can say that

x = atan (a12 / a22)

but that can generate tow solution as well since

tan (x) = tan (x + 180)

that is if x is in the first quadrant, the and angel in the third quadrant is also a solution.

Now we inforce the constraint that x can only be on the first or four quadrant.

This means

x = atan (a12 / a22)
where -90 < x < 90

witch you read the joint will never let x be outside the range.
If it ever does the solution will be wrong

From there angles y and z angle can be anything, and care must be take when y = += 90 degree.
That joint is now complete define and easy to implement my projection axis.

Now that joint is still have to implement since we still the for problem that
A02 = sin(y) = sin (180 – y)

So the more natural solution is to select the y axis to impose the constraint that
-90 < Y < 90

And the way we can eliminate two ambiguities with only one constraint, and this is a joint the is easy to implement by that that require some offset rotation to make it look like the y axis is the child twist axis.


It is perfectly possible to make a joint that can control three independent angles say pitch, yaw and roll as long as the yaw angle in restricted to +- 90 degree.

In practice yaw has to be much less the +- 90 degree since we start running into numerical errors every time the angle is close 90 degree, you can use 10 to 15 degree as safety margin.
If this is good for you I can post the joint code tonight.
Julio Jerez
Moderator
Moderator
 
Posts: 11154
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: NewtonUserJointAddLinearRow sanity check

Postby JoeWright » Fri Jan 02, 2009 7:21 pm

I think I'm following you. Could you please post the code and I'll look into it more.

Thanks, Joe
JoeWright
 
Posts: 69
Joined: Sat Apr 30, 2005 1:42 pm

Next

Return to General Discussion

Who is online

Users browsing this forum: No registered users and 4 guests