SturmSequence::SturmSequence(const Polynomial& polynomial) {
assert(f.size() == 0);
Polynomial pol0 = Polynomial(0.0);
f.push_back(polynomial);
f.push_back(polynomial.derivative());
if (!(f[1] == pol0)) {
uint32_t n = 2;
Polynomial remainder;
do {
f[n-2].division(f[n-1],remainder);
remainder = remainder * -1;
f.push_back(remainder);
n++;
} while (!(remainder == pol0));
};
}
int main(int argc, char *argv[]) {
// End time
double tf = 10.0;
// Dimensions
int nx = 3;
int np = 1;
// Declare variables
SX x = SX::sym("x",nx); // state
SX p = SX::sym("u",np); // control
// ODE right hand side function
SX ode = vertcat((1 - x(1)*x(1))*x(0) - x(1) + p,
x(0),
x(0)*x(0) + x(1)*x(1) + p*p);
SXDict dae = {{"x", x}, {"p", p}, {"ode", ode}};
// Number of finite elements
int n = 100;
// Size of the finite elements
double h = tf/n;
// Degree of interpolating polynomial
int d = 4;
// Choose collocation points
vector<double> tau_root = collocation_points(d, "legendre");
tau_root.insert(tau_root.begin(), 0);
// Nonlinear solver to use
string solver = "newton";
if (argc>1) solver = argv[1]; // chose a different solver from command line
// Coefficients of the collocation equation
vector<vector<double> > C(d+1,vector<double>(d+1,0));
// Coefficients of the continuity equation
vector<double> D(d+1,0);
// For all collocation points
for(int j=0; j<d+1; ++j){
// Construct Lagrange polynomials to get the polynomial basis at the collocation point
Polynomial p = 1;
for(int r=0; r<d+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] = p(1.0);
// 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<d+1; ++r){
C[j][r] = dp(tau_root[r]);
}
}
// Total number of variables for one finite element
MX X0 = MX::sym("X0",nx);
MX P = MX::sym("P",np);
MX V = MX::sym("V",d*nx);
// Get the state at each collocation point
vector<MX> X(1,X0);
for(int r=0; r<d; ++r){
X.push_back(V.nz(Slice(r*nx,(r+1)*nx)));
}
// Get the collocation quations (that define V)
Function f("f", {dae["x"], dae["p"]}, {dae["ode"]});
vector<MX> V_eq;
for(int j=1; j<d+1; ++j){
// Expression for the state derivative at the collocation point
MX xp_j = 0;
for(int r=0; r<d+1; ++r){
xp_j += C[r][j]*X[r];
}
// Append collocation equations
vector<MX> v = {X[j], P};
v = f(v);
V_eq.push_back(h*v[0] - xp_j);
}
// Root-finding function, implicitly defines V as a function of X0 and P
Function vfcn("vfcn", {V, X0, P}, {vertcat(V_eq)});
// Convert to sxfunction to decrease overhead
Function vfcn_sx = vfcn.expand("vfcn");
// Create a implicit function instance to solve the system of equations
Dict opts;
if (solver=="ipopt") {
// Use an NLP solver
opts["nlpsol"] = "ipopt";
opts["nlpsol_options"] = Dict{{"print_time", false}, {"ipopt.print_level", 0}};
solver = "nlpsol";
} else if (solver=="kinsol") {
opts["linear_solver_type"] = "user_defined";
}
Function ifcn = rootfinder("ifcn", solver, vfcn_sx, opts);
// Get an expression for the state at the end of the finite element
vector<MX> ifcn_arg = {MX(), X0, P};
V = ifcn(ifcn_arg).front();
X.resize(1);
for(int r=0; r<d; ++r){
X.push_back(V.nz(Slice(r*nx, (r+1)*nx)));
}
MX XF = 0;
for(int r=0; r<d+1; ++r){
XF += D[r]*X[r];
}
// Get the discrete time dynamics
Function F("F", {X0, P}, {XF});
// Do this iteratively for all finite elements
MX Xk = X0;
for(int i=0; i<n; ++i){
Xk = F(vector<MX>{Xk, P}).at(0);
}
// Fixed-step integrator
Function irk_integrator("irk_integrator", MXDict{{"x0", X0}, {"p", P}, {"xf", Xk}},
integrator_in(), integrator_out());
// Create a convensional integrator for reference
Function ref_integrator = integrator("ref_integrator",
"cvodes", dae, {{"tf", tf}});
// Test values
vector<double> x0_val = {0, 1, 0};
double p_val = 0.2;
// Make sure that both integrators give consistent results
for(int integ=0; integ<2; ++integ){
Function F = integ==0 ? irk_integrator : ref_integrator;
cout << "-------" << endl;
cout << "Testing " << F.name() << endl;
cout << "-------" << endl;
// Generate a new function that calculates forward and reverse directional derivatives
Function dF = F.factory("dF", {"x0", "p", "fwd:x0", "fwd:p", "adj:xf"},
{"xf", "fwd:xf", "adj:x0", "adj:p"});
// Arguments for evaluation
map<string, DM> arg, res;
arg["x0"] = x0_val;
arg["p"] = p_val;
// Forward sensitivity analysis, first direction: seed p and x0[0]
arg["fwd_x0"] = vector<double>{1, 0, 0};
arg["fwd_p"] = 1;
// Adjoint sensitivity analysis, seed xf[2]
arg["adj_xf"] = vector<double>{0, 0, 1};
// Integrate
res = dF(arg);
// Get the nondifferentiated results
cout << setw(15) << "xf = " << res.at("xf") << endl;
// Get the forward sensitivities
cout << setw(15) << "d(xf)/d(p)+d(xf)/d(x0[0]) = " << res.at("fwd_xf") << endl;
// Get the adjoint sensitivities
cout << setw(15) << "d(xf[2])/d(x0) = " << res.at("adj_x0") << endl;
cout << setw(15) << "d(xf[2])/d(p) = " << res.at("adj_p") << endl;
}
return 0;
}
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();
}
}
int main(int argc, char* argv[])
{
// quadraticFormulaTest();
// return 0;
//
// mullerTest2();
// return 0;
//
// mullerTest();
// return 0;
//
// deflationTest();
// return 0;
//
// deflationTest2();
// return 0;
if(argc < 2)
{
cerr << "No file specified... Aborting.\n";
return 1;
}
// RealDouble::setEpsilon(0);
// ifstream file(argv[1]);
// Polynomial<type> origP;
// file >> origP;
//
// RealDouble::setEpsilon(.00000001);
// const type scalar = 1000000000;
// cout << "Read in Polynomial:\n" << origP << '\n';
// cout << "p = " << scalar << " * origP.\n\n";
// Polynomial<type> p(scalar * origP);
RealDouble::setEpsilon(.00000001);
ifstream file(argv[1]);
Polynomial<type> p;
file >> p;
cout << "Original Polynomial:\n";
polynomialRootFinderTest( p );
cout << "First Derivative:\n";
polynomialRootFinderTest( p.derivative() );
cout << "Second Derivative:\n";
polynomialRootFinderTest( p.derivative().derivative() );
cout << "Third Derivative:\n";
polynomialRootFinderTest( p.derivative().derivative().derivative() );
cout << "Fourth Derivative:\n";
polynomialRootFinderTest( p.derivative().derivative().derivative().derivative() );
// Polynomial<type> firstDeriv = p.derivative();
// cout << "First Derivative: " << firstDeriv << '\n';
//
// Polynomial<type> secondDeriv = firstDeriv.derivative();
// cout << "Second Derivative: " << secondDeriv << '\n';
//
// Polynomial<type> thirdDeriv = secondDeriv.derivative();
// cout << "Third Derivative: " << thirdDeriv << '\n';
//// cout << "Third Derivative(7.16098) = " << thirdDeriv(7.16098) << '\n';
//// cout << "Third Derivative(2.85193) = " << thirdDeriv(2.85193) << '\n';
//
// cout << "Polynomial degree: " << p.degree() << '\n';
// cout << "First Derivative degree: " << firstDeriv.degree() << '\n';
// cout << "Second Derivative degree: " << secondDeriv.degree() << '\n';
// cout << "Third Derivative degree: " << thirdDeriv.degree() << '\n';
}
