spline_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_SPLINEMOTION_INL_H
39 #define FCL_CCD_SPLINEMOTION_INL_H
40 
42 
43 #include "fcl/common/unused.h"
44 
45 namespace fcl
46 {
47 
48 //==============================================================================
49 extern template
50 class FCL_EXPORT SplineMotion<double>;
51 
52 //==============================================================================
53 template <typename S>
55  const Vector3<S>& Td0, const Vector3<S>& Td1, const Vector3<S>& Td2, const Vector3<S>& Td3,
56  const Vector3<S>& Rd0, const Vector3<S>& Rd1, const Vector3<S>& Rd2, const Vector3<S>& Rd3)
57  : MotionBase<S>()
58 {
59  Td[0] = Td0;
60  Td[1] = Td1;
61  Td[2] = Td2;
62  Td[3] = Td3;
63 
64  Rd[0] = Rd0;
65  Rd[1] = Rd1;
66  Rd[2] = Rd2;
67  Rd[3] = Rd3;
68 
69  Rd0Rd0 = Rd[0].dot(Rd[0]);
70  Rd0Rd1 = Rd[0].dot(Rd[1]);
71  Rd0Rd2 = Rd[0].dot(Rd[2]);
72  Rd0Rd3 = Rd[0].dot(Rd[3]);
73  Rd1Rd1 = Rd[1].dot(Rd[1]);
74  Rd1Rd2 = Rd[1].dot(Rd[2]);
75  Rd1Rd3 = Rd[1].dot(Rd[3]);
76  Rd2Rd2 = Rd[2].dot(Rd[2]);
77  Rd2Rd3 = Rd[2].dot(Rd[3]);
78  Rd3Rd3 = Rd[3].dot(Rd[3]);
79 
80  TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0];
81  TB = (Td[0] - Td[1] * 2 + Td[2]) * 3;
82  TC = (Td[2] - Td[0]) * 3;
83 
84  RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0];
85  RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3;
86  RC = (Rd[2] - Rd[0]) * 3;
87 
88  tf.setIdentity();
89  integrate(0.0);
90 }
91 
92 //==============================================================================
93 template <typename S>
95  const Matrix3<S>& R1, const Vector3<S>& T1,
96  const Matrix3<S>& R2, const Vector3<S>& T2)
97  : MotionBase<S>()
98 {
99  FCL_UNUSED(R1);
100  FCL_UNUSED(T1);
101  FCL_UNUSED(R2);
102  FCL_UNUSED(T2);
103 
104  // TODO
105 }
106 
107 //==============================================================================
108 template <typename S>
110  const Transform3<S>& tf1, const Transform3<S>& tf2)
111  : MotionBase<S>()
112 {
113  FCL_UNUSED(tf1);
114  FCL_UNUSED(tf2);
115 
116  // TODO
117 }
118 
119 //==============================================================================
120 template <typename S>
122 {
123  if(dt > 1) dt = 1;
124 
125  Vector3<S> cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
126  Vector3<S> cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
127  S cur_angle = cur_w.norm();
128  if (cur_angle > 0.0) {
129  cur_w /= cur_angle;
130  }
131 
132  tf.linear() = AngleAxis<S>(cur_angle, cur_w).toRotationMatrix();
133  tf.translation() = cur_T;
134 
135  tf_t = dt;
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  // set tv
166  Vector3<S> c[4];
167  c[0] = (Td[0] + Td[1] * 4 + Td[2] + Td[3]) * (1/6.0);
168  c[1] = (-Td[0] + Td[2]) * (1/2.0);
169  c[2] = (Td[0] - Td[1] * 2 + Td[2]) * (1/2.0);
170  c[3] = (-Td[0] + Td[1] * 3 - Td[2] * 3 + Td[3]) * (1/6.0);
171  tv.setTimeInterval(this->getTimeInterval());
172  for(std::size_t i = 0; i < 3; ++i)
173  {
174  for(std::size_t j = 0; j < 4; ++j)
175  {
176  tv[i].coeff(j) = c[j][i];
177  }
178  }
179 
180  // set tm
182  // R(t) = R(t0) + R'(t0) (t-t0) + 1/2 R''(t0)(t-t0)^2 + 1 / 6 R'''(t0) (t-t0)^3 + 1 / 24 R''''(l)(t-t0)^4; t0 = 0.5
184  Vector3<S> Rt0 = (Rd[0] + Rd[1] * 23 + Rd[2] * 23 + Rd[3]) * (1 / 48.0);
185  S Rt0_len = Rt0.norm();
186  S inv_Rt0_len = 1.0 / Rt0_len;
187  S inv_Rt0_len_3 = inv_Rt0_len * inv_Rt0_len * inv_Rt0_len;
188  S inv_Rt0_len_5 = inv_Rt0_len_3 * inv_Rt0_len * inv_Rt0_len;
189  S theta0 = Rt0_len;
190  S costheta0 = cos(theta0);
191  S sintheta0 = sin(theta0);
192 
193  Vector3<S> Wt0 = Rt0 * inv_Rt0_len;
194  Matrix3<S> hatWt0;
195  hat(hatWt0, Wt0);
196  Matrix3<S> hatWt0_sqr = hatWt0 * hatWt0;
197  Matrix3<S> Mt0 = I + hatWt0 * sintheta0 + hatWt0_sqr * (1 - costheta0);
198  // TODO(JS): this could be improved by using exp(Wt0)
199 
201  Vector3<S> dRt0 = (-Rd[0] - Rd[1] * 5 + Rd[2] * 5 + Rd[3]) * (1 / 8.0);
202  S Rt0_dot_dRt0 = Rt0.dot(dRt0);
203  S dtheta0 = Rt0_dot_dRt0 * inv_Rt0_len;
204  Vector3<S> dWt0 = dRt0 * inv_Rt0_len - Rt0 * (Rt0_dot_dRt0 * inv_Rt0_len_3);
205  Matrix3<S> hatdWt0;
206  hat(hatdWt0, dWt0);
207  Matrix3<S> dMt0 = hatdWt0 * sintheta0 + hatWt0 * (costheta0 * dtheta0) + hatWt0_sqr * (sintheta0 * dtheta0) + (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (1 - costheta0);
208 
210  Vector3<S> ddRt0 = (Rd[0] - Rd[1] - Rd[2] + Rd[3]) * 0.5;
211  S Rt0_dot_ddRt0 = Rt0.dot(ddRt0);
212  S dRt0_dot_dRt0 = dRt0.squaredNorm();
213  S ddtheta0 = (Rt0_dot_ddRt0 + dRt0_dot_dRt0) * inv_Rt0_len - Rt0_dot_dRt0 * Rt0_dot_dRt0 * inv_Rt0_len_3;
214  Vector3<S> ddWt0 = ddRt0 * inv_Rt0_len - (dRt0 * (2 * Rt0_dot_dRt0) + Rt0 * (Rt0_dot_ddRt0 + dRt0_dot_dRt0)) * inv_Rt0_len_3 + (Rt0 * (3 * Rt0_dot_dRt0 * Rt0_dot_dRt0)) * inv_Rt0_len_5;
215  Matrix3<S> hatddWt0;
216  hat(hatddWt0, ddWt0);
217  Matrix3<S> ddMt0 =
218  hatddWt0 * sintheta0 +
219  hatWt0 * (costheta0 * dtheta0 - sintheta0 * dtheta0 * dtheta0 + costheta0 * ddtheta0) +
220  hatdWt0 * (costheta0 * dtheta0) +
221  (hatWt0 * hatdWt0 + hatdWt0 * hatWt0) * (sintheta0 * dtheta0 * 2) +
222  hatdWt0 * hatdWt0 * (2 * (1 - costheta0)) +
223  hatWt0 * hatWt0 * (costheta0 * dtheta0 * dtheta0 + sintheta0 * ddtheta0) +
224  (hatWt0 * hatddWt0 + hatddWt0 + hatWt0) * (1 - costheta0);
225 
226  tm.setTimeInterval(this->getTimeInterval());
227  for(std::size_t i = 0; i < 3; ++i)
228  {
229  for(std::size_t j = 0; j < 3; ++j)
230  {
231  tm(i, j).coeff(0) = Mt0(i, j) - dMt0(i, j) * 0.5 + ddMt0(i, j) * 0.25 * 0.5;
232  tm(i, j).coeff(1) = dMt0(i, j) - ddMt0(i, j) * 0.5;
233  tm(i, j).coeff(2) = ddMt0(i, j) * 0.5;
234  tm(i, j).coeff(3) = 0;
235 
236  tm(i, j).remainder() = Interval<S>(-1/48.0, 1/48.0);
237  }
238  }
239 }
240 
241 //==============================================================================
242 template <typename S>
244 {
245  // TODO(JS): Not implemented?
246 }
247 
248 //==============================================================================
249 template <typename S>
251 {
252  S Ta = TA.dot(n);
253  S Tb = TB.dot(n);
254  S Tc = TC.dot(n);
255 
256  std::vector<S> T_potential;
257  T_potential.push_back(tf_t);
258  T_potential.push_back(1);
259  if(Tb * Tb - 3 * Ta * Tc >= 0)
260  {
261  if(Ta == 0)
262  {
263  if(Tb != 0)
264  {
265  S tmp = -Tc / (2 * Tb);
266  if(tmp < 1 && tmp > tf_t)
267  T_potential.push_back(tmp);
268  }
269  }
270  else
271  {
272  S tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
273  S tmp1 = (-Tb + tmp_delta) / (3 * Ta);
274  S tmp2 = (-Tb - tmp_delta) / (3 * Ta);
275  if(tmp1 < 1 && tmp1 > tf_t)
276  T_potential.push_back(tmp1);
277  if(tmp2 < 1 && tmp2 > tf_t)
278  T_potential.push_back(tmp2);
279  }
280  }
281 
282  S T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
283  for(unsigned int i = 1; i < T_potential.size(); ++i)
284  {
285  S T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
286  if(T_bound_tmp > T_bound) T_bound = T_bound_tmp;
287  }
288 
289 
290  S cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
291 
292  T_bound -= cur_delta;
293  T_bound /= 6.0;
294 
295  return T_bound;
296 }
297 
298 //==============================================================================
299 template <typename S>
301 {
302  // first compute ||w'||
303  int a00[5] = {1,-4,6,-4,1};
304  int a01[5] = {-3,10,-11,4,0};
305  int a02[5] = {3,-8,6,0,-1};
306  int a03[5] = {-1,2,-1,0,0};
307  int a11[5] = {9,-24,16,0,0};
308  int a12[5] = {-9,18,-5,-4,0};
309  int a13[5] = {3,-4,0,0,0};
310  int a22[5] = {9,-12,-2,4,1};
311  int a23[5] = {-3,2,1,0,0};
312  int a33[5] = {1,0,0,0,0};
313 
314  S a[5];
315 
316  for(int i = 0; i < 5; ++i)
317  {
318  a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i]
319  + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i]
320  + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i]
321  + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i];
322  a[i] /= 4.0;
323  }
324 
325  // compute polynomial for ||w'||'
326  int da00[4] = {4,-12,12,-4};
327  int da01[4] = {-12,30,-22,4};
328  int da02[4] = {12,-24,12,0};
329  int da03[4] = {-4,6,-2,0};
330  int da11[4] = {36,-72,32,0};
331  int da12[4] = {-36,54,-10,-4};
332  int da13[4] = {12,-12,0,0};
333  int da22[4] = {36,-36,-4,4};
334  int da23[4] = {-12,6,2,0};
335  int da33[4] = {4,0,0,0};
336 
337  S da[4];
338  for(int i = 0; i < 4; ++i)
339  {
340  da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i]
341  + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i]
342  + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i]
343  + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i];
344  da[i] /= 4.0;
345  }
346 
347  S roots[3];
348 
349  int root_num = detail::PolySolver<S>::solveCubic(da, roots);
350 
351  S dWdW_max = a[0] * tf_t * tf_t * tf_t + a[1] * tf_t * tf_t * tf_t + a[2] * tf_t * tf_t + a[3] * tf_t + a[4];
352  S dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
353  if(dWdW_max < dWdW_1) dWdW_max = dWdW_1;
354  for(int i = 0; i < root_num; ++i)
355  {
356  S v = roots[i];
357 
358  if(v >= tf_t && v <= 1)
359  {
360  S value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
361  if(value > dWdW_max) dWdW_max = value;
362  }
363  }
364 
365  return sqrt(dWdW_max);
366 }
367 
368 //==============================================================================
369 template <typename S>
371 {
372  return tf_t;
373 }
374 
375 //==============================================================================
376 template <typename S>
378 {
379  return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0;
380 }
381 
382 //==============================================================================
383 template <typename S>
385 {
386  return (4 - 6 * t * t + 3 * t * t * t) / 6.0;
387 }
388 
389 //==============================================================================
390 template <typename S>
392 {
393  return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0;
394 }
395 
396 //==============================================================================
397 template <typename S>
399 {
400  return t * t * t / 6.0;
401 }
402 
403 } // namespace fcl
404 
405 #endif
fcl::TriangleMotionBoundVisitor
Definition: motion_base.h:52
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::SplineMotion::getCurrentTransform
void getCurrentTransform(Transform3< S > &tf_) const override
Get the rotation and translation in current step.
Definition: spline_motion-inl.h:156
fcl::SplineMotion::Rd1Rd2
S Rd1Rd2
Definition: spline_motion.h:96
fcl::SplineMotion::Rd3Rd3
S Rd3Rd3
Definition: spline_motion.h:96
fcl::TMatrix3::setTimeInterval
void setTimeInterval(const std::shared_ptr< TimeInterval< S >> &time_interval)
Definition: taylor_matrix-inl.h:431
fcl::SplineMotion::Rd1Rd1
S Rd1Rd1
Definition: spline_motion.h:96
unused.h
fcl::SplineMotion::RB
Vector3< S > RB
Definition: spline_motion.h:94
fcl::SplineMotion< double >
template class FCL_EXPORT SplineMotion< double >
fcl::SplineMotion::getCurrentTime
S getCurrentTime() const
Definition: spline_motion-inl.h:370
fcl::TVector3::setTimeInterval
void setTimeInterval(const std::shared_ptr< TimeInterval< S >> &time_interval)
Definition: taylor_vector-inl.h:331
fcl::SplineMotion::computeDWMax
S computeDWMax() const
Definition: spline_motion-inl.h:300
fcl::SplineMotion::SplineMotion
SplineMotion(const Vector3< S > &Td0, const Vector3< S > &Td1, const Vector3< S > &Td2, const Vector3< S > &Td3, const Vector3< S > &Rd0, const Vector3< S > &Rd1, const Vector3< S > &Rd2, const Vector3< S > &Rd3)
Construct motion from 4 deBoor points.
Definition: spline_motion-inl.h:54
fcl::AngleAxis
Eigen::AngleAxis< S > AngleAxis
Definition: types.h:97
fcl::SplineMotion::Rd0Rd3
S Rd0Rd3
Definition: spline_motion.h:96
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::SplineMotion::computeTBound
S computeTBound(const Vector3< S > &n) const
Definition: spline_motion-inl.h:250
fcl::SplineMotion::TA
Vector3< S > TA
Definition: spline_motion.h:93
spline_motion.h
fcl::Matrix3
Eigen::Matrix< S, 3, 3 > Matrix3
Definition: types.h:85
fcl::SplineMotion::Rd2Rd3
S Rd2Rd3
Definition: spline_motion.h:96
fcl::SplineMotion::Rd1Rd3
S Rd1Rd3
Definition: spline_motion.h:96
fcl::SplineMotion::getWeight3
S getWeight3(S t) const
Definition: spline_motion-inl.h:398
fcl::SplineMotion::Td
Vector3< S > Td[4]
Definition: spline_motion.h:90
fcl::MotionBase
Definition: bv_motion_bound_visitor.h:45
fcl::SplineMotion::RA
Vector3< S > RA
Definition: spline_motion.h:94
fcl::SplineMotion::integrate
bool integrate(S dt) const override
Integrate the motion from 0 to dt We compute the current transformation from zero point instead of fr...
Definition: spline_motion-inl.h:121
fcl::SplineMotion::RC
Vector3< S > RC
Definition: spline_motion.h:94
fcl::TriangleMotionBoundVisitor::visit
virtual S visit(const MotionBase< S > &motion) const
Definition: triangle_motion_bound_visitor.h:81
fcl::SplineMotion::getTaylorModel
void getTaylorModel(TMatrix3< S > &tm, TVector3< S > &tv) const override
Definition: spline_motion-inl.h:163
fcl::SplineMotion::getWeight0
S getWeight0(S t) const
Definition: spline_motion-inl.h:377
fcl::SplineMotion::getWeight1
S getWeight1(S t) const
Definition: spline_motion-inl.h:384
fcl::SplineMotion::Rd0Rd2
S Rd0Rd2
Definition: spline_motion.h:96
fcl::SplineMotion::Rd0Rd0
S Rd0Rd0
Definition: spline_motion.h:96
fcl::SplineMotion::TC
Vector3< S > TC
Definition: spline_motion.h:93
fcl::Interval
Interval class for [a, b].
Definition: interval.h:50
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::SplineMotion::Rd
Vector3< S > Rd[4]
Definition: spline_motion.h:91
fcl::TMatrix3
Definition: taylor_matrix.h:48
fcl::TVector3
Definition: taylor_vector.h:48
fcl::SplineMotion::Rd2Rd2
S Rd2Rd2
Definition: spline_motion.h:96
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
fcl::SplineMotion::computeSplineParameter
void computeSplineParameter()
Definition: spline_motion-inl.h:243
fcl::SplineMotion::getWeight2
S getWeight2(S t) const
Definition: spline_motion-inl.h:391
fcl::hat
template void hat(Matrix3d &mat, const Vector3d &vec)
fcl::SplineMotion::Rd0Rd1
S Rd0Rd1
Definition: spline_motion.h:96
fcl::SplineMotion::computeMotionBound
S computeMotionBound(const BVMotionBoundVisitor< S > &mb_visitor) const override
Compute the motion bound for a bounding volume along a given direction n, which is defined in the vis...
Definition: spline_motion-inl.h:142
fcl::SplineMotion::TB
Vector3< S > TB
Definition: spline_motion.h:93
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::SplineMotion::tf
Transform3< S > tf
Definition: spline_motion.h:98
fcl::detail::PolySolver::solveCubic
static int solveCubic(S c[4], S s[3])
Solve a cubic function with coefficients c, return roots s and number of roots.
Definition: polysolver-inl.h:103


fcl
Author(s):
autogenerated on Tue Dec 5 2023 03:40:49