Problems with dCustomKinematicController

A place to discuss everything related to Newton Dynamics.

Moderators: Sascha Willems, walaber

Problems with dCustomKinematicController

Postby Slin » Thu Jan 25, 2018 8:42 pm

Hello,

I am trying to have an object follow my hand in a VR project while colliding without going crazy or through walls. I started out by setting the velocity directly, but unless I applied a factor of 0.5 or smaller it tended to go crazy, also the interaction with walls and other held objects is very unreliable once the force grows a little. So now I am hoping that the kinematic controller joint can solve this for me.

The problem is that the joint is quite soft and lags behind more than I'd like it to. Currently the held object has a weight of 0.5 and both max angular and max linear friction are set to 40. It's not THAT bad, but it's also not good enough. How can I improve this?
Also the rotation does seem completely unaffected by the MaxAngularFriction and is very slow. Am I doing something wrong here? I also run into some assert in debug builds whenever I increase the max friction values to more than 1.0.
I don't care too much about performance as there won't be that many physics objects in the level, I'm mostly interested in reliable behavior.

Best behavior would be if the object would get to my hand within one update cycle, unless there is a collision somewhere ;).

Any help would be great,
Thanks!
Slin
 
Posts: 5
Joined: Thu Jan 25, 2018 8:10 pm

Re: Problems with dCustomKinematicController

Postby JoeJ » Fri Jan 26, 2018 10:14 am

Recently there was a bug in kinematic joint, and actually there happen some changes to joints, so you could try my implementation of kinematic joint below.
It's based on an older version from Newtons joint, let us know if it works better.

To make the object follow in a stiff way, you need to set high value for m_maxLinearFriction. This is the max force the joint will apply (you need to change this value in relation to the picked body each time a new object with different mass gets picked.)

The downside of high value is oszillation if you make fast moves and then stop instantly: The object will bounce a bit forth and back the hand until it finally stops as well. (Please describe if this is your problem, or is it latency?)

To improve results, you could create a more advanced control system that maps hand movement to a filtered joint target. In the worst case you could render the object at hand position even if actual physics simulation lags behind a bit - no one would notice, and it's not super easy to do that but surely possible. The idea would be that if you accept a lag, you could deaccelerate in time to get a nice stop.

You could also try to run physics at a higher rate, but that would slow things down and requires hand position extrapolation from its velocity, so i guess it introduces more problems than it solves.

Slin wrote:I started out by setting the velocity directly, but unless I applied a factor of 0.5 or smaller it tended to go crazy, also the interaction with walls and other held objects is very unreliable once the force grows a little.


If you set velocity directly, i guess the engine can not correct this value to prevent going through walls. Instead you should calculate a force that generates target velocity. Also you probably need to turn on continuous collision detection, which by default is off.

I use both the joint and force based methods to pick objects. Both require the same trade off between lag and oszillation, but the joint based method works with multibody systems (e.g. a ragdoll), while the naive force based method takes only mass of a single body into account and may be unable to lift the ragdoll up at all, so i suggest you focus on the joint method.



Code: Select all


class KCJoint
{
public:
   Body* m_body0;
   Joint* m_joint;

   int m_pickMode; // constrain orientation if 1
   dFloat m_maxLinearFriction;
   dFloat m_maxAngularFriction;
   dVector m_localHandle;
   dVector m_targetPosit;
   dQuaternion m_targetRot;


   KCJoint ()
   {
      m_body0 = 0;
      m_joint = 0;
      m_pickMode = 0;
      m_maxLinearFriction = 3500;
      m_maxAngularFriction = 1600;
      m_localHandle = dVector(0,0,0);
      m_targetPosit = dVector(0,20,0);
      m_targetRot = dQuaternion(0,1.0,0,0);
   }

