jacobiandouble.hpp
Go to the documentation of this file.
00001 /***************************************************************************
00002                         JacobianDouble.h -  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 
00029 #ifndef KDL_JACOBIANDOUBLE_H
00030 #define KDL_JACOBIANDOUBLE_H
00031 
00032 #include <kdl/jacobianexpr.hpp>
00033 #include <math.h>
00034 
00035 #define INLINE2 
00036  
00047 namespace KDL {
00048 /*****************************************************************************/
00049 template <>
00050 class BinaryOp<OpMult,double,double> {
00051 public:
00052     typedef double exprType;
00053         typedef double valueType;
00054         typedef double derivType;
00055         INLINE2 static double value(double a,double b)  {
00056                 return a*b;
00057         }
00058         INLINE2 static double derivVV(double a,double da,double b,double db) {
00059         return a*db + da*b;
00060         }
00061         INLINE2 static derivType derivCV(double a,double /* b*/,double db) {
00062         return a*db;
00063         }
00064         INLINE2 static derivType derivVC(double /*a*/,double da,double b) {
00065         return da*b;
00066         }
00067 };
00068 
00069 DEFBINARY_RTYPE(operator*,OpMult,Ref<Jacobian<double> >) 
00070 DEFBINARY_LTYPE(operator*,OpMult,Ref<Jacobian<double> >) 
00071 DEFBINARY_RLTYPE(operator*,OpMult,Ref<Jacobian<double> >,Ref<Jacobian<double> >)
00072 
00073 template <>
00074 class BinaryOp<OpDiv,double,double> {
00075 public:
00076     typedef double exprType;
00077         typedef double valueType;
00078         typedef double derivType;
00079         INLINE2 static double value(double a,double b)  {
00080                 return a/b;
00081         }
00082         INLINE2 static double derivVV(double a,double da,double b,double db) {
00083         return (b*da-a*db)/b/b;
00084         }
00085         INLINE2 static derivType derivCV(double a,double b,double db) {
00086         return -a/b/b*db;
00087         }
00088         INLINE2 static derivType derivVC(double /*a*/,double da,double b) {
00089         return da/b;
00090         }
00091 };
00092 
00093 DEFBINARY_RTYPE(operator/,OpDiv,Ref<Jacobian<double> >) 
00094 DEFBINARY_LTYPE(operator/,OpDiv,Ref<Jacobian<double> >) 
00095 DEFBINARY_RLTYPE(operator/,OpDiv,Ref<Jacobian<double> >,Ref<Jacobian<double> >)
00096 
00097 
00098 template <>
00099 class BinaryOp<OpAdd,double,double> {
00100 public:
00101     typedef double exprType;
00102         typedef double valueType;
00103         typedef double derivType;
00104         INLINE2 static double value(double a,double b)  {
00105                 return a+b;
00106         }
00107         INLINE2 static double derivVV(double ,double da,double ,double db) {
00108         return da+db;
00109         }
00110         INLINE2 static derivType derivCV(double ,double ,double db) {
00111         return db;
00112         }
00113         INLINE2 static derivType derivVC(double ,double da,double ) {
00114         return da;
00115         }
00116 };
00117 DEFBINARY_RTYPE(operator+,OpAdd,Ref<Jacobian<double> >) 
00118 DEFBINARY_LTYPE(operator+,OpAdd,Ref<Jacobian<double> >) 
00119 DEFBINARY_RLTYPE(operator+,OpAdd,Ref<Jacobian<double> >,Ref<Jacobian<double> >)
00120 
00121 template <>
00122 class BinaryOp<OpSub,double,double> {
00123 public:
00124     typedef double exprType;
00125         typedef double valueType;
00126         typedef double derivType;
00127         INLINE2 static double value(double a,double b)  {
00128                 return a-b;
00129         }
00130         INLINE2 static double derivVV(double /*a*/,double da,double /*b*/,double db) {
00131         return da-db;
00132         }
00133         INLINE2 static derivType derivCV(double /*a*/,double /*b*/,double db) {
00134         return db;
00135         }
00136         INLINE2 static derivType derivVC(double /*a*/,double da,double /*b*/) {
00137         return da;
00138         }
00139 };
00140 DEFBINARY_RTYPE(operator-,OpSub,Ref<Jacobian<double> >) 
00141 DEFBINARY_LTYPE(operator-,OpSub,Ref<Jacobian<double> >) 
00142 DEFBINARY_RLTYPE(operator-,OpSub,Ref<Jacobian<double> >,Ref<Jacobian<double> >)
00143 
00144 
00145 template <>
00146 class UnaryOp<OpNegate,double> {
00147 public:
00148     typedef double exprType;
00149         typedef double valueType;
00150         typedef double derivType;
00151         static INLINE2 valueType value(double F) {
00152                 return -F;
00153         }
00154         static INLINE2 derivType deriv(double /*a*/ ,double da) {
00155                 return -da;
00156         }
00157 };
00158 DEFUNARY_TYPE(operator-,OpNegate,Ref< Jacobian<double> >)
00159 
00160 template <>
00161 class UnaryOp<OpSin,double> {
00162 public:
00163     typedef double exprType;
00164         typedef double valueType;
00165         typedef double derivType;
00166         static INLINE2 valueType value(double a) {
00167         return ::sin(a);
00168         }
00169         static INLINE2 derivType deriv(double a,double da) {
00170         return ::cos(a)*da;
00171         }
00172 };
00173 DEFUNARY_TYPE(sin,OpSin,Ref< Jacobian<double> >)
00174 
00175 
00176 template <>
00177 class UnaryOp<OpAbs,double> {
00178 public:
00179     typedef double exprType;
00180         typedef double valueType;
00181         typedef double derivType;
00182         static INLINE2 valueType value(double a) {
00183         return ::fabs(a);
00184         }
00185         static INLINE2 derivType deriv(double a,double da) {
00186         return sign(a)*da;
00187         }
00188 };
00189 DEFUNARY_TYPE(abs,OpAbs,Ref< Jacobian<double> >)
00190 
00191 template <>
00192 class UnaryOp<OpTan,double> {
00193 public:
00194     typedef double exprType;
00195         typedef double valueType;
00196         typedef double derivType;
00197         static INLINE2 valueType value(double a) {
00198         return ::tan(a);
00199         }
00200         static INLINE2 derivType deriv(double a,double da) {
00201         double c = ::cos(a);
00202                 return da/c/c;
00203         }
00204 };
00205 DEFUNARY_TYPE(tan,OpTan,Ref< Jacobian<double> >)
00206 
00207 template <>
00208 class UnaryOp<OpCos,double> {
00209 public:
00210     typedef double exprType;
00211         typedef double valueType;
00212         typedef double derivType;
00213         static INLINE2 valueType value(double a) {
00214         return ::cos(a);
00215         }
00216         static INLINE2 derivType deriv(double a,double da) {
00217         return -::sin(a)*da;
00218         }
00219 };
00220 DEFUNARY_TYPE(cos,OpCos,Ref< Jacobian<double> >)
00221 
00222 template <>
00223 class UnaryOp<OpExp,double> {
00224 public:
00225     typedef double exprType;
00226         typedef double valueType;
00227         typedef double derivType;
00228         static INLINE2 valueType value(double a) {
00229         return ::exp(a);
00230         }
00231         static INLINE2 derivType deriv(double a,double da) {
00232         return ::exp(a)*da;
00233         }
00234 };
00235 DEFUNARY_TYPE(exp,OpExp,Ref< Jacobian<double> >)
00236 
00237 template <>
00238 class UnaryOp<OpLog,double> {
00239 public:
00240     typedef double exprType;
00241         typedef double valueType;
00242         typedef double derivType;
00243         static INLINE2 valueType value(double a) {
00244         return ::log(a);
00245         }
00246         static INLINE2 derivType deriv(double a,double da) {
00247                 return da/a;
00248         }
00249 };
00250 DEFUNARY_TYPE(log,OpLog,Ref< Jacobian<double> >)
00251 
00252 
00253 template<>
00254 class UnaryOp<OpSqrt,double> {
00255 public:
00256     typedef double exprType;
00257         typedef double valueType;
00258         typedef double derivType;
00259         static INLINE2 valueType value(double a) {
00260         return ::sqrt(a);
00261         }
00262         static INLINE2 derivType deriv(double a,double da) {
00263                 return 0.5/::sqrt(a)*da;
00264         }
00265 };
00266 DEFUNARY_TYPE(sqrt,OpSqrt,Ref< Jacobian<double> >)
00267 
00268 template<>
00269 class UnaryOp<OpAtan,double> {
00270 public:
00271     typedef double exprType;
00272         typedef double valueType;
00273         typedef double derivType;
00274         static INLINE2 valueType value(double a) {
00275         return ::atan(a);
00276         }
00277         static INLINE2 derivType deriv(double a,double da) {
00278                 return da/(1+a*a);
00279         }
00280 };
00281 DEFUNARY_TYPE(atan,OpAtan,Ref< Jacobian<double> >)
00282 
00283 template<>
00284 class UnaryOp<OpAsin,double> {
00285 public:
00286     typedef double exprType;
00287         typedef double valueType;
00288         typedef double derivType;
00289         static INLINE2 valueType value(double a) {
00290         return ::asin(a);
00291         }
00292         static INLINE2 derivType deriv(double a,double da) {
00293                 return da/::sqrt(1-a*a);
00294         }
00295 };
00296 DEFUNARY_TYPE(asin,OpAsin,Ref< Jacobian<double> >)
00297 
00298 template<>
00299 class UnaryOp<OpAcos,double> {
00300 public:
00301     typedef double exprType;
00302         typedef double valueType;
00303         typedef double derivType;
00304         static INLINE2 valueType value(double a) {
00305         return ::acos(a);
00306         }
00307         static INLINE2 derivType deriv(double a,double da) {
00308                 return -da/::sqrt(1-a*a);
00309         }
00310 };
00311 DEFUNARY_TYPE(acos,OpAcos,Ref< Jacobian<double> >)
00312 
00313 
00314 /*****************************************************************************/
00315 template<>
00316 class BinaryOp<OpAtan2,double,double> {
00317 public:
00318     typedef double exprType;
00319         typedef double valueType;
00320         typedef double derivType;
00321         INLINE2 static double value(double a,double b)  {
00322                 return ::atan2(a,b);
00323         }
00324         INLINE2 static double derivVV(double a,double da,double b,double db) {
00325         return (-a*db+b*da)/(a*a+b*b);
00326         }
00327         INLINE2 static derivType derivCV(double a,double b,double db) {
00328         return (-a*db)/(a*a+b*b);
00329         }
00330         INLINE2 static derivType derivVC(double a,double da,double b) {
00331         return (b*da)/(a*a+b*b);
00332         }
00333 };
00334 
00335 DEFBINARY_RTYPE(atan2,OpAtan2,Ref<Jacobian<double> >) 
00336 DEFBINARY_LTYPE(atan2,OpAtan2,Ref<Jacobian<double> >) 
00337 DEFBINARY_RLTYPE(atan2,OpAtan2,Ref<Jacobian<double> >,Ref<Jacobian<double> >)
00338 
00339 
00340 
00341 
00342 
00343 
00344 
00345 
00346 
00347 
00348 
00349 } // namespace
00350 #endif
00351 
00352 


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