MBSim

Project Page
casadi_function.h
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