   void KCJoint::SubmitConstraints (dFloat timestep, int threadIndex)
   {
      dFloat invTimestep = (timestep > 0.0f) ? 1.0f / timestep: 1.0f;

      if (!(propsPhysics::flags & propsPhysics::HAND_JOINT)) return;
      m_maxLinearFriction = (propsPhysics::f[propsPhysics::handLinMaxF]);
      
      dVector v;
      dVector w;
      dVector cg;
      dMatrix matrix0;
      // calculate the position of the pivot point and the Jacobian direction vectors, in global space.

      NewtonBodyGetOmega (m_body0, &w[0]);
      NewtonBodyGetVelocity (m_body0, &v[0]);
      NewtonBodyGetCentreOfMass (m_body0, &cg[0]);
      NewtonBodyGetMatrix (m_body0, &matrix0[0][0]);
   
      dVector p0 (matrix0.TransformVector (m_localHandle));

      dVector pointVeloc = v + w.CrossProduct(matrix0.RotateVector (m_localHandle - cg));
      dVector relPosit (m_targetPosit - p0);
      dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc);
      dVector relAccel (relVeloc.Scale (invTimestep * 0.3f));

      //////////////////
      //
      //  Modification: Use Gramm Schmidt Projection along error vector instead of body space
      //
      //////////////////

      dMatrix matrixCS;
      dVector alignTo = relPosit;
      if ((alignTo.DotProduct3(alignTo)) < 0.000001f) matrixCS = dGetIdentityMatrix();
      else matrixCS = dGrammSchmidt(alignTo);

      // Restrict the movement on the pivot point along all tree orthonormal direction
      NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrixCS.m_front[0]);
      NewtonUserJointSetRowAcceleration (m_joint, relAccel.DotProduct3(matrixCS.m_front));
      NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
      NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

      NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrixCS.m_up[0]);
      NewtonUserJointSetRowAcceleration (m_joint, relAccel.DotProduct3(matrixCS.m_up));
      NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
      NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

      NewtonUserJointAddLinearRow (m_joint, &p0[0], &m_targetPosit[0], &matrixCS.m_right[0]);
      NewtonUserJointSetRowAcceleration (m_joint, relAccel.DotProduct3(matrixCS.m_right));
      NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxLinearFriction);
      NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxLinearFriction);

      //////////////////
      //
      //////////////////

      if (m_pickMode)
      {
         dFloat mag;
         dQuaternion rotation;

         NewtonBodyGetRotation (m_body0, &rotation.m_q0);

         if (m_targetRot.DotProduct (rotation) < 0.0f) {
            rotation.m_q0 *= -1.0f;
            rotation.m_q1 *= -1.0f;
            rotation.m_q2 *= -1.0f;
            rotation.m_q3 *= -1.0f;
         }

         dVector relOmega (rotation.CalcAverageOmega (m_targetRot, invTimestep).Scale(0.3) - w);
         mag = relOmega.DotProduct3(relOmega);
         if (mag > 1.0e-6f)
         {
            dFloat relAlpha;
            dFloat relSpeed;
            dVector pin (relOmega.Scale (1.0f / mag));
            dMatrix basis (dGrammSchmidt (pin));    
            relSpeed = dSqrt (relOmega.DotProduct3(relOmega));
            relAlpha = relSpeed * invTimestep;

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_front[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha);
            NewtonUserJointSetRowStiffness (m_joint, 0.4f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_up[0]);
            NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
            NewtonUserJointSetRowStiffness (m_joint, 0.4f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &basis.m_right[0]);
            NewtonUserJointSetRowAcceleration (m_joint, 0.0f);
            NewtonUserJointSetRowStiffness (m_joint, 0.4f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);
         }
         else
         {
            dVector relAlpha = w.Scale (-invTimestep);
            NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha.DotProduct3(matrix0.m_front));
            NewtonUserJointSetRowStiffness (m_joint, 0.4f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha.DotProduct3(matrix0.m_up));
            NewtonUserJointSetRowStiffness (m_joint, 0.4f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);

            NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
            NewtonUserJointSetRowAcceleration (m_joint, relAlpha.DotProduct3(matrix0.m_right));
            NewtonUserJointSetRowStiffness (m_joint, 0.4f);
            NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction);
            NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction);
         }

      }
      else
      {
         // this is the single handle pick mode, add some angular friction

         dFloat const frictionFactor = 0.005f;

         dVector relAlpha = w.Scale (-invTimestep);

         NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_front[0]);
         NewtonUserJointSetRowAcceleration (m_joint, relAlpha.DotProduct3(matrix0.m_front));
         NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * frictionFactor);
         NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * frictionFactor);

         NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_up[0]);
         NewtonUserJointSetRowAcceleration (m_joint, relAlpha.DotProduct3(matrix0.m_up));
         NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * frictionFactor);
         NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * frictionFactor);

         NewtonUserJointAddAngularRow (m_joint, 0.0f, &matrix0.m_right[0]);
         NewtonUserJointSetRowAcceleration (m_joint, relAlpha.DotProduct3(matrix0.m_right));
         NewtonUserJointSetRowMinimumFriction (m_joint, -m_maxAngularFriction * frictionFactor);
         NewtonUserJointSetRowMaximumFriction (m_joint,  m_maxAngularFriction * frictionFactor);
      }
   }
};



