Refactoring joints

[Julio Jerez]

Ok Joe when you have some time, please Sync to the version that I commited
This is what was Dec 31, plus some bug fixed reported by few people.
It also has teh replacement of consts: 2.131592 with dPi or dgPi

I commited your test, please tell me if tshi is what you remember or what you see in your order version.
I do see the this is more stable specially when touching teh limits, but I am no sure anymore.

The has been a total disaster and waste of time.

anyway I am kind of glad because it was getting too much out of control.

[zak]

And in my tests, rotational part of hinge joint is more precise and robust with linear row, while angular row are slightly faster. I use the linear row method.

[JoeJ]

Much better, there is still strange behavior - it seems the joint climbs upwards while sliding over the planes in the arc region, and then falling down into the cone (that should connect the planes without a noticeable transition.) Unfortunately this is hard to see for you because there is no visualization of the limits - i added this later.

But this really looks much more like a bug in my joint code. Disabling that arc limit and using just one cone works well. I will test this newton version in my own app where i should have an alternate implementation of the same joint and let you know...
EDIT: After a quick test i can't say anything because nothing works in my own app, i haven't used this kind of joint for years and it may be just incompatible with something i've changed i guess.

To be sure you could download a version from july 2017, that's when i made the commit with the joint. (I would like to do this too, but i don't know how and if that's possible)

[JoeJ]

I made the code work in 3.13, and there i see the same mismatch between cone and arc sections, so this must be a bug of mine. (Not sure yet if i introduced that on first commit or yesterday.)

I looked also at the pulling-appart-behavior of 3.13, this has not become worse.

Pretty sure now that the December version is ok

[Julio Jerez]

I too saw the pop, but I cann't tell if is was the engine of the joint, I am not sure if after added the bug that the behavior changed. I will probably do it again maybe station form the day in July but is much harder that was about the time I try to migrate to I'm-gui, lot of changes happen since them.

do you have the day on July 2017?

[Julio Jerez]

Oh that good thinking we can test in t he last commited version of 3.13
That can be the standard and so no need to go tha far back.

I will continue adding the other high level changes, without touching the part in the SDK that move the derivative to the joints. then let us see who tha affect the behavior.
but first I will put that test in 3.13

[JoeJ]

do you have the day on July 2017?

The date of my commit was 22 July (still visible under closed pull requests on github)

I'll see what's wrong with the joint...

To test in 3.13 most work was to add Dot and Cross to dVector and some minor changes.

[JoeJ]

Ok, i've had multiple scale bugs (assuming vec.scale(0.5) already affects vec - i always get this wrong)

Embrassing i did not notice that back the time.

Works well now with December version, but still wrong with recent version before.
You should be able to narrow it down now

fixed code:

Code: Select all

struct JoesNewRagdollJoint: public dCustomJoint
{
   dQuaternion m_target; // relative target rotation to reach at next timestep

   // motor:
   dFloat m_reduceError;
   dFloat m_pin_length;
   dFloat m_angularFriction;
   dFloat m_stiffness;

   dFloat m_anim_speed;
   dFloat m_anim_offset;
   dFloat m_anim_time;

   // limits:
   bool m_isLimitJoint;

   dFloat m_minTwistAngle;
   dFloat m_maxTwistAngle;

   dFloat m_coneAngle;
   dFloat m_arcAngleCos;
   dFloat m_arcAngleSin;

   
   JoesNewRagdollJoint(NewtonBody* child, NewtonBody* parent, const dMatrix &localMatrix0, const dMatrix &localMatrix1, NewtonWorld *world, bool isLimitJoint = false)
      :dCustomJoint (6, child, parent)
   {
      m_localMatrix0 = localMatrix0;
      m_localMatrix1 = localMatrix1;

      m_target = dQuaternion(dVector(1.0f, 0, 0), 0.0f);
      m_reduceError = 0.95f; // amount of error to reduce per timestep (more -> oszillation)
      m_stiffness = 0.98f;
      m_angularFriction = 300.0f;

      m_anim_speed = 0.0f;
      m_anim_offset = 0.0f;
      m_anim_time = 0.0f;
     
      m_isLimitJoint = isLimitJoint;

      SetTwistSwingLimits (0.1f, 0.2f, -0.1f, 0.1f);
      //SetTwistSwingLimits (0.1f, 0.0f, -0.1f, 0.1f);
   }

