Kinematic controller hits assert (3.14)

[JoshKlint]

It seems that the custom kinematic controller hits an assert you set the joint target rotation 180 degrees away from the current orientation:

Code: Select all

dAssert(lateralDir.DotProduct3(lateralDir) > 1.0e-6f);

I guess this is because the solver does not know which way to turn? I can remove the assert statement, but then the joint is unresponsive at around 178 degrees.

Do you have any suggestions on how to handle this in a user-friendly way?

First reported here:
https://www.ultraengine.com/community/t ... r-message/

[Julio Jerez]

it is trying to make a coordinate frame to result the forces.
it takes one axis form one frame and another for the other, ideally general they should be perpendicular, but if they aren't then is just do a perpendicular frame allowing some slack.

if the tow axis are so misaligned that they are collinear, them is lose a degree for freedom and can no make the frame. This can happen on initialization or if some extreme violent movements happens.

you can apply a quick fix by just skipping the frame by changing the assert to this

Code: Select all

if (lateralDir.DotProduct3(lateralDir) > 1.0e-6f)
{
// do the operation. 
}


the joint will move and then the axis will not be collinear.

it could also be set that when that happens the cone between the two axis in either zero degree or 180 degrees base of how they are aligned, and that will be two special cases

but firs try that first quick fix, my guess is that tah will be enought

[JoshKlint]

I modified the code like so. It would be good if it could handle when the target rotation is 180 degrees opposite the current rotation, but I understand there is no perfect solution for that situation.

160:

Code: Select all

               //dAssert(lateralDir.DotProduct3(lateralDir) > 1.0e-6f);
               if (lateralDir.DotProduct3(lateralDir) > 1.0e-6f)
               {
                  lateralDir = lateralDir.Normalize();
                  dFloat coneAngle = dAcos(dClamp(cosAngle, dFloat(-1.0f), dFloat(1.0f)));
                  dMatrix coneRotation(dQuaternion(lateralDir, coneAngle), matrix1.m_posit);
                  dMatrix pitchMatrix(matrix1 * coneRotation * matrix0.Inverse());
                  pitchAngle = dAtan2(pitchMatrix[1][2], pitchMatrix[1][1]);
               }

Line 340:

Code: Select all

            //dAssert(lateralDir.DotProduct3(lateralDir) > 1.0e-6f);
            if (lateralDir.DotProduct3(lateralDir) > 1.0e-6f)
            {
               lateralDir = lateralDir.Normalize();
               dFloat coneAngle = dAcos(dClamp(cosAngle, dFloat(-1.0f), dFloat(1.0f)));
               dMatrix coneRotation(dQuaternion(lateralDir, coneAngle), matrix1.m_posit);

               if ((m_controlMode == m_linearAndCone) || (m_controlMode == m_full6dof)) {
                  NewtonUserJointAddAngularRow(m_joint, 0.0f, &lateralDir[0]);
                  NewtonUserJointGetRowJacobian(m_joint, &jacobian0.m_linear[0], &jacobian0.m_angular[0], &jacobian1.m_linear[0], &jacobian1.m_angular[0]);

                  dVector pointOmega(omega0 * jacobian0.m_angular + omega1 * jacobian1.m_angular);
                  dFloat relOmega = pointOmega.m_x + pointOmega.m_y + pointOmega.m_z;

                  dFloat w = m_maxOmega * dSign(coneAngle);
                  if ((coneAngle < maxAngle) && (coneAngle > -maxAngle)) {
                     w = damp * coneAngle * invTimestep;
                  }
                  dFloat relAlpha = (w + relOmega) * invTimestep;

                  NewtonUserJointSetRowAcceleration(m_joint, -relAlpha);
                  NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxAngularFriction);
                  NewtonUserJointSetRowMaximumFriction(m_joint, m_maxAngularFriction);


                  dVector sideDir(lateralDir.CrossProduct(matrix0.m_front));
                  NewtonUserJointAddAngularRow(m_joint, 0.0f, &sideDir[0]);
                  NewtonUserJointGetRowJacobian(m_joint, &jacobian0.m_linear[0], &jacobian0.m_angular[0], &jacobian1.m_linear[0], &jacobian1.m_angular[0]);
                  pointOmega = omega0 * jacobian0.m_angular + omega1 * jacobian1.m_angular;
                  relOmega = pointOmega.m_x + pointOmega.m_y + pointOmega.m_z;
                  relAlpha = relOmega * invTimestep;

                  NewtonUserJointSetRowAcceleration(m_joint, -relAlpha);
                  NewtonUserJointSetRowMinimumFriction(m_joint, -m_maxAngularFriction);
                  NewtonUserJointSetRowMaximumFriction(m_joint, m_maxAngularFriction);
               }

               dMatrix pitchMatrix(matrix1 * coneRotation * matrix0.Inverse());
               pitchAngle = -dAtan2(pitchMatrix[1][2], pitchMatrix[1][1]);
            }

