Forcing body to orientation with torque

[PJani]

My idea was to use joints to control motorics - joints are actually very good to abstract relation between two bodies in your engine(not sure if i can set DOF 0). Callbacks per body would make things complicated.

Is to use two joints one for constraining joint(hinge, ball joint), second for controller joint(for controlling motion with just two joints)) for same pair.

Using linear rows was my first attempt to steady motion and give it effect of weaknes and muscule response. I am discarding this idea with rows in controller joint, i will use torque PID or PD controller instead with quaternion error correction.

[PJani]

The typical scenario for this joint would be a control joint when the tree angle are be specified by the player.
say you want to make a upper Leg ball socked, and you want to create a kick.
here the angle Pitch, Yaw and Roll are always given as input.
The application want those angles to be the target and it does no care how they are generated all it care is that they are realized by that leg.

say the threes are; Pitch, Yaw, Roll
all of the Joint in the joint library, will try to achieve the target in one step.

however that causes a bit problem because the difficulty that, the tree torques that need to be generated by the solver are linear to cannot be proportional to the each respective error angle.
they have to be produce in one and only one order.

with the proposed joint, The joint code will get the same tree angle and it will clamp the to the "small angle"

now instead of solving for Pitch, Yaw, Roll, it solve for
Pitch0 = Clamp (Pitch, -smallAngle, smallAngle);
Yaw0 = Clamp (Yaw, -smallAngle, smallAngle);
Roll0 = Clamp (Roll, -smallAngle, smallAngle);

now the engine will emit three angle rows, in any order, and each oen will generate a torque the will be proportional to Pitch0, Yaw0 and Roll0 and because the fat that

Matrix(Pitch0) * Matrix (Yaw0) * Matrix (Roll0) ~= Matrix (Yaw0) * Matrix(Pitch0) * Matrix (Roll0) ~= any other permutation of the same angle

The effect is that, the joint will tend to correct the error, but It will require time to do the complete correction.

The effect is that you can control the angle independently at will, and without Gimbals Lock

Interesting. That means it would need angular rows If i am correct?

[JoeJ]

I remember the 6DOF idea and think it will do well for the powered ragdoll use case.
If the small solving angle really gets a problem (fast kung fu kick), we can mix it with a single angular row, if the angle of wanted rotation is too large.

@ PJani:
Using angular rows is highly recommended.
You could do a powered hinge using two pinvectors the linear way.
I've also extended this idea to free rotation too, and it worked ok for a snake with 2 joints,
but with more joints in one system in becomes instable and explosive.
This is no surprise, as it tries to solve a rotational problem with linear motion - small error with small angle, terrible error with large angle.

An example of angular powered hinge is here:
http://newtondynamics.com/forum/viewtopic.php?f=9&t=7469
I'm not willing to share how i did the powered cone twist, but as previously said, Julios idea will provide a better solution

[Julio Jerez]

@ PJani

I am still confused, can you make a sketch of what it is that you wna to achive?
you can use PNG file which do not require uploading then to any ftp site, teh forom can read then

Mean time I will make that joint so that you have an idea.

basically this joing let you use angular rows teh same way you use linear rows, by tshi I mean

say you issue three linear row, a, b, c
you get the same result if you issue b, a, c, or c, a, b, or any peomuation of the three linear row,
this is not the case if you issue two or three angular rows, the order in changes the joint behavior, and that is what makes the hard to use.

[PJani]

here is video what i am trying to achive.
http://www.youtube.com/watch?v=_yGJPoTn ... page#t=70s

First joint(hinge or ball joint) is just for keeping form with limits like regular "dead" ragdoll...controller joint "forces" two bodies into certain orientation like in the youtube video.

ok i will try to sketch in 2d what i mean with two joints per body pair.

[PJani]

This is my sketch...

[PJani]

I was playing with torque. For small angles(<45 deg) it works almost perfect but when i apply larger external force it just explodes.

This is my test creation code.

Code: Select all

