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/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
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210 }
00211
00212 #endif