[Julio Jerez]

Using cone and twist, this is a problem, for wants, if the joint passes the 180, it is possible that the meaning of the twist change sign, so it may snap. This is inevitable, even when using quat, and and step is too large.


It is possible to make handle 180 if the joint moves along the quaternion between the two frames.
That will be a short angle.
But the body of the joint would have to be different.

If I remember, in 4.00 it use the quaternion, not a cone and a twist angle.

[Julio Jerez]

I look at the code in 4:00
..\newton-4.00\sdk\dNewton\dJoints\ndJointKinematicController.cpp

and yes is use the quaternion for full rotation. at about lien 360

if you want to state in 3.14 you can look at the code in 4.00 copy the code, and either add a new option or replace the current code in 3.14.

in fact the code using quat, seems much easier.

[JoeJ]

Using cone and twist, this is a problem, for wants, if the joint passes the 180, it is possible that the meaning of the twist change sign, so it may snap. This is inevitable, even when using quat, and and step is too large.


It is possible to make handle 180 if the joint moves along the quaternion between the two frames.
That will be a short angle.
But the body of the joint would have to be different.

If I remember, in 4.00 it use the quaternion, not a cone and a twist angle.

This sounds like you're saying limit cone > 180 degrees is not possible, or if so a decomposition into cone / twist is no longer possible due to using quaternions.

But for me this works well. Posting code for reference, maybe it helps somebody.
It's confusing because i still mix math libraries and my cone limit is more complex. My cone is like a flat piece of pizza, but the pin is allowed to have some distance (angle) to that pizza, so i get kind of pizza extruded by cone. I do this for more natural human limits.
But the interesting part is the twist anyway.


EDIT: I remembered there is a problem with my quaternion joint, but i don't understand it fully yet.
The angle between min and max twist limits varies depending on the cone angle, and the effect is mostly noticeable when the cone is 90 degrees from its neutral position. So there is again a problem if the current cone forms 180 degrees.
Can't remember if the twist limit gets smaller or larger, but i think this creates a local minima / maxima, so likely it can cause discontinuities with simulation.
I did not notice this when testing the joint with a cone limit of 300 degrees, but i have never used such large limits beyond initial tests.
I guess it can be a practical solution where large limits are needed, but the method isn't perfect either.

