FCL  0.6.0
Flexible Collision Library
screw_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_SCREWMOTION_INL_H
39 #define FCL_CCD_SCREWMOTION_INL_H
40 
41 #include "fcl/math/motion/screw_motion.h"
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class ScrewMotion<double>;
49 
50 //==============================================================================
51 template <typename S>
53  : MotionBase<S>(), 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  linear_vel = 0;
62 }
63 
64 //==============================================================================
65 template <typename S>
67  const Matrix3<S>& R1, const Vector3<S>& T1,
68  const Matrix3<S>& R2, const Vector3<S>& T2)
69  : MotionBase<S>(),
70  tf1(Transform3<S>::Identity()),
71  tf2(Transform3<S>::Identity())
72 {
73  tf1.linear() = R1;
74  tf1.translation() = T1;
75 
76  tf2.linear() = R2;
77  tf2.translation() = T2;
78 
79  tf = tf1;
80 
81  computeScrewParameter();
82 }
83 
84 //==============================================================================
85 template <typename S>
87  const Transform3<S>& tf1_, const Transform3<S>& tf2_)
88  : tf1(tf1_), tf2(tf2_), tf(tf1)
89 {
90  computeScrewParameter();
91 }
92 
93 //==============================================================================
94 template <typename S>
95 bool ScrewMotion<S>::integrate(double dt) const
96 {
97  if(dt > 1) dt = 1;
98 
99  tf.linear() = absoluteRotation(dt).toRotationMatrix();
100 
101  Quaternion<S> delta_rot = deltaRotation(dt);
102  tf.translation() = p + axis * (dt * linear_vel) + delta_rot * (tf1.translation() - p);
103 
104  return true;
105 }
106 
107 //==============================================================================
108 template <typename S>
110  const BVMotionBoundVisitor<S>& mb_visitor) const
111 {
112  return mb_visitor.visit(*this);
113 }
114 
115 //==============================================================================
116 template <typename S>
118  const TriangleMotionBoundVisitor<S>& mb_visitor) const
119 {
120  return mb_visitor.visit(*this);
121 }
122 
123 //==============================================================================
124 template <typename S>
125 void ScrewMotion<S>::getCurrentTransform(Transform3<S>& tf_) const
126 {
127  tf_ = tf;
128 }
129 
130 //==============================================================================
131 template <typename S>
133 {
134  Matrix3<S> hat_axis;
135  hat(hat_axis, axis);
136 
137  TaylorModel<S> cos_model(this->getTimeInterval());
138  generateTaylorModelForCosFunc(cos_model, angular_vel, (S)0);
139 
140  TaylorModel<S> sin_model(this->getTimeInterval());
141  generateTaylorModelForSinFunc(sin_model, angular_vel, (S)0);
142 
143  TMatrix3<S> delta_R = hat_axis * sin_model
144  - (hat_axis * hat_axis).eval() * (cos_model - 1)
145  + Matrix3<S>::Identity();
146 
147  TaylorModel<S> a(this->getTimeInterval()), b(this->getTimeInterval()), c(this->getTimeInterval());
148  generateTaylorModelForLinearFunc(a, (S)0, linear_vel * axis[0]);
149  generateTaylorModelForLinearFunc(b, (S)0, linear_vel * axis[1]);
150  generateTaylorModelForLinearFunc(c, (S)0, linear_vel * axis[2]);
151  TVector3<S> delta_T = p - delta_R * p + TVector3<S>(a, b, c);
152 
153  tm = delta_R * tf1.linear().eval();
154  tv = delta_R * tf1.translation().eval() + delta_T;
155 }
156 
157 //==============================================================================
158 template <typename S>
160 {
161  const AngleAxis<S> aa(tf2.linear() * tf1.linear().transpose());
162 
163  axis = aa.axis();
164  angular_vel = aa.angle();
165 
166  if(angular_vel < 0)
167  {
169  axis = -axis;
170  }
171 
172  if(angular_vel < 1e-10)
173  {
174  angular_vel = 0;
175  axis = tf2.translation() - tf1.translation();
176  linear_vel = axis.norm();
177  p = tf1.translation();
178  }
179  else
180  {
181  Vector3<S> o = tf2.translation() - tf1.translation();
182  p = (tf1.translation() + tf2.translation() + axis.cross(o) * (1.0 / tan(angular_vel / 2.0))) * 0.5;
183  linear_vel = o.dot(axis);
184  }
185 }
186 
187 //==============================================================================
188 template <typename S>
189 Quaternion<S> ScrewMotion<S>::deltaRotation(S dt) const
190 {
191  return Quaternion<S>(AngleAxis<S>((S)(dt * angular_vel), axis));
192 }
193 
194 //==============================================================================
195 template <typename S>
196 Quaternion<S> ScrewMotion<S>::absoluteRotation(S dt) const
197 {
198  Quaternion<S> delta_t = deltaRotation(dt);
199 
200  return delta_t * Quaternion<S>(tf1.linear());
201 }
202 
203 //==============================================================================
204 template <typename S>
206 {
207  return linear_vel;
208 }
209 
210 //==============================================================================
211 template <typename S>
213 {
214  return angular_vel;
215 }
216 
217 //==============================================================================
218 template <typename S>
219 const Vector3<S>&ScrewMotion<S>::getAxis() const
220 {
221  return axis;
222 }
223 
224 //==============================================================================
225 template <typename S>
226 const Vector3<S>&ScrewMotion<S>::getAxisOrigin() const
227 {
228  return p;
229 }
230 
231 } // namespace fcl
232 
233 #endif
S angular_vel
angular velocity
Definition: screw_motion.h:107
Definition: motion_base.h:52
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
Vector3< S > p
A point on the axis.
Definition: screw_motion.h:101
Definition: bv_motion_bound_visitor.h:51
void getCurrentTransform(Transform3< S > &tf_) const
Get the rotation and translation in current step.
Definition: screw_motion-inl.h:125
ScrewMotion()
Default transformations are all identities.
Definition: screw_motion-inl.h:52
S linear_vel
linear velocity along the axis
Definition: screw_motion.h:104
Vector3< S > axis
screw axis
Definition: screw_motion.h:98
Transform3< S > tf
The transformation at current time t.
Definition: screw_motion.h:95
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: screw_motion-inl.h:109
Transform3< S > tf2
The transformation at time 1.
Definition: screw_motion.h:92
bool integrate(double dt) const
Integrate the motion from 0 to dt We compute the current transformation from zero point instead of fr...
Definition: screw_motion-inl.h:95
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
Definition: taylor_matrix.h:48
Transform3< S > tf1
The transformation at time 0.
Definition: screw_motion.h:89
Definition: taylor_vector.h:48
TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function over a t...
Definition: taylor_model.h:58