20 #ifndef SYMBOLIC_FUNCTION_H_
21 #define SYMBOLIC_FUNCTION_H_
23 #include <mbsim/functions/function.h>
24 #include <mbxmlutilshelper/casadiXML.h>
25 #include "mbsim/mbsim_event.h"
26 #include <casadi/core/function/function.hpp>
32 template<
typename Ret=
void>
33 casadi::SX
parDerSX(
const casadi::SX &f,
const casadi::SX &arg) {
34 assert(arg.size2()==1);
38 return casadi::SX::reshape(casadi::SX::jacobian(f, arg), f.size1(), f.size2());
41 return casadi::SX::nan();
45 return casadi::SX::jacobian(f, arg);
51 casadi::SX parDerSX<fmatvec::RotMat3>(
const casadi::SX &f,
const casadi::SX &arg) {
54 assert(arg.size2()==1);
55 casadi::SX ret(3, arg.size1());
56 for(
int i=0; i<arg.size1(); ++i) {
58 casadi::SX wtilde=casadi::SX::mtimes(
parDerSX(f, arg(i)), f.T());
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());
75 return jtimes(f, arg, argd);
77 return casadi::SX::reshape(jtimes(casadi::SX::reshape(f, f.size1()*f.size2(), 1), arg, argd), f.size1(), f.size2());
83 casadi::SX dirDerSX<fmatvec::RotMat3>(
const casadi::SX &f,
const casadi::SX &arg,
const casadi::SX &argd) {
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);
93 casadi::DM
c(
const double &x) {
100 casadi::DM y(x.size(), 1);
101 for(
int i=0; i<x.size(); i++)
110 static Ret cast(
const casadi::Matrix<double> &x) {
111 throw std::runtime_error(
"FromCasadi::cast not implemented for current type.");
120 for(
int i=0; i<x.size1(); i++)
121 y.e(i) = x(i,0).scalar();
131 for(
int i=0; i<x.size2(); i++)
132 y.e(i) = x(0,i).scalar();
137 template <
class Row,
class Col>
138 class FromCasadi<fmatvec::Matrix<fmatvec::General,Row,Col,double> > {
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();
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();
166 static double cast(
const casadi::Matrix<double> &x) {
173 T
c(
const casadi::DM &x) {
179 template<
typename Ret,
typename Arg>
184 mutable casadi::SX pd_;
185 casadi::SX pddd_, pdpd_;
186 casadi::Function f, pd, dd, pddd, pdpd, dddd;
189 SymbolicFunction(
const casadi::SX &ret_,
const casadi::SX &arg_) : ret(ret_), arg(arg_) {
190 checkFunctionIODim();
196 casadi::SX argd=casadi::SX::sym(
"argd", getArgSize());
197 casadi::SX argd_2=casadi::SX::sym(
"argd_2", getArgSize());
199 f = casadi::Function(
"noname", {arg}, {ret});
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_});
209 if(config.find(noDer)==config.end() && config.find(noDerDer)==config.end()) {
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_});
220 std::pair<int, int> getRetSize()
const override {
221 return std::make_pair(this->ret.size1(), this->ret.size2());
224 int getArgSize()
const override {
228 bool constParDer()
const override {
229 if(pd_.is_empty(
true))
230 pd_ = parDerSX<Ret>(ret, arg);
231 return pd_.is_constant();
234 Ret operator()(
const Arg& x)
override {
235 return c<Ret>(f(std::vector<casadi::SX>{
c(x)})[0]);
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]);
244 Ret dirDer(
const Arg &xd,
const Arg &x)
override {
245 return c<Ret>(dd(std::vector<casadi::SX>{
c(xd),
c(x)})[0]);
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]);
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]);
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]);
264 void initializeUsingXML(xercesc::DOMElement *element)
override {
265 auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
270 checkFunctionIODim();
275 void checkFunctionIODim() {
277 if(arg.size2()!=1) this->throwError(
"Matrix parameters are not allowed.");
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.");
288 template<
typename Ret,
typename Arg1,
typename Arg2>
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;
298 SymbolicFunction(
const casadi::SX &ret_,
const casadi::SX &arg1_,
const casadi::SX &arg2_) : ret(ret_), arg1(arg1_), arg2(arg2_) {
299 checkFunctionIODim();
305 casadi::SX arg1d=casadi::SX::sym(
"arg1d", getArg1Size());
306 casadi::SX arg2d=casadi::SX::sym(
"arg2d", getArg2Size());
308 f = casadi::Function(
"noname", {arg1, arg2}, {ret});
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_});
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);
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_});
336 std::pair<int, int> getRetSize()
const override {
337 return std::make_pair(ret.size1(), ret.size2());
340 int getArg1Size()
const override {
344 int getArg2Size()
const override {
348 bool constParDer1()
const override {
349 if(pd1_.is_empty(
true))
350 pd1_ = parDerSX<Ret>(ret, arg1);
351 return pd1_.is_constant();
354 bool constParDer2()
const override {
355 if(pd2_.is_empty(
true))
356 pd2_ = parDerSX<Ret>(ret, arg2);
357 return pd2_.is_constant();
360 Ret operator()(
const Arg1& x1,
const Arg2& x2)
override {
361 return c<Ret>(f(std::vector<casadi::SX>{
c(x1),
c(x2)})[0]);
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]);
370 Ret dirDer1(
const Arg1 &arg1Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
371 this->throwError(
"Not implemented");
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]);
380 Ret dirDer2(
const Arg2 &arg2Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
381 this->throwError(
"Not implemented");
384 typename B::DDRetDDArg1 parDer1ParDer1(
const Arg1 &arg1,
const Arg2 &arg2)
override {
385 this->throwError(
"Not implemented");
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]);
394 Ret dirDer1DirDer1(
const Arg1 &arg1Dir_1,
const Arg1 &arg1Dir_2,
const Arg1 &arg1,
const Arg2 &arg2)
override {
395 this->throwError(
"Not implemented");
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]);
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]);
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]);
416 Ret dirDer2DirDer2(
const Arg2 &arg2Dir_1,
const Arg2 &arg2Dir_2,
const Arg1 &arg1,
const Arg2 &arg2)
override {
417 this->throwError(
"Not implemented");
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]);
426 Ret dirDer2DirDer1(
const Arg2 &arg1Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
427 this->throwError(
"Not implemented");
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]);
436 void initializeUsingXML(xercesc::DOMElement *element)
override {
437 auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
443 checkFunctionIODim();
448 void checkFunctionIODim() {
450 if(arg1.size2()!=1) this->throwError(
"Matrix parameters are not allowed.");
451 if(arg2.size2()!=1) this->throwError(
"Matrix parameters are not allowed.");
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.");
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
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