Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 #include "rigidbodyinertia.hpp"
00023
00024 #include <Eigen/Core>
00025
00026 using namespace Eigen;
00027
00028 namespace KDL{
00029
00030 const static bool mhi=true;
00031
00032 RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi):
00033 m(m_),h(h_),I(I_)
00034 {
00035 }
00036
00037 RigidBodyInertia::RigidBodyInertia(double m_, const Vector& c_, const RotationalInertia& Ic):
00038 m(m_),h(m*c_){
00039
00040 Vector3d c_eig=Map<const Vector3d>(c_.data);
00041 Map<Matrix3d>(I.data)=Map<const Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity());
00042 }
00043
00044 RigidBodyInertia operator*(double a,const RigidBodyInertia& I){
00045 return RigidBodyInertia(a*I.m,a*I.h,a*I.I,mhi);
00046 }
00047
00048 RigidBodyInertia operator+(const RigidBodyInertia& Ia, const RigidBodyInertia& Ib){
00049 return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);
00050 }
00051
00052 Wrench operator*(const RigidBodyInertia& I,const Twist& t){
00053 return Wrench(I.m*t.vel-I.h*t.rot,I.I*t.rot+I.h*t.vel);
00054 }
00055
00056 RigidBodyInertia operator*(const Frame& T,const RigidBodyInertia& I){
00057 Frame X=T.Inverse();
00058
00059
00060
00061 Vector hmr = (I.h-I.m*X.p);
00062 Vector3d r_eig = Map<Vector3d>(X.p.data);
00063 Vector3d h_eig = Map<const Vector3d>(I.h.data);
00064 Vector3d hmr_eig = Map<Vector3d>(hmr.data);
00065 Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
00066 Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
00067 Matrix3d R = Map<Matrix3d>(X.M.data);
00068 RotationalInertia Ib;
00069 Map<Matrix3d>(Ib.data) = R*((Map<const Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());
00070
00071 return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi);
00072 }
00073
00074 RigidBodyInertia operator*(const Rotation& M,const RigidBodyInertia& I){
00075
00076
00077
00078 Matrix3d R = Map<const Matrix3d>(M.data);
00079 RotationalInertia Ib;
00080 Map<Matrix3d>(Ib.data) = R.transpose()*(Map<const Matrix3d>(I.I.data)*R);
00081
00082 return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
00083 }
00084
00085 RigidBodyInertia RigidBodyInertia::RefPoint(const Vector& p){
00086
00087
00088
00089 Vector hmr = (this->h-this->m*p);
00090 Vector3d r_eig = Map<const Vector3d>(p.data);
00091 Vector3d h_eig = Map<Vector3d>(this->h.data);
00092 Vector3d hmr_eig = Map<Vector3d>(hmr.data);
00093 Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
00094 Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
00095 RotationalInertia Ib;
00096 Map<Matrix3d>(Ib.data) = Map<Matrix3d>(this->I.data)+rcrosshcross+hmrcrossrcross;
00097
00098 return RigidBodyInertia(this->m,hmr,Ib,mhi);
00099 }
00100 }