//Zero mass object
      SimpleBody* b0 = bce->createSimpleBody(Ogre::Vector3(0,0,0), Ogre::Quaternion(Ogre::Degree(0),Ogre::Vector3(1,0,0)));
      b0->begin();
      b0->setBoxCollision(00, Ogre::Vector3(0.1, 0.2, 0.1) );
      b0->end();
      
//object with density 1000kg/m^3 and 100kg/m^3
      CompoundBody* b1 = bce->createCompoundBody(Ogre::Vector3(0,1.35,0));
      b1->begin();
      b1->addBoxCollision(0,1000, Ogre::Vector3(0.4, 0.2, 0.1), Ogre::Vector3(0.15,0,0) );
      b1->addBoxCollision(1,100, Ogre::Vector3(0.05, 1.15, 0.05), Ogre::Vector3(0,-1.35*0.5f + 0.25f,0) );
      b1->end();

      Ogre::Quaternion qut = Ogre::Quaternion::IDENTITY;//b0->getOrientation();

//this is what i mean by two joints for pair of bodies
      OgreNewt::BallAndSocket* bas = this->createBallAndSocket(b1, b0, this->getMainBody()->getPosition() + Ogre::Vector3(0, 0.25, 0));
      TorqueControllerJoint* tcj = new TorqueControllerJoint(b1, b0, this->getMainBody()->getPosition() + Ogre::Vector3(0, 0.25, 0));


This is TorqueControllerJoint constraint callback

Code: Select all

      void submitConstraint( Ogre::Real timeStep, int threadIndex )
      {

         Ogre::Quaternion glbl;
         Ogre::Vector3 global;
         localToGlobal(m_offset_orientation, m_global_point, glbl, global, 0);

         Ogre::Quaternion dori = m_parent->getOrientation() * m_child->getOrientation().Inverse();
         Ogre::Quaternion rot_err = dori.Inverse() * m_offset_orientation;

         Ogre::Real mass;
         Ogre::Vector3 I;
         getBody0()->getMassMatrix(mass, I);


         Ogre::Matrix3 mat;
         rot_err.ToRotationMatrix(mat);
         Ogre::Radian r0, r1, r2;
         mat.ToEulerAnglesXYZ(r0,r1,r2);


         Ogre::Vector3 torq(r0.valueRadians(),r1.valueRadians(), r2.valueRadians());

         if(torq.squaredLength() > 1e-9)
         {
            //Ogre::Vector3::ZERO; //
            Vector3 domega = (getBody1()->getOmega() - getBody0()->getOmega());
            Vector3 T = (torq*15000 - domega*850) * I;

            Ogre::Quaternion body0_ori = getBody0()->getOrientation();
            m_child->addTemporaryGlobalTorque(global, body0_ori * Ogre::Vector3::UNIT_X, -T.x * 0.5f);
            m_child->addTemporaryGlobalTorque(global, body0_ori * Ogre::Vector3::UNIT_Y, -T.y * 0.5f);
            m_child->addTemporaryGlobalTorque(global, body0_ori * Ogre::Vector3::UNIT_Z, -T.z * 0.5f);

            Ogre::Quaternion body1_ori = getBody1()->getOrientation();
            m_parent->addTemporaryGlobalTorque(global, body1_ori * Ogre::Vector3::UNIT_X, T.x * 0.5f);
            m_parent->addTemporaryGlobalTorque(global, body1_ori * Ogre::Vector3::UNIT_Y, T.y * 0.5f);
            m_parent->addTemporaryGlobalTorque(global, body1_ori * Ogre::Vector3::UNIT_Z, T.z * 0.5f);

         }

      }


[JoeJ]

One thing i see is:

Ogre::Quaternion rot_err = dori.Inverse() * m_offset_orientation;

That calculates realtive error, but does not care if it takes the long or the short arc of rotation,
and rapidly flipping between the two may cause explosive behaviour.
To avoid, take the dot product of quats to check their facing and invert if differs.

I use that func to do that all in one go, just to illustrate:

