Setting the direction of a HingeActuator instead of an angle

A place to discuss everything related to Newton Dynamics.

Moderators: Sascha Willems, walaber

Setting the direction of a HingeActuator instead of an angle

Postby Schmackbolzen » Wed Nov 30, 2016 8:26 am

Hi!

I have a direction vector in global space I want the moving body pointed to. Imagine a robotics arm or something similar (just an example). The way it is now I would have to save the direction vector at the creation of the joint as reference vector (zero position), transform it to global space, then project the direction vector onto the rotation plane and finally calculate the angle between them to get the relative angle. Is there possibly a simpler and faster way you can do this directly in the hinge actuator using already calculated values?

Thanks in advance!
Schmackbolzen
 
Posts: 26
Joined: Mon Feb 16, 2015 5:37 pm

Re: Setting the direction of a HingeActuator instead of an a

Postby Julio Jerez » Wed Nov 30, 2016 10:20 am

simpler but no faster unless a new joint is made just for that, are you using this joint CustomHingeActuator? if so them yes, you can subclass that joint and overload function

Code: Select all
void SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1);

them there you save with the class that data you nee to make up m_angle
somethog link
Code: Select all
void CustomHingeActuator::SubmitConstraintsFreeDof (dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
{
      m_angle = CalculateMyMtAngleFromVector(...);
      SubmitConstraintsFreeDof (timestep, matrix0, matrix1);
}


and function CalculateMyMtAngleFromVector you put the heuristic to explained in the post
I would no worry about the speed of that series of steps you mentioned because compared to the calculation the joint does is like comparing the mass of a rock with the mass of the planet, there is not contest.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Setting the direction of a HingeActuator instead of an a

Postby Schmackbolzen » Wed Nov 30, 2016 4:54 pm

Yes, I use the CustomHingeActuator. I will try to implement it this way. Can you explain to me what exactly CalculateGlobalMatrix does, since it uses NewtonBodyGetMatrix which already returns a global matrix? In what coordinate system do you work? Sadly there are no comments :(
Schmackbolzen
 
Posts: 26
Joined: Mon Feb 16, 2015 5:37 pm

Re: Setting the direction of a HingeActuator instead of an a

Postby Julio Jerez » Wed Nov 30, 2016 6:16 pm

CalculateGlobalMatrix calculate the pivot of the joint in global space.
when a joint is created you pass the parameter that define it.
one parameter is the pivot point (position and orientation)
These pivots are stored as a local matrices relative to each respective body forming the joint.

then as the body moves, to get the location of that pivot in global space, the engine uses that function which multiply the local matrices by its respective body matrices.
The error between the two global positions is the constraint violation that the engine tries to nullified.

there is always an error even when using good reduced coordinate solvers do to numerical error, because Newton is a reduce coordinate system physics engine, It always calculate reaction force to nullified degree of freedoms instead of Generalized systems which do the opposite and integrate alone the free degree of freedom.

what coordinate system? the engine keep it simple, the coordinate system is a global non moving inertial frame. (the origin of the world)
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Setting the direction of a HingeActuator instead of an a

Postby Schmackbolzen » Mon Dec 05, 2016 9:21 am

Thanks for the detailed explanation. That helped a lot.

While looking at the code from the CustomHinge class I noticed two things:

First, in line 198 you write
Code: Select all
CalculateAngle(matrix1.m_up, matrix0.m_up, matrix1.m_front, sinAngle, cosAngle);

shouldn't this be
Code: Select all
CalculateAngle(matrix1.m_up, matrix0.m_up, matrix0.m_front, sinAngle, cosAngle);

(the third matrix is different)?
And second, you calculate the angle afterwards with
Code: Select all
m_curJointAngle.Update(cosAngle, sinAngle);
.
This seems rather elaborate, since you already got the angle from the CalculateAngle return value. I don't see any benefit there and for me it looks like you should get a worse result because of rounding errors.

Regarding my problem, I managed to somewhat get my result, but the joint is oscillating. Looks like a bad feedback loop. Maybe I did a mistake. My code is:
Code: Select all
void CustomUserHingeActuator::SubmitConstraintsFreeDof(dFloat timestep, const dMatrix& matrix0, const dMatrix& matrix1)
   {
      if (m_NewTargetDirectionSet){      

         //Project target direction onto rotation plane
         dVector projDir = m_TargetDirection - matrix0.m_front.Scale(m_TargetDirection.DotProduct3(matrix0.m_front));      
         
         //Get angle between projected target direction and child pivot matrix direction
         dFloat angleDiff = CalculateAngle(projDir, matrix0.m_right, matrix0.m_front);

         //Get current joint angle
         dFloat angle = m_curJointAngle.GetAngle();

         //To rotate to desired direction, add the calculated angle to the current joint angle
         SetTargetAngle(angle + angleDiff);
         m_NewTargetDirectionSet = false;         
      }
      CustomHingeActuator::SubmitConstraintsFreeDof(timestep, matrix0, matrix1);
   }


As you can see I modified my method as I did not find an easy way to transform the starting child direction vector to global space, since there is no rotation matrix for the joint itself. Also this is a much simpler calculation.
Schmackbolzen
 
Posts: 26
Joined: Mon Feb 16, 2015 5:37 pm

Re: Setting the direction of a HingeActuator instead of an a

Postby Julio Jerez » Mon Dec 05, 2016 9:36 am

Schmackbolzen wrote:Thanks for the detailed explanation. That helped a lot.

While looking at the code from the CustomHinge class I noticed two things: ...

This seems rather elaborate, since you already got the angle from the CalculateAngle return value. I don't see any benefit there and for me it looks like you should get a worse result because of rounding errors.


actually no, that m_curJointAngle.Update do the exact opposite, it reduces numerical error.
basically when you do addiction or subtraction on periodic functions, there is always the chance to wrap around the period of the signal.
The function update takes care of that by keeping track of the length of the circumference by using the identity sin (a + b) and cos (a + b).
This also allows for cool effects like having arbitrary angle limits. For example you can make a hinge has a 1000 degree angle limit on one side and say -525 in the other.
without that periodic add function, that would be impossible.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Setting the direction of a HingeActuator instead of an a

Postby Schmackbolzen » Sat Dec 24, 2016 7:40 pm

Thanks for the explanation. It really would be nice if there were more comments in the code.

I am currently struggling with how to prevent the hinge actuator joints from oscillating. If you send one to a specific location it will not immediately stop, but oscillate around the target angle. I have tried different settings, even a PID control algorithm, but with not much success. Is this the intended behaviour?
Schmackbolzen
 
Posts: 26
Joined: Mon Feb 16, 2015 5:37 pm

Re: Setting the direction of a HingeActuator instead of an a

Postby Julio Jerez » Sat Dec 24, 2016 8:02 pm

the joint should be very, very stable.
do you have a test case, or can you recreate the demo.
many joint has serialization, maybe you could serialize your test and try loading it in the demo sandbox.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Setting the direction of a HingeActuator instead of an a

Postby JoeJ » Sun Dec 25, 2016 7:54 am

[quote="Schmackbolzen"]
//To rotate to desired direction, add the calculated angle to the current joint angle
SetTargetAngle(angle + angleDiff);
[/code]

Does this code set the target angle to the exact value?
Maybe this causes the oscillation: It reaches the exact target on the next update, but also creates velocity to go beyond that. Next update it will go back, but again it will have velocity so repeating that game infinite times.

Try SetTargetAngle(angle + angleDiff * 0.3f);

It will hit the target slower, but reduces the velocity so it should settle.
User avatar
JoeJ
 
Posts: 1453
Joined: Tue Dec 21, 2010 6:18 pm

Re: Setting the direction of a HingeActuator instead of an a

Postby Julio Jerez » Sun Dec 25, 2016 10:27 am

oh yes that's a very good point, that Joe pointed out.
remember that newton is a force base engine and does not clip velocities.
so each correction will add momentum to the bodies that nee to be corrected with another update.
this is no really a bug is part of how physics works, there is always stiffness at the boundaries.

Newton 3.15 I will experiment with a two pass solver, one for force and one for impulse.
this may help to solve this problem a lot.

wen using continued collision this is what newton already does, but it is not a formal implementation,
it uses a trick o passing zero as the time step to tell the use this is a impulse pass.
3.15 will formalize that to be the normal way of operation. and I will add another interface function when the custom joints can decide if it wants a acceleration or velocity base joint.

for now you need to do what Joes said.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Setting the direction of a HingeActuator instead of an a

Postby Schmackbolzen » Sun Dec 25, 2016 12:58 pm

Well, the oscillation is damped, so it does settle. I assumed it is similar to using servomotors, which actually speaks for the quality of the physics simulation. This is why I tried a PID-controller algorithm, since it is often used there. The problem is that you have to fine tune the parameters and that is not very practical. I would like to have that the rotated body stops immediately without any vibration. If it helps I can try to build a demo scene to make testing easier.

I tried JoeJ's suggestion but it just changes the targeted angle, not the behaviour. I've also made sure that it is only set once. So basically the joint just moves to the target angle.
Schmackbolzen
 
Posts: 26
Joined: Mon Feb 16, 2015 5:37 pm

Re: Setting the direction of a HingeActuator instead of an a

Postby Julio Jerez » Sun Dec 25, 2016 1:41 pm

has you tried using the skeletons?
you just create a newton skeleton and add the joints in a parent child relation.
this can make it better because it removes a lot of the solver convergent errors.

Please you using the skeleton see if it makes it better. Look at the standard joint demos for how to use it. In fact if your main concern is accuracy you should always wrap your joints contractions with a skeleton.
Julio Jerez
Moderator
Moderator
 
Posts: 12249
Joined: Sun Sep 14, 2003 2:18 pm
Location: Los Angeles

Re: Setting the direction of a HingeActuator instead of an a

Postby Schmackbolzen » Thu Dec 29, 2016 10:06 am

Yes, using a skeleton does the trick! Thanks a lot. It even works with a rather large time step of 30ms, which is great.

Btw.: You have a bug in the CustomHingeActuator class: Since you are overwriting the fields m_minAngle and m_maxAngle they work if you set them via constructor or SetMin/MaxAngularLimit, but not if you use SetLimits, since that calls the method from the parent class and uses the fields from that class. My solution was to not overwrite the variables, but to use the ones from the parent class. Maybe you had a reason for this?
Schmackbolzen
 
Posts: 26
Joined: Mon Feb 16, 2015 5:37 pm

Re: Setting the direction of a HingeActuator instead of an a

Postby Julio Jerez » Thu Dec 29, 2016 11:56 am

Ah I am glad is works for you now. yes the skeleton joint is a great addition to the engine since 3.13
is gives you the power of exact constraint solver for almost any conceivable contraction of joints and bodies. You have to be careful, like Spiderman said: "Great power requires great responsibility 8)"
because with exact solver also comes back the possibility of explosion if you connect loops that lead to singular mass matrices. This is a problem that iterative solver engine do not have because when solving an ill condition system with an iterative solver, the solver convergence rate becomes zero or even diverge.

I suggest that, if you are running at 30 fps then you should probably use this function
NewtonSetNumberOfSubsteps (world, 2)

this is like running two update but the are some saving, and in the future this will be even better because the solve will have two passes but not all the passes requires the same rate.
in 3.15 there will be an option so make collision executes one step, all of the solve overhead does one step, and only the solver does the sub stepping. in fact I would use
Code: Select all
NewtonSetNumberOfSubsteps (world, 4)
which is the equivalent to run at 120 fps.
you will be surprised how not much slower it is versus how much accuracy increases quadratically.

can you post the file and line function of that bug that you'd mentioned?

remember you can alway subclass from those base classes and override any parameter you want.
in fact what I do is that I make a joint class that wrapper that contain an pointer to the engine class something like.
Code: Select all
class myJoint
{
        public:
        myJoint (CustomJoint* joint)
                  :m_joint(joint)
         {
             ...
             ...
         }
         virtual ~myJoint ()
         {
                delete m_joint;
          }
         CustomJoint* m_joint;
}

Or some variant of that, this is what dNewton was meant to be but mot people prefer to use the c interface and the custom joints directly.

also, that way you separate your app from the physics engine and later you can even change the physics engine without affecting your app to much, if you decide to do later.
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 13 guests

cron