rigidbodyinertia.cpp
Go to the documentation of this file.
00001 // Copyright  (C)  2007  Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00002 
00003 // Version: 1.0
00004 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00005 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
00006 // URL: http://www.orocos.org/kdl
00007 
00008 // This library is free software; you can redistribute it and/or
00009 // modify it under the terms of the GNU Lesser General Public
00010 // License as published by the Free Software Foundation; either
00011 // version 2.1 of the License, or (at your option) any later version.
00012 
00013 // This library is distributed in the hope that it will be useful,
00014 // but WITHOUT ANY WARRANTY; without even the implied warranty of
00015 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00016 // Lesser General Public License for more details.
00017 
00018 // You should have received a copy of the GNU Lesser General Public
00019 // License along with this library; if not, write to the Free Software
00020 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
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         //I=Ic-c x c x
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         //mb=ma
00059         //hb=R*(h-m*r)
00060         //Ib = R(Ia+r x h x + (h-m*r) x r x)R'
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         //mb=ma
00076         //hb=R*h
00077         //Ib = R(Ia)R' with r=0
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         //mb=ma
00087         //hb=(h-m*r)
00088         //Ib = (Ia+r x h x + (h-m*r) x r x)
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 }//namespace


orocos_kdl
Author(s):
autogenerated on Wed Aug 26 2015 15:14:14