FCL  0.6.0
Flexible Collision Library
interp_motion-inl.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2011-2014, Willow Garage, Inc.
5  * Copyright (c) 2014-2016, Open Source Robotics Foundation
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of Open Source Robotics Foundation nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  */
35 
38 #ifndef FCL_CCD_INTERPMOTION_INL_H
39 #define FCL_CCD_INTERPMOTION_INL_H
40 
41 #include "fcl/math/motion/interp_motion.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class InterpMotion<double>;
49 
50 //==============================================================================
51 template <typename S>
53  : MotionBase<S>(), angular_axis(Vector3<S>::UnitX())
54 {
55  // Default angular velocity is zero
56  angular_vel = 0;
57 
58  // Default reference point is local zero point
59 
60  // Default linear velocity is zero
61 }
62 
63 //==============================================================================
64 template <typename S>
66  const Matrix3<S>& R1, const Vector3<S>& T1,
67  const Matrix3<S>& R2, const Vector3<S>& T2)
68  : MotionBase<S>(),
69  tf1(Transform3<S>::Identity()),
70  tf2(Transform3<S>::Identity())
71 {
72  tf1.linear() = R1;
73  tf1.translation() = T1;
74 
75  tf2.linear() = R2;
76  tf2.translation() = T2;
77 
78  tf = tf1;
79 
80  // Compute the velocities for the motion
81  computeVelocity();
82 }
83 
84 //==============================================================================
85 template <typename S>
87  const Transform3<S>& tf1_, const Transform3<S>& tf2_)
88  : MotionBase<S>(), tf1(tf1_), tf2(tf2_), tf(tf1)
89 {
90  // Compute the velocities for the motion
91  computeVelocity();
92 }
93 
94 //==============================================================================
95 template <typename S>
97  const Matrix3<S>& R1,
98  const Vector3<S>& T1,
99  const Matrix3<S>& R2,
100  const Vector3<S>& T2,
101  const Vector3<S>& O)
102  : MotionBase<S>(),
103  tf1(Transform3<S>::Identity()),
104  tf2(Transform3<S>::Identity()),
105  reference_p(O)
106 {
107  tf1.linear() = R1;
108  tf1.translation() = T1;
109 
110  tf2.linear() = R2;
111  tf2.translation() = T2;
112 
113  tf = tf1;
114 
115  // Compute the velocities for the motion
116  computeVelocity();
117 }
118 
119 //==============================================================================
120 template <typename S>
122  const Transform3<S>& tf1_, const Transform3<S>& tf2_, const Vector3<S>& O)
123  : MotionBase<S>(), tf1(tf1_), tf2(tf2_), tf(tf1), reference_p(O)
124 {
125  // Do nothing
126 }
127 
128 //==============================================================================
129 template <typename S>
130 bool InterpMotion<S>::integrate(double dt) const
131 {
132  if(dt > 1) dt = 1;
133 
134  tf.linear() = absoluteRotation(dt).toRotationMatrix();
135  tf.translation() = linear_vel * dt + tf1 * reference_p - tf.linear() * reference_p;
136 
137  return true;
138 }
139 
140 //==============================================================================
141 template <typename S>
143 {
144  return mb_visitor.visit(*this);
145 }
146 
147 //==============================================================================
148 template <typename S>
150 {
151  return mb_visitor.visit(*this);
152 }
153 
154 //==============================================================================
155 template <typename S>
156 void InterpMotion<S>::getCurrentTransform(Transform3<S>& tf_) const
157 {
158  tf_ = tf;
159 }
160 
161 //==============================================================================
162 template <typename S>
164 {
165  Matrix3<S> hat_angular_axis;
166  hat(hat_angular_axis, angular_axis);
167 
168  TaylorModel<S> cos_model(this->getTimeInterval());
169  generateTaylorModelForCosFunc(cos_model, angular_vel, (S)0);
170  TaylorModel<S> sin_model(this->getTimeInterval());
171  generateTaylorModelForSinFunc(sin_model, angular_vel, (S)0);
172 
173  TMatrix3<S> delta_R = hat_angular_axis * sin_model
174  - (hat_angular_axis * hat_angular_axis).eval() * (cos_model - 1)
175  + Matrix3<S>::Identity();
176 
177  TaylorModel<S> a(this->getTimeInterval()), b(this->getTimeInterval()), c(this->getTimeInterval());
178  generateTaylorModelForLinearFunc(a, (S)0, linear_vel[0]);
179  generateTaylorModelForLinearFunc(b, (S)0, linear_vel[1]);
180  generateTaylorModelForLinearFunc(c, (S)0, linear_vel[2]);
181  TVector3<S> delta_T(a, b, c);
182 
183  tm = delta_R * tf1.linear().eval();
184  tv = tf1 * reference_p
185  + delta_T
186  - delta_R * (tf1.linear() * reference_p).eval();
187 }
188 
189 //==============================================================================
190 template <typename S>
192 {
194 
195  const AngleAxis<S> aa(tf2.linear() * tf1.linear().transpose());
196  angular_axis = aa.axis();
197  angular_vel = aa.angle();
198 
199  if(angular_vel < 0)
200  {
202  angular_axis = -angular_axis;
203  }
204 }
205 
206 //==============================================================================
207 template <typename S>
208 Quaternion<S> InterpMotion<S>::deltaRotation(S dt) const
209 {
210  return Quaternion<S>(AngleAxis<S>((S)(dt * angular_vel), angular_axis));
211 }
212 
213 //==============================================================================
214 template <typename S>
215 Quaternion<S> InterpMotion<S>::absoluteRotation(S dt) const
216 {
217  Quaternion<S> delta_t = deltaRotation(dt);
218  return delta_t * Quaternion<S>(tf1.linear());
219 }
220 
221 //==============================================================================
222 template <typename S>
223 const Vector3<S>&InterpMotion<S>::getReferencePoint() const
224 {
225  return reference_p;
226 }
227 
228 //==============================================================================
229 template <typename S>
230 const Vector3<S>&InterpMotion<S>::getAngularAxis() const
231 {
232  return angular_axis;
233 }
234 
235 //==============================================================================
236 template <typename S>
238 {
239  return angular_vel;
240 }
241 
242 //==============================================================================
243 template <typename S>
244 const Vector3<S>&InterpMotion<S>::getLinearVelocity() const
245 {
246  return linear_vel;
247 }
248 
249 } // namespace fcl
250 
251 #endif
Definition: motion_base.h:52
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
void getCurrentTransform(Transform3< S > &tf_) const
Get the rotation and translation in current step.
Definition: interp_motion-inl.h:156
Linear interpolation motion Each Motion is assumed to have constant linear velocity and angular veloc...
Definition: bv_motion_bound_visitor.h:54
InterpMotion()
Default transformations are all identities.
Definition: interp_motion-inl.h:52
Transform3< S > tf
The transformation at current time t.
Definition: interp_motion.h:107
Transform3< S > tf1
The transformation at time 0.
Definition: interp_motion.h:101
Definition: bv_motion_bound_visitor.h:45
Compute the motion bound for a bounding volume, given the closest direction n between two query objec...
Definition: bv_motion_bound_visitor.h:62
Vector3< S > linear_vel
Linear velocity.
Definition: interp_motion.h:110
S computeMotionBound(const BVMotionBoundVisitor< S > &mb_visitor) const
Compute the motion bound for a bounding volume along a given direction n, which is defined in the vis...
Definition: interp_motion-inl.h:142
S angular_vel
Angular speed.
Definition: interp_motion.h:113
Definition: taylor_matrix.h:48
bool integrate(double dt) const
Integrate the motion from 0 to dt We compute the current transformation from zero point instead of fr...
Definition: interp_motion-inl.h:130
Definition: taylor_vector.h:48
Vector3< S > angular_axis
Angular velocity axis.
Definition: interp_motion.h:116
Vector3< S > reference_p
Reference point for the motion (in the object&#39;s local frame)
Definition: interp_motion.h:119
Transform3< S > tf2
The transformation at time 1.
Definition: interp_motion.h:104
TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function over a t...
Definition: taylor_model.h:58