Code: Select all

   void SubmitTwistSwingConstraints (ndConstraintDescritor& desc)
   {
      const ndFloat32 &timestep = desc.m_timestep;
      const ndFloat32 &invTimestep = desc.m_invTimestep;
      assert (timestep > 0.0f);

      ndMatrix matrix0 = m_body0->GetMatrix();
      ndMatrix matrix1 = m_body1->GetMatrix();
      Mat4 matrix0_, matrix1_;
      matrix0_ = (Mat4&)matrix0; matrix0_ *= (Mat4&)m_localMatrix0;      
      matrix1_ = (Mat4&)matrix1; matrix1_ *= (Mat4&)m_localMatrix1;      
      
      ndQuaternion q0 = m_body0->GetRotation();
      ndQuaternion q1 = m_body1->GetRotation();
      Quat q0_, q1_;
      q0_ = (Quat&)q0; q0_ *= localRot0;
      q1_ = (Quat&)q1; q1_ *= localRot1;      

      matrix0 = ToNdMatrix(matrix0_);
      matrix1 = ToNdMatrix(matrix1_);
      

      AddPointToPointConstraint (desc, matrix0, matrix1);



      const Vec3& dir0 = matrix0_[0];
      const Vec3& dir1 = matrix1_[0];
      float dot = dir0.Dot (dir1);

      if (dot < -0.999f) return;
   
      if (twistLimitAngleP >= twistLimitAngleN) // do the twist
      {

         // factor rotation about x axis: Quat qt = q0.Inversed() * q1; float halfTwistAngle = atan (qt[0] / qt[3]);
         float twistAngle = 2.0 * atan (
            ( ( ( (q0_[3] * q1_[0]) + (-q0_[0] * q1_[3]) ) + (-q0_[1] * q1_[2]) ) - (-q0_[2] * q1_[1]) ) /
            ( ( ( (q0_[3] * q1_[3]) - (-q0_[0] * q1_[0]) ) - (-q0_[1] * q1_[1]) ) - (-q0_[2] * q1_[2]) ) );
         //Vec3 twistAxis = dir1;
         Vec3 twistAxis_ = Vec3(dir0+dir1).Unit();
         ndVector twistAxis (twistAxis_[0], twistAxis_[1], twistAxis_[2], 0.f);

         if (twistLimitAngleP == twistLimitAngleN)
         {
            AddAngularRowJacobian(desc, twistAxis, ndFloat32(0.0f));
            //NewtonUserJointAddAngularRow (joint, twistAngle - twistLimitAngleP, (float*)&twistAxis);
         }
         else if (twistAngle > twistLimitAngleP)
         {
            AddAngularRowJacobian (desc, twistAxis, twistAngle - twistLimitAngleP);
            SetLowerFriction (desc, 0.f);
            //NewtonUserJointAddAngularRow (joint, twistAngle - twistLimitAngleP, (float*)&twistAxis);
            //NewtonUserJointSetRowMinimumFriction (joint, 0.0f);
         }
         else if (twistAngle < twistLimitAngleN)
         {
            AddAngularRowJacobian (desc, twistAxis, twistAngle - twistLimitAngleN);
            SetHighFriction (desc, 0.f);
         }
      }

      if (swingLimitAngle > 0 && dot < 0.999f) // do the swing (pizza cone limit)
      {
         Vec3 d = matrix1_.Unrotate (dir0);
         Vec3 cone = d; cone[1] = 0; cone.Normalize();

         if (cone[0] < swingSideAngleCos)
            cone = Vec3 (swingSideAngleCos, 0, ((cone[2]<0) ? -swingSideAngleSin : swingSideAngleSin));
         float angle = acos (max(-1.f, min(1.f, d.Dot (cone)))) - swingLimitAngle;
         if (angle > 0)
         {
            Vec3 swingAxis_ = (matrix1_.Rotate(d.Cross(cone))).Unit();
            ndVector swingAxis (swingAxis_[0], swingAxis_[1], swingAxis_[2], 0.f);
            AddAngularRowJacobian (desc, swingAxis, angle);
            SetLowerFriction (desc, 0.f);
         }         
      }
      
   }


Btw, i had very good progress on the ragdoll by replacing exact math with approximation to calculate target joint angular velocity from the pose my IK solver generates.
Here's a link: https://theorangeduck.com/page/exponential-map-angle-axis-angular-velocity

I knew about this before and wanted to try it for years, but i would not have thought what a difference it makes.
Using exact math there are problems with small changes because we need to normalize to get the rotation axis.
Now using the approximation instead those problems are gone, and accuracy and smoothness is much, much better. Actually my ragdoll animation looks so smooth it's no longer visible there is physics simulation involved. It looks like animation, and even too smooth to look realistic.

Really a game changer. Anyone trying to control stuff with motors wants to take a look at this, i propose.

[Julio Jerez]

the trick that I found seem to work best, is to just use the quaternion, to move along the shortest angle
but handle that the cone and twist by the joint instead by the solvers.

that is in each step instead to take large steps to reach a target, just read the current quat of angle and add a small fraction, then advance to that new target.
this is in effect an implementation of small angle approximation and seem to work quite well.