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 ));
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