Joints stiffness

[JoeJ]

edit: I see that you added a comment to you joint, after get your later, please post again so that I can take a look. Meant time I will on over the custom Power rag doll

You mean the the comment "Dont copy code, there are bugs"?
I have already fixed and posted in the next reply (not sure if the quote below here can capture all the code correctly).

I did some work on my smoothing idea and it seems promising. It may be a improvement anywhere where gramm schmidt is being used. It works by caching this orientation relative to the parent body, and allow it to rotate it only by a maximum acceleration.

Re: Joints stiffness

Post by JoeJ » Tue Jul 14, 2015 6:59 pm
I have fixed all bugs, now it should be a good test.
Animation and linear mode working now.
You can see my joints are less stiff than yours.
But it does not reproduce my problem that angular mode has for my ragdoll.
I will see if it's a matter of complexity, tweaking or bugs...

Joint code:

Code: Select all
struct JoesRagdollJoint
{
NewtonBody* m_body0;
NewtonBody* m_body1; // parent
dMatrix m_localMatrix0;
dMatrix m_localMatrix1;
NewtonJoint* m_joint;

dQuaternion m_target; // relative target rotatation to reach at next timestep

float m_reduceError;
float m_pin_length;
float m_angularFriction;
float m_linearFriction;
float m_stiffness;

bool m_useLienarMotor;
float m_anim_speed;
float m_anim_offset;
float m_anim_time;

JoesRagdollJoint (NewtonBody* child, NewtonBody* parent, const dMatrix &localMatrix0, const dMatrix &localMatrix1, NewtonWorld *world)
{
m_body0 = child;
m_body1 = parent;
m_localMatrix0 = localMatrix0;
m_localMatrix1 = localMatrix1;
m_target = dQuaternion (dVector(1.0f,0,0), 0.0f);
m_joint = NewtonConstraintCreateUserJoint (world, 6, JoesRagdollJoint::Callback, 0, child, parent);
NewtonJointSetUserData (m_joint, (void*) this);

m_reduceError = 0.2f; // amount of error to reduce per timestep (more -> oszillation)
m_pin_length = 10.0f;
m_angularFriction = 300.0f;
m_linearFriction = 100.0f;
m_stiffness = 0.9f;

m_useLienarMotor = false;
m_anim_speed = 0.0f;
m_anim_offset = 0.0f;
m_anim_time = 0.0f;
}

void SubmitConstraints (dFloat timestep, int threadIndex)
{

float invTimestep = 1.0f / timestep;

dMatrix body0Matrix; NewtonBodyGetMatrix(m_body0, &body0Matrix[0][0]);
dMatrix body1Matrix; NewtonBodyGetMatrix(m_body1, &body1Matrix[0][0]);
dMatrix matrix0 = m_localMatrix0 * body0Matrix;
dMatrix matrix1 = m_localMatrix1 * body1Matrix;

if (m_anim_speed != 0.0f) // some animation to illustrate purpose
{
m_anim_time += timestep * m_anim_speed;
float a0 = sin (m_anim_time);
float a1 = m_anim_offset * 3.14f;
dVector axis (sin(a1), 0.0f, cos(a1));
//dVector axis (1,0,0);
m_target = dQuaternion (axis, a0 * 0.5f);
}

// point to point constraint

dVector p0 (matrix0.m_posit);
dVector p1 (matrix1.m_posit);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_front[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_up[0]);
NewtonUserJointAddLinearRow (m_joint, &p0[0], &p1[0], &matrix1.m_right[0]);


// measure error

dQuaternion q0 (matrix0);
dQuaternion q1 (matrix1);

dQuaternion qt0 = m_target * q1;
dQuaternion qErr = (q0.DotProduct(qt0) < 0.0f
? dQuaternion (-q0.m_q0, q0.m_q1, q0.m_q2, q0.m_q3)
: dQuaternion ( q0.m_q0, -q0.m_q1, -q0.m_q2, -q0.m_q3)) * qt0;

float errorAngle = 2.0f * acos (max (-1.0f, min (1.0f, qErr.m_q0)));

if (errorAngle < 1.0e-10f)
{
// error is tiny - do we need to make a fixed joint / some damping here?
// To me it seems ok to do nothing.
}
else
{
dVector errorAxis (qErr.m_q1, qErr.m_q2, qErr.m_q3, 0.0f);
errorAxis = errorAxis.Scale(1.0f / dSqrt (errorAxis % errorAxis));

// fix rotation axis

dVector pin = p1 + errorAxis.Scale(m_pin_length);
dMatrix basis (dGrammSchmidt (errorAxis));

NewtonUserJointAddLinearRow (m_joint, (float*)&pin, (float*)&pin, (float*)&basis[1]);
NewtonUserJointAddLinearRow (m_joint, (float*)&pin, (float*)&pin, (float*)&basis[2]);

dVector angVel0, angVel1;
NewtonBodyGetOmega (m_body0, (float*)&angVel0);
NewtonBodyGetOmega (m_body1, (float*)&angVel1);

// motor

if (!m_useLienarMotor) // default angular motor
{
dVector errorAngVel = errorAxis.Scale (errorAngle * invTimestep * m_reduceError) - (angVel0 - angVel1);

NewtonUserJointAddAngularRow (m_joint, 0.0f, &errorAxis[0]);
NewtonUserJointSetRowAcceleration (m_joint, errorAxis % errorAngVel * invTimestep);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_angularFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_angularFriction);
NewtonUserJointSetRowStiffness (m_joint, m_stiffness);
}
else // linear motor
{
// construct two pins that remove error if they match
dVector pin0 (p1 + basis[1].Scale(m_pin_length));
dVector pin1 (p1 + ( basis[2].Scale(sin(errorAngle)) + basis[1].Scale(cos(errorAngle)) ).Scale(m_pin_length));
dVector diff = pin1 - pin0;

// calculate velocity at pins
dVector linVel0, linVel1;
NewtonBodyGetVelocity (m_body0, (float*)&linVel0);
NewtonBodyGetVelocity (m_body1, (float*)&linVel1);
dVector com0, com1;
NewtonBodyGetCentreOfMass (m_body0, (float*)&com0);
NewtonBodyGetCentreOfMass (m_body1, (float*)&com1);
com0 = body0Matrix.TransformVector (com0);
com1 = body1Matrix.TransformVector (com1);
dVector v0 (angVel0 * (pin0 - com0) + linVel0);
dVector v1 (angVel1 * (pin1 - com1) + linVel1);

dVector relAcc ( (diff.Scale(m_reduceError * invTimestep) - (v0 - v1)).Scale(invTimestep) );
diff = diff.Scale (1.0f / sqrt(diff % diff));

NewtonUserJointAddLinearRow (m_joint, (float*)&pin0, (float*)&pin1, (float*)&diff);
NewtonUserJointSetRowAcceleration (m_joint, relAcc % diff);
NewtonUserJointSetRowMinimumFriction (m_joint, -m_linearFriction);
NewtonUserJointSetRowMaximumFriction (m_joint, m_linearFriction);
NewtonUserJointSetRowStiffness (m_joint, m_stiffness);
}
}

}

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



