$search
00001 /*************************************************************************** 00002 JacobianExpr.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 #ifndef KDL_JACOBIANEXPR_H 00028 #define KDL_JACOBIANEXPR_H 00029 00030 #include <kdl/traits.h> 00031 #include <kdl/jacobian.hpp> 00032 #include <iostream> 00033 00034 00035 #define INLINE2 00036 namespace KDL { 00037 00048 template <typename T> 00049 class Ref { 00050 public: 00051 typedef typename T::exprType exprType; 00052 typedef typename T::derivType derivType; 00053 typedef typename T::valueType valueType; 00054 const T* ref; 00055 00056 00057 INLINE Ref(const T& _ref):ref(&_ref) {} 00058 INLINE Ref(const Ref<T>& arg):ref(arg.ref) {} 00059 Ref<T>& operator = (const Ref<T>& arg) { 00060 ref = arg.ref; 00061 } 00062 INLINE derivType deriv(int i) const{ 00063 return ref->deriv(i); 00064 } 00065 INLINE valueType value() const { 00066 return ref->value(); 00067 } 00068 INLINE int nrOfDeriv() const { 00069 return ref->nrOfDeriv(); 00070 } 00071 INLINE JacobianBitset getDerivs() const { 00072 return ref->getDerivs(); 00073 } 00074 INLINE bool hasDeriv(int i) const { 00075 return ref->hasDeriv(i); 00076 } 00077 }; 00078 00079 00122 template <typename OpID,typename A> 00123 class UnaryOp { 00124 }; 00125 00126 00133 template <class A,class OpID> 00134 class Unary { 00135 public: 00136 typedef UnaryOp<OpID,typename A::exprType> MyOp; 00137 typedef typename MyOp::exprType exprType; 00138 typedef typename MyOp::valueType valueType; 00139 typedef typename MyOp::derivType derivType; 00140 A a; 00141 00142 INLINE Unary(const A& _a):a(_a) {} 00143 INLINE derivType deriv(int i) const { 00144 assert(a.hasDeriv(i)); // should not be called when a is constant. 00145 return MyOp::deriv(a.value(),a.deriv(i)); 00146 } 00147 INLINE valueType value() const { 00148 return MyOp::value(a.value()); 00149 } 00150 INLINE JacobianBitset getDerivs() const { 00151 return a.getDerivs(); 00152 } 00153 INLINE bool hasDeriv(int i) const { 00154 return a.hasDeriv(i); 00155 } 00159 INLINE int nrOfDeriv() const { 00160 return a.nrOfDeriv(); 00161 } 00162 }; 00163 00164 #if 0 // OBSOLETE 00165 00171 template <class A,class OpID> 00172 class Unary { 00173 public: 00174 typedef UnaryOp<OpID,typename A::valueType> MyOp; 00175 typedef typename MyOp::valueType valueType; 00176 typedef typename MyOp::derivType derivType; 00177 A a; 00178 00179 INLINE Unary(const A& _a):a(_a) {} 00180 INLINE derivType deriv(int i) const { 00181 assert(!a.isConstant()); // should not be called when a is constant. 00182 return MyOp::deriv(a.value(),a.deriv(i)); 00183 } 00184 INLINE valueType value() const { 00185 return MyOp::value(a.value()); 00186 } 00187 INLINE JacobianBitset getDerivs() const { 00188 return a.getDerivs(); 00189 } 00190 INLINE bool hasDeriv(int i) const { 00191 return a.hasDeriv(i); 00192 } 00193 INLINE int nrOfDeriv() const { 00194 return a.nrOfDeriv(); 00195 } 00196 }; 00197 00198 #endif 00199 00200 00201 template <typename ID,typename A,typename B> 00202 class BinaryOp { 00203 public: 00204 }; 00205 00206 00207 00259 #if 0 //OBSOLETE 00260 template <class A,class B,class OpID> 00261 class Binary { 00262 public: 00263 typedef BinaryOp<OpID,typename A::valueType,typename B::valueType> MyOp; 00264 typedef typename MyOp::valueType valueType; 00265 typedef typename MyOp::derivType derivType; 00266 A a; 00267 B b; 00268 00269 INLINE Binary(const A& _a,const B& _b):a(_a),b(_b) {} 00270 INLINE derivType deriv(int i) const { 00271 if (a.isConstant()) { 00272 // deriv should not be called when the result is constant 00273 assert(!b.isConstant()); 00274 return MyOp::derivCV(a.value(),b.value(),b.deriv(i)); 00275 } 00276 if (b.isConstant()) { 00277 return MyOp::derivVC(a.value(),a.deriv(i),b.value()); 00278 } 00279 return MyOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)); 00280 } 00281 INLINE valueType value() const { 00282 return MyOp::value(a.value(),b.value()); 00283 } 00284 INLINE int nrOfDeriv() const { 00285 if (!a.isConstant()) return a.nrOfDeriv(); 00286 if (!b.isConstant()) return b.nrOfDeriv(); 00287 return 0; 00288 } 00289 INLINE bool isConstant() const { 00290 return a.isConstant() && b.isConstant(); 00291 } 00292 }; 00293 #endif // OBSOLETE 00294 00295 template <class A,class B,class OpID> 00296 class Binary { 00297 public: 00298 typedef BinaryOp<OpID,typename A::exprType,typename B::exprType> MyOp; 00299 00300 typedef typename MyOp::exprType exprType; 00301 typedef typename MyOp::valueType valueType; 00302 typedef typename MyOp::derivType derivType; 00303 A a; 00304 B b; 00305 JacobianBitset hasderiv; 00306 00307 INLINE Binary(const A& _a,const B& _b):a(_a),b(_b),hasderiv(_a.getDerivs()|_b.getDerivs()) {} 00308 INLINE derivType deriv(int i) const { 00309 if (a.hasDeriv(i)) { 00310 if (b.hasDeriv(i)) { 00311 return MyOp::derivVV(a.value(),a.deriv(i),b.value(),b.deriv(i)); 00312 } else { 00313 return MyOp::derivVC(a.value(),a.deriv(i),b.value()); 00314 } 00315 } else { 00316 assert(b.hasDeriv(i)); 00317 return MyOp::derivCV(a.value(),b.value(),b.deriv(i)); 00318 } 00319 } 00320 INLINE valueType value() const { 00321 return MyOp::value(a.value(),b.value()); 00322 } 00323 INLINE int nrOfDeriv() const { 00324 int r = a.nrOfDeriv(); 00325 if (r==0) r = b.nrOfDeriv(); 00326 return r; 00327 } 00328 INLINE JacobianBitset getDerivs() const { 00329 return hasderiv; 00330 } 00331 INLINE bool hasDeriv(int i) const { 00332 return hasderiv[i]; 00333 } 00334 }; 00335 00342 #define DEFUNARY_EXPR(NAME,IMPL) \ 00343 template <typename A>\ 00344 INLINE Expr< Unary< A, IMPL> >\ 00345 NAME (const Expr<A>& a) {\ 00346 typedef Unary< A, IMPL> MyType;\ 00347 return MyType(a.elem);\ 00348 } 00349 00358 #define DEFUNARY_TYPE(NAME,IMPL,TYPE) \ 00359 INLINE Expr< Unary< TYPE, IMPL> >\ 00360 NAME(const TYPE& a) {\ 00361 typedef Unary< TYPE , IMPL> MyType;\ 00362 return MyType(a);\ 00363 } 00364 00365 00366 00373 #define DEFBINARY_EXPR(NAME, IMPL) \ 00374 template <typename A,typename B>\ 00375 INLINE Expr< Binary< A, B, IMPL > >\ 00376 NAME (const Expr<A>& a,const Expr<B>& b) {\ 00377 typedef Binary<A,B, IMPL> MyType;\ 00378 return MyType(a.elem,b.elem);\ 00379 } 00380 00381 00390 #define DEFBINARY_RTYPE(NAME, IMPL, RTYPE) \ 00391 template <typename A>\ 00392 INLINE Expr< Binary< A, RTYPE, IMPL > >\ 00393 NAME (const Expr<A>& a,const RTYPE& b) {\ 00394 typedef Binary<A,RTYPE, IMPL> MyType;\ 00395 return MyType(a.elem,b);\ 00396 } 00397 00406 #define DEFBINARY_LTYPE(NAME, IMPL, LTYPE) \ 00407 template <typename B>\ 00408 INLINE Expr< Binary< LTYPE , B, IMPL > >\ 00409 NAME (const LTYPE& a,const Expr<B>& b) {\ 00410 typedef Binary< LTYPE , B, IMPL> MyType;\ 00411 return MyType(a,b.elem);\ 00412 } 00413 00423 #define DEFBINARY_RLTYPE(NAME, IMPL, LTYPE,RTYPE) \ 00424 INLINE Expr< Binary< LTYPE, RTYPE, IMPL > >\ 00425 NAME (const LTYPE& a,const RTYPE& b) {\ 00426 typedef Binary< LTYPE, RTYPE, IMPL > MyType;\ 00427 return MyType(a,b);\ 00428 } 00429 00430 /* obsolete 00431 #define DEFBINARY(NAME,IMPL,LTYPE,RTYPE)\ 00432 DEFBINARY_RTYPE(NAME,IMPL,RTYPE) \ 00433 DEFBINARY_LTYPE(NAME,IMPL,LTYPE) \ 00434 DEFBINARY_RLTYPE(NAME,IMPL,LTYPE,RTYPE) 00435 */ 00436 00437 00438 /*****************************************************************************/ 00439 00443 template <typename T> 00444 std::ostream& operator << (std::ostream& os, const Jacobian<T>& F) { 00445 os << "[ Value " << std::endl; 00446 os << F.value() << std::endl; 00447 if (F.hasDerivs()) { 00448 os << "Derivative[" << std::endl; 00449 for (int i = 0;i<F.nrOfDeriv();++i) 00450 if (F.hasDeriv(i)) { 00451 os << F.deriv(i) <<std::endl; 00452 } else { 00453 os << "CONSTANT" <<std::endl; 00454 } 00455 os << "]"; 00456 } else { 00457 os << "ALL CONSTANT" << std::endl; 00458 } 00459 os << "]"; 00460 return os; 00461 } 00462 00463 00464 // Some utility function to easily specify a constant : 00465 template <typename T> 00466 Jacobian<T> Constant(const T& arg) { 00467 return arg; 00468 } 00469 00470 /*****************************************************************************/ 00471 /* DEFINE Marker-classes for the operations */ 00472 /* These classes associate a TYPE with an OPERATOR */ 00473 /*****************************************************************************/ 00474 00475 // Binary operators : 00476 class OpMult{}; 00477 DEFBINARY_EXPR(operator*,OpMult); 00478 class OpDiv{}; 00479 DEFBINARY_EXPR(operator/,OpDiv); 00480 class OpAdd{}; 00481 DEFBINARY_EXPR(operator+,OpAdd); 00482 class OpSub{}; 00483 DEFBINARY_EXPR(operator-,OpSub); 00484 class OpDot {}; 00485 DEFBINARY_EXPR(dot,OpDot); 00486 00487 class OpDiff {}; 00488 DEFBINARY_EXPR(Diff,OpDiff); 00489 00490 class OpAtan2 {}; 00491 DEFBINARY_EXPR(atan2,OpAtan2); 00492 00493 class OpRefPoint{}; 00494 DEFBINARY_EXPR(RefPoint,OpRefPoint); 00495 00496 // Unary operators : 00497 class OpNegate {}; 00498 DEFUNARY_EXPR(operator-,OpNegate); 00499 class OpSin {}; 00500 DEFUNARY_EXPR(sin,OpSin); 00501 class OpCos {}; 00502 DEFUNARY_EXPR(cos,OpCos); 00503 class OpTan {}; 00504 DEFUNARY_EXPR(tan,OpTan); 00505 class OpExp {}; 00506 DEFUNARY_EXPR(exp,OpExp); 00507 class OpLog {}; 00508 DEFUNARY_EXPR(log,OpLog); 00509 class OpSqrt {}; 00510 DEFUNARY_EXPR(sqrt,OpSqrt); 00511 class OpAtan {}; 00512 DEFUNARY_EXPR(atan,OpAtan); 00513 class OpAsin {}; 00514 DEFUNARY_EXPR(asin,OpAsin); 00515 class OpAcos {}; 00516 DEFUNARY_EXPR(acos,OpAcos); 00517 class OpAbs {}; 00518 DEFUNARY_EXPR(abs,OpAbs); 00519 00520 class OpNorm {}; 00521 DEFUNARY_EXPR(norm,OpNorm); 00522 class OpInverse {}; 00523 DEFUNARY_EXPR(Inverse,OpInverse); 00524 class OpRotX {}; 00525 DEFUNARY_EXPR(RotX,OpRotX); 00526 class OpRotY {}; 00527 DEFUNARY_EXPR(RotY,OpRotY); 00528 class OpRotZ {}; 00529 DEFUNARY_EXPR(RotZ,OpRotZ); 00530 /* 00531 class OpSelectX {}; 00532 DEFUNARY_EXPR(SelectX,OpSelectX); 00533 class OpSelectY {}; 00534 DEFUNARY_EXPR(SelectY,OpSelectY); 00535 class OpSelectZ {}; 00536 DEFUNARY_EXPR(SelectZ,OpSelectZ); 00537 */ 00538 00539 class OpRotation {}; 00540 DEFUNARY_EXPR(RotMat,OpRotation); 00541 00542 class OpOrigin {}; 00543 DEFUNARY_EXPR(Origin,OpOrigin); 00544 00545 class OpUnitX {}; 00546 DEFUNARY_EXPR(UnitX,OpUnitX); 00547 00548 class OpUnitY {}; 00549 DEFUNARY_EXPR(UnitY,OpUnitY); 00550 00551 class OpUnitZ {}; 00552 DEFUNARY_EXPR(UnitZ,OpUnitZ); 00553 00554 00555 class OpCoordX {}; 00556 DEFUNARY_EXPR(CoordX,OpCoordX); 00557 00558 class OpCoordY {}; 00559 DEFUNARY_EXPR(CoordY,OpCoordY); 00560 00561 class OpCoordZ {}; 00562 DEFUNARY_EXPR(CoordZ,OpCoordZ); 00563 00564 00565 00566 } // namespace ORO_GEOMETRY 00567 #endif 00568 00569 00570