inline sQuat QuatFromAToB (const sQuat &qA, const sQuat &qB)
{
sQuat q;
if (qA.Dot(qB) < 0.0f)
{
q[0] = qA[0]; q[1] = qA[1]; q[2] = qA[2];
q[3] = -qA[3];
}
else
{
q[0] = -qA[0]; q[1] = -qA[1]; q[2] = -qA[2];
q[3] = qA[3];
}

return qB * q;
}

[PJani]

Hmm. Its same but more stable i think( i need to dampen oscilations more but will do that after i fix this ). I am feeling i am getting gimbal lock or something, when applying force to "twist" the child.

This is the effect i am getting when pulling on outer part of body(i am 99.999% sure my pulling/picking spring works).


Edit: i suspect that the Y torque is getting bigger than X torque when i pull child at side and that why child just bends the other way.

Any idea how to solve this?

[JoeJ]

Yes, that's the problem the whole discussion is all about.
You compute 3 angles in a predefined order using ToEulerAnglesXYZ.
You'd get different angles if you'd use ToEulerAnglesZXY... but you can not tell newton which order you want, because it tries to solve all constraints at once.
It's indeed gimbal lock related.

Julios Idea ist to use rotations that - no matter in what order you apply them - give the same result, but can't solve the problem at once.
He gave a me detailed example somewhere in the forum, maybe we could implement that ourself,
but i assume he also needs to modificate something at the solver or its interface.

After seeing the Unreal-video i see that you wanna attach the joints to a non-dynamic player controller.
That makes it musch easier for you than for my work, things that didn't work well for me may be good for you.
Have you tried this: Convert relative error to axis and angle, and use that for a single angular row.
That would be the right way avoiding the 3 angles problem.
It was not stable enough for me, because if the constraint is nearly satisfied, the axis jumps around rapidly.
But you could try to stabilize that situation with two additional linear or perpendicular zero-angle-angular rows...

[PJani]

Yes, that's the problem the whole discussion is all about.
You compute 3 angles in a predefined order using ToEulerAnglesXYZ.
You'd get different angles if you'd use ToEulerAnglesZXY... but you can not tell newton which order you want, because it tries to solve all constraints at once.
It's indeed gimbal lock related.

Sleep deprivation has sometimes negative impact on me. .

Is possible to drive torque via quaternion or matrix, not by XYZ axis as is now?

Julios Idea ist to use rotations that - no matter in what order you apply them - give the same result, but can't solve the problem at once.
He gave a me detailed example somewhere in the forum, maybe we could implement that ourself,
but i assume he also needs to modificate something at the solver or its interface.

i will try to find that post if is on forum

After seeing the Unreal-video i see that you wanna attach the joints to a non-dynamic player controller.
That makes it musch easier for you than for my work, things that didn't work well for me may be good for you.

non-dynamic? what do you mean by that? non self balancing ragdoll?

Have you tried this: Convert relative error to axis and angle, and use that for a single angular row.
That would be the right way avoiding the 3 angles problem.
It was not stable enough for me, because if the constraint is nearly satisfied, the axis jumps around rapidly.
But you could try to stabilize that situation with two additional linear or perpendicular zero-angle-angular rows...

I think i haven't yet. I could be wrong. I will try.

[JoeJ]

i found te post:
http://newtondynamics.com/forum/viewtopic.php?f=9&t=5634&hilit=any+order&start=30

... maybe it's a little like doing sheering instead of rotating... sheering is order independent and can approx small angle rotation, if you orthonormalize the result.
But i don't think so... Ask the master himself

Is possible to drive torque via quaternion or matrix, not by XYZ axis as is now?

For sure, but inside a joint you can not set a torque - only angular rows. Thus my axis and angle suggestion:
You can treat each rotation as a pair of an axis and and an angle and set a single angular row - working good for motion, but unstable at rest.
Usually each quaternion lib has axis / angle conversion build in.

