tbv_motion_bound_visitor-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_TBVMOTIONBOUNDVISITOR_INL_H
39 #define FCL_CCD_TBVMOTIONBOUNDVISITOR_INL_H
40 
42 
43 #include "fcl/common/unused.h"
44 
45 namespace fcl
46 {
47 
48 //==============================================================================
49 template<typename BV>
51  const BV& bv_, const Vector3<typename BV::S>& n_)
52  : bv(bv_), n(n_)
53 {
54  // Do nothing
55 }
56 
57 //==============================================================================
58 template <typename S, typename BV, typename MotionT>
60 {
61  static S run(
62  const TBVMotionBoundVisitor<BV>& /*visitor*/,
63  const MotionT& /*motion*/)
64  {
65  return 0;
66  }
67 };
68 
69 //==============================================================================
70 template<typename BV>
72  const MotionBase<S>& motion) const
73 {
74  FCL_UNUSED(motion);
75 
76  return 0;
77 }
78 
79 //==============================================================================
80 template<typename BV>
82  const SplineMotion<S>& motion) const
83 {
84  using S = typename BV::S;
85 
87  S, BV, SplineMotion<S>>::run(*this, motion);
88 }
89 
90 //==============================================================================
91 template<typename BV>
93  const ScrewMotion<S>& motion) const
94 {
95  using S = typename BV::S;
96 
98  S, BV, ScrewMotion<S>>::run(*this, motion);
99 }
100 
101 //==============================================================================
102 template<typename BV>
104  const InterpMotion<S>& motion) const
105 {
106  using S = typename BV::S;
107 
109  S, BV, InterpMotion<S>>::run(*this, motion);
110 }
111 
112 //==============================================================================
113 template<typename BV>
115  const TranslationMotion<S>& motion) const
116 {
117  using S = typename BV::S;
118 
120  S, BV, TranslationMotion<S>>::run(*this, motion);
121 }
122 
123 //==============================================================================
124 template <typename S>
126 {
127  static S run(
128  const TBVMotionBoundVisitor<RSS<S>>& visitor,
129  const SplineMotion<S>& motion)
130  {
131  S T_bound = motion.computeTBound(visitor.n);
132  S tf_t = motion.getCurrentTime();
133 
134  Vector3<S> c1 = visitor.bv.To;
135  Vector3<S> c2 = visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0];
136  Vector3<S> c3 = visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1];
137  Vector3<S> c4 = visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1];
138 
139  S tmp;
140  // max_i |c_i * n|
141  S cn_max = std::abs(c1.dot(visitor.n));
142  tmp = std::abs(c2.dot(visitor.n));
143  if(tmp > cn_max) cn_max = tmp;
144  tmp = std::abs(c3.dot(visitor.n));
145  if(tmp > cn_max) cn_max = tmp;
146  tmp = std::abs(c4.dot(visitor.n));
147  if(tmp > cn_max) cn_max = tmp;
148 
149  // max_i ||c_i||
150  S cmax = c1.squaredNorm();
151  tmp = c2.squaredNorm();
152  if(tmp > cmax) cmax = tmp;
153  tmp = c3.squaredNorm();
154  if(tmp > cmax) cmax = tmp;
155  tmp = c4.squaredNorm();
156  if(tmp > cmax) cmax = tmp;
157  cmax = sqrt(cmax);
158 
159  // max_i ||c_i x n||
160  S cxn_max = (c1.cross(visitor.n)).squaredNorm();
161  tmp = (c2.cross(visitor.n)).squaredNorm();
162  if(tmp > cxn_max) cxn_max = tmp;
163  tmp = (c3.cross(visitor.n)).squaredNorm();
164  if(tmp > cxn_max) cxn_max = tmp;
165  tmp = (c4.cross(visitor.n)).squaredNorm();
166  if(tmp > cxn_max) cxn_max = tmp;
167  cxn_max = sqrt(cxn_max);
168 
169  S dWdW_max = motion.computeDWMax();
170  S ratio = std::min(1 - tf_t, dWdW_max);
171 
172  S R_bound = 2 * (cn_max + cmax + cxn_max + 3 * visitor.bv.r) * ratio;
173 
174 
175  // std::cout << R_bound << " " << T_bound << std::endl;
176 
177  return R_bound + T_bound;
178  }
179 };
180 
181 //==============================================================================
186 template <typename S>
188 {
189  static S run(
190  const TBVMotionBoundVisitor<RSS<S>>& visitor,
191  const ScrewMotion<S>& motion)
192  {
193  Transform3<S> tf;
194  motion.getCurrentTransform(tf);
195 
196  const Vector3<S>& axis = motion.getAxis();
197  S linear_vel = motion.getLinearVelocity();
198  S angular_vel = motion.getAngularVelocity();
199  const Vector3<S>& p = motion.getAxisOrigin();
200 
201  S c_proj_max = ((tf.linear() * visitor.bv.To).cross(axis)).squaredNorm();
202  S tmp;
203  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0])).cross(axis)).squaredNorm();
204  if(tmp > c_proj_max) c_proj_max = tmp;
205  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1])).cross(axis)).squaredNorm();
206  if(tmp > c_proj_max) c_proj_max = tmp;
207  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1])).cross(axis)).squaredNorm();
208  if(tmp > c_proj_max) c_proj_max = tmp;
209 
210  c_proj_max = sqrt(c_proj_max);
211 
212  S v_dot_n = axis.dot(visitor.n) * linear_vel;
213  S w_cross_n = (axis.cross(visitor.n)).norm() * angular_vel;
214  S origin_proj = ((tf.translation() - p).cross(axis)).norm();
215 
216  S mu = v_dot_n + w_cross_n * (c_proj_max + visitor.bv.r + origin_proj);
217 
218  return mu;
219  }
220 };
221 
222 //==============================================================================
227 template <typename S>
229 {
230  static S run(
231  const TBVMotionBoundVisitor<RSS<S>>& visitor,
232  const InterpMotion<S>& motion)
233  {
234  Transform3<S> tf;
235  motion.getCurrentTransform(tf);
236 
237  const Vector3<S>& reference_p = motion.getReferencePoint();
238  const Vector3<S>& angular_axis = motion.getAngularAxis();
239  S angular_vel = motion.getAngularVelocity();
240  const Vector3<S>& linear_vel = motion.getLinearVelocity();
241 
242  S c_proj_max = ((tf.linear() * (visitor.bv.To - reference_p)).cross(angular_axis)).squaredNorm();
243  S tmp;
244  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] - reference_p)).cross(angular_axis)).squaredNorm();
245  if(tmp > c_proj_max) c_proj_max = tmp;
246  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(1) * visitor.bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
247  if(tmp > c_proj_max) c_proj_max = tmp;
248  tmp = ((tf.linear() * (visitor.bv.To + visitor.bv.axis.col(0) * visitor.bv.l[0] + visitor.bv.axis.col(1) * visitor.bv.l[1] - reference_p)).cross(angular_axis)).squaredNorm();
249  if(tmp > c_proj_max) c_proj_max = tmp;
250 
251  c_proj_max = std::sqrt(c_proj_max);
252 
253  S v_dot_n = linear_vel.dot(visitor.n);
254  S w_cross_n = (angular_axis.cross(visitor.n)).norm() * angular_vel;
255  S mu = v_dot_n + w_cross_n * (visitor.bv.r + c_proj_max);
256 
257  return mu;
258  }
259 };
260 
261 //==============================================================================
263 template <typename S>
265 {
266  static S run(
267  const TBVMotionBoundVisitor<RSS<S>>& visitor,
268  const TranslationMotion<S>& motion)
269  {
270  return motion.getVelocity().dot(visitor.n);
271  }
272 };
273 
274 } // namespace fcl
275 
276 #endif
fcl::SplineMotion
Definition: bv_motion_bound_visitor.h:48
fcl::Transform3
Eigen::Transform< S, 3, Eigen::Isometry > Transform3
Definition: types.h:91
fcl::TBVMotionBoundVisitor::S
typename BV::S S
Definition: tbv_motion_bound_visitor.h:68
fcl::TBVMotionBoundVisitorVisitImpl
Definition: tbv_motion_bound_visitor-inl.h:59
fcl::TBVMotionBoundVisitorVisitImpl< S, RSS< S >, SplineMotion< S > >::run
static S run(const TBVMotionBoundVisitor< RSS< S >> &visitor, const SplineMotion< S > &motion)
Definition: tbv_motion_bound_visitor-inl.h:127
fcl::ScrewMotion
Definition: bv_motion_bound_visitor.h:51
fcl::TBVMotionBoundVisitor::TBVMotionBoundVisitor
TBVMotionBoundVisitor(const BV &bv_, const Vector3< S > &n_)
Definition: tbv_motion_bound_visitor-inl.h:50
fcl::ScrewMotion::getAxisOrigin
const Vector3< S > & getAxisOrigin() const
Definition: screw_motion-inl.h:226
fcl::InterpMotion::getReferencePoint
const Vector3< S > & getReferencePoint() const
Definition: interp_motion-inl.h:223
unused.h
fcl::TranslationMotion
Definition: bv_motion_bound_visitor.h:57
fcl::SplineMotion::getCurrentTime
S getCurrentTime() const
Definition: spline_motion-inl.h:370
fcl::SplineMotion::computeDWMax
S computeDWMax() const
Definition: spline_motion-inl.h:300
fcl::ScrewMotion::getLinearVelocity
S getLinearVelocity() const
Definition: screw_motion-inl.h:205
fcl::RSS
A class for rectangle swept sphere bounding volume.
Definition: RSS.h:58
fcl::ScrewMotion::getAxis
const Vector3< S > & getAxis() const
Definition: screw_motion-inl.h:219
fcl::TranslationMotion::getVelocity
Vector3< S > getVelocity() const
Definition: translation_motion-inl.h:127
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::InterpMotion::getAngularVelocity
S getAngularVelocity() const
Definition: interp_motion-inl.h:237
fcl::TBVMotionBoundVisitorVisitImpl::run
static S run(const TBVMotionBoundVisitor< BV > &, const MotionT &)
Definition: tbv_motion_bound_visitor-inl.h:61
fcl::TBVMotionBoundVisitorVisitImpl< S, RSS< S >, ScrewMotion< S > >::run
static S run(const TBVMotionBoundVisitor< RSS< S >> &visitor, const ScrewMotion< S > &motion)
Definition: tbv_motion_bound_visitor-inl.h:189
fcl::MotionBase
Definition: bv_motion_bound_visitor.h:45
fcl::InterpMotion::getAngularAxis
const Vector3< S > & getAngularAxis() const
Definition: interp_motion-inl.h:230
fcl::TBVMotionBoundVisitor::visit
virtual S visit(const MotionBase< S > &motion) const
Definition: tbv_motion_bound_visitor-inl.h:71
fcl::TBVMotionBoundVisitor
Definition: tbv_motion_bound_visitor.h:65
fcl::TBVMotionBoundVisitorVisitImpl< S, RSS< S >, TranslationMotion< S > >::run
static S run(const TBVMotionBoundVisitor< RSS< S >> &visitor, const TranslationMotion< S > &motion)
Definition: tbv_motion_bound_visitor-inl.h:266
fcl::InterpMotion
Linear interpolation motion Each Motion is assumed to have constant linear velocity and angular veloc...
Definition: bv_motion_bound_visitor.h:54
fcl::TBVMotionBoundVisitorVisitImpl< S, RSS< S >, InterpMotion< S > >::run
static S run(const TBVMotionBoundVisitor< RSS< S >> &visitor, const InterpMotion< S > &motion)
Definition: tbv_motion_bound_visitor-inl.h:230
FCL_UNUSED
#define FCL_UNUSED(x)
Definition: unused.h:39
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
min
static T min(T x, T y)
Definition: svm.cpp:49
fcl::ScrewMotion::getCurrentTransform
void getCurrentTransform(Transform3< S > &tf_) const
Get the rotation and translation in current step.
Definition: screw_motion-inl.h:125
tbv_motion_bound_visitor.h
fcl
Main namespace.
Definition: broadphase_bruteforce-inl.h:45
fcl::ScrewMotion::getAngularVelocity
S getAngularVelocity() const
Definition: screw_motion-inl.h:212


fcl
Author(s):
autogenerated on Fri Apr 2 2021 02:38:01