ArticulatedBodyInertia ArticulatedBodyInertia::RefPoint(const Vector& p){
//mb=ma
//hb=R*(h-m*r)
//Ib = R(Ia+r x h x + (h-m*r) x r x)R'
Matrix3d rcross;
rcross << 0,-p[2],p[1],
p[2],0,-p[0],
-p[1],p[0],0;
Matrix3d HrM=this->H-rcross*this->M;
return ArticulatedBodyInertia(this->M,HrM,this->I-rcross*this->H.transpose()+HrM*rcross);
}
ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I){
Frame X=T.Inverse();
//mb=ma
//hb=R*(h-m*r)
//Ib = R(Ia+r x h x + (h-m*r) x r x)R'
Map<Matrix3d> E(X.M.data);
Matrix3d rcross;
rcross << 0,-X.p[2],X.p[1],
X.p[2],0,-X.p[0],
-X.p[1],X.p[0],0;
Matrix3d HrM=I.H-rcross*I.M;
return ArticulatedBodyInertia(E*I.M*E.transpose(),E*HrM*E.transpose(),E*(I.I-rcross*I.H.transpose()+HrM*rcross)*E.transpose());
}
ArticulatedBodyInertia TransformDerivative::transform(const Transform& transform,
ArticulatedBodyInertia& other)
{
// Inefficient implementation for the time being, this should not be a bottleneck
Matrix6x6 oldABI = other.asMatrix();
Matrix6x6 a_X_b_wrench = transform.asAdjointTransformWrench();
Matrix6x6 a_dX_b_wrench = this->asAdjointTransformWrenchDerivative(transform);
Eigen::Matrix<double,6,6,Eigen::RowMajor> b_X_a = toEigen(a_X_b_wrench).transpose();
Eigen::Matrix<double,6,6,Eigen::RowMajor> b_dX_a = toEigen(a_dX_b_wrench).transpose();
Matrix6x6 newABI;
toEigen(newABI) = toEigen(a_dX_b_wrench)*toEigen(oldABI)*(b_X_a)
+toEigen(a_X_b_wrench)*toEigen(oldABI)*(b_dX_a);
return ArticulatedBodyInertia(newABI.data(),6,6);
}
/**
* Creates an inertia with zero mass, and zero RotationalInertia
*/
static inline ArticulatedBodyInertia Zero(){
return ArticulatedBodyInertia(Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero(),Eigen::Matrix3d::Zero());
};
ArticulatedBodyInertia operator*(const Rotation& M,const ArticulatedBodyInertia& I){
Map<const Matrix3d> E(M.data);
return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E);
}
ArticulatedBodyInertia operator-(const RigidBodyInertia& Ia, const ArticulatedBodyInertia& Ib){
return ArticulatedBodyInertia(Ia)-Ib;
}
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia, const ArticulatedBodyInertia& Ib){
return ArticulatedBodyInertia(Ia.M-Ib.M,Ia.H-Ib.H,Ia.I-Ib.I);
}
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia, const ArticulatedBodyInertia& Ib){
return ArticulatedBodyInertia(Ia.M+Ib.M,Ia.H+Ib.H,Ia.I+Ib.I);
}
ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I){
return ArticulatedBodyInertia(a*I.M,a*I.H,a*I.I);
}
