Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
ForceVector.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_FORCEVECTOR_H_
31 #define _RL_MATH_FORCEVECTOR_H_
32 
33 #include <Eigen/Core>
34 
35 namespace rl
36 {
37  namespace math
38  {
39  namespace spatial
40  {
41  template< typename Scalar > class ArticulatedBodyInertia;
42  template< typename Scalar > class MotionVector;
43 
44  template< typename Scalar >
45  class ForceVector
46  {
47  public:
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
50  typedef Scalar ScalarType;
51 
52  typedef typename ::Eigen::Matrix< Scalar, 6, 1 > MatrixType;
53 
54  typedef const MatrixType ConstMatrixType;
55 
56  typedef ::Eigen::Block< MatrixType, 3, 1 > MomentType;
57 
58  typedef const ::Eigen::Block< ConstMatrixType, 3, 1 > ConstMomentType;
59 
60  typedef ::Eigen::Block< MatrixType, 3, 1 > ForceType;
61 
62  typedef const ::Eigen::Block< ConstMatrixType, 3, 1 > ConstForceType;
63 
65  {
66  }
67 
68  template< typename OtherDerived >
69  ForceVector(const ::Eigen::MatrixBase< OtherDerived >& other) :
70  data(other)
71  {
72  }
73 
74  virtual ~ForceVector()
75  {
76  }
77 
78  template< typename OtherScalar >
79  Scalar dot(const MotionVector< OtherScalar >& other) const;
80 
82  {
83  return data.template segment< 3 >(3);
84  }
85 
87  {
88  return data.template segment< 3 >(3);
89  }
90 
92  {
93  return data;
94  }
95 
97  {
98  return data.template segment< 3 >(0);
99  }
100 
102  {
103  return data.template segment< 3 >(0);
104  }
105 
106  template< typename OtherDerived >
107  ForceVector& operator=(const ::Eigen::MatrixBase< OtherDerived >& other)
108  {
109  data = other;
110  return *this;
111  }
112 
113  ForceVector operator+(const ForceVector& other) const
114  {
115  ForceVector res;
116  res.moment() = moment() + other.moment();
117  res.force() = force() + other.force();
118  return res;
119  }
120 
121  ForceVector operator-(const ForceVector& other) const
122  {
123  ForceVector res;
124  res.moment() = moment() - other.moment();
125  res.force() = force() - other.force();
126  return res;
127  }
128 
129  template< typename OtherScalar >
130  ForceVector operator*(const OtherScalar& other) const
131  {
132  ForceVector res;
133  res.moment() = moment() * other;
134  res.force() = force() * other;
135  return res;
136  }
137 
138  template< typename OtherScalar >
139  ForceVector operator/(const OtherScalar& other) const
140  {
141  ForceVector res;
142  res.moment() = moment() / other;
143  res.force() = force() / other;
144  return res;
145  }
146 
148 
149  void setZero()
150  {
151  moment().setZero();
152  force().setZero();
153  }
154 
155  protected:
156 
157  private:
159  };
160  }
161  }
162 }
163 
164 #endif // _RL_MATH_FORCEVECTOR_H_