motion.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00038 #ifndef FCL_CCD_MOTION_H
00039 #define FCL_CCD_MOTION_H
00040 
00041 #include "fcl/ccd/motion_base.h"
00042 #include "fcl/intersect.h"
00043 #include <iostream>
00044 #include <vector>
00045 
00046 namespace fcl
00047 {
00048 
00049 
00050 template<typename BV>
00051 class SplineMotion : public MotionBase<BV>
00052 {
00053 public:
00055   SplineMotion(const Vec3f& Td0, const Vec3f& Td1, const Vec3f& Td2, const Vec3f& Td3,
00056                const Vec3f& Rd0, const Vec3f& Rd1, const Vec3f& Rd2, const Vec3f& Rd3)
00057   {
00058     Td[0] = Td0;
00059     Td[1] = Td1;
00060     Td[2] = Td2;
00061     Td[3] = Td3;
00062 
00063     Rd[0] = Rd0;
00064     Rd[1] = Rd1;
00065     Rd[2] = Rd2;
00066     Rd[3] = Rd3;
00067 
00068     Rd0Rd0 = Rd[0].dot(Rd[0]);
00069     Rd0Rd1 = Rd[0].dot(Rd[1]);
00070     Rd0Rd2 = Rd[0].dot(Rd[2]);
00071     Rd0Rd3 = Rd[0].dot(Rd[3]);
00072     Rd1Rd1 = Rd[1].dot(Rd[1]);
00073     Rd1Rd2 = Rd[1].dot(Rd[2]);
00074     Rd1Rd3 = Rd[1].dot(Rd[3]);
00075     Rd2Rd2 = Rd[2].dot(Rd[2]);
00076     Rd2Rd3 = Rd[2].dot(Rd[3]);
00077     Rd3Rd3 = Rd[3].dot(Rd[3]);
00078 
00079     TA = Td[1] * 3 - Td[2] * 3 + Td[3] - Td[0];
00080     TB = (Td[0] - Td[1] * 2 + Td[2]) * 3;
00081     TC = (Td[2] - Td[0]) * 3;
00082 
00083     RA = Rd[1] * 3 - Rd[2] * 3 + Rd[3] - Rd[0];
00084     RB = (Rd[0] - Rd[1] * 2 + Rd[2]) * 3;
00085     RC = (Rd[2] - Rd[0]) * 3;
00086 
00087     integrate(0.0);
00088   }
00089 
00093   bool integrate(double dt)
00094   {
00095     if(dt > 1) dt = 1;
00096 
00097     Vec3f cur_T = Td[0] * getWeight0(dt) + Td[1] * getWeight1(dt) + Td[2] * getWeight2(dt) + Td[3] * getWeight3(dt);
00098     Vec3f cur_w = Rd[0] * getWeight0(dt) + Rd[1] * getWeight1(dt) + Rd[2] * getWeight2(dt) + Rd[3] * getWeight3(dt);
00099     FCL_REAL cur_angle = cur_w.length();
00100     cur_w.normalize();
00101 
00102     Quaternion3f cur_q;
00103     cur_q.fromAxisAngle(cur_w, cur_angle);
00104 
00105     tf.setTransform(cur_q, cur_T);
00106 
00107     tf_t = dt;
00108 
00109     return true;
00110   }
00111 
00115   FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
00116 
00117   FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
00118   {
00119     FCL_REAL T_bound = computeTBound(n);
00120 
00121     FCL_REAL R_bound = fabs(a.dot(n)) + a.length() + (a.cross(n)).length();
00122     FCL_REAL R_bound_tmp = fabs(b.dot(n)) + b.length() + (b.cross(n)).length();
00123     if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
00124     R_bound_tmp = fabs(c.dot(n)) + c.length() + (c.cross(n)).length();
00125     if(R_bound_tmp > R_bound) R_bound = R_bound_tmp;
00126 
00127     FCL_REAL dWdW_max = computeDWMax();
00128     FCL_REAL ratio = std::min(1 - tf_t, dWdW_max);
00129 
00130     R_bound *= 2 * ratio;
00131 
00132     // std::cout << R_bound << " " << T_bound << std::endl;
00133 
00134     return R_bound + T_bound;
00135   }
00136 
00138   void getCurrentTransform(Matrix3f& R, Vec3f& T) const
00139   {
00140     R = tf.getRotation();
00141     T = tf.getTranslation();
00142   }
00143 
00144   void getCurrentRotation(Matrix3f& R) const
00145   {
00146     R = tf.getRotation();
00147   }
00148 
00149   void getCurrentTranslation(Vec3f& T) const
00150   {
00151     T = tf.getTranslation();
00152   }
00153 
00154   void getCurrentTransform(Transform3f& tf_) const
00155   {
00156     tf_ = tf;
00157   }
00158 
00159 protected:
00160   void computeSplineParameter()
00161   {
00162 
00163   }
00164 
00165   FCL_REAL getWeight0(FCL_REAL t) const
00166   {
00167     return (1 - 3 * t + 3 * t * t - t * t * t) / 6.0;
00168   }
00169 
00170   FCL_REAL getWeight1(FCL_REAL t) const
00171   {
00172     return (4 - 6 * t * t + 3 * t * t * t) / 6.0;
00173   }
00174 
00175   FCL_REAL getWeight2(FCL_REAL t) const
00176   {
00177     return (1 + 3 * t + 3 * t * t - 3 * t * t * t) / 6.0;
00178   }
00179 
00180   FCL_REAL getWeight3(FCL_REAL t) const
00181   {
00182     return t * t * t / 6.0;
00183   }
00184 
00185   FCL_REAL computeTBound(const Vec3f& n) const
00186   {
00187     FCL_REAL Ta = TA.dot(n);
00188     FCL_REAL Tb = TB.dot(n);
00189     FCL_REAL Tc = TC.dot(n);
00190 
00191     std::vector<FCL_REAL> T_potential;
00192     T_potential.push_back(tf_t);
00193     T_potential.push_back(1);
00194     if(Tb * Tb - 3 * Ta * Tc >= 0)
00195     {
00196       if(Ta == 0)
00197       {
00198         if(Tb != 0)
00199         {
00200           FCL_REAL tmp = -Tc / (2 * Tb);
00201           if(tmp < 1 && tmp > tf_t)
00202             T_potential.push_back(tmp);
00203         }
00204       }
00205       else
00206       {
00207         FCL_REAL tmp_delta = sqrt(Tb * Tb - 3 * Ta * Tc);
00208         FCL_REAL tmp1 = (-Tb + tmp_delta) / (3 * Ta);
00209         FCL_REAL tmp2 = (-Tb - tmp_delta) / (3 * Ta);
00210         if(tmp1 < 1 && tmp1 > tf_t)
00211           T_potential.push_back(tmp1);
00212         if(tmp2 < 1 && tmp2 > tf_t)
00213           T_potential.push_back(tmp2);
00214       }
00215     }
00216 
00217     FCL_REAL T_bound = Ta * T_potential[0] * T_potential[0] * T_potential[0] + Tb * T_potential[0] * T_potential[0] + Tc * T_potential[0];
00218     for(unsigned int i = 1; i < T_potential.size(); ++i)
00219     {
00220       FCL_REAL T_bound_tmp = Ta * T_potential[i] * T_potential[i] * T_potential[i] + Tb * T_potential[i] * T_potential[i] + Tc * T_potential[i];
00221       if(T_bound_tmp > T_bound) T_bound = T_bound_tmp;
00222     }
00223 
00224 
00225     FCL_REAL cur_delta = Ta * tf_t * tf_t * tf_t + Tb * tf_t * tf_t + Tc * tf_t;
00226 
00227     T_bound -= cur_delta;
00228     T_bound /= 6.0;
00229 
00230     return T_bound;
00231   }
00232 
00233   FCL_REAL computeDWMax() const
00234   {
00235     // first compute ||w'||
00236     int a00[5] = {1,-4,6,-4,1};
00237     int a01[5] = {-3,10,-11,4,0};
00238     int a02[5] = {3,-8,6,0,-1};
00239     int a03[5] = {-1,2,-1,0,0};
00240     int a11[5] = {9,-24,16,0,0};
00241     int a12[5] = {-9,18,-5,-4,0};
00242     int a13[5] = {3,-4,0,0,0};
00243     int a22[5] = {9,-12,-2,4,1};
00244     int a23[5] = {-3,2,1,0,0};
00245     int a33[5] = {1,0,0,0,0};
00246 
00247     FCL_REAL a[5];
00248 
00249     for(int i = 0; i < 5; ++i)
00250     {
00251       a[i] = Rd0Rd0 * a00[i] + Rd0Rd1 * a01[i] + Rd0Rd2 * a02[i] + Rd0Rd3 * a03[i]
00252            + Rd0Rd1 * a01[i] + Rd1Rd1 * a11[i] + Rd1Rd2 * a12[i] + Rd1Rd3 * a13[i]
00253            + Rd0Rd2 * a02[i] + Rd1Rd2 * a12[i] + Rd2Rd2 * a22[i] + Rd2Rd3 * a23[i]
00254            + Rd0Rd3 * a03[i] + Rd1Rd3 * a13[i] + Rd2Rd3 * a23[i] + Rd3Rd3 * a33[i];
00255       a[i] /= 4.0;
00256     }
00257 
00258     // compute polynomial for ||w'||'
00259     int da00[4] = {4,-12,12,-4};
00260     int da01[4] = {-12,30,-22,4};
00261     int da02[4] = {12,-24,12,0};
00262     int da03[4] = {-4,6,-2,0};
00263     int da11[4] = {36,-72,32,0};
00264     int da12[4] = {-36,54,-10,-4};
00265     int da13[4] = {12,-12,0,0};
00266     int da22[4] = {36,-36,-4,4};
00267     int da23[4] = {-12,6,2,0};
00268     int da33[4] = {4,0,0,0};
00269 
00270     FCL_REAL da[4];
00271     for(int i = 0; i < 4; ++i)
00272     {
00273       da[i] = Rd0Rd0 * da00[i] + Rd0Rd1 * da01[i] + Rd0Rd2 * da02[i] + Rd0Rd3 * da03[i]
00274             + Rd0Rd1 * da01[i] + Rd1Rd1 * da11[i] + Rd1Rd2 * da12[i] + Rd1Rd3 * da13[i]
00275             + Rd0Rd2 * da02[i] + Rd1Rd2 * da12[i] + Rd2Rd2 * da22[i] + Rd2Rd3 * da23[i]
00276             + Rd0Rd3 * da03[i] + Rd1Rd3 * da13[i] + Rd2Rd3 * da23[i] + Rd3Rd3 * da33[i];
00277       da[i] /= 4.0;
00278     }
00279 
00280     FCL_REAL roots[3];
00281 
00282     int root_num = PolySolver::solveCubic(da, roots);
00283 
00284     FCL_REAL 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];
00285     FCL_REAL dWdW_1 = a[0] + a[1] + a[2] + a[3] + a[4];
00286     if(dWdW_max < dWdW_1) dWdW_max = dWdW_1;
00287     for(int i = 0; i < root_num; ++i)
00288     {
00289       FCL_REAL v = roots[i];
00290 
00291       if(v >= tf_t && v <= 1)
00292       {
00293         FCL_REAL value = a[0] * v * v * v * v + a[1] * v * v * v + a[2] * v * v + a[3] * v + a[4];
00294         if(value > dWdW_max) dWdW_max = value;
00295       }
00296     }
00297 
00298     return sqrt(dWdW_max);
00299   }
00300 
00301   Vec3f Td[4];
00302   Vec3f Rd[4];
00303 
00304   Vec3f TA, TB, TC;
00305   Vec3f RA, RB, RC;
00306 
00307   FCL_REAL Rd0Rd0, Rd0Rd1, Rd0Rd2, Rd0Rd3, Rd1Rd1, Rd1Rd2, Rd1Rd3, Rd2Rd2, Rd2Rd3, Rd3Rd3;
00309   Transform3f tf;
00310 
00312   FCL_REAL tf_t;
00313 };
00314 
00315 template<typename BV>
00316 class ScrewMotion : public MotionBase<BV>
00317 {
00318 public:
00320   ScrewMotion()
00321   {
00323     axis.setValue(1, 0, 0);
00324     angular_vel = 0;
00325 
00329     linear_vel = 0;
00330   }
00331 
00333   ScrewMotion(const Matrix3f& R1, const Vec3f& T1,
00334               const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
00335                                                      tf2(R2, T2),
00336                                                      tf(tf1)
00337   {
00338     computeScrewParameter();
00339   }
00340 
00342   ScrewMotion(const Transform3f& tf1_,
00343               const Transform3f& tf2_) : tf1(tf1_),
00344                                          tf2(tf2_),
00345                                          tf(tf1)
00346   {
00347     computeScrewParameter();
00348   }
00349 
00353   bool integrate(double dt)
00354   {
00355     if(dt > 1) dt = 1;
00356 
00357     tf.setQuatRotation(absoluteRotation(dt));
00358 
00359     Quaternion3f delta_rot = deltaRotation(dt);
00360     tf.setTranslation(p + axis * (dt * linear_vel) + delta_rot.transform(tf1.getTranslation() - p));
00361 
00362     return true;
00363   }
00364 
00368   FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
00369 
00370   FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
00371   {
00372     FCL_REAL proj_max = ((tf.getQuatRotation().transform(a) + tf.getTranslation() - p).cross(axis)).sqrLength();
00373     FCL_REAL tmp;
00374     tmp = ((tf.getQuatRotation().transform(b) + tf.getTranslation() - p).cross(axis)).sqrLength();
00375     if(tmp > proj_max) proj_max = tmp;
00376     tmp = ((tf.getQuatRotation().transform(c) + tf.getTranslation() - p).cross(axis)).sqrLength();
00377     if(tmp > proj_max) proj_max = tmp;
00378 
00379     proj_max = sqrt(proj_max);
00380 
00381     FCL_REAL v_dot_n = axis.dot(n) * linear_vel;
00382     FCL_REAL w_cross_n = (axis.cross(n)).length() * angular_vel;
00383     FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
00384 
00385     return mu;
00386   }
00387 
00389   void getCurrentTransform(Matrix3f& R, Vec3f& T) const
00390   {
00391     R = tf.getRotation();
00392     T = tf.getTranslation();
00393   }
00394 
00395   void getCurrentRotation(Matrix3f& R) const
00396   {
00397     R = tf.getRotation();
00398   }
00399 
00400   void getCurrentTranslation(Vec3f& T) const
00401   {
00402     T = tf.getTranslation();
00403   }
00404 
00405   void getCurrentTransform(Transform3f& tf_) const
00406   {
00407     tf_ = tf;
00408   }
00409 
00410 protected:
00411   void computeScrewParameter()
00412   {
00413     Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
00414     deltaq.toAxisAngle(axis, angular_vel);
00415     if(angular_vel < 0)
00416     {
00417       angular_vel = -angular_vel;
00418       axis = -axis;
00419     }
00420 
00421     if(angular_vel < 1e-10)
00422     {
00423       angular_vel = 0;
00424       axis = tf2.getTranslation() - tf1.getTranslation();
00425       linear_vel = axis.length();
00426       p = tf1.getTranslation();
00427     }
00428     else
00429     {
00430       Vec3f o = tf2.getTranslation() - tf1.getTranslation();
00431       p = (tf1.getTranslation() + tf2.getTranslation() + axis.cross(o) * (1.0 / tan(angular_vel / 2.0))) * 0.5;
00432       linear_vel = o.dot(axis);
00433     }
00434   }
00435 
00436   Quaternion3f deltaRotation(FCL_REAL dt) const
00437   {
00438     Quaternion3f res;
00439     res.fromAxisAngle(axis, (FCL_REAL)(dt * angular_vel));
00440     return res;
00441   }
00442 
00443   Quaternion3f absoluteRotation(FCL_REAL dt) const
00444   {
00445     Quaternion3f delta_t = deltaRotation(dt);
00446     return delta_t * tf1.getQuatRotation();
00447   }
00448 
00450   Transform3f tf1;
00451 
00453   Transform3f tf2;
00454 
00456   Transform3f tf;
00457 
00459   Vec3f axis;
00460 
00462   Vec3f p;
00463 
00465   FCL_REAL linear_vel;
00466 
00468   FCL_REAL angular_vel;
00469 };
00470 
00471 
00477 template<>
00478 FCL_REAL ScrewMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
00479 
00480 
00488 template<typename BV>
00489 class InterpMotion : public MotionBase<BV>
00490 {
00491 public:
00493   InterpMotion()
00494   {
00496     angular_axis.setValue(1, 0, 0);
00497     angular_vel = 0;
00498 
00502   }
00503 
00505   InterpMotion(const Matrix3f& R1, const Vec3f& T1,
00506                const Matrix3f& R2, const Vec3f& T2) : tf1(R1, T1),
00507                                                       tf2(R2, T2),
00508                                                       tf(tf1)
00509   {
00511     computeVelocity();
00512   }
00513 
00514 
00515   InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_) : tf1(tf1_),
00516                                                                    tf2(tf2_),
00517                                                                    tf(tf1)
00518   {
00520     computeVelocity();
00521   }
00522 
00525   InterpMotion(const Matrix3f& R1, const Vec3f& T1,
00526                const Matrix3f& R2, const Vec3f& T2,
00527                const Vec3f& O) : tf1(R1, T1),
00528                                  tf2(R2, T2),
00529                                  tf(tf1),
00530                                  reference_p(O)
00531   {
00533     computeVelocity();
00534   }
00535 
00536   InterpMotion(const Transform3f& tf1_, const Transform3f& tf2_, const Vec3f& O) : tf1(tf1_),
00537                                                                                    tf2(tf2_),
00538                                                                                    tf(tf1),
00539                                                                                    reference_p(O)
00540   {
00541   }
00542 
00543 
00547   bool integrate(double dt)
00548   {
00549     if(dt > 1) dt = 1;
00550 
00551     tf.setQuatRotation(absoluteRotation(dt));
00552     tf.setTranslation(linear_vel * dt + tf1.transform(reference_p) - tf.getQuatRotation().transform(reference_p));
00553 
00554     return true;
00555   }
00556 
00560   FCL_REAL computeMotionBound(const BV& bv, const Vec3f& n) const { return 0.0; }
00561 
00567   FCL_REAL computeMotionBound(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& n) const
00568   {
00569     FCL_REAL proj_max = ((tf.getQuatRotation().transform(a - reference_p)).cross(angular_axis)).sqrLength();
00570     FCL_REAL tmp;
00571     tmp = ((tf.getQuatRotation().transform(b - reference_p)).cross(angular_axis)).sqrLength();
00572     if(tmp > proj_max) proj_max = tmp;
00573     tmp = ((tf.getQuatRotation().transform(c - reference_p)).cross(angular_axis)).sqrLength();
00574     if(tmp > proj_max) proj_max = tmp;
00575 
00576     proj_max = sqrt(proj_max);
00577 
00578     FCL_REAL v_dot_n = linear_vel.dot(n);
00579     FCL_REAL w_cross_n = (angular_axis.cross(n)).length() * angular_vel;
00580     FCL_REAL mu = v_dot_n + w_cross_n * proj_max;
00581 
00582     return mu;
00583   }
00584 
00586   void getCurrentTransform(Matrix3f& R, Vec3f& T) const
00587   {
00588     R = tf.getRotation();
00589     T = tf.getTranslation();
00590   }
00591 
00592   void getCurrentRotation(Matrix3f& R) const
00593   {
00594     R = tf.getRotation();
00595   }
00596 
00597   void getCurrentTranslation(Vec3f& T) const
00598   {
00599     T = tf.getTranslation();
00600   }
00601 
00602   void getCurrentTransform(Transform3f& tf_) const
00603   {
00604     tf_ = tf;
00605   }
00606 
00607 protected:
00608 
00609   void computeVelocity()
00610   {
00611     linear_vel = tf2.transform(reference_p) - tf1.transform(reference_p);
00612     Quaternion3f deltaq = tf2.getQuatRotation() * inverse(tf1.getQuatRotation());
00613     deltaq.toAxisAngle(angular_axis, angular_vel);
00614     if(angular_vel < 0)
00615     {
00616       angular_vel = -angular_vel;
00617       angular_axis = -angular_axis;
00618     }
00619   }
00620 
00621 
00622   Quaternion3f deltaRotation(FCL_REAL dt) const
00623   {
00624     Quaternion3f res;
00625     res.fromAxisAngle(angular_axis, (FCL_REAL)(dt * angular_vel));
00626     return res;
00627   }
00628 
00629   Quaternion3f absoluteRotation(FCL_REAL dt) const
00630   {
00631     Quaternion3f delta_t = deltaRotation(dt);
00632     return delta_t * tf1.getQuatRotation();
00633   }
00634 
00636   Transform3f tf1;
00637 
00639   Transform3f tf2;
00640 
00642   Transform3f tf;
00643 
00645   Vec3f linear_vel;
00646 
00648   FCL_REAL angular_vel;
00649 
00651   Vec3f angular_axis;
00652 
00654   Vec3f reference_p;
00655 };
00656 
00657 
00663 template<>
00664 FCL_REAL InterpMotion<RSS>::computeMotionBound(const RSS& bv, const Vec3f& n) const;
00665 
00666 
00667 }
00668 
00669 #endif
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


fcl
Author(s): Jia Pan
autogenerated on Tue Jan 15 2013 16:05:30