00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
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 ,double db) {
00062 return a*db;
00063 }
00064 INLINE2 static derivType derivVC(double ,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 ,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 ,double da,double ,double db) {
00131 return da-db;
00132 }
00133 INLINE2 static derivType derivCV(double ,double ,double db) {
00134 return db;
00135 }
00136 INLINE2 static derivType derivVC(double ,double da,double ) {
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 ,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 }
00350 #endif
00351
00352