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 MBSimError(
"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 pd_ = parDerSX<Ret>(ret, arg);
200 casadi::SX dd_ = dirDerSX<Ret>(ret, arg, argd);
203 casadi::SX dddd_ =
dirDerSX(dd_, arg, argd_2);
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_});
214 std::pair<int, int> getRetSize()
const override {
215 return std::make_pair(this->ret.size1(), this->ret.size2());
218 int getArgSize()
const override {
222 bool constParDer()
const override {
223 if(pd_.is_empty(
true))
224 pd_ = parDerSX<Ret>(ret, arg);
225 return pd_.is_constant();
228 Ret operator()(
const Arg& x)
override {
229 return c<Ret>(f(std::vector<casadi::SX>{
c(x)})[0]);
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]);
238 Ret dirDer(
const Arg &xd,
const Arg &x)
override {
239 return c<Ret>(dd(std::vector<casadi::SX>{
c(xd),
c(x)})[0]);
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]);
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]);
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]);
258 void initializeUsingXML(xercesc::DOMElement *element)
override {
259 auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
264 checkFunctionIODim();
269 void checkFunctionIODim() {
271 if(arg.size2()!=1) THROW_MBSIMERROR(
"Matrix parameters are not allowed.");
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.");
282 template<
typename Ret,
typename Arg1,
typename Arg2>
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;
292 SymbolicFunction(
const casadi::SX &ret_,
const casadi::SX &arg1_,
const casadi::SX &arg2_) : ret(ret_), arg1(arg1_), arg2(arg2_) {
293 checkFunctionIODim();
299 casadi::SX arg1d=casadi::SX::sym(
"arg1d", getArg1Size());
300 casadi::SX arg2d=casadi::SX::sym(
"arg2d", getArg2Size());
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);
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_});
325 std::pair<int, int> getRetSize()
const override {
326 return std::make_pair(ret.size1(), ret.size2());
329 int getArg1Size()
const override {
333 int getArg2Size()
const override {
337 bool constParDer1()
const override {
338 if(pd1_.is_empty(
true))
339 pd1_ = parDerSX<Ret>(ret, arg1);
340 return pd1_.is_constant();
343 bool constParDer2()
const override {
344 if(pd2_.is_empty(
true))
345 pd2_ = parDerSX<Ret>(ret, arg2);
346 return pd2_.is_constant();
349 Ret operator()(
const Arg1& x1,
const Arg2& x2)
override {
350 return c<Ret>(f(std::vector<casadi::SX>{
c(x1),
c(x2)})[0]);
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]);
359 Ret dirDer1(
const Arg1 &arg1Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
360 THROW_MBSIMERROR(
"Not implemented");
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]);
369 Ret dirDer2(
const Arg2 &arg2Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
370 THROW_MBSIMERROR(
"Not implemented");
373 typename B::DDRetDDArg1 parDer1ParDer1(
const Arg1 &arg1,
const Arg2 &arg2)
override {
374 THROW_MBSIMERROR(
"Not implemented");
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]);
383 Ret dirDer1DirDer1(
const Arg1 &arg1Dir_1,
const Arg1 &arg1Dir_2,
const Arg1 &arg1,
const Arg2 &arg2)
override {
384 THROW_MBSIMERROR(
"Not implemented");
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]);
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]);
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]);
405 Ret dirDer2DirDer2(
const Arg2 &arg2Dir_1,
const Arg2 &arg2Dir_2,
const Arg1 &arg1,
const Arg2 &arg2)
override {
406 THROW_MBSIMERROR(
"Not implemented");
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]);
415 Ret dirDer2DirDer1(
const Arg2 &arg1Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
416 THROW_MBSIMERROR(
"Not implemented");
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]);
425 void initializeUsingXML(xercesc::DOMElement *element) {
426 auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
432 checkFunctionIODim();
437 void checkFunctionIODim() {
439 if(arg1.size2()!=1) THROW_MBSIMERROR(
"Matrix parameters are not allowed.");
440 if(arg2.size2()!=1) THROW_MBSIMERROR(
"Matrix parameters are not allowed.");
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.");
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