00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
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
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
00124 BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg>::size2==1);
00125
00126 if(f.inputExpr(0).size2()!=1) THROW_MBSIMERROR("Matrix parameter are not allowed.");
00127
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
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
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
00216 BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg1>::size2==1);
00217 BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg2>::size2==1);
00218
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
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
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392 }
00393
00394 #endif