void JointPath::calcJacobian(Eigen::MatrixXd& out_J) const { const int n = joints.size(); out_J.resize(6, n); if(n > 0){ //! \todo compare the efficiency for the following codes if(false){ setJacobian<0x3f, 0, 0>(*this, linkPath.endLink(), out_J); } else { Link* targetLink = linkPath.endLink(); for(int i=0; i < n; ++i){ Link* link = joints[i]; switch(link->jointType()){ case Link::ROTATIONAL_JOINT: { Vector3 omega = link->R() * link->a(); const Vector3 arm = targetLink->p() - link->p(); if(!isJointDownward(i)){ omega = -omega; } out_J.col(i) << omega.cross(arm), omega; } break; case Link::SLIDE_JOINT: { Vector3 dp = link->R() * link->d(); if(!isJointDownward(i)){ dp = -dp; } out_J.col(i) << dp, Vector3::Zero(); } break; default: out_J.col(i).setZero(); } } } } }
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; } } }