Featherstone articulated body algorithm

[boldt]

Hi

We have some problems with Featherstones Articulated body algorithm. We have implemented it and also uses it to compute the dynamics for articulated bodies with a floating base link.

We have a articulated body consisting of a base link and one child. The velocity in the joint is zero. Both links are affected by gravity which are the only external force acting in the system.

We then compute the zero acceleration force on the base link, and it is here our problems start. From the children a small angular force are given on to the base link.

the contribution to the zero acceleration force from the child to the base link are computed as

Code: Select all

mLinks[h]->ZA_s += (mLinks[i]->hXi * ( mLinks[i]->ZA_s + mLinks[i]->IA_s * mLinks[i]->c_s + (mLinks[i]->csu/mLinks[i]->csd) * mLinks[i]->csh ));

where mLinks[h] are the base link and mLinks are the child link. All computations are done in link coordinates.

The angular contribution comes from (mLinks->csu/mLinks->csd) * mLinks->csh )), which are the common subexpressions as defined by featherstone.

We are pretty sure this is wrong, but are not certain. Does anybody have a hint to the problems.

Best Regards
Niels

[vangelis]

Hi Niels,
Are you forgetting maybe to add the gravity force to your base link?

mLinks[h]->ZA_s should be initialized with -g_h (spatial gravity force in the local h coordinate frame) before you loop through the children of h to add their contributions. That term should, I believe, cancel out the contribution from (mLinks->csu/mLinks->csd) * mLinks->csh )) as mLinks->csu = -s_i * g_i .

I find it easier to troubleshoot Featherstone's algorithm when I look at the equations in pseudo-code form. If you take a look here:

http://research.scea.com/research/pdfs/ ... DC2004.pdf

on page 4 you'll find a more compact representation of the loops involved in Featherstone's algorithm.

Hope this helps...

Cheers,
Vangelis

[boldt]

Hi Vangelis

Thanks for your answer, I already knew your article and it is very interesting We have done quite a lot of debugging the last couple of days, a our problem probably has another cause, that the one we describes in the post. We already initialises the external force with the gravity.

So far we have narrowed the problem down to the following:

We use the equation a_s = - I_a * Z_a to compute the spatial acceleration of the base link. But then we are not sure how we should do the integration step. Could we just keep the spatial velocity as one state variable and integrate as v_s += dt * a_s. And how do we get v_s converted to a velocity in the global frame, so the position can be updated.

All articles describes this as trivial, but so far we have not been able to solve the problem.

Thanks
Niels

[vangelis]

Hi Niels,
Glad to hear that the paper was not completely wasted ...

Just out of curiosity, what kind of joint connects the base to the first link? The reason I'm asking is that if it is a revolute joint then you should expect mLinks->csu to be zero... Don't forget that the spatial transpose operator that's used to compute U = S' . P flips the first and second 3-vectors within the spatial joint vector S. So if S = [ s0 s1 ] then S' = [s1' s0'] ... A revolute joint has s1 = 0. If gravity is the only force acting on the link, P = [ p0 p1 ] where p1 = 0. Therefore U = S' . P = 0

Regarding your second question, here's what I do..

Assume V = [ w v ], A = [ wd a ] are the spatial velocity and acceleration of the base in local space. If R is the rotation matrix from local to world space (for the base) then :

v_W = R v => a_W = Rd v + R a

where v_W and a_W are the linear velocity and acceleration in world space and Rd = (R w)xR is the time derivative of R ... So that gives you the expression for the linear acceleration in world space.

Similarly, the angular velocity in world space is:

w_W = R w => wd_W = Rd w + R wd

In my code I use a quaternion to store the orientation of the base

Cheers,
Vangelis

[vangelis]

Ooops, I take back the first thing that I said in the previous posting... U will be zero on a link with a revolute joint only if the gravity force does not produce a torque about the joint axis.

Cheers,
Vangelis

[boldt]

Hi Vangelis

Thanks again . But

Assume V = [ w v ], A = [ wd a ] are the spatial velocity and acceleration of the base in local space. If R is the rotation matrix from local to world space (for the base) then :

Just to be sure! By spatial velocity and acceleration you denote vectors that are given in a frame rotating with the base link?

where v_W and a_W are the linear velocity and acceleration in world space and Rd = (R w)xR is the time derivative of R ...

I'm a not so sure on the notation you use here, but I understand it the following way. By (R w)x you mean the current rotation multiplied with the angular velocity in spatial coodinates and the we create the skew symmetric cross product matrix from the result which we multiply with the current rotation?

Could you also use quaternions instead to describe the derivative of the orientation, and using q_d = 0.5 * [0,w] * q, where q are the current orientation of the base link and qd are the derivative of q.

In my code I use a quaternion to store the orientation of the base.

But how do you store the velocity of the base link between each integration. Is it the spatial velocity or do you treat the base link as a rigid body and store linear and angular momentum in world coordinates. In that case, which inertia tensor do you use. Just the isolated inertia tensor?

I have investigated the issue with the angular contribution from the child links to the base links further and made a strange observation. The angular contribution from the child links does not change when I remove the gravity from the base link. I definitely feel this is an error in my code, isn't that right?

And still i would expect no angular contribution from the child link as all bodies are falling freely. Would it be better to leave out the gravity from all links and then apply the acceleration only to the base link after we have computed joint accelerations.


Thanks
Niels

[boldt]

I just forgot a small thing

Before I start the forward dynamics loop for the articulated body I initialize the base link the following way

Code: Select all

bRw = mLinks[0]->GetOrientation().toMatrix3().transpose();
VectorS f_s_external( bRw * mGravity * mLinks[0]->GetMass() , vec3( 0.0, 0.0, 0.0 ) );
mLinks[0]->ZA_s = mLinks[0]->GetSpatialVelocity().cross( mLinks[0]->IA_s * mLinks[0]->GetSpatialVelocity() ) - f_s_external;

In pseudo code it would be something like


Z_{a,0} = V_{s,0} x I_{a,s} * V_{s,0} + f_s_external


Should I do anymore than this

Thanks
Niels

[vangelis]

I'm a not so sure on the notation you use here, but I understand it the following way. By (R w)x you mean the current rotation multiplied with the angular velocity in spatial coordinates and the we create the skew symmetric cross product matrix from the result which we multiply with the current rotation?

Sorry about the confusing notation. I meant exactly what you said above.

Could you also use quaternions instead to describe the derivative of the orientation, and using q_d = 0.5 * [0,w] * q, where q are the current orientation of the base link and qd are the derivative of q.

Yes, that state of the base link consists of the position, linear velocity, orientation quaternion and angular velocity (all in world space). I use the equation you mentioned for q_d to update q during the integration step. To update the spatial velocity of the first link I convert the velocity vectors from world to local space by multiplying them with R^-1.

I have investigated the issue with the angular contribution from the child links to the base links further and made a strange observation. The angular contribution from the child links does not change when I remove the gravity from the base link. I definitely feel this is an error in my code, isn't that right?

And still i would expect no angular contribution from the child link as all bodies are falling freely. Would it be better to leave out the gravity from all links and then apply the acceleration only to the base link after we have computed joint accelerations.

You know, I never noticed this behavior but you are right! After looking more closely into the equations it seems that the only way Featherstone's A = IA^-1 * P would work is if the joint center of the child joint is coincident with the COM of the base link ... Otherwise you end up with these angular acceleration components transferred through the P vector of the child link.


Cheers,
Vangelis