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 <fmatvec/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 fmatvec::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
00118
00119
00120
00121
00122
00123 void init() {
00124
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
00128 BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg>::size2==1);
00129
00130 if(f.inputExpr(0).size2()!=1) throw MBSimError("Matrix parameter are not allowed.");
00131
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
00212 template<typename Ret, typename Arg1, typename Arg2>
00213 class SymbolicFunction<Ret(Arg1, Arg2)> : public fmatvec::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
00221
00222
00223
00224
00225
00226
00227
00228 void init() {
00229
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
00233 BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg1>::size2==1);
00234 BOOST_STATIC_ASSERT(fmatvec::StaticSize<Arg2>::size2==1);
00235
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
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 fmatvec::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
00377
00378
00379
00380
00381
00382
00383
00384
00385
00386
00387
00388
00389
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416 }
00417
00418 #endif