All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Pages
symbolic_function.h
1 /* Copyright (C) 2004-2009 MBSim Development Team
2  *
3  * This library is free software; you can redistribute it and/or
4  * modify it under the terms of the GNU Lesser General Public
5  * License as published by the Free Software Foundation; either
6  * version 2.1 of the License, or (at your option) any later version.
7  *
8  * This library is distributed in the hope that it will be useful,
9  * but WITHOUT ANY WARRANTY; without even the implied warranty of
10  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
11  * Lesser General Public License for more details.
12  *
13  * You should have received a copy of the GNU Lesser General Public
14  * License along with this library; if not, write to the Free Software
15  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
16  *
17  * Contact: martin.o.foerg@googlemail.com
18  */
19 
20 #ifndef SYMBOLIC_FUNCTION_H_
21 #define SYMBOLIC_FUNCTION_H_
22 
23 #include <mbsim/functions/function.h>
24 #include <mbxmlutilshelper/casadiXML.h>
25 #include "mbsim/mbsim_event.h"
26 #include <casadi/core/function/function.hpp>
27 
28 namespace MBSim {
29 
31  casadi::SX jac(const casadi::SX &f, const casadi::SX &arg) {
32  if(f.size2()>1) { // f is a matrix
33  if(arg.size1()==1) { // arg is a scalar -> jac is a matrix but this cannot be handled by casadi directly
34  // reshape to column vector -> calcualte jacobian using casadi -> reshape back to original shape
35  casadi::SX fr=casadi::SX::reshape(f, f.size1()*f.size2(), 1);
36  casadi::SX frj=casadi::SX::jacobian(fr, arg);
37  return casadi::SX::reshape(frj, f.size1(), f.size2());
38  }
39  else { // arg is a vector -> not possible (would be a tensor of order 3 which cannot be handled by casadi and fmatvec)
40  return casadi::SX::nan(); // return a scalar NaN -> will throw later if this is tried to evaluate
41  }
42  }
43  else // f is a vector -> jac may get a matrix if arg is a vector (handled by casadi)
44  return casadi::SX::jacobian(f, arg);
45  }
46 
48  casadi::DM c(const double &x) {
49  return casadi::DM(x);
50  }
51 
53  template<class Col>
54  casadi::DM c(const fmatvec::Vector<Col,double> &x) {
55  casadi::DM y(x.size(), 1);
56  for(int i=0; i<x.size(); i++)
57  y(i) = x.e(i);
58  return y;
59  }
60 
61  // helper classes to convert casadi::DM to any value
62  template <class Ret>
63  class FromCasadi {
64  public:
65  static Ret cast(const casadi::Matrix<double> &x) {
66  throw MBSimError("FromCasadi::cast not implemented for current type.");
67  }
68  };
69 
70  template <class Col>
71  class FromCasadi<fmatvec::Vector<Col,double> > {
72  public:
73  static fmatvec::Vector<Col,double> cast(const casadi::Matrix<double> &x) {
74  fmatvec::Vector<Col,double> y(x.size1(),fmatvec::NONINIT);
75  for(int i=0; i<x.size1(); i++)
76  y.e(i) = x(i,0).scalar();
77  return y;
78  }
79  };
80 
81  template <class Col>
82  class FromCasadi<fmatvec::RowVector<Col,double> > {
83  public:
84  static fmatvec::RowVector<Col,double> cast(const casadi::Matrix<double> &x) {
85  fmatvec::RowVector<Col,double> y(x.size2(),fmatvec::NONINIT);
86  for(int i=0; i<x.size2(); i++)
87  y.e(i) = x(0,i).scalar();
88  return y;
89  }
90  };
91 
92  template <class Row, class Col>
93  class FromCasadi<fmatvec::Matrix<fmatvec::General,Row,Col,double> > {
94  public:
95  static fmatvec::Matrix<fmatvec::General,Row,Col,double> cast(const casadi::Matrix<double> &A) {
96  fmatvec::Matrix<fmatvec::General,Row,Col,double> B(A.size1(),A.size2(),fmatvec::NONINIT);
97  for(int i=0; i<A.size1(); i++)
98  for(int j=0; j<A.size2(); j++)
99  B.e(i,j) = A(i,j).scalar();
100  return B;
101  }
102  };
103 
104  template <>
105  class FromCasadi<double> {
106  public:
107  static double cast(const casadi::Matrix<double> &x) {
108  return x.scalar();
109  }
110  };
111 
113  template<class T>
114  T c(const casadi::DM &x) {
115  return FromCasadi<T>::cast(x);
116  }
117 
118  template<typename Sig> class SymbolicFunction;
119 
120  template<typename Ret, typename Arg>
121  class SymbolicFunction<Ret(Arg)> : public Function<Ret(Arg)> {
122  private:
124  casadi::SX ret, arg;
125  mutable casadi::SX pd_;
126  casadi::SX pddd_, pdpd_;
127  casadi::Function f, pd, dd, pddd, pdpd, dddd;
128  public:
129  SymbolicFunction() {}
130  SymbolicFunction(const casadi::SX &ret_, const casadi::SX &arg_) : ret(ret_), arg(arg_) {
131  checkFunctionIODim();
132  }
133 
134  void init(Element::InitStage stage) {
136  if(stage == Element::preInit) {
137  casadi::SX argd=casadi::SX::sym("argd", getArgSize());
138  casadi::SX argd_2=casadi::SX::sym("argd_2", getArgSize());
139 
140  pd_ = jac(ret, arg);
141  casadi::SX dd_ = jtimes(ret, arg, argd);
142  pddd_ = jac(dd_, arg);
143  pdpd_ = jac(pd_, arg);
144  casadi::SX dddd_ = jtimes(dd_, arg, argd_2);
145 
146  f = casadi::Function("noname", {arg}, {ret});
147  pd = casadi::Function("noname", {arg}, {pd_});
148  dd = casadi::Function("noname", {argd, arg}, {dd_});
149  pddd = casadi::Function("noname", {argd, arg}, {pddd_});
150  pdpd = casadi::Function("noname", {arg}, {pdpd_});
151  dddd = casadi::Function("noname", {argd_2, argd, arg}, {dddd_});
152  }
153  }
154 
155  std::pair<int, int> getRetSize() const override {
156  return std::make_pair(this->ret.size1(), this->ret.size2());
157  }
158 
159  int getArgSize() const override {
160  return arg.size1();
161  }
162 
163  bool constParDer() const override {
164  if(pd_.is_empty(true))
165  pd_ = jac(ret, arg);
166  return pd_.is_constant();
167  }
168 
169  Ret operator()(const Arg& x) override {
170  return c<Ret>(f(std::vector<casadi::SX>{c(x)})[0]);
171  }
172 
173  typename B::DRetDArg parDer(const Arg &x) override {
174  if(pd_(0,0).scalar().isNan())
175  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
176  return c<typename B::DRetDArg>(pd(std::vector<casadi::SX>{c(x)})[0]);
177  }
178 
179  Ret dirDer(const Arg &xd, const Arg &x) override {
180  return c<Ret>(dd(std::vector<casadi::SX>{c(xd), c(x)})[0]);
181  }
182 
183  typename B::DRetDArg parDerDirDer(const Arg &xd, const Arg &x) override {
184  if(pddd_(0,0).scalar().isNan())
185  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
186  return c<typename B::DRetDArg>(pddd(std::vector<casadi::SX>{c(xd), c(x)})[0]);
187  }
188 
189  typename B::DDRetDDArg parDerParDer(const Arg &x) override {
190  if(pdpd_(0,0).scalar().isNan())
191  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
192  return c<typename B::DDRetDDArg>(pdpd(std::vector<casadi::SX>{c(x)})[0]);
193  }
194 
195  Ret dirDerDirDer(const Arg &argDir_1, const Arg &argDir_2, const Arg &arg) override {
196  return c<Ret>(dddd(std::vector<casadi::SX>{c(argDir_1), c(argDir_2), c(arg)})[0]);
197  }
198 
199  void initializeUsingXML(xercesc::DOMElement *element) override {
200  auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
201  arg=io.first[0];
202  ret=io.second[0];
203  // check symbolic function arguments: we need to throw errors during initializeUsingXML to enable the ObjectFactory
204  // to test other possible combinations (more general ones)
205  checkFunctionIODim();
206  }
207 
208  private:
209 
210  void checkFunctionIODim() {
211  // check function: only scalar and vector arguments are supported
212  if(arg.size2()!=1) THROW_MBSIMERROR("Matrix parameters are not allowed.");
213  // check function <-> template argument dimension
214  if(this->argSize!=0 && arg.size1()!=this->argSize)
215  THROW_MBSIMERROR("The dimension of the parameter does not match.");
216  if(this->retSize1!=0 && ret.size1()!=this->retSize1)
217  THROW_MBSIMERROR("The output row dimension does not match.");
218  if(this->retSize2!=0 && ret.size2()!=this->retSize2)
219  THROW_MBSIMERROR("The output column dimension does not match.");
220  }
221  };
222 
223  template<typename Ret, typename Arg1, typename Arg2>
224  class SymbolicFunction<Ret(Arg1, Arg2)> : public Function<Ret(Arg1, Arg2)> {
225  private:
227  casadi::SX ret, arg1, arg2;
228  mutable casadi::SX pd1_, pd2_;
229  casadi::SX pd1dd1_, pd1dd2_, pd1pd2_, pd2dd1_, pd2dd2_, pd2pd2_;
230  casadi::Function f, pd1, pd2, pd1dd1, pd1dd2, pd1pd2, pd2dd1, pd2dd2, pd2pd2;
231  public:
232  SymbolicFunction() {}
233  SymbolicFunction(const casadi::SX &ret_, const casadi::SX &arg1_, const casadi::SX &arg2_) : ret(ret_), arg1(arg1_), arg2(arg2_) {
234  checkFunctionIODim();
235  }
236 
237  void init(Element::InitStage stage) {
239  if(stage == Element::preInit) {
240  casadi::SX arg1d=casadi::SX::sym("arg1d", getArg1Size());
241  casadi::SX arg2d=casadi::SX::sym("arg2d", getArg2Size());
242 
243  pd1_ = jac(ret, arg1);
244  pd2_ = jac(ret, arg2);
245  casadi::SX dd1_ = jtimes(ret, arg1, arg1d);
246  casadi::SX dd2_ = jtimes(ret, arg2, arg2d);
247  pd1dd1_ = jac(dd1_, arg1);
248  pd1dd2_ = jac(dd2_, arg1);
249  pd1pd2_ = jac(pd1_, arg2);
250  pd2dd1_ = jac(dd1_, arg2);
251  pd2dd2_ = jac(dd2_, arg2);
252  pd2pd2_ = jac(pd2_, arg2);
253 
254  f = casadi::Function("noname", {arg1, arg2}, {ret});
255  pd1 = casadi::Function("noname", {arg1, arg2}, {pd1_});
256  pd2 = casadi::Function("noname", {arg1, arg2}, {pd2_});
257  pd1dd1 = casadi::Function("noname", {arg1d, arg1, arg2}, {pd1dd1_});
258  pd1dd2 = casadi::Function("noname", {arg2d, arg1, arg2}, {pd1dd2_});
259  pd1pd2 = casadi::Function("noname", {arg1, arg2}, {pd1pd2_});
260  pd2dd1 = casadi::Function("noname", {arg1d, arg1, arg2}, {pd2dd1_});
261  pd2dd2 = casadi::Function("noname", {arg2d, arg1, arg2}, {pd2dd2_});
262  pd2pd2 = casadi::Function("noname", {arg1, arg2}, {pd2pd2_});
263  }
264  }
265 
266  std::pair<int, int> getRetSize() const override {
267  return std::make_pair(ret.size1(), ret.size2());
268  }
269 
270  int getArg1Size() const override {
271  return arg1.size1();
272  }
273 
274  int getArg2Size() const override {
275  return arg2.size1();
276  }
277 
278  bool constParDer1() const override {
279  if(pd1_.is_empty(true))
280  pd1_ = jac(ret, arg1);
281  return pd1_.is_constant();
282  }
283 
284  bool constParDer2() const override {
285  if(pd2_.is_empty(true))
286  pd2_ = jac(ret, arg2);
287  return pd2_.is_constant();
288  }
289 
290  Ret operator()(const Arg1& x1, const Arg2& x2) override {
291  return c<Ret>(f(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
292  }
293 
294  typename B::DRetDArg1 parDer1(const Arg1 &x1, const Arg2 &x2) override {
295  if(pd1_(0,0).scalar().isNan())
296  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
297  return c<typename B::DRetDArg1>(pd1(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
298  }
299 
300  Ret dirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) override {
301  THROW_MBSIMERROR("Not implemented");
302  }
303 
304  typename B::DRetDArg2 parDer2(const Arg1 &x1, const Arg2 &x2) override {
305  if(pd2_(0,0).scalar().isNan())
306  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
307  return c<typename B::DRetDArg2>(pd2(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
308  }
309 
310  Ret dirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) override {
311  THROW_MBSIMERROR("Not implemented");
312  }
313 
314  typename B::DDRetDDArg1 parDer1ParDer1(const Arg1 &arg1, const Arg2 &arg2) override {
315  THROW_MBSIMERROR("Not implemented");
316  }
317 
318  typename B::DRetDArg1 parDer1DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) override {
319  if(pd1dd1_(0,0).scalar().isNan())
320  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
321  return c<typename B::DRetDArg1>(pd1dd1(std::vector<casadi::SX>{c(xd1), c(x1), c(x2)})[0]);
322  }
323 
324  Ret dirDer1DirDer1(const Arg1 &arg1Dir_1, const Arg1 &arg1Dir_2, const Arg1 &arg1, const Arg2 &arg2) override {
325  THROW_MBSIMERROR("Not implemented");
326  }
327 
328  typename B::DDRetDDArg2 parDer2ParDer2(const Arg1 &arg1, const Arg2 &arg2) override {
329  if(pd2pd2_(0,0).scalar().isNan())
330  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
331  return c<typename B::DDRetDDArg2>(pd2pd2(std::vector<casadi::SX>{c(arg1), c(arg2)})[0]);
332  }
333 
334  typename B::DRetDArg1 parDer1DirDer2(const Arg2 &xd2, const Arg1 &x1, const Arg2 &x2) override {
335  if(pd1dd2_(0,0).scalar().isNan())
336  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
337  return c<typename B::DRetDArg1>(pd1dd2(std::vector<casadi::SX>{c(xd2), c(x1), c(x2)})[0]);
338  }
339 
340  typename B::DRetDArg2 parDer2DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) override {
341  if(pd2dd1_(0,0).scalar().isNan())
342  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
343  return c<typename B::DRetDArg2>(pd2dd1(std::vector<casadi::SX>{c(xd1), c(x1), c(x2)})[0]);
344  }
345 
346  Ret dirDer2DirDer2(const Arg2 &arg2Dir_1, const Arg2 &arg2Dir_2, const Arg1 &arg1, const Arg2 &arg2) override {
347  THROW_MBSIMERROR("Not implemented");
348  }
349 
350  typename B::DDRetDArg1DArg2 parDer1ParDer2(const Arg1 &arg1, const Arg2 &arg2) override {
351  if(pd1pd2_(0,0).scalar().isNan())
352  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
353  return c<typename B::DDRetDArg1DArg2>(pd1pd2(std::vector<casadi::SX>{c(arg1), c(arg2)})[0]);
354  }
355 
356  Ret dirDer2DirDer1(const Arg2 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) override {
357  THROW_MBSIMERROR("Not implemented");
358  }
359 
360  typename B::DRetDArg2 parDer2DirDer2(const Arg2 &xd2, const Arg1 &x1, const Arg2 &x2) override {
361  if(pd2dd2_(0,0).scalar().isNan())
362  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
363  return c<typename B::DRetDArg2>(pd2dd2(std::vector<casadi::SX>{c(xd2), c(x1), c(x2)})[0]);
364  }
365 
366  void initializeUsingXML(xercesc::DOMElement *element) {
367  auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
368  arg1=io.first[0];
369  arg2=io.first[1];
370  ret=io.second[0];
371  // check symbolic function arguments: we need to throw errors during initializeUsingXML to enable the ObjectFactory
372  // to test other possible combinations (more general ones)
373  checkFunctionIODim();
374  }
375 
376  private:
377 
378  void checkFunctionIODim() {
379  // check function: only scalar and vector arguments are supported
380  if(arg1.size2()!=1) THROW_MBSIMERROR("Matrix parameters are not allowed.");
381  if(arg2.size2()!=1) THROW_MBSIMERROR("Matrix parameters are not allowed.");
382  // check function <-> template argument dimension
383  if(this->arg1Size!=0 && arg1.size1()!=this->arg1Size)
384  THROW_MBSIMERROR("The dimension of the first parameter does not match.");
385  if(this->arg2Size!=0 && arg2.size1()!=this->arg2Size)
386  THROW_MBSIMERROR("The dimension of the second parameter does not match.");
387  if(this->retSize1!=0 && ret.size1()!=this->retSize1)
388  THROW_MBSIMERROR("The output row dimension does not match.");
389  if(this->retSize2!=0 && ret.size2()!=this->retSize2)
390  THROW_MBSIMERROR("The output column dimension does not match.");
391  }
392  };
393 
394 }
395 
396 #endif
Definition: symbolic_function.h:118
void init(Element::InitStage stage)
plots time series header
Definition: symbolic_function.h:134
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:48
InitStage
The stages of the initialization.
Definition: element.h:97
Definition: planar_contour.h:31
void init(Element::InitStage stage)
plots time series header
Definition: symbolic_function.h:237
Definition: element.h:100
basic error class for mbsim
Definition: mbsim_event.h:38
casadi::SX jac(const casadi::SX &f, const casadi::SX &arg)
A fmatvec like jacobian using casadi.
Definition: symbolic_function.h:31
Definition: symbolic_function.h:63

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Valid HTML