jacobianframevel.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002                         jacobianframe.cpp -  description
00003                        -------------------------
00004     begin                : June 2006
00005     copyright            : (C) 2006 Erwin Aertbelien
00006     email                : firstname.lastname@mech.kuleuven.ac.be
00007 
00008  History (only major changes)( AUTHOR-Description ) :
00009  
00010  ***************************************************************************
00011  *   This library is free software; you can redistribute it and/or         *
00012  *   modify it under the terms of the GNU Lesser General Public            *
00013  *   License as published by the Free Software Foundation; either          *
00014  *   version 2.1 of the License, or (at your option) any later version.    *
00015  *                                                                         *
00016  *   This library is distributed in the hope that it will be useful,       *
00017  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00018  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
00019  *   Lesser General Public License for more details.                       *
00020  *                                                                         *
00021  *   You should have received a copy of the GNU Lesser General Public      *
00022  *   License along with this library; if not, write to the Free Software   *
00023  *   Foundation, Inc., 59 Temple Place,                                    *
00024  *   Suite 330, Boston, MA  02111-1307  USA                                *
00025  *                                                                         *
00026  ***************************************************************************/
00027 
00028 #include <kdl/jacobianframe.hpp>
00029 
00030 namespace KDL {
00031 
00032 
00033 #if 0
00034 
00042  int GetEulerZYX(const Jacobian<Rotation>& JR, 
00043                                   Jacobian<double>& gamma, 
00044                                   Jacobian<double>& beta, 
00045                                   Jacobian<double>& alpha,
00046                                   double eps) {
00047         // position-domain :
00048         JR.value().GetEulerZYX(alpha.value(),beta.value(),gamma.value());
00049         double ca = ::cos(alpha.value()); 
00050         double sa = ::sin(alpha.value()); 
00051         double cb = ::cos(beta.value()); 
00052         double sb = ::sin(beta.value()); 
00053         // velocity-domain :
00054         assert(gamma.nrOfDeriv()==JR.nrOfDeriv());
00055         assert(beta.nrOfDeriv()==JR.nrOfDeriv());
00056         assert(alpha.nrOfDeriv()==JR.nrOfDeriv());
00057     for (int i=0;i<JR.nrOfDeriv();++i) {
00058                 if (JR.hasDeriv(i) ) {
00059                         Vector w = JR.deriv(i);                 
00060                         double dgamma = (ca*w[0]+sa*w[1]);
00061                         if (fabs(cb) < eps) { 
00062                                 if (fabs(dgamma)< eps) {
00063                                         dgamma = 0.0;  // arbitrary choice
00064                                 } else {
00065                                         return 1;
00066                                 }
00067                         } else {
00068                                 dgamma /= cb;
00069                         }
00070                         double dalpha = w[2] + sb*dgamma;
00071                         double dbeta  = -sa*w[0]+ca*w[1];
00072                         gamma.deriv(i)=dgamma;
00073                         alpha.deriv(i)=dalpha;
00074                         beta.deriv(i) =dbeta;
00075                 } else {
00076                         gamma.setDeriv(i,false);
00077                         beta.setDeriv(i,false);
00078                         alpha.setDeriv(i,false);
00079                 }
00080         }
00081         return 0;
00082  }
00090  void SetEulerZYX(const Jacobian<double>& gamma, 
00091                                   const Jacobian<double>& beta, 
00092                                   const Jacobian<double>& alpha,
00093                                   Jacobian<Rotation>& JR) {
00094         // position-domain :
00095         JR.value() = Rotation::EulerZYX(alpha.value(),beta.value(),gamma.value());
00096         double ca = ::cos(alpha.value()); 
00097         double sa = ::sin(alpha.value()); 
00098         double cb = ::cos(beta.value()); 
00099         double sb = ::sin(beta.value()); 
00100         // velocity-domain :
00101         assert(gamma.nrOfDeriv()==JR.nrOfDeriv());
00102         assert(beta.nrOfDeriv()==JR.nrOfDeriv());
00103         assert(alpha.nrOfDeriv()==JR.nrOfDeriv());
00104     for (int i=0;i<gamma.nrOfDeriv();++i) {
00105                 if (gamma.hasDeriv(i) || beta.hasDeriv(i) || alpha.hasDeriv(i) ) {
00106                         Vector w;
00107                         double dalpha  = alpha.deriv(i);
00108                         double dbeta   = beta.deriv(i);
00109                         double dgamma  = gamma.deriv(i);
00110                         w[0] = -sa*dbeta + ca*cb*dgamma;
00111                         w[1] =  ca*dbeta + sa*cb*dgamma;
00112                         w[2] =  dalpha - sb*dgamma; 
00113                         JR.deriv(i) = w;
00114                 } else {
00115                         JR.setDeriv(i,false);
00116                 }
00117         }
00118 }
00119 
00120 #endif
00121 } // end of namespace


orocos_kdl
Author(s): Ruben Smits, Erwin Aertbelien, Orocos Developers
autogenerated on Sat Dec 28 2013 17:17:25