Setup code:

Code: Select all
void AddJoesPoweredRagDoll (DemoEntityManager* const scene, const dVector& origin, const int jointType = 1, const float animSpeed = 0.0f, const int numSegments = 4)
{
float height = 1.0f;
float width = 4.0f;

dVector size (width, height, width);
NewtonBody* parent = CreateBox (scene, origin + dVector (0.0f, 0.5f, 0.0f, 0.0f), size);

#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainer* const skeleton = NewtonSkeletonContainerCreate (scene->GetNewton(), parent, NULL);
#endif

for (int i=0; i<numSegments; i++)
{
float height = 1.0f;
float width = 0.5f;

dVector size (width, height, width);
NewtonBody* child = CreateBox (scene, origin + dVector (0.0f, 0.5f + height * float(i+1), 0.0f, 0.0f), size);

if (jointType == 0) // Julios
{
dMatrix pinMatrix = dGrammSchmidt (dVector (0.0f, -1.0f, 1.0f, 0.0f));
dMatrix matrix0; NewtonBodyGetMatrix (parent, &matrix0[0][0]);
dMatrix matrix1; NewtonBodyGetMatrix (child, &matrix1[0][0]);
pinMatrix.m_posit = (matrix0.m_posit + matrix1.m_posit).Scale (0.5f);
CustomControlledBallAndSocket* const joint = new CustomControlledBallAndSocket (pinMatrix, child, parent);
}
else
{
dMatrix matrix0 = dGetIdentityMatrix(); matrix0.m_posit = dVector (0.0f, height*-0.5f, 0.0f, 1.0f);
dMatrix matrix1 = dGetIdentityMatrix(); matrix1.m_posit = dVector (0.0f, height*0.5f, 0.0f, 1.0f);
JoesRagdollJoint* joint = new JoesRagdollJoint (child, parent, matrix0, matrix1, scene->GetNewton());

if (jointType == 2) joint->m_useLienarMotor = true;
if (animSpeed != 0.0f) joint->m_anim_speed = animSpeed, joint->m_anim_offset = float(i) / float(numSegments); // animated
}

#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainerAttachBone (skeleton, child, parent);
#endif

parent = child;
}


#ifdef _USE_HARD_JOINTS
NewtonSkeletonContainerFinalize(skeleton);
#endif

}



