Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
RigidBodyInertia.h
Go to the documentation of this file.
1 //
2 // Copyright (c) 2009, Markus Rickert
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of the Technische Universitaet Muenchen nor the names of
14 // its contributors may be used to endorse or promote products derived from
15 // this software without specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 
30 #ifndef _RL_MATH_RIGIDBODYINERTIA_H_
31 #define _RL_MATH_RIGIDBODYINERTIA_H_
32 
33 #include <Eigen/Core>
34 
35 namespace rl
36 {
37  namespace math
38  {
39  namespace spatial
40  {
41  template< typename Scalar > class MotionVector;
42 
43  template< typename Scalar >
44  class RigidBodyInertia
45  {
46  public:
47  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 
49  typedef Scalar ScalarType;
50 
51  typedef ::Eigen::Matrix< Scalar, 6, 6 > MatrixType;
52 
53  typedef ::Eigen::Matrix< Scalar, 3, 1 > CenterOfGravityType;
54 
56 
57  typedef ::Eigen::Matrix< Scalar, 3, 3 > InertiaType;
58 
60 
61  typedef Scalar MassType;
62 
63  typedef const MassType ConstMassType;
64 
66  {
67  }
68 
69  template< typename OtherDerived >
70  RigidBodyInertia(const ::Eigen::DenseBase< OtherDerived >& other) :
71  centerOfGravityData(other.template topRightCorner< 3, 3 >().cross3()),
72  inertiaData(other.template topLeftCorner< 3, 3 >()),
73  massData(other.template bottomRightCorner< 3, 3 >().diagonal().mean())
74  {
75  }
76 
78  {
79  }
80 
82  {
83  return centerOfGravityData;
84  }
85 
87  {
88  return centerOfGravityData;
89  }
90 
92  {
93  return inertiaData;
94  }
95 
97  {
98  return inertiaData;
99  }
100 
102  {
103  return massData;
104  }
105 
107  {
108  return massData;
109  }
110 
112  {
113  MatrixType res;
114  res.template topLeftCorner< 3, 3 >() = inertia();
115  res.template topRightCorner< 3, 3 >() = cog().cross33();
116  res.template bottomLeftCorner< 3, 3 >() = -cog().cross33();
117  res.template bottomRightCorner< 3, 3 >() = ::Eigen::Matrix< Scalar, 3, 3 >::Identity() * mass();
118  return res;
119  }
120 
121  template< typename OtherDerived >
122  RigidBodyInertia& operator=(const ::Eigen::MatrixBase< OtherDerived >& other)
123  {
124  cog() = other.template topRightCorner< 3, 3 >().cross3();
125  inertia() = other.template topLeftCorner< 3, 3 >();
126  mass() = other.template bottomRightCorner< 3, 3 >().diagonal().mean();
127  return *this;
128  }
129 
131  {
132  RigidBodyInertia res;
133  res.cog() = cog() + other.cog();
134  res.inertia() = inertia() + other.inertia();
135  res.mass() = mass() + other.mass();
136  return res;
137  }
138 
140  {
141  RigidBodyInertia res;
142  res.cog() = cog() - other.cog();
143  res.inertia() = inertia() - other.inertia();
144  res.mass() = mass() - other.mass();
145  return res;
146  }
147 
148  template< typename OtherScalar >
149  RigidBodyInertia operator*(const OtherScalar& other) const
150  {
151  RigidBodyInertia res;
152  res.cog() = cog() * other;
153  res.inertia() = inertia() * other;
154  res.mass() = mass() * other;
155  return res;
156  }
157 
158  template< typename OtherScalar >
160 
161  template< typename OtherScalar >
162  RigidBodyInertia operator/(const OtherScalar& other) const
163  {
164  RigidBodyInertia res;
165  res.cog() = cog() / other;
166  res.inertia() = inertia() / other;
167  res.mass() = mass() / other;
168  return res;
169  }
170 
171  void setIdentity()
172  {
173  cog().setZero();
174  inertia().setIdentity();
175  mass() = 1;
176  }
177 
178  void setZero()
179  {
180  cog().setZero();
181  inertia().setZero();
182  mass() = 0;
183  }
184 
185  protected:
186 
187  private:
189 
191 
193  };
194  }
195  }
196 }
197 
198 #endif // _RL_MATH_RIGIDBODYINERTIA_H_