Kinematic controller joint rotation broken?

[pHySiQuE]

The Kinematic controller joint rotation doesn't seem to be working right. Has anything changed here? No matter what rotation I give it, it wants to rotate to a single wrong orientation. This used to work perfectly.

[Julio Jerez]

I added a new constructor so that I can used with inverse dynamics articulations.
that should nor change the behavior in anyway.
last time this happened was because the joint was going to sleep and I added this

Code: Select all

void dCustomKinematicController::Init (NewtonBody* const body, const dMatrix& matrix)
{
   CalculateLocalMatrix(matrix, m_localMatrix0, m_localMatrix1);

   m_autoSleepState = NewtonBodyGetSleepState(body);
   //NewtonBodySetAutoSleep(body, 0);
   NewtonBodySetSleepState(body, 0);


but that joint should not be the default behavior that should left to the high level code,
sync and you will see a new function
void dCustomKinematicController::ResetAutoSleep ()

The reason for this is that kinematic joints can be part of many things, take for example a tree of a telephone pole. such that and actor soudl hit it with certain force in order to brake rather that touch9ng and teh pole fly around. that one effect but other more important is that Kinematic joints were made to serve as end effector of complex contractions and on that case you do not wan the to be active all the time. If the default behavior is to activate the body, then he engine will sped lot of time doing useless work and what you wnat is that the funtionality is there but noty at the expence of slowing everything down.
I am making a demo to demonstrate this functionality

Sync and call tha function and it should be fine.

[pHySiQuE]

The problem persists.

Here is my code:

Code: Select all

   void NewtonDynamicsJoint::SetTargetRotation(const Quat& rotation, const float blend)
   {
      if (type == KINEMATIC)
      {
            dQuaternion rotf;
            Quat q = entity[0]->body->nextquat.Slerp(rotation, blend);
            q = q.Normalize();
         rotf.m_q0 = q.w;
            rotf.m_q1 = -q.x;
            rotf.m_q2 = -q.y;
            rotf.m_q3 = -q.z;
            ((dCustomKinematicController*)newtonuserjoint)->SetTargetRotation(rotf);
         ((dCustomKinematicController*)newtonuserjoint)->ResetAutoSleep();
       }
   }

Here is a demo:
https://www.leadwerks.com/testgame.zip

• Select "New Game".
• Walk up to a crate and press "E" to pick it up.
• Move the mouse around a bit to see the rotation.

[Julio Jerez]

I don't know why is not working, I when back to the version I had on nov 30, and the behavior is the same, the has not been any change that can cause that.
is as if the math library is wrong. bu I have no chnge any of the stuff.

you must has changed target the quaternion calculation. why do you do this? this is just wrong.

Code: Select all

 rotf.m_q0 = q.w;
            rotf.m_q1 = -q.x;
            rotf.m_q2 = -q.y;
            rotf.m_q3 = -q.z;

[pHySiQuE]

I think your quaternions and my quaternions are different.

I'll take a closer look tomorrow and see if anything is wrong on my end.

[JoeJ]

The problem persists.

Here is my code:

Code: Select all

   void NewtonDynamicsJoint::SetTargetRotation(const Quat& rotation, const float blend)
   {
      if (type == KINEMATIC)
      {
            dQuaternion rotf;
            Quat q = entity[0]->body->nextquat.Slerp(rotation, blend);
            q = q.Normalize();
         rotf.m_q0 = q.w;
            rotf.m_q1 = -q.x;
            rotf.m_q2 = -q.y;
            rotf.m_q3 = -q.z;
            ((dCustomKinematicController*)newtonuserjoint)->SetTargetRotation(rotf);
         ((dCustomKinematicController*)newtonuserjoint)->ResetAutoSleep();
       }
   }

Here is a demo:
https://www.leadwerks.com/testgame.zip

• Select "New Game".
• Walk up to a crate and press "E" to pick it up.
• Move the mouse around a bit to see the rotation.

By inverting the quat as you do you move away from the target instead towards it. The negation should not be there, and nextquat should be the current orientation if the body in world space. Maybe the bug is there. (you don't do this negation also when moving body quat to your entity?)


Ah wait, Julio there seems indeed a bug:

Code: Select all

if (m_pickMode) {
      dQuaternion rotation (matrix0);
      if (m_targetRot.DotProduct (rotation) < 0.0f) {
         rotation.Scale(-1.0f);
      }

      dFloat dot = rotation.DotProduct(m_targetRot);
      if (dot < 0.9995f) {
         dMatrix rot (dGrammSchmidt(dVector (rotation.m_q1, rotation.m_q2, rotation.m_q3)));
         dFloat angle = 2.0f * dAcos(dClamp(rotation.m_q0, dFloat(-1.0f), dFloat(1.0f)));

         NewtonUserJointAddAngularRow (m_joint, -angle, &rot.m_front[0]);
         NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
         NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);


Here you do not involve m_targetRot at all. It seems you intended to make 'rotation' the rotation between 'rotation' and 'm_targetRot', but probably you forgot to calculate it and continued using unmodified 'rotation' instead, which is still the bodies orientation at this point?

[Julio Jerez]

oh yes that seem quite wrong, I think I made that change a very long time ago, and I do not remember why. but yes that's not right.
I try to fix it or see if there was a reason for it.

but in any case what physics is doing is not correct he is passing the inverse of the matrix which will causes the oscillation that he sees.

he also is using a gravity of 25 and the body mass is 2 which mean the weight is 50, but the friction of the joint is 100, this will be the lag that he sees.
he needs to set the joint friction proportional to the weight, that was also a change I made long time ago, no sure why he noticed now.
old code

Code: Select all

void dCustomKinematicController::SetMaxLinearFriction(dFloat accel)
{
   dFloat Ixx;
   dFloat Iyy;
   dFloat Izz;
   dFloat mass;

   NewtonBodyGetMass (m_body0, &mass, &Ixx, &Iyy, &Izz);
   m_maxLinearFriction = dAbs (accel) * mass;
}

new code

Code: Select all

void dCustomKinematicController::SetMaxLinearFriction(dFloat frictionForce)
{
   m_maxLinearFriction = dAbs (frictionForce);
}

void dCustomKinematicController::SetMaxAngularFriction(dFloat frictionTorque)
{
   m_maxAngularFriction = dAbs (frictionTorque);
}


but that changed was about six month ago.
I am trying to remove lot of the babysitting that I have added to the engine because they have a way to come back and bite me in the ass many time.

[pHySiQuE]

That makes sense because our last version release was six months ago, and I have not done extensive testing since then.

[Julio Jerez]

I see if I can fix my side tonight.

Joe try to get the latest so that you can see what we are working with Sweenie.

[JoeJ]

Joe try to get the latest so that you can see what we are working with Sweenie.

Nice, so inverse dynamics in use already. I need to look the demo a bit closer to see if i get it...

Something different, here is the paper that i think extends the instant meshes algorithm i talked about in another thread to volumes:
https://gaoxifeng.github.io/papers/2017/Robust-Meshes-2017.pdf
Project on github as well, so this might be interesting for you to generate softbodies.
(instant meshes is not robust for very low tesselation - that would be bad but worth a try)

[Julio Jerez]

Yes please check out the demo, and tell me what you think.
also I just committed the a refinement.

I finally go to fix the bugs on the solver, there are few pending feature like the handling limits, and adding vicus friction, but we can live with that as long as the end effector is keep inside the work space of the articulation.

I want the get that functionality in for the final release, so I decided for a simpler demo, and I am glad I did be4cause I found plaint for bug that otherwise will take me a long time to solve.
We will leave the softbody stuff for 3.15.

I have been working this with Sweenie and he suggested some idea that even I got scared of at first but now I start to get hope again.

[pHySiQuE]

Is there another update coming or was that the fix for me?

[pHySiQuE]

Looks like the kinematic joint rotation is still acting wrong. We're due to release a new update next Wednesday.

[Julio Jerez]

is fixed now, sync

you need to make sure the friction is proportional to teh mass of the grabbed body,
five to ten time the weight more of less.
you value is only two time the weight.

test it and tell me if it works as before, no sure why was changed or if it ever worked before,
but I am sure I have it working.
In fact I am planning to use that joint for all the more advance inverse dynamics that I'm planning for next version and the last two demos.
That was one of the the purpose of that joint in the first place.

also you need to change this function

Code: Select all

   void NewtonDynamicsJoint::SetTargetRotation(const Quat& rotation, const float blend)
   {
      if (type == KINEMATIC)
      {
            dQuaternion rotf;
            Quat q = entity[0]->body->nextquat.Slerp(rotation, blend);
            q = q.Normalize();
         rotf.m_q0 = q.w;
            rotf.m_q1 = -q.x;
            rotf.m_q2 = -q.y;
            rotf.m_q3 = -q.z;
            ((dCustomKinematicController*)newtonuserjoint)->SetTargetRotation(rotf);
         ((dCustomKinematicController*)newtonuserjoint)->ResetAutoSleep();
       }
   }

Code: Select all

   void NewtonDynamicsJoint::SetTargetRotation(const Quat& rotation, const float blend)
   {
      if (type == KINEMATIC)
      {
            dQuaternion rotf;
            Quat q = entity[0]->body->nextquat.Slerp(rotation, blend);
            q = q.Normalize();
         rotf.m_q0 = q.w;
            rotf.m_q1 = q.x;
            rotf.m_q2 = q.y;
            rotf.m_q3 = q.z;
            ((dCustomKinematicController*)newtonuserjoint)->SetTargetRotation(rotf);
         ((dCustomKinematicController*)newtonuserjoint)->ResetAutoSleep();
       }
   }

basically all you nee to do is pass the target rotation and position. the negation makes the inverse matrix and that why the all code oscillated even with the bug.

[pHySiQuE]

Unfortunately I see no difference in behavior. The rotation is still totally off no matter what I do.