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>
31 casadi::SX
jac(
const casadi::SX &f,
const casadi::SX &arg) {
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());
40 return casadi::SX::nan();
44 return casadi::SX::jacobian(f, arg);
48 casadi::DM
c(
const double &x) {
55 casadi::DM y(x.size(), 1);
56 for(
int i=0; i<x.size(); i++)
65 static Ret cast(
const casadi::Matrix<double> &x) {
66 throw MBSimError(
"FromCasadi::cast not implemented for current type.");
75 for(
int i=0; i<x.size1(); i++)
76 y.e(i) = x(i,0).scalar();
86 for(
int i=0; i<x.size2(); i++)
87 y.e(i) = x(0,i).scalar();
92 template <
class Row,
class Col>
93 class FromCasadi<fmatvec::Matrix<fmatvec::General,Row,Col,double> > {
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();
107 static double cast(
const casadi::Matrix<double> &x) {
114 T
c(
const casadi::DM &x) {
120 template<
typename Ret,
typename Arg>
125 mutable casadi::SX pd_;
126 casadi::SX pddd_, pdpd_;
127 casadi::Function f, pd, dd, pddd, pdpd, dddd;
130 SymbolicFunction(
const casadi::SX &ret_,
const casadi::SX &arg_) : ret(ret_), arg(arg_) {
131 checkFunctionIODim();
137 casadi::SX argd=casadi::SX::sym(
"argd", getArgSize());
138 casadi::SX argd_2=casadi::SX::sym(
"argd_2", getArgSize());
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);
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_});
155 std::pair<int, int> getRetSize()
const override {
156 return std::make_pair(this->ret.size1(), this->ret.size2());
159 int getArgSize()
const override {
163 bool constParDer()
const override {
164 if(pd_.is_empty(
true))
166 return pd_.is_constant();
169 Ret operator()(
const Arg& x)
override {
170 return c<Ret>(f(std::vector<casadi::SX>{
c(x)})[0]);
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]);
179 Ret dirDer(
const Arg &xd,
const Arg &x)
override {
180 return c<Ret>(dd(std::vector<casadi::SX>{
c(xd),
c(x)})[0]);
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]);
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]);
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]);
199 void initializeUsingXML(xercesc::DOMElement *element)
override {
200 auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
205 checkFunctionIODim();
210 void checkFunctionIODim() {
212 if(arg.size2()!=1) THROW_MBSIMERROR(
"Matrix parameters are not allowed.");
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.");
223 template<
typename Ret,
typename Arg1,
typename Arg2>
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;
233 SymbolicFunction(
const casadi::SX &ret_,
const casadi::SX &arg1_,
const casadi::SX &arg2_) : ret(ret_), arg1(arg1_), arg2(arg2_) {
234 checkFunctionIODim();
240 casadi::SX arg1d=casadi::SX::sym(
"arg1d", getArg1Size());
241 casadi::SX arg2d=casadi::SX::sym(
"arg2d", getArg2Size());
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);
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_});
266 std::pair<int, int> getRetSize()
const override {
267 return std::make_pair(ret.size1(), ret.size2());
270 int getArg1Size()
const override {
274 int getArg2Size()
const override {
278 bool constParDer1()
const override {
279 if(pd1_.is_empty(
true))
280 pd1_ =
jac(ret, arg1);
281 return pd1_.is_constant();
284 bool constParDer2()
const override {
285 if(pd2_.is_empty(
true))
286 pd2_ =
jac(ret, arg2);
287 return pd2_.is_constant();
290 Ret operator()(
const Arg1& x1,
const Arg2& x2)
override {
291 return c<Ret>(f(std::vector<casadi::SX>{
c(x1),
c(x2)})[0]);
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]);
300 Ret dirDer1(
const Arg1 &arg1Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
301 THROW_MBSIMERROR(
"Not implemented");
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]);
310 Ret dirDer2(
const Arg2 &arg2Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
311 THROW_MBSIMERROR(
"Not implemented");
314 typename B::DDRetDDArg1 parDer1ParDer1(
const Arg1 &arg1,
const Arg2 &arg2)
override {
315 THROW_MBSIMERROR(
"Not implemented");
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]);
324 Ret dirDer1DirDer1(
const Arg1 &arg1Dir_1,
const Arg1 &arg1Dir_2,
const Arg1 &arg1,
const Arg2 &arg2)
override {
325 THROW_MBSIMERROR(
"Not implemented");
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]);
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]);
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]);
346 Ret dirDer2DirDer2(
const Arg2 &arg2Dir_1,
const Arg2 &arg2Dir_2,
const Arg1 &arg1,
const Arg2 &arg2)
override {
347 THROW_MBSIMERROR(
"Not implemented");
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]);
356 Ret dirDer2DirDer1(
const Arg2 &arg1Dir,
const Arg1 &arg1,
const Arg2 &arg2)
override {
357 THROW_MBSIMERROR(
"Not implemented");
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]);
366 void initializeUsingXML(xercesc::DOMElement *element) {
367 auto io=casadi::createCasADiFunctionFromXML(element->getFirstElementChild());
373 checkFunctionIODim();
378 void checkFunctionIODim() {
380 if(arg1.size2()!=1) THROW_MBSIMERROR(
"Matrix parameters are not allowed.");
381 if(arg2.size2()!=1) THROW_MBSIMERROR(
"Matrix parameters are not allowed.");
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.");
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