MBSim

Project Page
symbolic_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 SYMBOLIC_FUNCTION_H_
00021 #define SYMBOLIC_FUNCTION_H_
00022 
00023 #include <mbsim/utils/function.h>
00024 #include <casadi/symbolic/fx/sx_function.hpp>
00025 #include "casadi/symbolic/matrix/matrix_tools.hpp"
00026 #include "mbxmlutilstinyxml/casadiXML.h"
00027 
00028 namespace MBSim {
00029 
00030   template <class Arg>
00031   class ToCasadi {
00032   };
00033 
00034   template <>
00035   class ToCasadi<double> {
00036     public:
00037       static double cast(const double &x) {
00038         return x;
00039       }
00040   };
00041 
00042   template <class Col>
00043   class ToCasadi<fmatvec::Vector<Col,double> > {
00044     public:
00045       static std::vector<double> cast(const fmatvec::Vector<Col,double> &x) {
00046         std::vector<double> y(x.size());
00047         for(int i=0; i<x.size(); i++)
00048           y[i] = x.e(i);
00049         return y; 
00050       }
00051   };
00052 
00053   template <class Ret>
00054   class FromCasadi {
00055   };
00056 
00057   template <class Col>
00058   class FromCasadi<fmatvec::Vector<Col,double> > {
00059     public:
00060       static fmatvec::Vector<Col,double> cast(const CasADi::Matrix<double> &x) {
00061         fmatvec::Vector<Col,double> y(x.size1(),fmatvec::NONINIT);
00062         for(int i=0; i<x.size1(); i++)
00063           y.e(i) = x(i,0).toScalar();
00064         return y;
00065       }
00066   };
00067 
00068   template <class Row>
00069   class FromCasadi<fmatvec::Matrix<fmatvec::General,Row,fmatvec::Var,double> > {
00070     public:
00071       static fmatvec::Matrix<fmatvec::General,Row,fmatvec::Var,double> cast(const CasADi::Matrix<double> &A) {
00072         fmatvec::Matrix<fmatvec::General,Row,fmatvec::Var,double> B(A.size1(),A.size2(),fmatvec::NONINIT);
00073         for(int i=0; i<A.size1(); i++)
00074           for(int j=0; j<A.size2(); j++)
00075             B.e(i,j) = A(i,j).toScalar();
00076         return B;
00077       }
00078   };
00079 
00080   template <>
00081   class FromCasadi<double> {
00082     public:
00083       static double cast(const CasADi::Matrix<double> &x) {
00084         return x.toScalar();
00085       }
00086   };
00087 
00088   template <class Ret, class Arg>
00089   class SymbolicFunction1 : public Function1<Ret,Arg> {
00090     CasADi::SXFunction f;
00091     public:
00092     SymbolicFunction1() {}
00093     SymbolicFunction1(const CasADi::SXFunction &f_) : f(f_) {
00094       f.init();
00095     }
00096     SymbolicFunction1(const CasADi::FX &f_) : f(CasADi::SXFunction(f_)) {
00097       f.init();
00098     }
00099     CasADi::SXFunction& getSXFunction() {return f;} 
00100 
00101     std::string getType() const { return "SymbolicFunction1"; }
00102 
00103     Ret operator()(const Arg& x, const void * =NULL) {
00104       f.setInput(ToCasadi<Arg>::cast(x));
00105       f.evaluate();
00106       return FromCasadi<Ret>::cast(f.output());
00107     }
00108 
00109     void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00110       f=CasADi::createCasADiSXFunctionFromXML(element->FirstChildElement());
00111       f.init();
00112       assert(f.getNumInputs()==1);
00113       assert(f.getNumOutputs()==1);
00114     }
00115   };
00116 
00117   template <class Ret, class Arg1, class Arg2>
00118   class SymbolicFunction2 : public Function2<Ret,Arg1,Arg2> {
00119     CasADi::SXFunction f;
00120     public:
00121     SymbolicFunction2() {}
00122     SymbolicFunction2(const CasADi::SXFunction &f_) : f(f_) {
00123       f.init();
00124     }
00125     SymbolicFunction2(const CasADi::FX &f_) : f(CasADi::SXFunction(f_)) {
00126       f.init();
00127     }
00128     CasADi::SXFunction& getSXFunction() {return f;} 
00129 
00130     std::string getType() const { return "SymbolicFunction2"; }
00131 
00132     Ret operator()(const Arg1& x1, const Arg2& x2, const void * =NULL) {
00133       f.setInput(ToCasadi<Arg1>::cast(x1),0);
00134       f.setInput(ToCasadi<Arg2>::cast(x2),1);
00135       f.evaluate();
00136       return FromCasadi<Ret>::cast(f.output());
00137     }
00138 
00139     void initializeUsingXML(MBXMLUtils::TiXmlElement *element) {
00140       f=CasADi::createCasADiSXFunctionFromXML(element->FirstChildElement());
00141       f.init();
00142       assert(f.getNumInputs()==2);
00143       assert(f.getNumOutputs()==1);
00144     }
00145   };
00146 
00147   template <class Ret, class Arg1, class Arg2, class Arg3>
00148   class SymbolicFunction3 : public Function3<Ret,Arg1,Arg2,Arg3> {
00149     CasADi::SXFunction f;
00150     public:
00151     SymbolicFunction3(const CasADi::SXFunction &f_) : f(f_) {
00152       f.init();
00153     }
00154     SymbolicFunction3(const CasADi::FX &f_) : f(CasADi::SXFunction(f_)) {
00155       f.init();
00156     }
00157     CasADi::SXFunction& getSXFunction() {return f;} 
00158 
00159     std::string getType() const { return "SymbolicFunction3"; }
00160 
00161     Ret operator()(const Arg1& x1, const Arg2& x2, const Arg3& x3, const void * =NULL) {
00162       f.setInput(ToCasadi<Arg1>::cast(x1),0);
00163       f.setInput(ToCasadi<Arg2>::cast(x2),1);
00164       f.setInput(ToCasadi<Arg3>::cast(x3),2);
00165       f.evaluate();
00166       return FromCasadi<Ret>::cast(f.output());
00167     }
00168   };
00169 
00170   // The following classes may be faster when calculating the function value
00171   // together with the first and second derivative
00172  
00173 //  template <class Ret>
00174 //  class SymbolicFunctionDerivatives1 : public Function1<Ret,double> {
00175 //    private:
00176 //      CasADi::SXFunction f;
00177 //      CasADi::FX fder1, fder2;
00178 //    public:
00179 //    SymbolicFunctionDerivatives1(const CasADi::SXFunction &f_) : f(f_) {
00180 //      f.init();
00181 //      fder1 = f.jacobian();
00182 //      fder1.init();
00183 //      fder2 = fder1.jacobian();
00184 //      fder2.init();
00185 //    }
00186 //    std::string getType() const { return "CasadiDiffFunction"; }
00187 //
00188 //    CasADi::FX& getFXFunction() {return fder2;} 
00189 //
00190 //    Ret operator()(const double& x_, const void * =NULL) {
00191 //      fder2.setInput(x_);
00192 //      fder2.evaluate();
00193 //      return FromCasadi<Ret>::cast(fder2.output(2));
00194 //    }
00195 //  };
00196 //
00197 //  template <class Ret>
00198 //  class SymbolicFXFunction : public Function1<Ret,double> {
00199 //    private:
00200 //    CasADi::FX &f;
00201 //    int index;
00202 //    public:
00203 //    SymbolicFXFunction(CasADi::FX &f_, int index_=0) : f(f_), index(index_) {}
00204 //
00205 //    Ret operator()(const double& x_, const void * =NULL) {
00206 //      return FromCasadi<Ret>::cast(f.output(2-index));
00207 //    }
00208 //  };
00209 
00210 }
00211 
00212 #endif