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