   void SetTwistSwingLimits (const float coneAngle, const float arcAngle, const float minTwistAngle, const float maxTwistAngle)
   {
      float const maxAng = 2.8f; // to prevent flipping on the pole on the backside

      m_coneAngle = min (maxAng, coneAngle);
      float angle = max (0.0f, min (maxAng, arcAngle + m_coneAngle) - m_coneAngle);
      m_arcAngleCos = cos (angle);
      m_arcAngleSin = sin (angle);

      m_minTwistAngle = minTwistAngle;
      m_maxTwistAngle = maxTwistAngle;
   }

   dVector BodyGetPointVelocity(const NewtonBody* const body, const dVector &point)
   {
      dMatrix matrix;
      dVector v(0.0f);
      dVector w(0.0f);
      dVector c(0.0f);
      NewtonBodyGetVelocity(body, &v[0]);
      NewtonBodyGetOmega(body, &w[0]);
      NewtonBodyGetMatrix(body, &matrix[0][0]);
      c = matrix.m_posit; // TODO: Does not handle COM offset !!!
      return v + w.CrossProduct(point - c);
   }

   static void SubmitConstraintsCallback (const NewtonJoint* const userJoint, dFloat timestep, int threadIndex)
   {
      JoesNewRagdollJoint *joint = (JoesNewRagdollJoint*) NewtonJointGetUserData (userJoint);
      joint->SubmitConstraints(timestep, threadIndex);
   }

