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 
32  template<typename Ret=void>
33  casadi::SX parDerSX(const casadi::SX &f, const casadi::SX &arg) {
34  assert(arg.size2()==1);
35  if(f.size2()>1) { // f is a matrix
36  if(arg.size1()==1) { // arg is a scalar -> jac is a matrix but this cannot be handled by casadi directly
37  // calcualte jacobian using casadi -> reshape to matrix
38  return casadi::SX::reshape(casadi::SX::jacobian(f, arg), f.size1(), f.size2());
39  }
40  else { // arg is a vector -> not possible (would be a tensor of order 3 which cannot be handled by casadi and fmatvec)
41  return casadi::SX::nan(); // return a scalar NaN -> will throw later if this is tried to evaluate
42  }
43  }
44  else // f is a vector -> jac may get a matrix if arg is a vector (handled by casadi)
45  return casadi::SX::jacobian(f, arg);
46  }
47 
50  template<>
51  casadi::SX parDerSX<fmatvec::RotMat3>(const casadi::SX &f, const casadi::SX &arg) {
52  assert(f.size1()==3);
53  assert(f.size2()==3);
54  assert(arg.size2()==1);
55  casadi::SX ret(3, arg.size1());
56  for(int i=0; i<arg.size1(); ++i) {
57  // wtilde = df/dargi * f
58  casadi::SX wtilde=casadi::SX::mtimes(parDerSX(f, arg(i)), f.T());
59  // ret = tilde(wtilde)
60  ret(0,i)=wtilde(2,1);
61  ret(1,i)=wtilde(0,2);
62  ret(2,i)=wtilde(1,0);
63  }
64  return ret;
65  }
66 
69  template<typename Ret=void>
70  casadi::SX dirDerSX(const casadi::SX &f, const casadi::SX &arg, const casadi::SX &argd) {
71  assert(arg.size2()==1);
72  assert(argd.size2()==1);
73  assert(arg.size1()==argd.size1());
74  if(f.size2()==1)
75  return jtimes(f, arg, argd);
76  else
77  return casadi::SX::reshape(jtimes(casadi::SX::reshape(f, f.size1()*f.size2(), 1), arg, argd), f.size1(), f.size2());
78  }
79 
82  template<>
83  casadi::SX dirDerSX<fmatvec::RotMat3>(const casadi::SX &f, const casadi::SX &arg, const casadi::SX &argd) {
84  assert(f.size1()==3);
85  assert(f.size2()==3);
86  assert(arg.size2()==1);
87  assert(argd.size2()==1);
88  assert(arg.size1()==argd.size1());
89  return casadi::SX::mtimes(parDerSX<fmatvec::RotMat3>(f, arg), argd);
90  }
91 
93  casadi::DM c(const double &x) {
94  return casadi::DM(x);
95  }
96 
98  template<class Col>
99  casadi::DM c(const fmatvec::Vector<Col,double> &x) {
100  casadi::DM y(x.size(), 1);
101  for(int i=0; i<x.size(); i++)
102  y(i) = x.e(i);
103  return y;
104  }
105 
106  // helper classes to convert casadi::DM to any value
107  template <class Ret>
108  class FromCasadi {
109  public:
110  static Ret cast(const casadi::Matrix<double> &x) {
111  throw std::runtime_error("FromCasadi::cast not implemented for current type.");
112  }
113  };
114 
115  template <class Col>
116  class FromCasadi<fmatvec::Vector<Col,double> > {
117  public:
118  static fmatvec::Vector<Col,double> cast(const casadi::Matrix<double> &x) {
119  fmatvec::Vector<Col,double> y(x.size1(),fmatvec::NONINIT);
120  for(int i=0; i<x.size1(); i++)
121  y.e(i) = x(i,0).scalar();
122  return y;
123  }
124  };
125 
126  template <class Col>
127  class FromCasadi<fmatvec::RowVector<Col,double> > {
128  public:
129  static fmatvec::RowVector<Col,double> cast(const casadi::Matrix<double> &x) {
130  fmatvec::RowVector<Col,double> y(x.size2(),fmatvec::NONINIT);
131  for(int i=0; i<x.size2(); i++)
132  y.e(i) = x(0,i).scalar();
133  return y;
134  }
135  };
136 
137  template <class Row, class Col>
138  class FromCasadi<fmatvec::Matrix<fmatvec::General,Row,Col,double> > {
139  public:
140  static fmatvec::Matrix<fmatvec::General,Row,Col,double> cast(const casadi::Matrix<double> &A) {
141  fmatvec::Matrix<fmatvec::General,Row,Col,double> B(A.size1(),A.size2(),fmatvec::NONINIT);
142  for(int i=0; i<A.size1(); i++)
143  for(int j=0; j<A.size2(); j++)
144  B.e(i,j) = A(i,j).scalar();
145  return B;
146  }
147  };
148 
149  template<>
150  class FromCasadi<fmatvec::RotMat3> {
151  public:
152  static fmatvec::RotMat3 cast(const casadi::Matrix<double> &A) {
153  assert(A.size1()==3);
154  assert(A.size2()==3);
155  fmatvec::RotMat3 B(fmatvec::NONINIT);
156  for(int i=0; i<3; i++)
157  for(int j=0; j<3; j++)
158  B.e(i,j) = A(i,j).scalar();
159  return B;
160  }
161  };
162 
163  template <>
164  class FromCasadi<double> {
165  public:
166  static double cast(const casadi::Matrix<double> &x) {
167  return x.scalar();
168  }
169  };
170 
172  template<class T>
173  T c(const casadi::DM &x) {
174  return FromCasadi<T>::cast(x);
175  }
176 
177  template<typename Sig> class SymbolicFunction;
178 
179  template<typename Ret, typename Arg>
180  class SymbolicFunction<Ret(Arg)> : public Function<Ret(Arg)> {
181  private:
183  casadi::SX ret, arg;
184  mutable casadi::SX pd_;
185  casadi::SX pddd_, pdpd_;
186  casadi::Function f, pd, dd, pddd, pdpd, dddd;
187  public:
188  SymbolicFunction() = default;
189  SymbolicFunction(const casadi::SX &ret_, const casadi::SX &arg_) : ret(ret_), arg(arg_) {
190  checkFunctionIODim();
191  }
192 
193  void init(Element::InitStage stage, const InitConfigSet &config) override {
194  Function<Ret(Arg)>::init(stage, config);
195  if(stage == Element::preInit) {
196  casadi::SX argd=casadi::SX::sym("argd", getArgSize());
197  casadi::SX argd_2=casadi::SX::sym("argd_2", getArgSize());
198 
199  f = casadi::Function("noname", {arg}, {ret});
200 
201  casadi::SX dd_;
202  if(config.find(noDer)==config.end()) {
203  pd_ = parDerSX<Ret>(ret, arg);
204  dd_ = dirDerSX<Ret>(ret, arg, argd);
205  pd = casadi::Function("noname", {arg}, {pd_});
206  dd = casadi::Function("noname", {argd, arg}, {dd_});
207  }
208 
209  if(config.find(noDer)==config.end() && config.find(noDerDer)==config.end()) {
210  pddd_ = parDerSX(dd_, arg);
211  pdpd_ = parDerSX(pd_, arg);
212  casadi::SX dddd_ = dirDerSX(dd_, arg, argd_2);
213  pddd = casadi::Function("noname", {argd, arg}, {pddd_});
214  pdpd = casadi::Function("noname", {arg}, {pdpd_});
215  dddd = casadi::Function("noname", {argd_2, argd, arg}, {dddd_});
216  }
217  }
218  }
219 
220  std::pair<int, int> getRetSize() const override {
221  return std::make_pair(this->ret.size1(), this->ret.size2());
222  }
223 
224  int getArgSize() const override {
225  return arg.size1();
226  }
227 
228  bool constParDer() const override {
229  if(pd_.is_empty(true))
230  pd_ = parDerSX<Ret>(ret, arg);
231  return pd_.is_constant();
232  }
233 
234  Ret operator()(const Arg& x) override {
235  return c<Ret>(f(std::vector<casadi::SX>{c(x)})[0]);
236  }
237 
238  typename B::DRetDArg parDer(const Arg &x) override {
239  if(pd_(0,0).scalar().isNan())
240  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
241  return c<typename B::DRetDArg>(pd(std::vector<casadi::SX>{c(x)})[0]);
242  }
243 
244  Ret dirDer(const Arg &xd, const Arg &x) override {
245  return c<Ret>(dd(std::vector<casadi::SX>{c(xd), c(x)})[0]);
246  }
247 
248  typename B::DRetDArg parDerDirDer(const Arg &xd, const Arg &x) override {
249  if(pddd_(0,0).scalar().isNan())
250  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
251  return c<typename B::DRetDArg>(pddd(std::vector<casadi::SX>{c(xd), c(x)})[0]);
252  }
253 
254  typename B::DDRetDDArg parDerParDer(const Arg &x) override {
255  if(pdpd_(0,0).scalar().isNan())
256  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
257  return c<typename B::DDRetDDArg>(pdpd(std::vector<casadi::SX>{c(x)})[0]);
258  }
259 
260  Ret dirDerDirDer(const Arg &argDir_1, const Arg &argDir_2, const Arg &arg) override {
261  return c<Ret>(dddd(std::vector<casadi::SX>{c(argDir_1), c(argDir_2), c(arg)})[0]);
262  }
263 
264  void initializeUsingXML(xercesc::DOMElement *element) override {
265  auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
266  arg=io.first[0];
267  ret=io.second[0];
268  // check symbolic function arguments: we need to throw errors during initializeUsingXML to enable the ObjectFactory
269  // to test other possible combinations (more general ones)
270  checkFunctionIODim();
271  }
272 
273  private:
274 
275  void checkFunctionIODim() {
276  // check function: only scalar and vector arguments are supported
277  if(arg.size2()!=1) this->throwError("Matrix parameters are not allowed.");
278  // check function <-> template argument dimension
279  if(this->argSize!=0 && arg.size1()!=this->argSize)
280  this->throwError("The dimension of the parameter does not match.");
281  if(this->retSize1!=0 && ret.size1()!=this->retSize1)
282  this->throwError("The output row dimension does not match.");
283  if(this->retSize2!=0 && ret.size2()!=this->retSize2)
284  this->throwError("The output column dimension does not match.");
285  }
286  };
287 
288  template<typename Ret, typename Arg1, typename Arg2>
289  class SymbolicFunction<Ret(Arg1, Arg2)> : public Function<Ret(Arg1, Arg2)> {
290  private:
292  casadi::SX ret, arg1, arg2;
293  mutable casadi::SX pd1_, pd2_;
294  casadi::SX pd1dd1_, pd1dd2_, pd1pd2_, pd2dd1_, pd2dd2_, pd2pd2_;
295  casadi::Function f, pd1, pd2, pd1dd1, pd1dd2, pd1pd2, pd2dd1, pd2dd2, pd2pd2;
296  public:
297  SymbolicFunction() = default;
298  SymbolicFunction(const casadi::SX &ret_, const casadi::SX &arg1_, const casadi::SX &arg2_) : ret(ret_), arg1(arg1_), arg2(arg2_) {
299  checkFunctionIODim();
300  }
301 
302  void init(Element::InitStage stage, const InitConfigSet &config) override {
303  Function<Ret(Arg1, Arg2)>::init(stage, config);
304  if(stage == Element::preInit) {
305  casadi::SX arg1d=casadi::SX::sym("arg1d", getArg1Size());
306  casadi::SX arg2d=casadi::SX::sym("arg2d", getArg2Size());
307 
308  f = casadi::Function("noname", {arg1, arg2}, {ret});
309 
310  if(config.find(noDer)==config.end()) {
311  pd1_ = parDerSX<Ret>(ret, arg1);
312  pd2_ = parDerSX<Ret>(ret, arg2);
313  pd1 = casadi::Function("noname", {arg1, arg2}, {pd1_});
314  pd2 = casadi::Function("noname", {arg1, arg2}, {pd2_});
315  }
316 
317  if(config.find(noDer)==config.end() && config.find(noDerDer)==config.end()) {
318  casadi::SX dd1_ = dirDerSX<Ret>(ret, arg1, arg1d);
319  casadi::SX dd2_ = dirDerSX<Ret>(ret, arg2, arg2d);
320  pd1dd1_ = parDerSX(dd1_, arg1);
321  pd1dd2_ = parDerSX(dd2_, arg1);
322  pd1pd2_ = parDerSX(pd1_, arg2);
323  pd2dd1_ = parDerSX(dd1_, arg2);
324  pd2dd2_ = parDerSX(dd2_, arg2);
325  pd2pd2_ = parDerSX(pd2_, arg2);
326  pd1dd1 = casadi::Function("noname", {arg1d, arg1, arg2}, {pd1dd1_});
327  pd1dd2 = casadi::Function("noname", {arg2d, arg1, arg2}, {pd1dd2_});
328  pd1pd2 = casadi::Function("noname", {arg1, arg2}, {pd1pd2_});
329  pd2dd1 = casadi::Function("noname", {arg1d, arg1, arg2}, {pd2dd1_});
330  pd2dd2 = casadi::Function("noname", {arg2d, arg1, arg2}, {pd2dd2_});
331  pd2pd2 = casadi::Function("noname", {arg1, arg2}, {pd2pd2_});
332  }
333  }
334  }
335 
336  std::pair<int, int> getRetSize() const override {
337  return std::make_pair(ret.size1(), ret.size2());
338  }
339 
340  int getArg1Size() const override {
341  return arg1.size1();
342  }
343 
344  int getArg2Size() const override {
345  return arg2.size1();
346  }
347 
348  bool constParDer1() const override {
349  if(pd1_.is_empty(true))
350  pd1_ = parDerSX<Ret>(ret, arg1);
351  return pd1_.is_constant();
352  }
353 
354  bool constParDer2() const override {
355  if(pd2_.is_empty(true))
356  pd2_ = parDerSX<Ret>(ret, arg2);
357  return pd2_.is_constant();
358  }
359 
360  Ret operator()(const Arg1& x1, const Arg2& x2) override {
361  return c<Ret>(f(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
362  }
363 
364  typename B::DRetDArg1 parDer1(const Arg1 &x1, const Arg2 &x2) override {
365  if(pd1_(0,0).scalar().isNan())
366  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
367  return c<typename B::DRetDArg1>(pd1(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
368  }
369 
370  Ret dirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) override {
371  this->throwError("Not implemented");
372  }
373 
374  typename B::DRetDArg2 parDer2(const Arg1 &x1, const Arg2 &x2) override {
375  if(pd2_(0,0).scalar().isNan())
376  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
377  return c<typename B::DRetDArg2>(pd2(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
378  }
379 
380  Ret dirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) override {
381  this->throwError("Not implemented");
382  }
383 
384  typename B::DDRetDDArg1 parDer1ParDer1(const Arg1 &arg1, const Arg2 &arg2) override {
385  this->throwError("Not implemented");
386  }
387 
388  typename B::DRetDArg1 parDer1DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) override {
389  if(pd1dd1_(0,0).scalar().isNan())
390  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
391  return c<typename B::DRetDArg1>(pd1dd1(std::vector<casadi::SX>{c(xd1), c(x1), c(x2)})[0]);
392  }
393 
394  Ret dirDer1DirDer1(const Arg1 &arg1Dir_1, const Arg1 &arg1Dir_2, const Arg1 &arg1, const Arg2 &arg2) override {
395  this->throwError("Not implemented");
396  }
397 
398  typename B::DDRetDDArg2 parDer2ParDer2(const Arg1 &arg1, const Arg2 &arg2) override {
399  if(pd2pd2_(0,0).scalar().isNan())
400  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
401  return c<typename B::DDRetDDArg2>(pd2pd2(std::vector<casadi::SX>{c(arg1), c(arg2)})[0]);
402  }
403 
404  typename B::DRetDArg1 parDer1DirDer2(const Arg2 &xd2, const Arg1 &x1, const Arg2 &x2) override {
405  if(pd1dd2_(0,0).scalar().isNan())
406  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
407  return c<typename B::DRetDArg1>(pd1dd2(std::vector<casadi::SX>{c(xd2), c(x1), c(x2)})[0]);
408  }
409 
410  typename B::DRetDArg2 parDer2DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) override {
411  if(pd2dd1_(0,0).scalar().isNan())
412  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
413  return c<typename B::DRetDArg2>(pd2dd1(std::vector<casadi::SX>{c(xd1), c(x1), c(x2)})[0]);
414  }
415 
416  Ret dirDer2DirDer2(const Arg2 &arg2Dir_1, const Arg2 &arg2Dir_2, const Arg1 &arg1, const Arg2 &arg2) override {
417  this->throwError("Not implemented");
418  }
419 
420  typename B::DDRetDArg1DArg2 parDer1ParDer2(const Arg1 &arg1, const Arg2 &arg2) override {
421  if(pd1pd2_(0,0).scalar().isNan())
422  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
423  return c<typename B::DDRetDArg1DArg2>(pd1pd2(std::vector<casadi::SX>{c(arg1), c(arg2)})[0]);
424  }
425 
426  Ret dirDer2DirDer1(const Arg2 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) override {
427  this->throwError("Not implemented");
428  }
429 
430  typename B::DRetDArg2 parDer2DirDer2(const Arg2 &xd2, const Arg1 &x1, const Arg2 &x2) override {
431  if(pd2dd2_(0,0).scalar().isNan())
432  this->throwError("Cannot calculate this partial derivative, would be a tensor of order 3.");
433  return c<typename B::DRetDArg2>(pd2dd2(std::vector<casadi::SX>{c(xd2), c(x1), c(x2)})[0]);
434  }
435 
436  void initializeUsingXML(xercesc::DOMElement *element) override {
437  auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
438  arg1=io.first[0];
439  arg2=io.first[1];
440  ret=io.second[0];
441  // check symbolic function arguments: we need to throw errors during initializeUsingXML to enable the ObjectFactory
442  // to test other possible combinations (more general ones)
443  checkFunctionIODim();
444  }
445 
446  private:
447 
448  void checkFunctionIODim() {
449  // check function: only scalar and vector arguments are supported
450  if(arg1.size2()!=1) this->throwError("Matrix parameters are not allowed.");
451  if(arg2.size2()!=1) this->throwError("Matrix parameters are not allowed.");
452  // check function <-> template argument dimension
453  if(this->arg1Size!=0 && arg1.size1()!=this->arg1Size)
454  this->throwError("The dimension of the first parameter does not match.");
455  if(this->arg2Size!=0 && arg2.size1()!=this->arg2Size)
456  this->throwError("The dimension of the second parameter does not match.");
457  if(this->retSize1!=0 && ret.size1()!=this->retSize1)
458  this->throwError("The output row dimension does not match.");
459  if(this->retSize2!=0 && ret.size2()!=this->retSize2)
460  this->throwError("The output column dimension does not match.");
461  }
462  };
463 
464 }
465 
466 #endif
void init(Element::InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: symbolic_function.h:193
Definition: symbolic_function.h:177
casadi::SX dirDerSX(const casadi::SX &f, const casadi::SX &arg, const casadi::SX &argd)
Definition: symbolic_function.h:70
casadi::DM c(const double &x)
convert double to casadi::DM
Definition: symbolic_function.h:93
InitStage
The stages of the initialization.
Definition: element.h:62
Definition: planar_contour.h:30
Definition: element.h:64
void init(Element::InitStage stage, const InitConfigSet &config) override
plots time series header
Definition: symbolic_function.h:302
casadi::SX parDerSX(const casadi::SX &f, const casadi::SX &arg)
Definition: symbolic_function.h:33
Definition: symbolic_function.h:108

Impressum / Disclaimer / Datenschutz Generated by doxygen 1.8.5 Home