void KCJointCallback (const NewtonJoint* joint, float timestep, int threadIndex)
{
   KCJoint* custom = (KCJoint*) NewtonJointGetUserData (joint);
   custom->SubmitConstraints (timestep, threadIndex);
}


KCJoint kineJoint;
kineJoint.m_body0 = pickedBody;
kineJoint.m_joint = NewtonConstraintCreateUserJoint (world->world, 6, KCJointCallback, pickedBody, 0);
NewtonUserJointSetSolverModel (kineJoint.m_joint, 2);
NewtonJointSetUserData (kineJoint.m_joint, &kineJoint);
NewtonBodySetAutoSleep (pickedBody, 0);
Last edited by JoeJ on Fri Jan 26, 2018 3:38 pm, edited 1 time in total.
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Problems with dCustomKinematicController

Postby JoeJ » Fri Jan 26, 2018 11:04 am

I thought a bit more about the problem, and i realize what's missing from kinematic joint: We need to be able to set target velocity as well. Form target position, target velocity and max force one could easily calculate deacceleration to get a nice stop. Just feed real world values from VR controller. (Not sure yet if we need to specify allowed latency as well, or if that is already defined from those parameters which i guess. It should also automatically care for noise if there is one.)

This would have nice application for any user interaction - VR or mouse, so i'll do it anyways some time. If you are interested i can do that the next days or so, and you can test it. Interface will be as code above. If it turns out nicely i can add functionality to the Newton joint and propose to include it there.
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Problems with dCustomKinematicController

Postby Julio Jerez » Fri Jan 26, 2018 11:38 am

does the VR give velocity information back.
I know nothing about those devices works but I know some people has got newton working with then usefully, check out this video
https://www.youtube.com/watch?time_cont ... lhJbHqVDu0

It can even though objects, and I do not think is use anything other that target matric, O kwno because I last debug the last bug in the joint.

but yes you are correct if the velocity information is available, the joint will be more accurate, you can see that the code simple assume target velocity is set zero, is just a matter to get that info, but I will be very surprised if that was a feature of the device.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Problems with dCustomKinematicController

Postby Slin » Fri Jan 26, 2018 12:00 pm

Thanks for your great answer Joej, I won't be able to test until Sunday, but I'll let you know how it goes and if you got any possible improvements I'd love to give that a try too ;).
Just took a look at the oculus documentation and they expose acceleration and velocity data, not sure if raw or smoothed or whatever, but it does exist in some form. Pretty sure OpenVR has it too.

Thanks again!
Slin
 
Posts: 5
Joined: Thu Jan 25, 2018 8:10 pm

Re: Problems with dCustomKinematicController

Postby JoeJ » Fri Jan 26, 2018 12:14 pm

The video looks very good, but he uses very low velocity in comparision what i do in my app: I pick objects at 10 meter distance, move mouse quickly and i get oszillation issues from that high acceleration. Also i have only 30 fps but run physics at 120 Hz, so the same target for 4 updates which makes it worse, but that's a good test to see if my idea handles such extreme conditions...
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Problems with dCustomKinematicController

Postby Julio Jerez » Fri Jan 26, 2018 12:40 pm

There is something we need to consider.
A physic simulator is a mathematical model of the law of physics, is not exact but try to do better approximation than just animations.