   void SubmitConstraints(dFloat timestep, int threadIndex)
   {
      dFloat invTimestep = 1.0f / timestep;

      dMatrix matrix0;
      dMatrix matrix1;

      //CalculateGlobalMatrix(matrix0, matrix1);
      dMatrix body0Matrix;
      // Get the global matrices of each rigid body.
      NewtonBodyGetMatrix(m_body0, &body0Matrix[0][0]);

      dMatrix body1Matrix (dGetIdentityMatrix());
      if (m_body1) {
         NewtonBodyGetMatrix(m_body1, &body1Matrix[0][0]);
      }
      matrix0 = m_localMatrix0 * body0Matrix;
      matrix1 = m_localMatrix1 * body1Matrix;



      const dVector& p0 = matrix0.m_posit;
      const dVector& p1 = matrix1.m_posit;
      NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_front[0]);
      NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_up[0]);
      NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix0.m_right[0]);



      if (m_isLimitJoint)
      {

         //dCustomBallAndSocket::SubmitConstraints(timestep, threadIndex);


         const dVector& coneDir0 = matrix0.m_front;
         const dVector& coneDir1 = matrix1.m_front;
         float dot = coneDir0.DotProduct3(coneDir1);
         if (dot < -0.999) return; // should never happen

         // do the twist

         if (m_maxTwistAngle >= m_minTwistAngle) // twist restricted?
         {
            dQuaternion quat0(matrix0), quat1(matrix1);     
            float *q0 = (float*)&quat0;
            float *q1 = (float*)&quat1;

            // factor rotation about x axis between quat0 and quat1. Code is an optimization of this: qt = q0.Inversed() * q1; halfTwistAngle = atan (qt.x / qt.w);
            float twistAngle = 2.0f * atan (
               ( ( ( (q0[0] * q1[1]) + (-q0[1] * q1[0]) ) + (-q0[2] * q1[3]) ) - (-q0[3] * q1[2]) ) /
               ( ( ( (q0[0] * q1[0]) - (-q0[1] * q1[1]) ) - (-q0[2] * q1[2]) ) - (-q0[3] * q1[3]) ) );

            // select an axis for the twist - any on the unit arc from coneDir0 to coneDir1 would do - average seemed best after some tests
            dVector twistAxis = coneDir0 + coneDir1;
            twistAxis = twistAxis.Scale (1.0f / sqrt (twistAxis.DotProduct3(twistAxis)));

            if (m_maxTwistAngle == m_minTwistAngle) // no freedom for any twist
            {
               NewtonUserJointAddAngularRow (m_joint, twistAngle - m_maxTwistAngle, (float*)&twistAxis);
            }
            else if (twistAngle > m_maxTwistAngle)
            {
               NewtonUserJointAddAngularRow (m_joint, twistAngle - m_maxTwistAngle, (float*)&twistAxis);
               NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
            }
            else if (twistAngle < m_minTwistAngle)
            {
               NewtonUserJointAddAngularRow (m_joint, twistAngle - m_minTwistAngle, (float*)&twistAxis);
               NewtonUserJointSetRowMaximumFriction (m_joint, 0.0f);
            }
         }

         // do the swing
#if 0
         // simple cone limit:

         float angle = acos (dot) - m_coneAngle;
         if (angle > 0)
         {
            dVector swingAxis = (coneDir0.CrossProduct(coneDir1));
            swingAxis = swingAxis.Scale (1.0 / sqrt (swingAxis.DotProduct3(swingAxis)));
            NewtonUserJointAddAngularRow (m_joint, angle, (float*)&swingAxis);
            NewtonUserJointSetRowMinimumFriction (m_joint, 0.0);
         }
#else
         // cone / arc limit - think of an piece of pizza (arc) and an allowed max distance from it (cone):

         if (m_coneAngle > 0.0f && dot < 0.999f)
         {
            // project current axis to the arc plane (y)
            dVector d = matrix1.UnrotateVector (matrix0.m_front);
            dVector cone = d; cone.m_y = 0; cone = cone.Scale (1.0f / sqrt (cone.DotProduct3(cone)));

            // clamp the result to be within the arc angle
            if (cone.m_x < m_arcAngleCos)
               cone = dVector (m_arcAngleCos, 0.0f, ( (cone.m_z < 0.0f) ? -m_arcAngleSin : m_arcAngleSin));

            // do a regular cone constraint from that
            float angle = acos (max(-1.0f, min(1.0f, d.DotProduct3(cone)))) - m_coneAngle;
            if (angle > 0.0f)
            {
               dVector swingAxis = matrix1.RotateVector(d.CrossProduct(cone));
               swingAxis = swingAxis.Scale (1.0f / sqrt (swingAxis.DotProduct3(swingAxis)));
               NewtonUserJointAddAngularRow (m_joint, angle, (float*)&swingAxis);
               NewtonUserJointSetRowMinimumFriction (m_joint, 0.0f);
            }   
         }
#endif
      }
      else
      {

         if (m_anim_speed != 0.0f) // some animation to illustrate purpose
         {
            m_anim_time += timestep * m_anim_speed;
            dFloat a0 = sin(m_anim_time);
            dFloat a1 = m_anim_offset * 3.14f;
            dVector axis(sin(a1), 0.0f, cos(a1));
            //dVector axis (1,0,0);
            m_target = dQuaternion(axis, a0 * 0.5f);
         }

         // measure error
         dQuaternion q0(matrix0);
         dQuaternion q1(matrix1);
         dQuaternion qt0 = m_target * q1;
         dQuaternion qErr = ((q0.DotProduct(qt0) < 0.0f)   ? dQuaternion(-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3) : dQuaternion(q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;
         qErr.Normalize();

         dFloat errorAngle = 2.0f * acos(dMax(dFloat(-1.0f), dMin(dFloat(1.0f), qErr.m_q0)));
         dVector errorAngVel(0, 0, 0);

         dMatrix basis;
         if (errorAngle > 1.0e-10f) {
            dVector errorAxis(qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
            errorAxis = errorAxis.Scale(1.0f / dSqrt(errorAxis.DotProduct3(errorAxis)));
            errorAngVel = errorAxis.Scale(errorAngle * invTimestep);

            basis = dGrammSchmidt(errorAxis);
         } else {
            basis = dMatrix(qt0, dVector(0.0f, 0.0f, 0.0f, 1.0f));
         }

         dVector angVel0(0.0f);
         dVector angVel1(0.0f);
         NewtonBodyGetOmega(m_body0, (dFloat*)&angVel0);
         NewtonBodyGetOmega(m_body1, (dFloat*)&angVel1);

         dVector angAcc = (errorAngVel.Scale(m_reduceError) - (angVel0 - angVel1)).Scale(invTimestep);

         //dCustomBallAndSocket::SubmitConstraints(timestep, threadIndex);
         // motors
         for (int n = 0; n < 3; n++) {
            // calculate the desired acceleration
            dVector &axis = basis[n];
            dFloat relAccel = angAcc.DotProduct3(axis);

            NewtonUserJointAddAngularRow(m_joint, 0.0f, &axis[0]);
            NewtonUserJointSetRowAcceleration(m_joint, relAccel);
            NewtonUserJointSetRowMinimumFriction(m_joint, -m_angularFriction);
            NewtonUserJointSetRowMaximumFriction(m_joint, m_angularFriction);
            NewtonUserJointSetRowStiffness(m_joint, m_stiffness);
         }

      }
   }
};


[Julio Jerez]

Ha excellent, I just replace the old one. and yes thsi is more like I remember the where.

what I am going to do is to complete adding teh changes that are not related to the joints and bring all demos to work again.
The we can try see what went wrong with the joint.

The fact that is stable does no mean is totally right, teh fail at high speed, teh problem is that a low speed the joint derivative vanish or have very lithe effect,
so if a joint is required to deal with violent acceleration they are jus to soft to respond.

That's the theory, let use see if is correct, but thsi time will maintain the test as standard.

worse thing is that if we can no get it, we can always calculate the derivative in teh submit constraint for the joints that we need. like the inverse dynamics.

[JoeJ]

The fact that is stable does no mean is totally right, teh fail at high speed, teh problem is that a low speed the joint derivative vanish or have very lithe effect,
so if a joint is required to deal with violent acceleration they are jus to soft to respond.

Interesting!

However - i'm afraid i was too optimistc After remembering id all those scale fixed already i niticed i picked the wrong pull request. Few days later i submitted an updated version with fixes and visuals.
I can even download that from here: https://github.com/JoeJGit/newton-dynamics

This shows a lineup of 5 limit joint configuration and they are all stable.
After porting this to December version the bouncing shows up again, especially in the 3 body setup with 2 joints.

[Julio Jerez]

But that pull request was applied, can you made a new one with the changes that you thing generate the most stable setup?

I will explain why the rotation are soft and mostly has not effect on joints that move at low speed of the control one or two angle.

The equation that the solve resolve for torque is this
Let L be the angular momentum I the inertia and w the angular velocity.
We have the Euler equation that says:

L = I * W

Some engine works on the momentum space because is linear but that's a huge mistake, since they have not way to generate velocity and angular velocity accelerations correction terms that arise from the derivatives of and are necessary for maintaining the laws that angular momentum and angular energy must be conserved. This is why as hard as it is, I do the step further and resolve for acceleration instead of for impulses.
Engines that solve for impulse must apply the force and torque as independent quantities and then solve for the values that the are need to clip the delta momentum added to the bodies.
I do not know of anything that could be more erronues than that, yet is the approached used by all game engines.

In Newton I apply the derivatives of the angular and linear momentum, that is the derivatives of a product. for angular momentum the derivative is this Euler equation.

T = L' = I * w' + I' * w

The time derivative of the angular momentum is equal to the external torque.
Now we know that w' is the angular velocity, but is not clear what I' is.

It turns out I' is a externally complex expression, but if we assume that the body principal axis are aligned to the frame of rotation, then many quantity vanish and the expression reduces to the cross product operator of angular velocity time the angular momentum, so we get

T = I * a + w x L

That's the first torque equation for the solver.
I continue in the next post

[JoeJ]

But that pull request was applied, can you made a new one with the changes that you thing generate the most stable setup?

I will fix more things like guarantee to submit all rows, check in old version and then port to new version so we can keep comparing in both...

It turn out I' is a externally complex expression, but if we assume that the body principal axis is aligned to the frame of rotation, the many quantity vanish and the expression reduces to the cross product operator of angular velocity time the angular momentum

Do you talk about how approximating moment of inertia with an ellipsoid helps to calculate its value on any axis, or do you mean something like a need to 'rotate equations' to a space where they can be solved easier?

[Julio Jerez]

no, I mean that is the moment of inertia are chose such that the principal axis cancel the cross inertia the equation become simpler.
I used to derive this thing in the pass but now with awesomee Wikipedia all we need to do is look for what we need: https://en.wikipedia.org/wiki/Euler%27s ... y_dynamics)

yes if the center of mass of a body is not located at the center of the principal asis this will not work, but that is the assumption that newton makes.

There was an option to compiled with arbitary inertia DG_USE_FULL_INERTIA_MATRIX
but I think only one person requested and I reluctantly added after explaining that most simulation are base on object that do hae axis of symetry so handling abyrtary ionerat is just a waste of time.
There were making a flght simulator and after I added the feature I never heard from them.

Anyway if these assumptions are correct, (the way the assumption is enforced is that when we calculate the inertia for a shape, we take the principal axis but we just ignore teh frame tah align the inertia to the principal axis)
so if soem one take for example a cylinder and twist 30 degress along the y or z axis, the cylinder will act as if it was aligned the the x axis.

Ok next I will continue teh derivation so that we know where it went wrong or what mistake I made.

[JoeJ]

Ok, that's what i meant with the ellipsoid

I've just finished fixing row count with my limit joint and it became noticeable better
I noticed i was wrong when i said swing and twist axis are not orthogonal
So i added motor on those axis and the resulting third - works without any issues!

Now i'm only worried about accuracy if rotation axis as badly aligned to any of them, but it's really perfect stable and robust for now.

I'm still on that older July version. Tomorrow i'll test if recent versions still show problems and do some cleanup...

[Julio Jerez]

alright,
can you get tested with the latest that I committed to gitub. if the one works we use that a the base for the inverse dynamics joint.