void ODESimulatorItemImpl::addBody(ODEBody* odeBody)
{
Body& body = *odeBody->body();
Link* rootLink = body.rootLink();
rootLink->v().setZero();
rootLink->dv().setZero();
rootLink->w().setZero();
rootLink->dw().setZero();
for(int i=0; i < body.numJoints(); ++i){
Link* joint = body.joint(i);
joint->u() = 0.0;
joint->dq() = 0.0;
joint->ddq() = 0.0;
}
body.clearExternalForces();
body.calcForwardKinematics(true, true);
odeBody->createBody(this);
}
/**
calculate the mass matrix using the unit vector method
\todo replace the unit vector method here with a more efficient method
The motion equation (dv != dvo)
| | | dv | | | | fext |
| out_M | * | dw | + | b1 | = | tauext |
| | |ddq | | | | u |
*/
void calcMassMatrix(Body* body, const Vector3& g, Eigen::MatrixXd& out_M)
{
const int nj = body->numJoints();
Link* rootLink = body->rootLink();
const int totaldof = rootLink->isFixedJoint() ? nj : nj + 6;
out_M.resize(totaldof, totaldof);
// preserve and clear the joint accelerations
VectorXd ddqorg(nj);
VectorXd uorg(nj);
for(int i = 0; i < nj; ++i){
Link* joint = body->joint(i);
ddqorg[i] = joint->ddq();
uorg [i] = joint->u();
joint->ddq() = 0.0;
}
// preserve and clear the root link acceleration
const Vector3 dvorg = rootLink->dv();
const Vector3 dworg = rootLink->dw();
//const Vector3 root_w_x_v = rootLink->w().cross(rootLink->vo() + rootLink->w().cross(rootLink->p()));
rootLink->dv() = g;
rootLink->dw().setZero();
MatrixXd b1(totaldof, 1);
setColumnOfMassMatrix(body, b1, 0);
if(!rootLink->isFixedJoint()){
for(int i=0; i < 3; ++i){
rootLink->dv()[i] += 1.0;
setColumnOfMassMatrix(body, out_M, i);
rootLink->dv()[i] -= 0.0;
}
for(int i=0; i < 3; ++i){
rootLink->dw()[i] = 1.0;
setColumnOfMassMatrix(body, out_M, i + 3);
rootLink->dw()[i] = 0.0;
}
}
for(int i = 0; i < nj; ++i){
Link* joint = body->joint(i);
joint->ddq() = 1.0;
int j = i + 6;
setColumnOfMassMatrix(body, out_M, j);
out_M(j, j) += joint->Jm2(); // motor inertia
joint->ddq() = 0.0;
}
// subtract the constant term
for(int i = 0; i < out_M.cols(); ++i){
out_M.col(i) -= b1;
}
// recover state
for(int i = 0; i < nj; ++i){
Link* joint = body->joint(i);
joint->ddq() = ddqorg[i];
joint->u() = uorg [i];
}
rootLink->dv() = dvorg;
rootLink->dw() = dworg;
}
void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const
{
Vector3 arm;
int i;
for(i=1; i <= numUpwardConnections; ++i){
Link* link = links[i];
const Link* child = links[i-1];
switch(child->jointType()){
case Link::ROTATIONAL_JOINT:
link->R().noalias() = child->R() * AngleAxisd(child->q(), child->a()).inverse();
arm.noalias() = link->R() * child->b();
link->p().noalias() = child->p() - arm;
if(calcVelocity){
const Vector3 sw(link->R() * child->a());
link->w() = child->w() - child->dq() * sw;
link->v() = child->v() - link->w().cross(arm);
if(calcAcceleration){
link->dw() = child->dw() - child->dq() * child->w().cross(sw) - (child->ddq() * sw);
link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);
}
}
break;
case Link::SLIDE_JOINT:
link->R() = child->R();
arm.noalias() = link->R() * (child->b() + child->q() * child->d());
link->p().noalias() = child->p() - arm;
if(calcVelocity){
const Vector3 sv(link->R() * child->d());
link->w() = child->w();
link->v().noalias() = child->v() - child->dq() * sv;
if(calcAcceleration){
link->dw() = child->dw();
link->dv().noalias() =
child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm)
- 2.0 * child->dq() * child->w().cross(sv) - child->ddq() * sv;
}
}
break;
case Link::FIXED_JOINT:
default:
arm.noalias() = link->R() * child->b();
link->R() = child->R();
link->p().noalias() = child->p() - arm;
if(calcVelocity){
link->w() = child->w();
link->v() = child->v() - link->w().cross(arm);
if(calcAcceleration){
link->dw() = child->dw();
link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);;
}
}
break;
}
}
const int n = links.size();
for( ; i < n; ++i){
Link* link = links[i];
const Link* parent = link->parent();
switch(link->jointType()){
case Link::ROTATIONAL_JOINT:
link->R().noalias() = parent->R() * AngleAxisd(link->q(), link->a());
arm.noalias() = parent->R() * link->b();
link->p().noalias() = parent->p() + arm;
if(calcVelocity){
const Vector3 sw(parent->R() * link->a());
link->w().noalias() = parent->w() + sw * link->dq();
link->v().noalias() = parent->v() + parent->w().cross(arm);
if(calcAcceleration){
link->dw().noalias() = parent->dw() + link->dq() * parent->w().cross(sw) + (link->ddq() * sw);
link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
}
}
break;
case Link::SLIDE_JOINT:
link->R() = parent->R();
arm.noalias() = parent->R() * (link->b() + link->q() * link->d());
link->p() = parent->p() + arm;
if(calcVelocity){
const Vector3 sv(parent->R() * link->d());
link->w() = parent->w();
link->v().noalias() = parent->v() + sv * link->dq();
if(calcAcceleration){
link->dw() = parent->dw();
link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm)
+ 2.0 * link->dq() * parent->w().cross(sv) + link->ddq() * sv;
}
}
break;
case Link::FIXED_JOINT:
default:
arm.noalias() = parent->R() * link->b();
link->R() = parent->R();
link->p().noalias() = arm + parent->p();
if(calcVelocity){
link->w() = parent->w();
link->v() = parent->v() + parent->w().cross(arm);;
if(calcAcceleration){
link->dw() = parent->dw();
link->dv().noalias() = parent->dv() +
parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
}
}
break;
}
}
}
