00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "articulatedbodyinertia.hpp"
00023
00024 #include <Eigen/Core>
00025
00026 using namespace Eigen;
00027
00028 namespace KDL{
00029
00030 ArticulatedBodyInertia::ArticulatedBodyInertia(const RigidBodyInertia& rbi)
00031 {
00032 this->M=Matrix3d::Identity()*rbi.m;
00033 this->I=Map<const Matrix3d>(rbi.I.data);
00034 this->H << 0,-rbi.h[2],rbi.h[1],
00035 rbi.h[2],0,-rbi.h[0],
00036 -rbi.h[1],rbi.h[0],0;
00037 }
00038
00039 ArticulatedBodyInertia::ArticulatedBodyInertia(double m, const Vector& c, const RotationalInertia& Ic)
00040 {
00041 *this = RigidBodyInertia(m,c,Ic);
00042 }
00043
00044 ArticulatedBodyInertia::ArticulatedBodyInertia(const Matrix3d& M, const Matrix3d& H, const Matrix3d& I)
00045 {
00046 this->M=M;
00047 this->I=I;
00048 this->H=H;
00049 }
00050
00051 ArticulatedBodyInertia operator*(double a,const ArticulatedBodyInertia& I){
00052 return ArticulatedBodyInertia(a*I.M,a*I.H,a*I.I);
00053 }
00054
00055 ArticulatedBodyInertia operator+(const ArticulatedBodyInertia& Ia, const ArticulatedBodyInertia& Ib){
00056 return ArticulatedBodyInertia(Ia.M+Ib.M,Ia.H+Ib.H,Ia.I+Ib.I);
00057 }
00058
00059 ArticulatedBodyInertia operator+(const RigidBodyInertia& Ia, const ArticulatedBodyInertia& Ib){
00060 return ArticulatedBodyInertia(Ia)+Ib;
00061 }
00062 ArticulatedBodyInertia operator-(const ArticulatedBodyInertia& Ia, const ArticulatedBodyInertia& Ib){
00063 return ArticulatedBodyInertia(Ia.M-Ib.M,Ia.H-Ib.H,Ia.I-Ib.I);
00064 }
00065
00066 ArticulatedBodyInertia operator-(const RigidBodyInertia& Ia, const ArticulatedBodyInertia& Ib){
00067 return ArticulatedBodyInertia(Ia)-Ib;
00068 }
00069
00070 Wrench operator*(const ArticulatedBodyInertia& I,const Twist& t){
00071 Wrench result;
00072 Vector3d::Map(result.force.data)=I.M*Vector3d::Map(t.vel.data)+I.H.transpose()*Vector3d::Map(t.rot.data);
00073 Vector3d::Map(result.torque.data)=I.I*Vector3d::Map(t.rot.data)+I.H*Vector3d::Map(t.vel.data);
00074 return result;
00075 }
00076
00077 ArticulatedBodyInertia operator*(const Frame& T,const ArticulatedBodyInertia& I){
00078 Frame X=T.Inverse();
00079
00080
00081
00082 Map<Matrix3d> E(X.M.data);
00083 Matrix3d rcross;
00084 rcross << 0,-X.p[2],X.p[1],
00085 X.p[2],0,-X.p[0],
00086 -X.p[1],X.p[0],0;
00087
00088 Matrix3d HrM=I.H-rcross*I.M;
00089 return ArticulatedBodyInertia(E*I.M*E.transpose(),E*HrM*E.transpose(),E*(I.I-rcross*I.H.transpose()+HrM*rcross)*E.transpose());
00090 }
00091
00092 ArticulatedBodyInertia operator*(const Rotation& M,const ArticulatedBodyInertia& I){
00093 Map<const Matrix3d> E(M.data);
00094 return ArticulatedBodyInertia(E.transpose()*I.M*E,E.transpose()*I.H*E,E.transpose()*I.I*E);
00095 }
00096
00097 ArticulatedBodyInertia ArticulatedBodyInertia::RefPoint(const Vector& p){
00098
00099
00100
00101 Matrix3d rcross;
00102 rcross << 0,-p[2],p[1],
00103 p[2],0,-p[0],
00104 -p[1],p[0],0;
00105
00106 Matrix3d HrM=this->H-rcross*this->M;
00107 return ArticulatedBodyInertia(this->M,HrM,this->I-rcross*this->H.transpose()+HrM*rcross);
00108 }
00109 }