Function simpleRK(Function f, int N, int order) {
// Consistency check
casadi_assert_message(N>=1, "Parameter N (number of steps) must be at least 1, but got "
<< N << ".");
casadi_assert_message(order==4, "Only RK order 4 is supported now.");
casadi_assert_message(f.n_in()==2, "Function must have two inputs: x and p");
casadi_assert_message(f.n_out()==1, "Function must have one outputs: dot(x)");
MX x0 = MX::sym("x0", f.sparsity_in(0));
MX p = MX::sym("p", f.sparsity_in(1));
MX h = MX::sym("h");
std::vector<double> b(order);
b[0]=1.0/6;
b[1]=1.0/3;
b[2]=1.0/3;
b[3]=1.0/6;
std::vector<double> c(order);
c[0]=0;
c[1]=1.0/2;
c[2]=1.0/2;
c[3]=1;
std::vector< std::vector<double> > A(order-1);
A[0].resize(1);
A[0][0]=1.0/2;
A[1].resize(2);
A[1][0]=0;A[1][1]=1.0/2;
A[2].resize(3);
A[2][0]=0;
A[2][1]=0;A[2][2]=1;
// Time step
MX dt = h/N;
std::vector<MX> k(order);
vector<MX> f_arg(2);
// Integrate
MX xf = x0;
for (int i=0; i<N; ++i) {
for (int j=0; j<order; ++j) {
MX xL = 0;
for (int jj=0; jj<j; ++jj) {
xL += k.at(jj)*A.at(j-1).at(jj);
}
f_arg[0] = xf+xL;
f_arg[1] = p;
k[j] = dt*f(f_arg).at(0);
}
for (int j=0; j<order; ++j) {
xf += b.at(j)*k.at(j);
}
}
// Form discrete-time dynamics
return Function("F", {x0, p, h}, {xf}, {"x0", "p", "h"}, {"xf"});
}
octave_caller_t::octave_caller_t()
{
wordexp_t p;
std::string test_path = "$PRACSYS_PATH/prx_utilities/prx/utilities/octave_interface/functions/";
wordexp(test_path.c_str(), &p, 0);
std::string dir(p.we_wordv[0]);
f_arg(0) = dir;
const char * argvv[] = {"", "-q"};
octave_main(2, (char**)argvv, true);
feval("cd", f_arg);
}
/*
* call-seq:
* cmp ** numeric -> complex
*
* Performs exponentiation.
*
* Complex('i') ** 2 #=> (-1+0i)
* Complex(-8) ** Rational(1, 3) #=> (1.0000000000000002+1.7320508075688772i)
*/
static VALUE
nucomp_expt(VALUE self, VALUE other)
{
if (k_numeric_p(other) && k_exact_zero_p(other))
return f_complex_new_bang1(CLASS_OF(self), ONE);
if (k_rational_p(other) && f_one_p(f_denominator(other)))
other = f_numerator(other); /* c14n */
if (k_complex_p(other)) {
get_dat1(other);
if (k_exact_zero_p(dat->imag))
other = dat->real; /* c14n */
}
if (k_complex_p(other)) {
VALUE r, theta, nr, ntheta;
get_dat1(other);
r = f_abs(self);
theta = f_arg(self);
nr = m_exp_bang(f_sub(f_mul(dat->real, m_log_bang(r)),
f_mul(dat->imag, theta)));
ntheta = f_add(f_mul(theta, dat->real),
f_mul(dat->imag, m_log_bang(r)));
return f_complex_polar(CLASS_OF(self), nr, ntheta);
}
if (k_fixnum_p(other)) {
if (f_gt_p(other, ZERO)) {
VALUE x, z;
long n;
x = self;
z = x;
n = FIX2LONG(other) - 1;
while (n) {
long q, r;
while (1) {
get_dat1(x);
q = n / 2;
r = n % 2;
if (r)
break;
x = nucomp_s_new_internal(CLASS_OF(self),
f_sub(f_mul(dat->real, dat->real),
f_mul(dat->imag, dat->imag)),
f_mul(f_mul(TWO, dat->real), dat->imag));
n = q;
}
z = f_mul(z, x);
n--;
}
return z;
}
return f_expt(f_reciprocal(self), f_negate(other));
}
if (k_numeric_p(other) && f_real_p(other)) {
VALUE r, theta;
if (k_bignum_p(other))
rb_warn("in a**b, b may be too big");
r = f_abs(self);
theta = f_arg(self);
return f_complex_polar(CLASS_OF(self), f_expt(r, other),
f_mul(theta, other));
}
return rb_num_coerce_bin(self, other, id_expt);
}
/*
* call-seq:
* num.polar -> array
*
* Returns an array; [num.abs, num.arg].
*/
static VALUE
numeric_polar(VALUE self)
{
return rb_assoc_new(f_abs(self), f_arg(self));
}
void CollocationIntegratorInternal::setupFG() {
// Interpolation order
deg_ = getOption("interpolation_order");
// All collocation time points
std::vector<long double> tau_root = collocationPointsL(deg_, getOption("collocation_scheme"));
// Coefficients of the collocation equation
vector<vector<double> > C(deg_+1, vector<double>(deg_+1, 0));
// Coefficients of the continuity equation
vector<double> D(deg_+1, 0);
// Coefficients of the quadratures
vector<double> B(deg_+1, 0);
// For all collocation points
for (int j=0; j<deg_+1; ++j) {
// Construct Lagrange polynomials to get the polynomial basis at the collocation point
Polynomial p = 1;
for (int r=0; r<deg_+1; ++r) {
if (r!=j) {
p *= Polynomial(-tau_root[r], 1)/(tau_root[j]-tau_root[r]);
}
}
// Evaluate the polynomial at the final time to get the
// coefficients of the continuity equation
D[j] = zeroIfSmall(p(1.0L));
// Evaluate the time derivative of the polynomial at all collocation points to
// get the coefficients of the continuity equation
Polynomial dp = p.derivative();
for (int r=0; r<deg_+1; ++r) {
C[j][r] = zeroIfSmall(dp(tau_root[r]));
}
// Integrate polynomial to get the coefficients of the quadratures
Polynomial ip = p.anti_derivative();
B[j] = zeroIfSmall(ip(1.0L));
}
// Symbolic inputs
MX x0 = MX::sym("x0", f_.input(DAE_X).sparsity());
MX p = MX::sym("p", f_.input(DAE_P).sparsity());
MX t = MX::sym("t", f_.input(DAE_T).sparsity());
// Implicitly defined variables (z and x)
MX v = MX::sym("v", deg_*(nx_+nz_));
vector<int> v_offset(1, 0);
for (int d=0; d<deg_; ++d) {
v_offset.push_back(v_offset.back()+nx_);
v_offset.push_back(v_offset.back()+nz_);
}
vector<MX> vv = vertsplit(v, v_offset);
vector<MX>::const_iterator vv_it = vv.begin();
// Collocated states
vector<MX> x(deg_+1), z(deg_+1);
for (int d=1; d<=deg_; ++d) {
x[d] = reshape(*vv_it++, this->x0().shape());
z[d] = reshape(*vv_it++, this->z0().shape());
}
casadi_assert(vv_it==vv.end());
// Collocation time points
vector<MX> tt(deg_+1);
for (int d=0; d<=deg_; ++d) {
tt[d] = t + h_*tau_root[d];
}
// Equations that implicitly define v
vector<MX> eq;
// Quadratures
MX qf = MX::zeros(f_.output(DAE_QUAD).sparsity());
// End state
MX xf = D[0]*x0;
// For all collocation points
for (int j=1; j<deg_+1; ++j) {
//for (int j=deg_; j>=1; --j) {
// Evaluate the DAE
vector<MX> f_arg(DAE_NUM_IN);
f_arg[DAE_T] = tt[j];
f_arg[DAE_P] = p;
f_arg[DAE_X] = x[j];
f_arg[DAE_Z] = z[j];
vector<MX> f_res = f_.call(f_arg);
// Get an expression for the state derivative at the collocation point
MX xp_j = C[0][j] * x0;
for (int r=1; r<deg_+1; ++r) {
xp_j += C[r][j] * x[r];
}
// Add collocation equation
eq.push_back(vec(h_*f_res[DAE_ODE] - xp_j));
// Add the algebraic conditions
eq.push_back(vec(f_res[DAE_ALG]));
// Add contribution to the final state
xf += D[j]*x[j];
// Add contribution to quadratures
qf += (B[j]*h_)*f_res[DAE_QUAD];
}
// Form forward discrete time dynamics
vector<MX> F_in(DAE_NUM_IN);
F_in[DAE_T] = t;
F_in[DAE_X] = x0;
F_in[DAE_P] = p;
F_in[DAE_Z] = v;
vector<MX> F_out(DAE_NUM_OUT);
F_out[DAE_ODE] = xf;
F_out[DAE_ALG] = vertcat(eq);
F_out[DAE_QUAD] = qf;
F_ = MXFunction(F_in, F_out);
F_.init();
// Backwards dynamics
// NOTE: The following is derived so that it will give the exact adjoint
// sensitivities whenever g is the reverse mode derivative of f.
if (!g_.isNull()) {
// Symbolic inputs
MX rx0 = MX::sym("x0", g_.input(RDAE_RX).sparsity());
MX rp = MX::sym("p", g_.input(RDAE_RP).sparsity());
// Implicitly defined variables (rz and rx)
MX rv = MX::sym("v", deg_*(nrx_+nrz_));
vector<int> rv_offset(1, 0);
for (int d=0; d<deg_; ++d) {
rv_offset.push_back(rv_offset.back()+nrx_);
rv_offset.push_back(rv_offset.back()+nrz_);
}
vector<MX> rvv = vertsplit(rv, rv_offset);
vector<MX>::const_iterator rvv_it = rvv.begin();
// Collocated states
vector<MX> rx(deg_+1), rz(deg_+1);
for (int d=1; d<=deg_; ++d) {
rx[d] = reshape(*rvv_it++, this->rx0().shape());
rz[d] = reshape(*rvv_it++, this->rz0().shape());
}
casadi_assert(rvv_it==rvv.end());
// Equations that implicitly define v
eq.clear();
// Quadratures
MX rqf = MX::zeros(g_.output(RDAE_QUAD).sparsity());
// End state
MX rxf = D[0]*rx0;
// For all collocation points
for (int j=1; j<deg_+1; ++j) {
// Evaluate the backward DAE
vector<MX> g_arg(RDAE_NUM_IN);
g_arg[RDAE_T] = tt[j];
g_arg[RDAE_P] = p;
g_arg[RDAE_X] = x[j];
g_arg[RDAE_Z] = z[j];
g_arg[RDAE_RX] = rx[j];
g_arg[RDAE_RZ] = rz[j];
g_arg[RDAE_RP] = rp;
vector<MX> g_res = g_.call(g_arg);
// Get an expression for the state derivative at the collocation point
MX rxp_j = -D[j]*rx0;
for (int r=1; r<deg_+1; ++r) {
rxp_j += (B[r]*C[j][r]) * rx[r];
}
// Add collocation equation
eq.push_back(vec(h_*B[j]*g_res[RDAE_ODE] - rxp_j));
// Add the algebraic conditions
eq.push_back(vec(g_res[RDAE_ALG]));
// Add contribution to the final state
rxf += -B[j]*C[0][j]*rx[j];
// Add contribution to quadratures
rqf += h_*B[j]*g_res[RDAE_QUAD];
}
// Form backward discrete time dynamics
vector<MX> G_in(RDAE_NUM_IN);
G_in[RDAE_T] = t;
G_in[RDAE_X] = x0;
G_in[RDAE_P] = p;
G_in[RDAE_Z] = v;
G_in[RDAE_RX] = rx0;
G_in[RDAE_RP] = rp;
G_in[RDAE_RZ] = rv;
vector<MX> G_out(RDAE_NUM_OUT);
G_out[RDAE_ODE] = rxf;
G_out[RDAE_ALG] = vertcat(eq);
G_out[RDAE_QUAD] = rqf;
G_ = MXFunction(G_in, G_out);
G_.init();
}
}
fun_t * a_sustain(fun_t *sample){
return f_arg(ARG_SUSTAIN,sample);
}
fun_t * a_delay(fun_t *sample){
return f_arg(ARG_DELAY,sample);
}
fun_t * a_vol(fun_t *vol){
return f_arg(ARG_VOLUME,vol);
}
fun_t * a_freq(fun_t *hz){
return f_arg(ARG_FREQ,hz);
}