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