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 MBSimError("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() {}
189  SymbolicFunction(const casadi::SX &ret_, const casadi::SX &arg_) : ret(ret_), arg(arg_) {
190  checkFunctionIODim();
191  }
192 
193  void init(Element::InitStage stage) {
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  pd_ = parDerSX<Ret>(ret, arg);
200  casadi::SX dd_ = dirDerSX<Ret>(ret, arg, argd);
201  pddd_ = parDerSX(dd_, arg);
202  pdpd_ = parDerSX(pd_, arg);
203  casadi::SX dddd_ = dirDerSX(dd_, arg, argd_2);
204 
205  f = casadi::Function("noname", {arg}, {ret});
206  pd = casadi::Function("noname", {arg}, {pd_});
207  dd = casadi::Function("noname", {argd, arg}, {dd_});
208  pddd = casadi::Function("noname", {argd, arg}, {pddd_});
209  pdpd = casadi::Function("noname", {arg}, {pdpd_});
210  dddd = casadi::Function("noname", {argd_2, argd, arg}, {dddd_});
211  }
212  }
213 
214  std::pair<int, int> getRetSize() const override {
215  return std::make_pair(this->ret.size1(), this->ret.size2());
216  }
217 
218  int getArgSize() const override {
219  return arg.size1();
220  }
221 
222  bool constParDer() const override {
223  if(pd_.is_empty(true))
224  pd_ = parDerSX<Ret>(ret, arg);
225  return pd_.is_constant();
226  }
227 
228  Ret operator()(const Arg& x) override {
229  return c<Ret>(f(std::vector<casadi::SX>{c(x)})[0]);
230  }
231 
232  typename B::DRetDArg parDer(const Arg &x) override {
233  if(pd_(0,0).scalar().isNan())
234  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
235  return c<typename B::DRetDArg>(pd(std::vector<casadi::SX>{c(x)})[0]);
236  }
237 
238  Ret dirDer(const Arg &xd, const Arg &x) override {
239  return c<Ret>(dd(std::vector<casadi::SX>{c(xd), c(x)})[0]);
240  }
241 
242  typename B::DRetDArg parDerDirDer(const Arg &xd, const Arg &x) override {
243  if(pddd_(0,0).scalar().isNan())
244  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
245  return c<typename B::DRetDArg>(pddd(std::vector<casadi::SX>{c(xd), c(x)})[0]);
246  }
247 
248  typename B::DDRetDDArg parDerParDer(const Arg &x) override {
249  if(pdpd_(0,0).scalar().isNan())
250  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
251  return c<typename B::DDRetDDArg>(pdpd(std::vector<casadi::SX>{c(x)})[0]);
252  }
253 
254  Ret dirDerDirDer(const Arg &argDir_1, const Arg &argDir_2, const Arg &arg) override {
255  return c<Ret>(dddd(std::vector<casadi::SX>{c(argDir_1), c(argDir_2), c(arg)})[0]);
256  }
257 
258  void initializeUsingXML(xercesc::DOMElement *element) override {
259  auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
260  arg=io.first[0];
261  ret=io.second[0];
262  // check symbolic function arguments: we need to throw errors during initializeUsingXML to enable the ObjectFactory
263  // to test other possible combinations (more general ones)
264  checkFunctionIODim();
265  }
266 
267  private:
268 
269  void checkFunctionIODim() {
270  // check function: only scalar and vector arguments are supported
271  if(arg.size2()!=1) THROW_MBSIMERROR("Matrix parameters are not allowed.");
272  // check function <-> template argument dimension
273  if(this->argSize!=0 && arg.size1()!=this->argSize)
274  THROW_MBSIMERROR("The dimension of the parameter does not match.");
275  if(this->retSize1!=0 && ret.size1()!=this->retSize1)
276  THROW_MBSIMERROR("The output row dimension does not match.");
277  if(this->retSize2!=0 && ret.size2()!=this->retSize2)
278  THROW_MBSIMERROR("The output column dimension does not match.");
279  }
280  };
281 
282  template<typename Ret, typename Arg1, typename Arg2>
283  class SymbolicFunction<Ret(Arg1, Arg2)> : public Function<Ret(Arg1, Arg2)> {
284  private:
286  casadi::SX ret, arg1, arg2;
287  mutable casadi::SX pd1_, pd2_;
288  casadi::SX pd1dd1_, pd1dd2_, pd1pd2_, pd2dd1_, pd2dd2_, pd2pd2_;
289  casadi::Function f, pd1, pd2, pd1dd1, pd1dd2, pd1pd2, pd2dd1, pd2dd2, pd2pd2;
290  public:
291  SymbolicFunction() {}
292  SymbolicFunction(const casadi::SX &ret_, const casadi::SX &arg1_, const casadi::SX &arg2_) : ret(ret_), arg1(arg1_), arg2(arg2_) {
293  checkFunctionIODim();
294  }
295 
296  void init(Element::InitStage stage) {
298  if(stage == Element::preInit) {
299  casadi::SX arg1d=casadi::SX::sym("arg1d", getArg1Size());
300  casadi::SX arg2d=casadi::SX::sym("arg2d", getArg2Size());
301 
302  pd1_ = parDerSX<Ret>(ret, arg1);
303  pd2_ = parDerSX<Ret>(ret, arg2);
304  casadi::SX dd1_ = dirDerSX<Ret>(ret, arg1, arg1d);
305  casadi::SX dd2_ = dirDerSX<Ret>(ret, arg2, arg2d);
306  pd1dd1_ = parDerSX(dd1_, arg1);
307  pd1dd2_ = parDerSX(dd2_, arg1);
308  pd1pd2_ = parDerSX(pd1_, arg2);
309  pd2dd1_ = parDerSX(dd1_, arg2);
310  pd2dd2_ = parDerSX(dd2_, arg2);
311  pd2pd2_ = parDerSX(pd2_, arg2);
312 
313  f = casadi::Function("noname", {arg1, arg2}, {ret});
314  pd1 = casadi::Function("noname", {arg1, arg2}, {pd1_});
315  pd2 = casadi::Function("noname", {arg1, arg2}, {pd2_});
316  pd1dd1 = casadi::Function("noname", {arg1d, arg1, arg2}, {pd1dd1_});
317  pd1dd2 = casadi::Function("noname", {arg2d, arg1, arg2}, {pd1dd2_});
318  pd1pd2 = casadi::Function("noname", {arg1, arg2}, {pd1pd2_});
319  pd2dd1 = casadi::Function("noname", {arg1d, arg1, arg2}, {pd2dd1_});
320  pd2dd2 = casadi::Function("noname", {arg2d, arg1, arg2}, {pd2dd2_});
321  pd2pd2 = casadi::Function("noname", {arg1, arg2}, {pd2pd2_});
322  }
323  }
324 
325  std::pair<int, int> getRetSize() const override {
326  return std::make_pair(ret.size1(), ret.size2());
327  }
328 
329  int getArg1Size() const override {
330  return arg1.size1();
331  }
332 
333  int getArg2Size() const override {
334  return arg2.size1();
335  }
336 
337  bool constParDer1() const override {
338  if(pd1_.is_empty(true))
339  pd1_ = parDerSX<Ret>(ret, arg1);
340  return pd1_.is_constant();
341  }
342 
343  bool constParDer2() const override {
344  if(pd2_.is_empty(true))
345  pd2_ = parDerSX<Ret>(ret, arg2);
346  return pd2_.is_constant();
347  }
348 
349  Ret operator()(const Arg1& x1, const Arg2& x2) override {
350  return c<Ret>(f(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
351  }
352 
353  typename B::DRetDArg1 parDer1(const Arg1 &x1, const Arg2 &x2) override {
354  if(pd1_(0,0).scalar().isNan())
355  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
356  return c<typename B::DRetDArg1>(pd1(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
357  }
358 
359  Ret dirDer1(const Arg1 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) override {
360  THROW_MBSIMERROR("Not implemented");
361  }
362 
363  typename B::DRetDArg2 parDer2(const Arg1 &x1, const Arg2 &x2) override {
364  if(pd2_(0,0).scalar().isNan())
365  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
366  return c<typename B::DRetDArg2>(pd2(std::vector<casadi::SX>{c(x1), c(x2)})[0]);
367  }
368 
369  Ret dirDer2(const Arg2 &arg2Dir, const Arg1 &arg1, const Arg2 &arg2) override {
370  THROW_MBSIMERROR("Not implemented");
371  }
372 
373  typename B::DDRetDDArg1 parDer1ParDer1(const Arg1 &arg1, const Arg2 &arg2) override {
374  THROW_MBSIMERROR("Not implemented");
375  }
376 
377  typename B::DRetDArg1 parDer1DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) override {
378  if(pd1dd1_(0,0).scalar().isNan())
379  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
380  return c<typename B::DRetDArg1>(pd1dd1(std::vector<casadi::SX>{c(xd1), c(x1), c(x2)})[0]);
381  }
382 
383  Ret dirDer1DirDer1(const Arg1 &arg1Dir_1, const Arg1 &arg1Dir_2, const Arg1 &arg1, const Arg2 &arg2) override {
384  THROW_MBSIMERROR("Not implemented");
385  }
386 
387  typename B::DDRetDDArg2 parDer2ParDer2(const Arg1 &arg1, const Arg2 &arg2) override {
388  if(pd2pd2_(0,0).scalar().isNan())
389  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
390  return c<typename B::DDRetDDArg2>(pd2pd2(std::vector<casadi::SX>{c(arg1), c(arg2)})[0]);
391  }
392 
393  typename B::DRetDArg1 parDer1DirDer2(const Arg2 &xd2, const Arg1 &x1, const Arg2 &x2) override {
394  if(pd1dd2_(0,0).scalar().isNan())
395  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
396  return c<typename B::DRetDArg1>(pd1dd2(std::vector<casadi::SX>{c(xd2), c(x1), c(x2)})[0]);
397  }
398 
399  typename B::DRetDArg2 parDer2DirDer1(const Arg1 &xd1, const Arg1 &x1, const Arg2 &x2) override {
400  if(pd2dd1_(0,0).scalar().isNan())
401  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
402  return c<typename B::DRetDArg2>(pd2dd1(std::vector<casadi::SX>{c(xd1), c(x1), c(x2)})[0]);
403  }
404 
405  Ret dirDer2DirDer2(const Arg2 &arg2Dir_1, const Arg2 &arg2Dir_2, const Arg1 &arg1, const Arg2 &arg2) override {
406  THROW_MBSIMERROR("Not implemented");
407  }
408 
409  typename B::DDRetDArg1DArg2 parDer1ParDer2(const Arg1 &arg1, const Arg2 &arg2) override {
410  if(pd1pd2_(0,0).scalar().isNan())
411  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
412  return c<typename B::DDRetDArg1DArg2>(pd1pd2(std::vector<casadi::SX>{c(arg1), c(arg2)})[0]);
413  }
414 
415  Ret dirDer2DirDer1(const Arg2 &arg1Dir, const Arg1 &arg1, const Arg2 &arg2) override {
416  THROW_MBSIMERROR("Not implemented");
417  }
418 
419  typename B::DRetDArg2 parDer2DirDer2(const Arg2 &xd2, const Arg1 &x1, const Arg2 &x2) override {
420  if(pd2dd2_(0,0).scalar().isNan())
421  THROW_MBSIMERROR("Cannot calculate this partial derivative, would be a tensor of order 3.");
422  return c<typename B::DRetDArg2>(pd2dd2(std::vector<casadi::SX>{c(xd2), c(x1), c(x2)})[0]);
423  }
424 
425  void initializeUsingXML(xercesc::DOMElement *element) {
426  auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
427  arg1=io.first[0];
428  arg2=io.first[1];
429  ret=io.second[0];
430  // check symbolic function arguments: we need to throw errors during initializeUsingXML to enable the ObjectFactory
431  // to test other possible combinations (more general ones)
432  checkFunctionIODim();
433  }
434 
435  private:
436 
437  void checkFunctionIODim() {
438  // check function: only scalar and vector arguments are supported
439  if(arg1.size2()!=1) THROW_MBSIMERROR("Matrix parameters are not allowed.");
440  if(arg2.size2()!=1) THROW_MBSIMERROR("Matrix parameters are not allowed.");
441  // check function <-> template argument dimension
442  if(this->arg1Size!=0 && arg1.size1()!=this->arg1Size)
443  THROW_MBSIMERROR("The dimension of the first parameter does not match.");
444  if(this->arg2Size!=0 && arg2.size1()!=this->arg2Size)
445  THROW_MBSIMERROR("The dimension of the second parameter does not match.");
446  if(this->retSize1!=0 && ret.size1()!=this->retSize1)
447  THROW_MBSIMERROR("The output row dimension does not match.");
448  if(this->retSize2!=0 && ret.size2()!=this->retSize2)
449  THROW_MBSIMERROR("The output column dimension does not match.");
450  }
451  };
452 
453 }
454 
455 #endif
Definition: symbolic_function.h:177
void init(Element::InitStage stage)
plots time series header
Definition: symbolic_function.h:193
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:97
Definition: planar_contour.h:31
void init(Element::InitStage stage)
plots time series header
Definition: symbolic_function.h:296
Definition: element.h:100
basic error class for mbsim
Definition: mbsim_event.h:38
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