MBSim

Project Page
symbolic_functions.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/functions/function.h>
00024 #include <casadi/symbolic/fx/sx_function.hpp>
00025 #include <casadi/symbolic/matrix/matrix_tools.hpp>
00026 #include <mbxmlutilshelper/casadiXML.h>
00027 #include "mbsim/mbsim_event.h"
00028 #include <boost/static_assert.hpp>
00029 
00030 namespace MBSim {
00031 
00032   template <class Arg>
00033   class ToCasadi {
00034   };
00035 
00036   template <>
00037   class ToCasadi<double> {
00038     public:
00039       static double cast(const double &x) {
00040         return x;
00041       }
00042   };
00043 
00044   template <class Col>
00045   class ToCasadi<fmatvec::Vector<Col,double> > {
00046     public:
00047       static std::vector<double> cast(const fmatvec::Vector<Col,double> &x) {
00048         std::vector<double> y(x.size());
00049         for(int i=0; i<x.size(); i++)
00050           y[i] = x.e(i);
00051         return y; 
00052       }
00053   };
00054 
00055   template <class Ret>
00056   class FromCasadi {
00057     public:
00058       static Ret cast(const CasADi::Matrix<double> &x) {
00059         throw MBSimError("FromCasadi::cast not implemented for current type.");
00060         return Ret();
00061       }
00062   };
00063 
00064   template <class Col>
00065   class FromCasadi<fmatvec::Vector<Col,double> > {
00066     public:
00067       static fmatvec::Vector<Col,double> cast(const CasADi::Matrix<double> &x) {
00068         fmatvec::Vector<Col,double> y(x.size1(),fmatvec::NONINIT);
00069         for(int i=0; i<x.size1(); i++)
00070           y.e(i) = x(i,0).toScalar();
00071         return y;
00072       }
00073   };
00074 
00075   template <class Col>
00076   class FromCasadi<fmatvec::RowVector<Col,double> > {
00077     public:
00078       static fmatvec::RowVector<Col,double> cast(const CasADi::Matrix<double> &x) {
00079         fmatvec::RowVector<Col,double> y(x.size2(),fmatvec::NONINIT);
00080         for(int i=0; i<x.size2(); i++)
00081           y.e(i) = x(0,i).toScalar();
00082         return y;
00083       }
00084   };
00085 
00086   template <class Row>
00087   class FromCasadi<fmatvec::Matrix<fmatvec::General,Row,fmatvec::Var,double> > {
00088     public:
00089       static fmatvec::Matrix<fmatvec::General,Row,fmatvec::Var,double> cast(const CasADi::Matrix<double> &A) {
00090         fmatvec::Matrix<fmatvec::General,Row,fmatvec::Var,double> B(A.size1(),A.size2(),fmatvec::NONINIT);
00091         for(int i=0; i<A.size1(); i++)
00092           for(int j=0; j<A.size2(); j++)
00093             B.e(i,j) = A(i,j).toScalar();
00094         return B;
00095       }
00096   };
00097 
00098   template <>
00099   class FromCasadi<double> {
00100     public:
00101       static double cast(const CasADi::Matrix<double> &x) {
00102         return x.toScalar();
00103       }
00104   };
00105 
00106 template<typename Sig>
00107 class SymbolicFunction;
00108 
00109 template<typename Ret, typename Arg>
00110   class SymbolicFunction<Ret(Arg)> : public Function<Ret(Arg)> {
00111     CasADi::SXFunction f, pd, dd, pddd, pdpd;
00112     public:
00113     SymbolicFunction() {}
00114     SymbolicFunction(const CasADi::SXFunction &f_) : f(f_) {
00115       init();
00116     }
00117 //    SymbolicFunction(const CasADi::FX &f_) : f(CasADi::SXFunction(f_)) {
00118 //      f.init();
00119 //      pd = CasADi::SXFunction(f.inputExpr(),f.jac(0));
00120 //      pd.init();
00121 //    }
00122 
00123     void init() {
00124       // check function: number in inputs and outputs
00125       if(f.getNumInputs()!=1) THROW_MBSIMERROR("Function must have only 1 argument.");
00126       if(f.getNumOutputs()!=1) THROW_MBSIMERROR("Function must have only 1 output.");
00127       // check template arguments: only scalar and vector arguments are supported
00128       BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg>::size2==1);
00129       // check function: only scalar and vector arguments are supported
00130       if(f.inputExpr(0).size2()!=1) THROW_MBSIMERROR("Matrix parameter are not allowed.");
00131       // check function <-> template argument dimension
00132       if(fmatvec::StaticSize<Arg>::size1!=0 && f.inputExpr(0).size1()!=fmatvec::StaticSize<Arg>::size1)
00133         THROW_MBSIMERROR("The dimension of a parameter does not match.");
00134       if(fmatvec::StaticSize<Ret>::size1!=0 && f.outputExpr(0).size1()!=fmatvec::StaticSize<Ret>::size1)
00135         THROW_MBSIMERROR("The output dimension does not match.");
00136       if(fmatvec::StaticSize<Ret>::size2!=0 && f.outputExpr(0).size2()!=fmatvec::StaticSize<Ret>::size2)
00137         THROW_MBSIMERROR("The output dimension does not match.");
00138 
00139       f.init();
00140       pd = CasADi::SXFunction(f.inputExpr(),f.jac());
00141       pd.init();
00142       int nq = getArgSize();
00143       std::vector<CasADi::SX> sqd(nq);
00144       for(int i=0; i<nq; i++) {
00145         std::stringstream stream;
00146         stream << "qd" << i;
00147         sqd[i] = CasADi::SX(stream.str());
00148       }
00149       std::vector<CasADi::SXMatrix> input2(3);
00150       input2[0] = sqd;
00151       input2[1] = f.inputExpr(0);
00152       dd = CasADi::SXFunction(input2,f.jac().mul(sqd));
00153       dd.init();
00154       int n = f.outputExpr(0).size();
00155       CasADi::SXMatrix Jd(n,nq);
00156       for(int j=0; j<nq; j++) {
00157         Jd(CasADi::Slice(0,n),CasADi::Slice(j,j+1)) = pd.jac(0)(CasADi::Slice(j,nq*n,nq),CasADi::Slice(0,nq)).mul(sqd);
00158       }
00159       pddd = CasADi::SXFunction(input2,Jd);
00160       pddd.init();
00161       pdpd = CasADi::SXFunction(f.inputExpr(),pd.jac());
00162       pdpd.init();
00163     }
00164 
00165     CasADi::SXFunction& getSXFunction() {return f;} 
00166 
00167     typename fmatvec::Size<Arg>::type getArgSize() const {
00168       return f.inputExpr(0).size();
00169     }
00170 
00171     std::string getType() const { return "SymbolicFunction2"; }
00172 
00173     Ret operator()(const Arg& x) {
00174       f.setInput(ToCasadi<Arg>::cast(x),0);
00175       f.evaluate();
00176       return FromCasadi<Ret>::cast(f.output());
00177     }
00178 
00179     typename fmatvec::Der<Ret, Arg>::type parDer(const Arg &x) {
00180       pd.setInput(ToCasadi<Arg>::cast(x),0);
00181       pd.evaluate();
00182       return FromCasadi<typename fmatvec::Der<Ret, Arg>::type>::cast(pd.output());
00183     }
00184 
00185     Ret dirDer(const Arg &xd, const Arg &x) {
00186       dd.setInput(ToCasadi<Arg>::cast(xd),0);
00187       dd.setInput(ToCasadi<Arg>::cast(x),1);
00188       dd.evaluate();
00189       return FromCasadi<Ret>::cast(dd.output());
00190     }
00191 
00192     typename fmatvec::Der<Ret, Arg>::type parDerDirDer(const Arg &xd, const Arg &x) {
00193       pddd.setInput(ToCasadi<Arg>::cast(xd),0);
00194       pddd.setInput(ToCasadi<Arg>::cast(x),1);
00195       pddd.evaluate();
00196       return FromCasadi<typename fmatvec::Der<Ret, Arg>::type>::cast(pddd.output());
00197     }
00198 
00199     typename fmatvec::Der<typename fmatvec::Der<Ret, Arg>::type, Arg>::type parDerParDer(const Arg &x) {
00200       pdpd.setInput(ToCasadi<Arg>::cast(x),0);
00201       pdpd.evaluate();
00202       return FromCasadi<typename fmatvec::Der<typename fmatvec::Der<Ret, Arg>::type, Arg>::type>::cast(pdpd.output());
00203     }
00204 
00205     void initializeUsingXML(xercesc::DOMElement *element) {
00206       f=CasADi::createCasADiSXFunctionFromXML(element->getFirstElementChild());
00207       init();
00208     }
00209   };
00210 
00211 // TODO mit double vorbelegen
00212 template<typename Ret, typename Arg1, typename Arg2>
00213   class SymbolicFunction<Ret(Arg1, Arg2)> : public Function<Ret(Arg1, Arg2)> {
00214     CasADi::SXFunction f, pd1, pd2, pd1dd1, pd1pd2, pd2dd1, pd2pd2;
00215     public:
00216     SymbolicFunction() {}
00217     SymbolicFunction(const CasADi::SXFunction &f_) : f(f_) {
00218       init();
00219     }
00220 //    SymbolicFunction(const CasADi::FX &f_) : f(CasADi::SXFunction(f_)) {
00221 //      f.init();
00222 //      pd1 = CasADi::SXFunction(f.inputExpr(),f.jac(0));
00223 //      pd1.init();
00224 //      pd2 = CasADi::SXFunction(f.inputExpr(),f.jac(1));
00225 //      pd2.init();
00226 //    }
00227 
00228     void init() {
00229       // check function: number in inputs and outputs
00230       if(f.getNumInputs()!=2) THROW_MBSIMERROR("Function has must have exact 2 arguments.");
00231       if(f.getNumOutputs()!=1) THROW_MBSIMERROR("Function has must have only 1 output.");
00232       // check template arguments: only scalar and vector arguments are supported
00233       BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg1>::size2==1);
00234       BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg2>::size2==1);
00235       // check function: only scalar and vector arguments are supported
00236       if(f.inputExpr(0).size2()!=1) THROW_MBSIMERROR("Matrix parameter are not allowed.");
00237       if(f.inputExpr(1).size2()!=1) THROW_MBSIMERROR("Matrix parameter are not allowed.");
00238       // check function <-> template argument dimension
00239       if(fmatvec::StaticSize<Arg1>::size1!=0 && f.inputExpr(0).size1()!=fmatvec::StaticSize<Arg1>::size1)
00240         THROW_MBSIMERROR("The dimension of a parameter does not match.");
00241       if(fmatvec::StaticSize<Arg2>::size1!=0 && f.inputExpr(1).size1()!=fmatvec::StaticSize<Arg2>::size1)
00242         THROW_MBSIMERROR("The dimension of a parameter does not match.");
00243       if(fmatvec::StaticSize<Ret>::size1!=0 && f.outputExpr(0).size1()!=fmatvec::StaticSize<Ret>::size1)
00244         THROW_MBSIMERROR("The output dimension does not match.");
00245       if(fmatvec::StaticSize<Ret>::size2!=0 && f.outputExpr(0).size2()!=fmatvec::StaticSize<Ret>::size2)
00246         THROW_MBSIMERROR("The output dimension does not match.");
00247 
00248       f.init();
00249       pd1 = CasADi::SXFunction(f.inputExpr(),f.jac(0));
00250       pd1.init();
00251       pd2 = CasADi::SXFunction(f.inputExpr(),f.jac(1));
00252       pd2.init();
00253       int nq = getArg1Size();
00254       std::vector<CasADi::SX> sqd(nq);
00255       for(int i=0; i<nq; i++) {
00256         std::stringstream stream;
00257         stream << "qd" << i;
00258         sqd[i] = CasADi::SX(stream.str());
00259       }
00260       std::vector<CasADi::SXMatrix> input2(3);
00261       input2[0] = sqd;
00262       input2[1] = f.inputExpr(0);
00263       input2[2] = f.inputExpr(1);
00264       int n = f.outputExpr(0).size1();
00265       CasADi::SXMatrix Jd1(n,nq);
00266       CasADi::SXMatrix Jd2(n,nq);
00267       for(int j=0; j<nq; j++) {
00268         Jd1(CasADi::Slice(0,n),CasADi::Slice(j,j+1)) = pd1.jac(0)(CasADi::Slice(j,nq*n,nq),CasADi::Slice(0,nq)).mul(sqd);
00269         Jd2(CasADi::Slice(0,n),CasADi::Slice(j,j+1)) = pd1.jac(1)(CasADi::Slice(j,nq*n,nq),CasADi::Slice(0,1));
00270       }
00271       pd1dd1 = CasADi::SXFunction(input2,Jd1);
00272       pd1dd1.init();
00273       pd1pd2 = CasADi::SXFunction(f.inputExpr(),Jd2);
00274       pd1pd2.init();
00275 
00276       CasADi::SXMatrix djT1 = pd2.jac(0).mul(sqd);
00277       CasADi::SXMatrix djT2 = pd2.jac(1);
00278       pd2dd1 = CasADi::SXFunction(input2,djT1);
00279       pd2dd1.init();
00280       pd2pd2 = CasADi::SXFunction(f.inputExpr(),djT2);
00281       pd2pd2.init();
00282     }
00283 
00284     CasADi::SXFunction& getSXFunction() {return f;} 
00285 
00286     typename fmatvec::Size<Arg1>::type getArg1Size() const {
00287       return f.inputExpr(0).size();
00288     }
00289 
00290     typename fmatvec::Size<Arg2>::type getArg2Size() const {
00291       return f.inputExpr(1).size();
00292     }
00293 
00294     std::string getType() const { return "SymbolicFunction2"; }
00295 
00296     Ret operator()(const Arg1& x1, const Arg2& x2) {
00297       f.setInput(ToCasadi<Arg1>::cast(x1),0);
00298       f.setInput(ToCasadi<Arg2>::cast(x2),1);
00299       f.evaluate();
00300       return FromCasadi<Ret>::cast(f.output());
00301     }
00302 
00303     typename fmatvec::Der<Ret, Arg1>::type parDer1(const Arg1 &x1, const Arg2 &x2) {
00304       pd1.setInput(ToCasadi<Arg1>::cast(x1),0);
00305       pd1.setInput(ToCasadi<Arg2>::cast(x2),1);
00306       pd1.evaluate();
00307       return FromCasadi<typename fmatvec::Der<Ret, Arg1>::type>::cast(pd1.output());
00308     }
00309 
00310     typename fmatvec::Der<Ret, Arg2>::type parDer2(const Arg1 &x1, const Arg2 &x2) {
00311       pd2.setInput(ToCasadi<Arg1>::cast(x1),0);
00312       pd2.setInput(ToCasadi<Arg2>::cast(x2),1);
00313       pd2.evaluate();
00314       return FromCasadi<typename fmatvec::Der<Ret, Arg2>::type>::cast(pd2.output());
00315     }
00316 
00317     typename fmatvec::Der<Ret, Arg1>::type parDer1DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) {
00318       pd1dd1.setInput(ToCasadi<Arg1>::cast(xd1),0);
00319       pd1dd1.setInput(ToCasadi<Arg1>::cast(x1),1);
00320       pd1dd1.setInput(ToCasadi<Arg2>::cast(x2),2);
00321       pd1dd1.evaluate();
00322       return FromCasadi<typename fmatvec::Der<Ret, Arg1>::type>::cast(pd1dd1.output());
00323     }
00324 
00325     typename fmatvec::Der<typename fmatvec::Der<Ret, Arg1>::type, Arg2>::type parDer1ParDer2(const Arg1 &x1, const Arg2 &x2) {
00326       pd1pd2.setInput(ToCasadi<Arg1>::cast(x1),0);
00327       pd1pd2.setInput(ToCasadi<Arg2>::cast(x2),1);
00328       pd1pd2.evaluate();
00329       return FromCasadi<typename fmatvec::Der<typename fmatvec::Der<Ret, Arg1>::type, Arg2>::type>::cast(pd1pd2.output());
00330     }
00331 
00332     typename fmatvec::Der<Ret, Arg2>::type parDer2DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) {
00333       pd2dd1.setInput(ToCasadi<Arg1>::cast(xd1),0);
00334       pd2dd1.setInput(ToCasadi<Arg1>::cast(x1),1);
00335       pd2dd1.setInput(ToCasadi<Arg2>::cast(x2),2);
00336       pd2dd1.evaluate();
00337       return FromCasadi<typename fmatvec::Der<Ret, Arg2>::type>::cast(pd2dd1.output());
00338     }
00339 
00340     typename fmatvec::Der<typename fmatvec::Der<Ret, Arg2>::type, Arg2>::type parDer2ParDer2(const Arg1 &x1, const Arg2 &x2) {
00341       pd2pd2.setInput(ToCasadi<Arg1>::cast(x1),0);
00342       pd2pd2.setInput(ToCasadi<Arg2>::cast(x2),1);
00343       pd2pd2.evaluate();
00344       return FromCasadi<typename fmatvec::Der<typename fmatvec::Der<Ret, Arg2>::type, Arg2>::type>::cast(pd2pd2.output());
00345     }
00346 
00347     void initializeUsingXML(xercesc::DOMElement *element) {
00348       f=CasADi::createCasADiSXFunctionFromXML(element->getFirstElementChild());
00349       init();
00350     }
00351   };
00352 
00353   template <class Ret, class Arg1, class Arg2, class Arg3>
00354   class SymbolicFunction3 : public Function<Ret(Arg1,Arg2,Arg3)> {
00355     CasADi::SXFunction f;
00356     public:
00357     SymbolicFunction3(const CasADi::SXFunction &f_) : f(f_) {
00358       f.init();
00359     }
00360     SymbolicFunction3(const CasADi::FX &f_) : f(CasADi::SXFunction(f_)) {
00361       f.init();
00362     }
00363     CasADi::SXFunction& getSXFunction() {return f;} 
00364 
00365     std::string getType() const { return "SymbolicFunction3"; }
00366 
00367     Ret operator()(const Arg1& x1, const Arg2& x2, const Arg3& x3, const void * =NULL) {
00368       f.setInput(ToCasadi<Arg1>::cast(x1),0);
00369       f.setInput(ToCasadi<Arg2>::cast(x2),1);
00370       f.setInput(ToCasadi<Arg3>::cast(x3),2);
00371       f.evaluate();
00372       return FromCasadi<Ret>::cast(f.output());
00373     }
00374   };
00375 
00376   // The following classes may be faster when calculating the function value
00377   // together with the first and second derivative
00378  
00379 //  template <class Ret>
00380 //  class SymbolicFunctionDerivatives1 : public Function1<Ret,double> {
00381 //    private:
00382 //      CasADi::SXFunction f;
00383 //      CasADi::FX fder1, fder2;
00384 //    public:
00385 //    SymbolicFunctionDerivatives1(const CasADi::SXFunction &f_) : f(f_) {
00386 //      f.init();
00387 //      fder1 = f.jacobian();
00388 //      fder1.init();
00389 //      fder2 = fder1.jacobian();
00390 //      fder2.init();
00391 //    }
00392 //    std::string getType() const { return "CasadiDiffFunction"; }
00393 //
00394 //    CasADi::FX& getFXFunction() {return fder2;} 
00395 //
00396 //    Ret operator()(const double& x_, const void * =NULL) {
00397 //      fder2.setInput(x_);
00398 //      fder2.evaluate();
00399 //      return FromCasadi<Ret>::cast(fder2.output(2));
00400 //    }
00401 //  };
00402 //
00403 //  template <class Ret>
00404 //  class SymbolicFXFunction : public Function1<Ret,double> {
00405 //    private:
00406 //    CasADi::FX &f;
00407 //    int index;
00408 //    public:
00409 //    SymbolicFXFunction(CasADi::FX &f_, int index_=0) : f(f_), index(index_) {}
00410 //
00411 //    Ret operator()(const double& x_, const void * =NULL) {
00412 //      return FromCasadi<Ret>::cast(f.output(2-index));
00413 //    }
00414 //  };
00415 
00416 }
00417 
00418 #endif