/**
assuming Link::v,w is already computed by calcForwardKinematics(true);
assuming Link::wc is already computed by calcCM();
*/
void Body::calcTotalMomentum(Vector3& out_P, Vector3& out_L)
{
out_P.setZero();
out_L.setZero();
Vector3 dwc; // Center of mass speed in world frame
Vector3 P; // Linear momentum of the link
Vector3 L; // Angular momentum with respect to the world frame origin
Vector3 Llocal; // Angular momentum with respect to the center of mass of the link
int n = linkTraverse_.numLinks();
for(int i=0; i < n; i++){
Link* link = linkTraverse_[i];
dwc = link->v + link->w.cross(link->R * link->c);
P = link->m * dwc;
//L = cross(link->wc, P) + link->R * link->I * trans(link->R) * link->w;
Llocal.noalias() = link->I * link->R.transpose()*link->w;
L = link->wc.cross(P) + link->R * Llocal;
out_P += P;
out_L += L;
}
}
Vector3 LeggedBodyHelper::homeCopOfSoles() const
{
Vector3 p = Vector3::Zero();
int n = footInfos.size();
if(n == 0){
throw "LeggedBodyHelper::homeCopOfSoles(): not foot information";
} else {
for(size_t i=0; i < footInfos.size(); ++i){
const FootInfo& info = footInfos[i];
p.noalias() += info.link->p() + info.link->R() * info.homeCop;
}
}
return p / n;
}
const Vector3& Body::calcCenterOfMass()
{
double m = 0.0;
Vector3 mc = Vector3::Zero();
int n = linkTraverse_.numLinks();
for(int i=0; i < n; i++){
Link* link = linkTraverse_[i];
link->wc().noalias() = link->R() * link->c() + link->p();
mc.noalias() += link->m() * link->wc();
m += link->m();
}
impl->centerOfMass = mc / m;
impl->mass = m;
return impl->centerOfMass;
}
void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const
{
Vector3 arm;
int i;
for(i=1; i <= numUpwardConnections; ++i){
Link* link = links[i];
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 = child->p - arm;
if(calcVelocity){
child->sw.noalias() = link->R * child->a;
link->w = child->w - child->dq * child->sw;
link->v = child->v - link->w.cross(arm);
if(calcAcceleration){
link->dw = child->dw - child->dq * child->w.cross(child->sw) - (child->ddq * child->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 = child->p - arm;
if(calcVelocity){
child->sv.noalias() = link->R * child->d;
link->w = child->w;
link->v = child->v - child->dq * child->sv;
if(calcAcceleration){
link->dw = child->dw;
link->dv = child->dv - child->w.cross(child->w.cross(arm)) - child->dw.cross(arm)
- 2.0 * child->dq * child->w.cross(child->sv) - child->ddq * child->sv;
}
}
break;
case Link::FIXED_JOINT:
default:
link->R = child->R;
link->p.noalias() = child->p - (link->R * child->b);
if(calcVelocity){
link->w = child->w;
link->v = child->v;
if(calcAcceleration){
link->dw = child->dw;
link->dv = child->dv;
}
}
break;
}
}
int n = links.size();
for( ; i < n; ++i){
Link* link = links[i];
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 = parent->p + arm;
if(calcVelocity){
link->sw.noalias() = parent->R * link->a;
link->w = parent->w + link->sw * link->dq;
link->v = parent->v + parent->w.cross(arm);
if(calcAcceleration){
link->dw = parent->dw + link->dq * parent->w.cross(link->sw) + (link->ddq * link->sw);
link->dv = 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){
link->sv.noalias() = parent->R * link->d;
link->w = parent->w;
link->v = parent->v + link->sv * link->dq;
if(calcAcceleration){
link->dw = parent->dw;
link->dv = parent->dv + parent->w.cross(parent->w.cross(arm)) + parent->dw.cross(arm)
+ 2.0 * link->dq * parent->w.cross(link->sv) + link->ddq * link->sv;
}
}
break;
case Link::FIXED_JOINT:
default:
link->R = parent->R;
link->p.noalias() = parent->R * link->b + parent->p;
if(calcVelocity){
link->w = parent->w;
link->v = parent->v;
if(calcAcceleration){
link->dw = parent->dw;
link->dv = parent->dv;
}
}
break;
}
}
}
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;
}
}
}
