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 using namespace Eigen;
27 
28 namespace KDL{
29 
30  const static bool mhi=true;
31 
32  RigidBodyInertia::RigidBodyInertia(double m_,const Vector& h_,const RotationalInertia& I_,bool mhi):
33  m(m_),h(h_),I(I_)
34  {
35  }
36 
38  m(m_),h(m*c_){
39  //I=Ic-c x c x
40  Vector3d c_eig=Map<const Vector3d>(c_.data);
41  Map<Matrix3d>(I.data)=Map<const Matrix3d>(Ic.data)-m_*(c_eig*c_eig.transpose()-c_eig.dot(c_eig)*Matrix3d::Identity());
42  }
43 
45  return RigidBodyInertia(a*I.m,a*I.h,a*I.I,mhi);
46  }
47 
49  return RigidBodyInertia(Ia.m+Ib.m,Ia.h+Ib.h,Ia.I+Ib.I,mhi);
50  }
51 
53  return Wrench(I.m*t.vel-I.h*t.rot,I.I*t.rot+I.h*t.vel);
54  }
55 
57  Frame X=T.Inverse();
58  //mb=ma
59  //hb=R*(h-m*r)
60  //Ib = R(Ia+r x h x + (h-m*r) x r x)R'
61  Vector hmr = (I.h-I.m*X.p);
62  Vector3d r_eig = Map<Vector3d>(X.p.data);
63  Vector3d h_eig = Map<const Vector3d>(I.h.data);
64  Vector3d hmr_eig = Map<Vector3d>(hmr.data);
65  Matrix3d rcrosshcross = h_eig *r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
66  Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
67  Matrix3d R = Map<Matrix3d>(X.M.data);
69  Map<Matrix3d>(Ib.data) = R*((Map<const Matrix3d>(I.I.data)+rcrosshcross+hmrcrossrcross)*R.transpose());
70 
71  return RigidBodyInertia(I.m,T.M*hmr,Ib,mhi);
72  }
73 
75  //mb=ma
76  //hb=R*h
77  //Ib = R(Ia)R' with r=0
78  Matrix3d R = Map<const Matrix3d>(M.data);
80  Map<Matrix3d>(Ib.data) = R.transpose()*(Map<const Matrix3d>(I.I.data)*R);
81 
82  return RigidBodyInertia(I.m,M*I.h,Ib,mhi);
83  }
84 
86  //mb=ma
87  //hb=(h-m*r)
88  //Ib = (Ia+r x h x + (h-m*r) x r x)
89  Vector hmr = (this->h-this->m*p);
90  Vector3d r_eig = Map<const Vector3d>(p.data);
91  Vector3d h_eig = Map<Vector3d>(this->h.data);
92  Vector3d hmr_eig = Map<Vector3d>(hmr.data);
93  Matrix3d rcrosshcross = h_eig * r_eig.transpose()-r_eig.dot(h_eig)*Matrix3d::Identity();
94  Matrix3d hmrcrossrcross = r_eig*hmr_eig.transpose()-hmr_eig.dot(r_eig)*Matrix3d::Identity();
96  Map<Matrix3d>(Ib.data) = Map<Matrix3d>(this->I.data)+rcrosshcross+hmrcrossrcross;
97 
98  return RigidBodyInertia(this->m,hmr,Ib,mhi);
99  }
100 }//namespace
represents rotations in 3 dimensional space.
Definition: frames.hpp:301
Vector vel
The velocity of that point.
Definition: frames.hpp:722
friend double dot(const Vector &lhs, const Vector &rhs)
Definition: frames.hpp:1013
6D Inertia of a rigid body
RigidBodyInertia RefPoint(const Vector &p)
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
represents both translational and rotational velocities.
Definition: frames.hpp:720
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.hpp:423
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:160
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
double data[3]
Definition: frames.hpp:163
static const bool mhi
RigidBodyInertia(double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
Vector p
origine of the Frame
Definition: frames.hpp:572
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:570
represents both translational and rotational acceleration.
Definition: frames.hpp:878
friend RigidBodyInertia operator+(const RigidBodyInertia &Ia, const RigidBodyInertia &Ib)
friend RigidBodyInertia operator*(double a, const RigidBodyInertia &I)
double data[9]
Definition: frames.hpp:304


orocos_kdl
Author(s):
autogenerated on Tue Sep 22 2020 03:17:50