When we pick objects from the screen there is lot of information that is lost in the process of rendering. The first is that the perspective projection loses one dimension.
Pixel represent a solid volume that get wider as the collision point is farther away from the pick point. So any velocity or acceleration recovered from the screen will have lot of horrendous quantization errors.

That't one thing, the second is that a simulation can do things that people can't do in reality, people do not pick objects with picking rods that are several meters long, the closer you can get to that is using a fishing rod and you see how they bend no matter how strong they are. This is because objects that pivot far away from an origin are subject to high centripelal acceleration and velocity. And most people are only familiar with picking objects from the reach of their bodies.

This is why I think the video works and the velocity is not a significan factor.

But yes if there if velocity info the better.
I would not dare to recover it from inverse projection of perspective projection of a pixel thought.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Problems with dCustomKinematicController

Postby JoeJ » Fri Jan 26, 2018 3:57 pm

Yeah, quickly tried my idea but it did not show improvement. Target velocity would not be very important, setting it to 0 would only make the joint most careful. The idea is to deaccelerate towards the target in time to prevent overshoot and oszillation. At slow speed i can see my math is right but at high speed the discrete timesteps become too large in relation. :cry:
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Problems with dCustomKinematicController

Postby Slin » Mon Jan 29, 2018 6:34 am

I finally got to try your joint JoeJ.
It works quite well, with your default values it follows my hands with no noticeable lag and a tiny bit of oscillation which I can probably fix with some value tweaking.
What does the pick mode do? Rotation is only working if I set it to 1.

If I would use lower maximum friction and don't adjust it to the objects weight, would heavier objects lag behind more than light ones?
I got two different use cases, one is the users fists following the real users fists and the other are objects the user picks up, which are things like swords and shields and I actually want those to lag behind if the hands are accelerating faster than the users strength allows based on the weapons weight. (I might still end up adjusting my target position/rotation for this to get some better control).
Since mostly objects are just following the tracked controllers, the only unnatural accelerations happen when snap turning, starting to move/run, snap turning or picking up objects from a small distance as all of these just change speed immediately but some lag is fine in these cases.

The swords origin is in it's grip and if the tip of the swords blade collides I would expect it stay at the collision point but the sword to rotate with the grip still following my hand, instead it just stays as it collided. Not always in all situations but most of the time if I don't try too hard to make it rotate. Is this due to too much max angular friction?

I'm running the simulation at 90Hz with 4 substeps, could that cause any issues, how does this effect the joints (in your first reply you said this may not be good)? The swords can be quite thin and if I don't use at least 4 substeps it's easy to have two pass through each other. I can make the collision mesh a bit thicker, but not THAT much or it will look strange.

I'll try tweaking things a bit and hopefully have something to show soon :).
Slin
 
Posts: 5
Joined: Thu Jan 25, 2018 8:10 pm

Re: Problems with dCustomKinematicController

Postby JoeJ » Mon Jan 29, 2018 1:18 pm

Slin wrote:I'm running the simulation at 90Hz with 4 substeps, could that cause any issues, how does this effect the joints (in your first reply you said this may not be good)?


That's fine, what i meant being bad would be running physics with more Hz than the controller provides (because you'd need to interpolate or extrapolate controller state to give Newton good data).

Slin wrote:The swords can be quite thin and if I don't use at least 4 substeps it's easy to have two pass through each other.


You need to use continuous collision detection to prevent objects from tunneling from each other.

(You might want to tweak moment of inertia values for long / thin objects if they feel like being hard to control by the player.)

Slin wrote:What does the pick mode do? Rotation is only working if I set it to 1.

Yes, pick_mode 0 means just orientation is free ans almost unaffected (some damping happens).

Slin wrote:If I would use lower maximum friction and don't adjust it to the objects weight, would heavier objects lag behind more than light ones?

Yes. But the larger effect would be tat you can't pick up a heavy object at all if the maximum is too small.
You could add automatic adaption of those parameters to the joint code, but i assume it's a thing that requires manual tweaking in some cases.

