$search
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