and this at the bottom:

Code: Select all
AddJoesPoweredRagDoll (scene, dVector (0.0f, 0.0f, -15.0f), 1, 1.5f); // angular + animated
AddJoesPoweredRagDoll (scene, dVector (0.0f, 0.0f, -5.0f), 1); // angular
AddJoesPoweredRagDoll (scene, dVector (0.0f, 0.0f, 5.0f), 2); // linear
AddJoesPoweredRagDoll (scene, dVector (0.0f, 0.0f, 15.0f), 0); // Julio

[Julio Jerez]

just added the powered ragdoll, it turn out I had tow bugs, now they are even stronger, if you believe it.
I am now completing the last one , the path follow, and the I will check out yours.
and with the that I think we are done.

there is thing left, and that is the, the joint seem soft at the base, because the new system doe no handle bodies with infinite mass. what I do is the I attach the base to the world with the fix jo9nt that is no part of the skeleton.

But this week end I will bring back the option using exact solve of selected joints, that will make that base a lot more stronger, the only thing that will brake them will be numerical instability.

[JoeJ]

Some interesting things i found out:

The powered joints are perfectly stiff if i do not use NewtonUserJointSetRowMinimumFriction.
(Independent of the given parameter or my reduceError factor)

NewtonUserJointSetRowMinimumFriction is dangerous.
If i use it for axis stabilization, it blows up (dos not fly to nirwana but jitters extremely and can't pose)

Skeleton is more sensible to the order you add rows. You need to stabilize axis first.
(Without skeleton i have the impression that order does not matter)

just added the powered ragdoll, it turn out I had tow bugs, now they are even stronger, if you believe it.

Whooo! To bad i have to leave now. Will try later...

[Julio Jerez]

Code: Select all

The powered joints are perfectly stiff if i do not use NewtonUserJointSetRowMinimumFriction.
(Independent of the given parameter or my reduceError factor)
NewtonUserJointSetRowMinimumFriction is dangerous.

yes that that is correct, but it is not dangerous. it may be violating the limit and let it move, this is what the function of for. That was wan of the error I say. on mine.
The second error was that when the joint was at the I was changing the sign of the angles by choosing the base as the reference instead of the target as I was on the second part of the if, so the joint we jittering a the at the point.
Met the target, move out, then mod to the target and move out again and so on. Now that fixed

[JoeJ]

The powered joints are perfectly stiff if i do not use NewtonUserJointSetRowMinimumFriction.
(Independent of the given parameter or my reduceError factor)

NewtonUserJointSetRowMinimumFriction is dangerous.
If i use it for axis stabilization, it blows up (dos not fly to nirwana but jitters extremely and can't pose)

I mean my joint, you will see when you try it out. (The issues are still there with the newest Newton)
I'm pretty sure when you remove the setting of max friction, it will be as stiff as yours with the advantage that it's faster and moves along the predicted arc.
But max friction is important to avoid infinite strong characters or other robotic elements.

Meanwhile i found that the joint i have posted is not the best i had. The oldest one i used is much better for balancing (very smooth motion), and uses 3 angular rows, each setting accel and max friction.
If you like the joint i have posted a can post the better one too, but first i need to improve something i don't like about it.

I will also try your improved joint on my ragdoll (need to do copy paste...)

I saw the other improvements you made on the joint lib

[Julio Jerez]

...with the advantage that it's faster and moves along the predicted arc.

I think you are right, using small angular approximating doe no generate a nice trajectory.

I will review it to see If there is a bug, if no I will just use your version.

[Julio Jerez]

I was looking at the implementation and believe I must have some errors there.
I am not even go to debugged I will simple do it again using the small angular approximation.
The small angular approximation is is act a very robust idea, it should work near perfect and be very robust.
The only difference is that I think I am using it for the wrong joint, I should use that alagorth for the 3dof instead.
The idea is this.
Say you have to matrices, M0 and M1
Say you set matrix on as your base and you stablish the kinematic relation

M0 = L * M1

Where L is the rotation matrix that turns M1 into M0

After you calculate L, you can get the quaternion there represent matrix L
Now this quaternion represents the arch on the unit sphere the any point on the unit sphere will follows, it is a on demotion circulate path, because the trajectory is on dimensional, this mean that is can be decomposed into a series of annual rotation along the quaternion axis of rotation,
And all the rotations are comutatives

That is day Q = quaterion (L)

The Q can be decomposed into to ration that are commutative, that is

Q = q0 * q1 = q1 * q0

An the is the theory that can up the claim
So if the is the case the you can plug plug Q = q0 * q1 it the matrix expression

M0 = Q0 * Q1 * M1

Then you do

M0 = Q0 * (Q1 * M1)

And there join is implement as

1-calcualte Q
2-calculate Q0 and Q1 from Q
Q1 is the fraction that is determined by the joint max speed along the arch Q and the time step.

The new expression is now

Q0’ * M0 = Q1 * M1

M’0 = Q1 * M1
And we submit the angular rotation

That part is common to both joints, the idea of the small angles approximation Is that
If Q1 is near identity, then the three Euler angles can be commuted and the error is small.
the mean that the rotations alone x, y, z are nearly independent, and can be taken on any order.

at the point what the small angle approximation does is that It gets the euler angles from Q1
And pass three rows along the three axis of M1, and in any order.
The other benefit is that you can get the angles by accumulation the from the euler
It should works, I must have a bug some where.

[JoeJ]

I think that joint can be useful if it follows the shortest arc at least closely, but also it is necessary to set the exact time duration until it hits the target.
Even if that works the velocity will be constant.
This makes it not good for locomotion problems, where acceleration should be constant but not velocity. And constant acceleration is also only a side effect (something that proofs the control algorithm is good), but not a parameter that you can give to a joint an leave it unchanged until it hits the target.

A good example is the one legged jumping robot that i made. Meanwhile it uses a complete analytic model much better than the complicated ragdoll. No tweaking, no state machine. Every timestep it analyses velocity of itself and enviroment, predicts when it will land to shorten the leg, or when it should take off to reach a given height (This way it also works when the ground has unpredictable velocity).
Finally it seems like the robot has constant acceleration all the time, but each timestep it needs to make small adjustments to react to simulation error or enviromental changes.

Also, the robust angular approximation is a nice idea, but it is not necessary anymore.
The simulation with hard joints is as robust as it is stiff.
Today i finished to rework my joint, not using the skeleton i can see small improvements - less jitter caused by balancing, smoother motion etc. But still i can only resolve 20% of error while balancing to avoid oszillation.
But with the skeleton i can set the gravity to 100m/s*s(!) and error resolving to 100%(!).
Simulation is total robust - ragdoll can fall with full weight on its small hand and it won't bend.
And there is zero jitter, just low frequency drift in the range of less than 1mm, caused probably by resting contact.
It is unbelievable, but this works with a joint that does any rotation in one step.

All problems we know for years are gone. Instead we have new ones: It's simply too stiff.
(Yeah, i'm the first one complaining about that

Like said:
I have to remove all RowMaxFriction calls to avoid explosions.
You may be lucky to use it in one row (like in my demo joint), but then you loose hard stiffness for that row.


I just wanna sum up - no hurry with this. Just trying to push in the right direction a bit.
I know you expected that problems, but i expect more than a bit of work left to take advantage from that holy grail
Actually it hurts ragdoll can't do anything useful - it jumps 1m by moving a toe.
Next thing i'll try is to make the point to point constraint flexible...

[Julio Jerez]

...
it is unbelievable, but this works with a joint that does any rotation in one step.
All problems we know for years are gone. Instead we have new ones: It's simply too stiff.
(Yeah, i'm the first one complaining about that
I just wanna sum up - no hurry with this. Just trying to push in the right direction a bit.
I know you expected that problems, but i expect more than a bit of work left to take advantage from that holy grail
Actually it hurts ragdoll can't do anything useful - it jumps 1m by moving a toe.
Next thing i'll try is to make the point to point constraint flexible...

is not that amassing how strong joints using that feature are. This is a case where we think if we can only get that feature, and then wen we get it we start to see that it is not all glory.
For years and year we have learned how to work with errors and we now think the error it the norm but the show no be. But to me this is a huge win, we just need to learn how to work with it.

I have to come up with the a way to add stiffness, They are simple too strong now,
I actually do have the functionality in the code, but is commented out, I nee to add stiffness and dampening as well and I have not figured out how to incorporate that.

I just checked it the change to make handle root node with infinite mass and now if you check the demos out will see that the root node nodes do not moves all.

This afternoon I will add the code that will make the normal joint as stiffer than they are now, and at the same time they will not be any slower.

we what are seen now is the unforeseen problems that new features always bring with them,
Basically unlimited stiffness combined with discrete time step will inevitable lead to some numerical instability, so I must come up with the a way add some controllable softness.

[Julio Jerez]

Also, the robust angular approximation is a nice idea, but it is not necessary anymore.
The simulation with hard joints is as robust as it is stiff.

I agree I am abandoning that idea, I spend the morning try to get to work, but it just not worth the effort.

I also leaned that, the learn row in some case do the just just fine, I does not has to be an all or nothing, we can use linear row when we need then and angular rows we need then.

[Julio Jerez]

Joe so that you can see the power of this new feature.
I am making a Robot with thread, an many, many articulate joints.

Ih have three skeleton, 1 for the body and one for each thread, it is no functional yet because I have to add limit and a way for the read to get out of the retails.
But I is not blow up, and these is a complex baby.

I was thinking It was not going to even work, I know google sketchy physics people do many of this but they do not run in real time, I believe this feature can make it possible to run this is real time.
But maybe I picked a test case that is fat too complex.

[JoeJ]

That's lots of bodies

I'm always thinking about robotic level design, somehow like this:


Platform, puzzle, sandbox game... there's a lot of possibilities.

The possibility to make a static root is great for this

Started working on the walking ragdoll again...

[Julio Jerez]

wow that's a lot of noise on the video, By I believe that is quite possible with the right tool.

I now have the robot basically working, but It need more polish.
I find it quiet hard to turn, and it make essence all tow track link in contact with the ground each one has a small grip, but when combine the offer a formidable torque to overcome.
I wonder of real tank have that problem, I realized that Tank are extremely inefficient vehicles, there requires lot or power to turn in places.

I also see that it nee reals suspension, other wise only few links and in full support of the vehicle so it drift, but all in all it does work and is stable

and yes this is a lot of interconnected bodies for one feature, so fat 160 and counting, it is more a proof of concept of the technology.

[Julio Jerez]

later to make more practical I will scale up some of the body part, I when too far.

at 128 for the link are so small that a relatively small speed the track link overwhelm the supports Cog and the track get derailed.
I will make the link 25% bigger eh small tires 50% bigger, that will make more practical

also another problem that we solve is the perennial mass ratio, that baby has mass ration that rage for 100 to 1 to 1000 to 1. and it is smooth and stable as silk.

[Sweenie]

wow, i know it isn't finished yet but that thing is a monster
Couldn't steer it though so i tried to drag it with the mouse, but the mousepicking still generates absurd amounts of force i guess so i managed to rip the treads off.

Awesome work