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