Robotics Library  0.6.0
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
PlueckerTransform.hxx
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_PLUECKERTRANSFORM_HXX_
31 #define _RL_MATH_PLUECKERTRANSFORM_HXX_
32 
33 namespace rl
34 {
35  namespace math
36  {
37  namespace spatial
38  {
39  template< typename Scalar >
40  template< typename OtherScalar >
41  inline
42  ForceVector< OtherScalar >
44  {
46  res.force() = rotation() * other.force();
47  res.moment() = rotation() * (other.moment() - translation().cross(other.force()));
48  return res;
49  }
50 
51  template< typename Scalar >
52  template< typename OtherScalar >
53  inline
56  {
58  res.linear() = rotation() * (other.linear() - translation().cross(other.angular()));
59  res.angular() = rotation() * other.angular();
60  return res;
61  }
62 
63  template< typename Scalar >
64  template< typename OtherScalar >
65  inline
68  {
70  res.cog() = rotation() * (other.cog() - other.mass() * translation());
71  res.inertia() = rotation() * (other.inertia() + translation().cross(other.cog()).cross33() + (other.cog() - other.mass() * translation()).cross(translation()).cross33()) * rotation().transpose();
72  res.mass() = other.mass();
73  return res;
74  }
75 
76  template< typename Scalar >
77  template< typename OtherScalar >
78  inline
81  {
83  res.cog() = rotation() * (other.cog() - translation().cross33() * other.mass()) * rotation().transpose();
84  res.inertia() = rotation() * (other.inertia() - translation().cross33() * other.cog().transpose() + (other.cog() - translation().cross33() * other.mass()) * translation().cross33()) * rotation().transpose();
85  res.mass() = rotation() * other.mass() * rotation().transpose();
86  return res;
87  }
88 
89  template< typename Scalar >
90  template< typename OtherScalar >
91  inline
94  {
96  res.force() = rotation().transpose() * other.force();
97  res.moment() = rotation().transpose() * other.moment() + translation().cross(rotation().transpose() * other.force());
98  return res;
99  }
100 
101  template< typename Scalar >
102  template< typename OtherScalar >
103  inline
106  {
108  res.linear() = rotation().transpose() * other.linear() + translation().cross(rotation().transpose() * other.angular());
109  res.angular() = rotation().transpose() * other.angular();
110  return res;
111  }
112 
113  template< typename Scalar >
114  template< typename OtherScalar >
115  inline
118  {
120  res.cog() = rotation().transpose() * other.cog() + other.mass() * translation();
121  res.inertia() = rotation().transpose() * other.inertia() * rotation() - translation().cross(rotation().transpose() * other.cog()).cross33() - (rotation().transpose() * other.cog() + other.mass() * translation()).cross(translation()).cross33();
122  res.mass() = other.mass();
123  return res;
124  }
125 
126  template< typename Scalar >
127  template< typename OtherScalar >
128  inline
131  {
133  typename ArticulatedBodyInertia< OtherScalar >::CenterOfGravityType cog = rotation().transpose() * other.cog() * rotation();
134  typename ArticulatedBodyInertia< OtherScalar >::MassType mass = rotation().transpose() * other.mass() * rotation();
135  res.cog() = cog + translation().cross33() * mass;
136  res.inertia() = rotation().transpose() * other.inertia() * rotation() + translation().cross33() * cog.transpose() - (cog + translation().cross33() * mass) * translation().cross33();
137  res.mass() = mass;
138  return res;
139  }
140  }
141  }
142 }
143 
144 #endif // _RL_MATH_PLUECKERTRANSFORM_HXX_