interp_motion-inl.h
Go to the documentation of this file.
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 
42 
43 namespace fcl
44 {
45 
46 //==============================================================================
47 extern template
48 class FCL_EXPORT 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
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
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>
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)
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 {
193  linear_vel = tf2 * reference_p - tf1 * reference_p;
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  {
201  angular_vel = -angular_vel;
202  angular_axis = -angular_axis;
203  }
204 }
205 
206 //==============================================================================
207 template <typename S>
209 {
210  return Quaternion<S>(AngleAxis<S>((S)(dt * angular_vel), angular_axis));
211 }
212 
213 //==============================================================================
214 template <typename S>
216 {
217  Quaternion<S> delta_t = deltaRotation(dt);
218  return delta_t * Quaternion<S>(tf1.linear());
219 }
220 
221 //==============================================================================
222 template <typename S>
224 {
225  return reference_p;
226 }
227 
228 //==============================================================================
229 template <typename S>
231 {
232  return angular_axis;
233 }
234 
235 //==============================================================================
236 template <typename S>
238 {
239  return angular_vel;
240 }
241 
242 //==============================================================================
243 template <typename S>
245 {
246  return linear_vel;
247 }
248 
249 } // namespace fcl
250 
251 #endif
fcl::InterpMotion::angular_vel
S angular_vel
Angular speed.
Definition: interp_motion.h:113
fcl::TriangleMotionBoundVisitor
Definition: motion_base.h:52
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::generateTaylorModelForLinearFunc
template void generateTaylorModelForLinearFunc(TaylorModel< double > &tm, double p, double v)
interp_motion.h
fcl::InterpMotion::tf2
Transform3< S > tf2
The transformation at time 1.
Definition: interp_motion.h:104
fcl::Quaternion
Eigen::Quaternion< S > Quaternion
Definition: types.h:88
fcl::InterpMotion::computeVelocity
void computeVelocity()
Definition: interp_motion-inl.h:191
fcl::InterpMotion::tf1
Transform3< S > tf1
The transformation at time 0.
Definition: interp_motion.h:101
fcl::InterpMotion::getReferencePoint
const Vector3< S > & getReferencePoint() const
Definition: interp_motion-inl.h:223
fcl::generateTaylorModelForSinFunc
template void generateTaylorModelForSinFunc(TaylorModel< double > &tm, double w, double q0)
fcl::InterpMotion::computeMotionBound
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
fcl::AngleAxis
Eigen::AngleAxis< S > AngleAxis
Definition: types.h:97
fcl::BVMotionBoundVisitor::visit
virtual S visit(const MotionBase< S > &motion) const =0
fcl::Vector3
Eigen::Matrix< S, 3, 1 > Vector3
Definition: types.h:70
fcl::InterpMotion::getTaylorModel
void getTaylorModel(TMatrix3< S > &tm, TVector3< S > &tv) const
Definition: interp_motion-inl.h:163
fcl::InterpMotion::getAngularVelocity
S getAngularVelocity() const
Definition: interp_motion-inl.h:237
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::MotionBase
Definition: bv_motion_bound_visitor.h:45
fcl::InterpMotion::getAngularAxis
const Vector3< S > & getAngularAxis() const
Definition: interp_motion-inl.h:230
fcl::InterpMotion::deltaRotation
Quaternion< S > deltaRotation(S dt) const
Definition: interp_motion-inl.h:208
fcl::TriangleMotionBoundVisitor::visit
virtual S visit(const MotionBase< S > &motion) const
Definition: triangle_motion_bound_visitor.h:81
fcl::generateTaylorModelForCosFunc
template void generateTaylorModelForCosFunc(TaylorModel< double > &tm, double w, double q0)
fcl::InterpMotion::absoluteRotation
Quaternion< S > absoluteRotation(S dt) const
Definition: interp_motion-inl.h:215
fcl::TaylorModel
TaylorModel implements a third order Taylor model, i.e., a cubic approximation of a function over a t...
Definition: taylor_model.h:58
fcl::InterpMotion::tf
Transform3< S > tf
The transformation at current time t.
Definition: interp_motion.h:107
fcl::BVMotionBoundVisitor
Compute the motion bound for a bounding volume, given the closest direction n between two query objec...
Definition: bv_motion_bound_visitor.h:62
fcl::TMatrix3
Definition: taylor_matrix.h:48
fcl::TVector3
Definition: taylor_vector.h:48
fcl::InterpMotion::getCurrentTransform
void getCurrentTransform(Transform3< S > &tf_) const
Get the rotation and translation in current step.
Definition: interp_motion-inl.h:156
fcl::InterpMotion::getLinearVelocity
const Vector3< S > & getLinearVelocity() const
Definition: interp_motion-inl.h:244
fcl::hat
template void hat(Matrix3d &mat, const Vector3d &vec)
fcl::InterpMotion::integrate
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
fcl::InterpMotion< double >
template class FCL_EXPORT InterpMotion< double >
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::InterpMotion::InterpMotion
InterpMotion()
Default transformations are all identities.
Definition: interp_motion-inl.h:52


fcl
Author(s):
autogenerated on Fri Jun 11 2021 02:38:03