Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
ArticulatedBodyInertia.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_ARTICULATEDBODYINERTIA_H_
31 #define _RL_MATH_ARTICULATEDBODYINERTIA_H_
32 
33 #include <Eigen/Core>
34 
35 namespace rl
36 {
37  namespace math
38  {
39  namespace spatial
40  {
41  template< typename Scalar > class ForceVector;
42  template< typename Scalar > class MotionVector;
43  template< typename Scalar > class RigidBodyInertia;
44 
45  template< typename Scalar >
47  {
48  public:
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 
51  typedef Scalar ScalarType;
52 
53  typedef ::Eigen::Matrix< Scalar, 6, 6 > MatrixType;
54 
55  typedef ::Eigen::Matrix< Scalar, 3, 3 > CenterOfGravityType;
56 
58 
59  typedef ::Eigen::Matrix< Scalar, 3, 3 > InertiaType;
60 
62 
63  typedef ::Eigen::Matrix< Scalar, 3, 3 > MassType;
64 
65  typedef const MassType ConstMassType;
66 
68  {
69  }
70 
71  template< typename OtherDerived >
72  ArticulatedBodyInertia(const ::Eigen::DenseBase< OtherDerived >& other) :
73  centerOfGravityData(other.template topRightCorner< 3, 3 >()),
74  inertiaData(other.template topLeftCorner< 3, 3 >()),
75  massData(other.template bottomRightCorner< 3, 3 >())
76  {
77  }
78 
80  {
81  }
82 
84  {
85  return centerOfGravityData;
86  }
87 
89  {
90  return centerOfGravityData;
91  }
92 
94  {
95  return inertiaData;
96  }
97 
99  {
100  return inertiaData;
101  }
102 
104  {
105  return massData;
106  }
107 
109  {
110  return massData;
111  }
112 
114  {
115  MatrixType res;
116  res.template topLeftCorner< 3, 3 >() = inertia();
117  res.template topRightCorner< 3, 3 >() = cog();
118  res.template bottomLeftCorner< 3, 3 >() = cog().transpose();
119  res.template bottomRightCorner< 3, 3 >() = mass();
120  return res;
121  }
122 
123  template< typename OtherDerived >
124  ArticulatedBodyInertia& operator=(const ::Eigen::MatrixBase< OtherDerived >& other)
125  {
126  cog() = other.template topRightCorner< 3, 3 >();
127  inertia() = other.template topLeftCorner< 3, 3 >();
128  mass() = other.template bottomRightCorner< 3, 3 >();
129  return *this;
130  }
131 
132  template< typename OtherScalar >
134 
136  {
138  res.cog() = cog() + other.cog();
139  res.inertia() = inertia() + other.inertia();
140  res.mass() = mass() + other.mass();
141  return res;
142  }
143 
145  {
147  res.cog() = cog() - other.cog();
148  res.inertia() = inertia() - other.inertia();
149  res.mass() = mass() - other.mass();
150  return res;
151  }
152 
153  template< typename OtherScalar >
154  ArticulatedBodyInertia operator*(const OtherScalar& other) const
155  {
157  res.cog() = cog() * other;
158  res.inertia() = inertia() * other;
159  res.mass() = mass() * other;
160  return res;
161  }
162 
163  template< typename OtherScalar >
165 
166  template< typename OtherScalar >
167  ArticulatedBodyInertia operator/(const OtherScalar& other) const
168  {
170  res.cog() = cog() / other;
171  res.inertia() = inertia() / other;
172  res.mass() = mass() / other;
173  return res;
174  }
175 
176  void setIdentity()
177  {
178  cog().setZero();
179  inertia().setIdentity();
180  mass().setIdentity();
181  }
182 
183  void setZero()
184  {
185  cog().setZero();
186  inertia().setZero();
187  mass().setZero();
188  }
189 
190  protected:
191 
192  private:
194 
196 
198  };
199  }
200  }
201 }
202 
203 #endif // _RL_MATH_ARTICULATEDBODYINERTIA_H_