With non-dynamic i mean the character in the video is not balancing. It's limbs are attached to a user controlled player controller - that's clearly visible.
If you have a real balancing ragdoll that's hit by a heavy obstacle, the complete ragdoll will tumble, fall down, needs to stand up again... no player controller, no animation.
So what of the two (or a mix of those) is your goal?

[PJani]

My first idea was to use motion planning to correct center of gravity but then i figured it would be faster to make and less a performance kill to use ragdoll animation following.

I want to drive ragdoll from animation final state machine(FSM). On events(pushes, pulls, falls,... actual external force) certain state will trigger animation and ragdoll will follow that animation with limited force, feedback from delayed limbs would auto correct, pause,... animation. My ragdoll actually stands using little trick to keep it upwards, but it has unstable limbs(hands, feet, head) because of linear rows. Ragdoll actually picks its self up in some fall in reverse mode XD. NO ANIMATIONS!

[PJani]

I am running out of luck, julio did you have any luck with that joint?

[PJani]

I figured the easiest way to stabilize both bodies is to do this...buti dont think i cant control torque

Code: Select all

         Ogre::Quaternion body0_ori = getBody0()->getOrientation();
         Ogre::Quaternion body1_ori = getBody1()->getOrientation();

         Ogre::Matrix3 mat0;
         body0_ori.ToRotationMatrix(mat0);

         Ogre::Vector3 mat0_x = mat0.GetColumn(0);
         Ogre::Vector3 mat0_y = mat0.GetColumn(1);
         Ogre::Vector3 mat0_z = mat0.GetColumn(2);

         Ogre::Matrix3 mat1;
         body1_ori.ToRotationMatrix(mat1);

         Ogre::Vector3 mat1_x = mat1.GetColumn(0);
         Ogre::Vector3 mat1_y = mat1.GetColumn(1);
         Ogre::Vector3 mat1_z = mat1.GetColumn(2);

         dbg->insertDebugLine(((UINT32)this)+0, global, global + mat0.GetColumn(0), 0xFFFF0000, 0xFFFF0000);
         dbg->insertDebugLine(((UINT32)this)+1, global, global + mat0.GetColumn(1), 0xFF00FF00, 0xFF00FF00);
         dbg->insertDebugLine(((UINT32)this)+2, global, global + mat0.GetColumn(2), 0xFF0000FF, 0xFF0000FF);

         dbg->insertDebugLine(((UINT32)this)+0+3, global, global + mat1.GetColumn(0), 0xFFFF0000, 0xFFFF0000);
         dbg->insertDebugLine(((UINT32)this)+1+3, global, global + mat1.GetColumn(1), 0xFF00FF00, 0xFF00FF00);
         dbg->insertDebugLine(((UINT32)this)+2+3, global, global + mat1.GetColumn(2), 0xFF0000FF, 0xFF0000FF);

         if(mat0_x.absDotProduct(mat1_x) < (1-(1e-9)))
         {
            Ogre::Vector3 perp = mat1_x.crossProduct(mat0_x).normalisedCopy();
            Ogre::Radian angle = singedAngleBetweenVectors(mat0_x, mat1_x, perp);
            addAngularRow(angle, perp);
         }

         if(mat0_y.absDotProduct(mat1_y) < (1-(1e-9)))
         {
            Ogre::Vector3 perp = mat1_y.crossProduct(mat0_y).normalisedCopy();
            Ogre::Radian angle = singedAngleBetweenVectors(mat0_y, mat1_y, perp);
            addAngularRow(angle, perp);
         }

         if(mat0_z.absDotProduct(mat1_z) < (1-(1e-9)))
         {
            Ogre::Vector3 perp = mat1_z.crossProduct(mat0_z).normalisedCopy();
            Ogre::Radian angle = singedAngleBetweenVectors(mat0_z, mat1_z, perp);
            addAngularRow(angle, perp);
         }


One thing...if i have angular rows...how can i use torque for muscular strength?

Example: lower arm lift torque is 100Nm(if i persume lenght of lower arm is 30cm and that arm part can maximaly lift 30kg(weight of arm included)).