Slin wrote: I actually want those to lag behind if the hands are accelerating faster than the users strength allows based on the weapons weight.

Yes, this is one such case :)

Slin wrote:accelerations happen when snap turning

Is this pushing a button to turn 180 degrees instantly?
If this is a problem you could do this:
Detect if there are objects in the player neighborhood other than his sword.
No -> Teleport the sword by setting body matrix directly.
Yes -> Keep simulation running and accept its results.
And of course you can render objects independent of their physics state.

Slin wrote:The swords origin is in it's grip and if the tip of the swords blade collides I would expect it stay at the collision point but the sword to rotate with the grip still following my hand, instead it just stays as it collided. Not always in all situations but most of the time if I don't try too hard to make it rotate. Is this due to too much max angular friction?


If i get you right this is a matter of angular friction values, yes, but i don't get what you mean with 'instead it just stays as it collided.'

It is also a matter of moment of inertia (how hard is it to rotate the object around 3 axis), and where the center of mass is in relation to the object. Those values are important of how those objects feel to handle, but have little influence on a object stuck by contact and a joint. Contact behavior is also affected my material friction values and restitution.

If a sword gets constantly stuck, maybe replacing it by raycasting or using a soft sword becomes an option.
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Problems with dCustomKinematicController

Postby Slin » Mon Jan 29, 2018 1:33 pm

CCD is already enabled but it doesn't seem to stop two convex colliders to eventually pass through each other if they are thin enough (they are both something between 1 and 2 cm).

Reducing the angular friction already solved that thing with the grip following correctly.

What do you mean by "soft sword"? I tried implementing sword behavior before using sweep tests and while it kinda worked there just were way more edge cases than I'd like to handle :)
Slin
 
Posts: 5
Joined: Thu Jan 25, 2018 8:10 pm

Re: Problems with dCustomKinematicController

Postby Slin » Mon Jan 29, 2018 3:14 pm

Is there a way to make the constrained object lag behind? Because now it always overshoots and it overshoots more the lower the friction values are and then takes longer to settle, I'd prefer it to just lag behind a little bit instead. Would changing damping values or something for the body help?
Slin
 
Posts: 5
Joined: Thu Jan 25, 2018 8:10 pm

Re: Problems with dCustomKinematicController

Postby JoeJ » Mon Jan 29, 2018 5:27 pm

Slin wrote:Is there a way to make the constrained object lag behind?


Yes, look at those lines:

dVector relPosit (m_targetPosit - p0);

dVector relVeloc (relPosit.Scale (invTimestep) - pointVeloc); // calculate velocity that brings bodies together in one timestep, accounting for current velocity as well

dVector relAccel (relVeloc.Scale (invTimestep * 0.3f)); // calculate acceleration from this and use 30 % of it

If you look further the rest of the code is just to construct a typical space with 3 orthogonal axis, distriubting our target acceleration to those directions, telling Newton to solve for forces that will achieve this.

If you change this to apply only 5% of acceleration, you should see more lag.
If you still don't like the behavior, note that you have full control of the body at this point, e.g. you could create a spline and calculate velocity to follow it, whatever.
The angular part is harder to manage due to rotations, but in principle the same.
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Problems with dCustomKinematicController

Postby JoeJ » Tue Jan 30, 2018 12:04 pm

Slin wrote:CCD is already enabled but it doesn't seem to stop two convex colliders to eventually pass through each other if they are thin enough (they are both something between 1 and 2 cm)


Hm, i have not much experience with CCD and don't know how this can work at all :) Maybe it's expected small objects can fail, maybe it's a bug. Eventually start a new thread about this.

Slin wrote:What do you mean by "soft sword"


I mean a flexible sword, so it bends if the player constantly pokes things unintendently, and it remains more plausible to be still in it's hands (while with a rigid sword he would feel a strong push against his hand).
This could be done with multiple joined bodies.
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Problems with dCustomKinematicController

Postby Julio Jerez » Tue Jan 30, 2018 12:29 pm

I do not think the engine will handle and object that is one or 2 cm on diameter, try 4
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles


Return to General Discussion

Who is online

Users browsing this forum: No registered users and 17 guests

cron