orocos_kdl
rigidbodyinertia.cpp
Go to the documentation of this file.
1 // Copyright (C) 2007 Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
5 // Maintainer: Ruben Smits <ruben dot smits at mech dot kuleuven dot be>
6 // URL: http://www.orocos.org/kdl
7 
8 // This library is free software; you can redistribute it and/or
9 // modify it under the terms of the GNU Lesser General Public
10 // License as published by the Free Software Foundation; either
11 // version 2.1 of the License, or (at your option) any later version.
12 
13 // This library is distributed in the hope that it will be useful,
14 // but WITHOUT ANY WARRANTY; without even the implied warranty of
15 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
16 // Lesser General Public License for more details.
17 
18 // You should have received a copy of the GNU Lesser General Public
19 // License along with this library; if not, write to the Free Software
20 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
21 
22 #include "rigidbodyinertia.hpp"
23 
24 #include <Eigen/Core>
25 
26 namespace KDL{
27 
28  const static bool mhi=true;
29 
30  RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi):
31  m(m_),h(h_),I(I_)
32  {
33  }
34 
36  m(m_),h(m*c_){
37  //I=Ic-c x c x
38  Eigen::Vector3d c_eig=Eigen::Map<const Eigen::Vector3d>(c_.data);
39  Eigen::Map<Eigen::Matrix3d>(I.data)=Eigen::Map<const Eigen::Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Eigen::Matrix3d::Identity());
40  }
41 
43  return RigidBodyInertia(a*I.m,a*I.h,a*I.I,mhi);
44  }
45 
47  return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);
48  }
49 
50  Wrench operator*(const RigidBodyInertia& I,const Twist& t){
51  return Wrench(I.m*t.vel-I.h*t.rot,I.I*t.rot+I.h*t.vel);
52  }
53 
55  Frame X=T.Inverse();
56  //mb=ma
57  //hb=R*(h-m*r)
58  //Ib = R(Ia+r x h x + (h-m*r) x r x)R'
59  Vector hmr = (I.h-I.m*X.p);
60  Eigen::Vector3d r_eig = Eigen::Map<Eigen::Vector3d>(X.p.data);
61  Eigen::Vector3d h_eig = Eigen::Map<const Eigen::Vector3d>(I.h.data);
62  Eigen::Vector3d hmr_eig = Eigen::Map<Eigen::Vector3d>(hmr.data);
63  Eigen::Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity();
64  Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity();
65  Eigen::Matrix3d R = Eigen::Map<Eigen::Matrix3d>(X.M.data);
67  Eigen::Map<Eigen::Matrix3d>(Ib.data) = R*((Eigen::Map<const Eigen::Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());
68 
69  return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi);
70  }
71 
73  //mb=ma
74  //hb=R*h
75  //Ib = R(Ia)R' with r=0
76  Eigen::Matrix3d R = Eigen::Map<const Eigen::Matrix3d>(M.data);
78  Eigen::Map<Eigen::Matrix3d>(Ib.data) = R.transpose()*(Eigen::Map<const Eigen::Matrix3d>(I.I.data)*R);
79 
80  return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
81  }
82 
84  //mb=ma
85  //hb=(h-m*r)
86  //Ib = (Ia+r x h x + (h-m*r) x r x)
87  Vector hmr = (this->h-this->m*p);
88  Eigen::Vector3d r_eig = Eigen::Map<const Eigen::Vector3d>(p.data);
89  Eigen::Vector3d h_eig = Eigen::Map<Eigen::Vector3d>(this->h.data);
90  Eigen::Vector3d hmr_eig = Eigen::Map<Eigen::Vector3d>(hmr.data);
91  Eigen::Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Eigen::Matrix3d::Identity();
92  Eigen::Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Eigen::Matrix3d::Identity();
94  Eigen::Map<Eigen::Matrix3d>(Ib.data) = Eigen::Map<Eigen::Matrix3d>(this->I.data)+rcrosshcross+hmrcrossrcross;
95 
96  return RigidBodyInertia(this->m,hmr,Ib,mhi);
97  }
98 }//namespace
KDL::Vector::data
double data[3]
Definition: frames.hpp:165
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
KDL::operator*
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Definition: articulatedbodyinertia.cpp:49
KDL::RigidBodyInertia::RefPoint
RigidBodyInertia RefPoint(const Vector &p)
Definition: rigidbodyinertia.cpp:83
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::mhi
const static bool mhi
Definition: rigidbodyinertia.cpp:28
KDL::RigidBodyInertia::h
Vector h
Definition: rigidbodyinertia.hpp:101
KDL::RigidBodyInertia::m
double m
Definition: rigidbodyinertia.hpp:100
KDL::RigidBodyInertia
6D Inertia of a rigid body
Definition: rigidbodyinertia.hpp:37
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
rigidbodyinertia.hpp
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::RotationalInertia
Definition: rotationalinertia.hpp:34
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::Rotation::data
double data[9]
Definition: frames.hpp:306
KDL::RigidBodyInertia::RigidBodyInertia
RigidBodyInertia(double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
Definition: rigidbodyinertia.cpp:35
KDL::RotationalInertia::data
double data[9]
Definition: rotationalinertia.hpp:65
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
KDL::RigidBodyInertia::I
RotationalInertia I
Definition: rigidbodyinertia.hpp:102
KDL::operator+
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition: articulatedbodyinertia.cpp:53
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303