MBSim |
Project Page |
00001 /* Copyright (C) 2004-2009 MBSim Development Team 00002 * 00003 * This library is free software; you can redistribute it and/or 00004 * modify it under the terms of the GNU Lesser General Public 00005 * License as published by the Free Software Foundation; either 00006 * version 2.1 of the License, or (at your option) any later version. 00007 * 00008 * This library is distributed in the hope that it will be useful, 00009 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00010 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 00011 * Lesser General Public License for more details. 00012 * 00013 * You should have received a copy of the GNU Lesser General Public 00014 * License along with this library; if not, write to the Free Software 00015 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 00016 * 00017 * Contact: martin.o.foerg@googlemail.com 00018 */ 00019 00020 #ifndef DIFFERENTIABLE_FUNCTION_LIBRARY_H_ 00021 #define DIFFERENTIABLE_FUNCTION_LIBRARY_H_ 00022 00023 #include <mbsim/utils/function.h> 00024 #include <casadi/symbolic/fx/sx_function.hpp> 00025 00026 namespace MBSim { 00027 00028 template <class Ret> 00029 class Casadi { 00030 }; 00031 00032 template <class Col> 00033 class Casadi<fmatvec::Vector<Col,double> > { 00034 public: 00035 static fmatvec::Vector<Col,double> cast(const CasADi::Matrix<double> &x) { 00036 fmatvec::Vector<Col,double> y(x.size1(),fmatvec::NONINIT); 00037 for(int i=0; i<x.size1(); i++) 00038 y.e(i) = x(i,0).toScalar(); 00039 return y; 00040 } 00041 }; 00042 00043 template <> 00044 class Casadi<double> { 00045 public: 00046 static double cast(const CasADi::Matrix<double> &x) { 00047 return x.toScalar(); 00048 } 00049 }; 00050 00051 template <class Ret> 00052 class CasadiFunction1 : public Function1<Ret,double> { 00053 CasADi::SXFunction f; 00054 public: 00055 CasadiFunction1(const CasADi::SXFunction &f_) : f(f_) { 00056 f.init(); 00057 } 00058 CasadiFunction1(const CasADi::FX &f_) : f(CasADi::SXFunction(f_)) { 00059 f.init(); 00060 } 00061 CasADi::SXFunction& getSXFunction() {return f;} 00062 00063 std::string getType() const { return "CasadiFunction1"; } 00064 00065 Ret operator()(const double& x_, const void * =NULL) { 00066 f.setInput(x_); 00067 f.evaluate(); 00068 return Casadi<Ret>::cast(f.output()); 00069 } 00070 }; 00071 00072 // The following classes may be faster when calculating the function value 00073 // together with the first and second derivative 00074 00075 // template <class Ret> 00076 // class CasadiFunctionDerivatives1 : public Function1<Ret,double> { 00077 // private: 00078 // CasADi::SXFunction f; 00079 // CasADi::FX fder1, fder2; 00080 // public: 00081 // CasadiFunctionDerivatives1(const CasADi::SXFunction &f_) : f(f_) { 00082 // f.init(); 00083 // fder1 = f.jacobian(); 00084 // fder1.init(); 00085 // fder2 = fder1.jacobian(); 00086 // fder2.init(); 00087 // } 00088 // std::string getType() const { return "CasadiDiffFunction"; } 00089 // 00090 // CasADi::FX& getFXFunction() {return fder2;} 00091 // 00092 // Ret operator()(const double& x_, const void * =NULL) { 00093 // fder2.setInput(x_); 00094 // fder2.evaluate(); 00095 // return Casadi<Ret>::cast(fder2.output(2)); 00096 // } 00097 // }; 00098 // 00099 // template <class Ret> 00100 // class CasadiFXFunction : public Function1<Ret,double> { 00101 // private: 00102 // CasADi::FX &f; 00103 // int index; 00104 // public: 00105 // CasadiFXFunction(CasADi::FX &f_, int index_=0) : f(f_), index(index_) {} 00106 // 00107 // Ret operator()(const double& x_, const void * =NULL) { 00108 // return Casadi<Ret>::cast(f.output(2-index)); 00109 // } 00110 // }; 00111 00112 } 00113 00114 #endif
1.8.0