C++ SX类代码示例

  void SymbolicQr::evaluateSXGen(const SXPtrV& input, SXPtrV& output, bool tr) {
    // Get arguments
    casadi_assert(input.at(0)!=0);
    SX r = *input.at(0);
    casadi_assert(input.at(1)!=0);
    SX A = *input.at(1);

    // Number of right hand sides
    int nrhs = r.size2();

    // Factorize A
    vector<SX> v = fact_fcn_(A);

    // Select solve function
    Function& solv = tr ? solv_fcn_T_ : solv_fcn_N_;

    // Solve for every right hand side
    vector<SX> resv;
    v.resize(3);
    for (int i=0; i<nrhs; ++i) {
      v[2] = r(Slice(), i);
      resv.push_back(solv(v).at(0));
    }

    // Collect the right hand sides
    casadi_assert(output[0]!=0);
    *output.at(0) = horzcat(resv);
  }

bool SX::isEqual(const SX& ex, int depth) const{
  if(node==ex.get())
    return true;
  else if(depth>0)
    return node->isEqual(ex.get(),depth);
  else
    return false;
}

SX MultipleShooting::getOutput(string o)
{
     SX ret = ssym(o, N);
     for (int k=0; k<N; k++)
	  ret.at(k) = getOutput(o, k);
	
     return ret;
}

    /** \brief  Create a binary expression */
    inline static SX create(unsigned char op, const SX& dep0, const SX& dep1){
      if(dep0.isConstant() && dep1.isConstant()){
	// Evaluate constant
	double dep0_val = dep0.getValue();
	double dep1_val = dep1.getValue();
	double ret_val;
	casadi_math<double>::fun(op,dep0_val,dep1_val,ret_val);
	return ret_val;
      } else {
	// Expression containing free variables
	return SX::create(new BinarySX(op,dep0,dep1));
      }
    }

bool SX::isEquivalent(const SX& y, int depth) const{
  if (isEqual(y)) return true;
  if (isConstant() && y.isConstant()) return y.getValue()==getValue();
  if (depth==0) return false;
  
  if (hasDep() && y.hasDep() && getOp()==y.getOp()) {
    if (getDep(0).isEquivalent(y.getDep(0),depth-1)  && getDep(1).isEquivalent(y.getDep(1),depth-1)) return true;
    return (operation_checker<CommChecker>(getOp()) && getDep(0).isEquivalent(y.getDep(1),depth-1)  && getDep(1).isEquivalent(y.getDep(0),depth-1));
  }
  return false;
}

  void QpToNlp::init(const Dict& opts) {
    // Initialize the base classes
    Qpsol::init(opts);

    // Default options
    string nlpsol_plugin;
    Dict nlpsol_options;

    // Read user options
    for (auto&& op : opts) {
      if (op.first=="nlpsol") {
        nlpsol_plugin = op.second.to_string();
      } else if (op.first=="nlpsol_options") {
        nlpsol_options = op.second;
      }
    }

    // Create a symbolic matrix for the decision variables
    SX X = SX::sym("X", n_, 1);

    // Parameters to the problem
    SX H = SX::sym("H", sparsity_in(QPSOL_H));
    SX G = SX::sym("G", sparsity_in(QPSOL_G));
    SX A = SX::sym("A", sparsity_in(QPSOL_A));

    // Put parameters in a vector
    std::vector<SX> par;
    par.push_back(H.nonzeros());
    par.push_back(G.nonzeros());
    par.push_back(A.nonzeros());

    // The nlp looks exactly like a mathematical description of the NLP
    SXDict nlp = {{"x", X}, {"p", vertcat(par)},
                  {"f", mtimes(G.T(), X) + 0.5*mtimes(mtimes(X.T(), H), X)},
                  {"g", mtimes(A, X)}};

    // Create an Nlpsol instance
    casadi_assert_message(!nlpsol_plugin.empty(), "'nlpsol' option has not been set");
    solver_ = nlpsol("nlpsol", nlpsol_plugin, nlp, nlpsol_options);
    alloc(solver_);

    // Allocate storage for NLP solver  parameters
    alloc_w(solver_.nnz_in(NLPSOL_P), true);
  }

  SX SXFunctionInternal::hess(int iind, int oind) {
    casadi_assert_message(output(oind).numel() == 1, "Function must be scalar");
    SX g = grad(iind, oind);
    g.makeDense();
    if (verbose())  userOut() << "SXFunctionInternal::hess: calculating gradient done " << endl;

    // Create function
    Dict opts;
    opts["verbose"] = getOption("verbose");
    SXFunction gfcn("gfcn", make_vector(inputv_.at(iind)),
                    make_vector(g), opts);

    // Calculate jacobian of gradient
    if (verbose()) {
      userOut() << "SXFunctionInternal::hess: calculating Jacobian " << endl;
    }
    SX ret = gfcn.jac(0, 0, false, true);
    if (verbose()) {
      userOut() << "SXFunctionInternal::hess: calculating Jacobian done" << endl;
    }

    // Return jacobian of the gradient
    return ret;
  }

int main(){
  cout << "program started" << endl;
      
  // Dimensions
  int nu = 20;  // Number of control segments
  int nj = 100; // Number of integration steps per control segment

  // optimization variable
  SX u = SX::sym("u", nu); // control

  SX s_0 = 0; // initial position
  SX v_0 = 0; // initial speed
  SX m_0 = 1; // initial mass
  
  SX dt = 10.0/(nj*nu); // time step
  SX alpha = 0.05; // friction
  SX beta = 0.1; // fuel consumption rate

  // Trajectory
  SX s_traj = SX::zeros(nu);
  SX v_traj = SX::zeros(nu);
  SX m_traj = SX::zeros(nu);

  // Integrate over the interval with Euler forward
  SX s = s_0, v = v_0, m = m_0;
  for(int k=0; k<nu; ++k){
    for(int j=0; j<nj; ++j){
      s += dt*v;
      v += dt / m * (u[k]- alpha * v*v);
      m += -dt * beta*u[k]*u[k];
    }
    s_traj[k] = s;
    v_traj[k] = v;
    m_traj[k] = m;
  }

  // Objective function
  SX f = inner_prod(u, u);
    
  // Terminal constraints
  SX g;
  g.append(s);
  g.append(v);
  g.append(v_traj);
  
  // Create the NLP
  SXFunction nlp("nlp", nlpIn("x", u), nlpOut("f", f, "g", g));
  
  // Allocate an NLP solver and buffers
  NlpSolver solver("solver", "ipopt", nlp);
  std::map<std::string, DMatrix> arg, res;

  // Bounds on u and initial condition
  arg["lbx"] = -10;
  arg["ubx"] = 10;
  arg["x0"] = 0.4;
  
  // Bounds on g
  vector<double> gmin(2), gmax(2);
  gmin[0] = gmax[0] = 10;
  gmin[1] = gmax[1] =  0;
  gmin.resize(2+nu, -numeric_limits<double>::infinity());
  gmax.resize(2+nu, 1.1);
  arg["lbg"] = gmin;
  arg["ubg"] = gmax;

  // Solve the problem
  res = solver(arg);
  
  // Print the optimal cost
  double cost(res.at("f"));
  cout << "optimal cost: " << cost << endl;

  // Print the optimal solution
  vector<double> uopt(res.at("x"));
  cout << "optimal control: " << uopt << endl;

  // Get the state trajectory
  vector<double> sopt(nu), vopt(nu), mopt(nu);
  SXFunction xfcn("xfcn", make_vector(u), make_vector(s_traj, v_traj, m_traj));
  assign_vector(sopt, vopt, mopt, xfcn(make_vector(res.at("x"))));
  cout << "position: " << sopt << endl;
  cout << "velocity: " << vopt << endl;
  cout << "mass:     " << mopt << endl;

  // Create Matlab script to plot the solution
  ofstream file;
  string filename = "rocket_ipopt_results.m";
  file.open(filename.c_str());
  file << "% Results file from " __FILE__ << endl;
  file << "% Generated " __DATE__ " at " __TIME__ << endl;
  file << endl;
  file << "cost = " << cost << ";" << endl;
  file << "u = " << uopt << ";" << endl;

  // Save results to file
  file << "t = linspace(0,10.0," << nu << ");"<< endl;
  file << "s = " << sopt << ";" << endl;
  file << "v = " << vopt << ";" << endl;
  file << "m = " << mopt << ";" << endl;
//.........这里部分代码省略.........

  void Sqpmethod::init() {
    // Call the init method of the base class
    NlpSolverInternal::init();

    // Read options
    max_iter_ = getOption("max_iter");
    max_iter_ls_ = getOption("max_iter_ls");
    c1_ = getOption("c1");
    beta_ = getOption("beta");
    merit_memsize_ = getOption("merit_memory");
    lbfgs_memory_ = getOption("lbfgs_memory");
    tol_pr_ = getOption("tol_pr");
    tol_du_ = getOption("tol_du");
    regularize_ = getOption("regularize");
    exact_hessian_ = getOption("hessian_approximation")=="exact";
    min_step_size_ = getOption("min_step_size");

    // Get/generate required functions
    gradF();
    jacG();
    if (exact_hessian_) {
      hessLag();
    }

    // Allocate a QP solver
    Sparsity H_sparsity = exact_hessian_ ? hessLag().output().sparsity()
        : Sparsity::dense(nx_, nx_);
    H_sparsity = H_sparsity + Sparsity::diag(nx_);
    Sparsity A_sparsity = jacG().isNull() ? Sparsity(0, nx_)
        : jacG().output().sparsity();

    // QP solver options
    Dict qp_solver_options;
    if (hasSetOption("qp_solver_options")) {
      qp_solver_options = getOption("qp_solver_options");
    }

    // Allocate a QP solver
    qp_solver_ = QpSolver("qp_solver", getOption("qp_solver"),
                          make_map("h", H_sparsity, "a", A_sparsity),
                          qp_solver_options);

    // Lagrange multipliers of the NLP
    mu_.resize(ng_);
    mu_x_.resize(nx_);

    // Lagrange gradient in the next iterate
    gLag_.resize(nx_);
    gLag_old_.resize(nx_);

    // Current linearization point
    x_.resize(nx_);
    x_cand_.resize(nx_);
    x_old_.resize(nx_);

    // Constraint function value
    gk_.resize(ng_);
    gk_cand_.resize(ng_);

    // Hessian approximation
    Bk_ = DMatrix::zeros(H_sparsity);

    // Jacobian
    Jk_ = DMatrix::zeros(A_sparsity);

    // Bounds of the QP
    qp_LBA_.resize(ng_);
    qp_UBA_.resize(ng_);
    qp_LBX_.resize(nx_);
    qp_UBX_.resize(nx_);

    // QP solution
    dx_.resize(nx_);
    qp_DUAL_X_.resize(nx_);
    qp_DUAL_A_.resize(ng_);

    // Gradient of the objective
    gf_.resize(nx_);

    // Create Hessian update function
    if (!exact_hessian_) {
      // Create expressions corresponding to Bk, x, x_old, gLag and gLag_old
      SX Bk = SX::sym("Bk", H_sparsity);
      SX x = SX::sym("x", input(NLP_SOLVER_X0).sparsity());
      SX x_old = SX::sym("x", x.sparsity());
      SX gLag = SX::sym("gLag", x.sparsity());
      SX gLag_old = SX::sym("gLag_old", x.sparsity());

      SX sk = x - x_old;
      SX yk = gLag - gLag_old;
      SX qk = mul(Bk, sk);

      // Calculating theta
      SX skBksk = inner_prod(sk, qk);
      SX omega = if_else(inner_prod(yk, sk) < 0.2 * inner_prod(sk, qk),
                               0.8 * skBksk / (skBksk - inner_prod(sk, yk)),
                               1);
      yk = omega * yk + (1 - omega) * qk;
      SX theta = 1. / inner_prod(sk, yk);
      SX phi = 1. / inner_prod(qk, sk);
//.........这里部分代码省略.........

int main(){
  cout << "program started" << endl;

  std::ofstream resfile;
  resfile.open ("results_biegler_10_1.txt");

  // Test with different number of elements
  for(int N=1; N<=10; ++N){
  
  // Degree of interpolating polynomial
  int K = 2;
  
  // Legrandre roots
  vector<double> tau_root(K+1);
  tau_root[0] = 0.;
  tau_root[1] = 0.211325;
  tau_root[2] = 0.788675;

  // Radau roots (K=3)
  /*  tau_root[0] = 0;
  tau_root[1] = 0.155051;
  tau_root[2] = 0.644949;
  tau_root[3] = 1;*/

  
  // Time
  SX t("t");
  
  // Differential equation
  SX z("z");
  SXFunction F(z,z*z - 2*z + 1);
  F.setOption("name","dz/dt");
  F.init();
  cout << F << endl;
  
  double z0 = -3;
  
  // Analytic solution
  SXFunction z_analytic(t, (4*t-3)/(3*t+1));
  z_analytic.setOption("name","analytic solution");
  z_analytic.init();
  cout << z_analytic << endl;
  
  // Collocation point
  SX tau("tau");

  // Step size
  double h = 1.0/N;
  
  // Lagrange polynomials
  vector<SXFunction> l(K+1);
  for(int j=0; j<=K; ++j){
    SX L = 1;
    for(int k=0; k<=K; ++k)
      if(k != j)
        L *= (tau-tau_root[k])/(tau_root[j]-tau_root[k]);
  
    l[j] = SXFunction(tau,L);
    stringstream ss;
    ss << "l(" << j << ")";
    l[j].setOption("name",ss.str());    
    l[j].init();
    cout << l[j] << endl;
  }
  
  // Get the coefficients of the continuity equation
  vector<double> D(K+1);
  for(int j=0; j<=K; ++j){
    l[j].setInput(1.0);
    l[j].evaluate();
    l[j].getOutput(D[j]);
  }
  cout << "D = " << D << endl;

  // Get the coefficients of the collocation equation
  vector<vector<double> > C(K+1);
  for(int j=0; j<=K; ++j){
    C[j].resize(K+1);
    for(int k=0; k<=K; ++k){
      l[j].setInput(tau_root[k]);
      l[j].setFwdSeed(1.0);
      l[j].evaluate(1,0);
      l[j].getFwdSens(C[j][k]);
    }
  }
  cout << "C = " << C << endl;
  
  // Collocated states
  SX Z = ssym("Z",N,K+1);
  
  // State at final time
// SX ZF("ZF");
  
  // All variables
  SX x;
  x << vec(trans(Z));
  // x << vec(ZF);  
  cout << "x = " << x << endl;
  
  // Construct the "NLP"
//.........这里部分代码省略.........

 DM value(const SX& x, const std::vector<MX>& values=std::vector<MX>()) const {
   return DM::nan(x.sparsity());
 }

int main(){
  cout << "program started" << endl;

  // Dimensions
  int nk = 100;  // Number of control segments
  int nj = 100; // Number of integration steps per control segment

  // Control
  SX u = ssym("u",nk); // control

  // Number of states
  int nx = 3;
  
  // Intermediate variables with initial values and bounds
  SX v, v_def;
  DMatrix v_init, v_min, v_max;
  
  // Initial values and bounds for the state at the different stages
  DMatrix x_k_init =  DMatrix::zeros(nx);
  DMatrix x_k_min  = -DMatrix::inf(nx); 
  DMatrix x_k_max  =  DMatrix::inf(nx);
  
  // Initial conditions
  DMatrix x_0 = DMatrix::zeros(nx);
  x_0[0] = 0; // x
  x_0[1] = 1; // y
  x_0[2] = 0; // lterm
  
  double tf = 10;
  SX dt = tf/(nj*nk); // time step

  // For all the shooting intervals
  SX x_k = x_0;
  SX ode_rhs(x_k.sparsity(),0);
  for(int k=0; k<nk; ++k){
    // Get control
    SX u_k = u[k].at(0);
    
    // Integrate over the interval with Euler forward
    for(int j=0; j<nj; ++j){
      
      // ODE right hand side
      ode_rhs[0] = (1 - x_k[1]*x_k[1])*x_k[0] - x_k[1] + u_k;
      ode_rhs[1] = x_k[0];
      ode_rhs[2] = x_k[0]*x_k[0] + x_k[1]*x_k[1];
      
      // Take a step
      x_k += dt*ode_rhs;
    }
    
    // Lift x
    v_def.append(x_k);
    v_init.append(x_k_init);
    v_min.append(x_k_min);
    v_max.append(x_k_max);
    
    // Allocate intermediate variables
    stringstream ss;
    ss << "v_" << k;
    x_k = ssym(ss.str(),nx);
    v.append(x_k);
  }

  // Objective function
  SX f = x_k[2] + (tf/nk)*inner_prod(u,u);
  
  // Terminal constraints
  SX g;
  g.append(x_k[0]);
  g.append(x_k[1]);

  // Bounds on g
  DMatrix g_min = DMatrix::zeros(2);
  DMatrix g_max = DMatrix::zeros(2);

  // Bounds on u and initial condition
  DMatrix u_min  = -0.75*DMatrix::ones(nk);
  DMatrix u_max  =  1.00*DMatrix::ones(nk);
  DMatrix u_init =       DMatrix::zeros(nk);
  DMatrix xv_min = vertcat(u_min,v_min);
  DMatrix xv_max = vertcat(u_max,v_max);
  DMatrix xv_init = vertcat(u_init,v_init);
  DMatrix gv_min = vertcat(DMatrix::zeros(v.size()),g_min);
  DMatrix gv_max = vertcat(DMatrix::zeros(v.size()),g_max);
  
  // Formulate the full-space NLP
  SXFunction ffcn(vertcat(u,v),f);
  SXFunction gfcn(vertcat(u,v),vertcat(v_def-v,g));

  Dictionary qp_solver_options;
  qp_solver_options["printLevel"] = "none";
  
  // Solve using multiple NLP solvers
  enum Tests{IPOPT, LIFTED_SQP, FULLSPACE_SQP, OLD_SQP_METHOD, NUM_TESTS};
  for(int test=0; test<NUM_TESTS; ++test){
    // Get the nlp solver and NLP solver options
    NLPSolver nlp_solver;
    switch(test){
      case IPOPT:
	cout << "Testing IPOPT" << endl;
//.........这里部分代码省略.........

void fill(SXMatrix& mat, const SX& val){
  if(val->isZero())    mat.makeEmpty(mat.size1(),mat.size2());
  else                 mat.makeDense(mat.size1(),mat.size2(),val);
}

  void AmplInterface::init(const Dict& opts) {
    // Call the init method of the base class
    Nlpsol::init(opts);

    // Set default options
    solver_ = "ipopt";

    // Read user options
    for (auto&& op : opts) {
      if (op.first=="solver") {
        solver_ = op.first;
      }
    }

    // Extract the expressions
    casadi_assert(oracle().is_a("SXFunction"),
                  "Only SX supported currently.");
    vector<SX> xp = oracle().sx_in();
    vector<SX> fg = oracle()(xp);

    // Get x, p, f and g
    SX x = xp.at(NL_X);
    SX p = xp.at(NL_P);
    SX f = fg.at(NL_F);
    SX g = fg.at(NL_G);
    casadi_assert(p.is_empty(), "'p' currently not supported");

    // Names of the variables, constraints
    vector<string> x_name, g_name;
    for (casadi_int i=0; i<nx_; ++i) x_name.push_back("x[" + str(i) + "]");
    for (casadi_int i=0; i<ng_; ++i) g_name.push_back("g[" + str(i) + "]");
    casadi_int max_x_name = x_name.back().size();
    casadi_int max_g_name = g_name.empty() ? 0 : g_name.back().size();

    // Calculate the Jacobian, gradient
    Sparsity jac_g = SX::jacobian(g, x).sparsity();
    Sparsity jac_f = SX::jacobian(f, x).sparsity();

    // Extract the shared subexpressions
    vector<SX> ex = {f, g}, v, vdef;
    shared(ex, v, vdef);
    f = ex[0];
    g = ex[1];

    // Header
    nl_init_ << "g3 1 1 0\n";
    // Type of constraints
    nl_init_ << nx_ << " " // number of variables
       << ng_ << " " // number of constraints
       << 1 << " " // number of objectives
       << 0 << " " // number of ranges
       << 0 << " " // ?
       << 0 << "\n"; // number of logical constraints

    // Nonlinearity - assume all nonlinear for now TODO: Detect
    nl_init_ << ng_ << " "  // nonlinear constraints
       << 1 << "\n"; // nonlinear objectives

    // Network constraints
    nl_init_ << 0 << " " // nonlinear
       << 0 << "\n"; // linear

    // Nonlinear variables
    nl_init_ << nx_ << " " // in constraints
       << nx_ << " " // in objectives
       << nx_ << "\n"; // in both

    // Linear network ..
    nl_init_ << 0 << " " // .. variables ..
       << 0 << " " // .. arith ..
       << 0 << " " // .. functions ..
       << 0 << "\n"; // .. flags

    // Discrete variables
    nl_init_ << 0 << " " // binary
       << 0 << " " // integer
       << 0 << " " // nonlinear in both
       << 0 << " " // nonlinear in constraints
       << 0 << "\n"; // nonlinear in objective

    // Nonzeros in the Jacobian, gradients
    nl_init_ << jac_g.nnz() << " " // nnz in Jacobian
       << jac_f.nnz() << "\n"; // nnz in gradients

    // Maximum name length
    nl_init_ << max_x_name << " " // constraints
       << max_g_name << "\n"; // variables

    // Shared subexpressions
    nl_init_ << v.size() << " " // both
       << 0 << " " // constraints
       << 0 << " " // objective
       << 0 << " " // c1 - constaint, but linear?
       << 0 << "\n"; // o1 - objective, but linear?

    // Create a function which evaluates f and g
    Function F("F", {vertcat(v), x}, {vertcat(vdef), f, g},
              {"v", "x"}, {"vdef", "f", "g"});

    // Iterate over the algoritm
//.........这里部分代码省略.........

void
dxdt(map<string,SX> &xDot, map<string,SX> &outputs, map<string,SX> state, map<string,SX> action, map<string,SX> param, SX t)
{
	double g  = 9.8;
	double L1 = 0.1;
	double L2 = 0.1;
	double m0 = 0.1;
	double mp = 0.03;
	double m1 = mp;
	double m2 = mp;
     
	double d1 = m0 + m1 + m2;
	double d2 = (0.5*m1 + m2)*L1;
	double d3 = 0.5 * m2 * L2;
	double d4 = ( m1/3 + m2 )*SQR(L1);
	double d5 = 0.5 * m2 * L1 * L2;
	double d6 = m2 * SQR(L2)/3;
	double f1 = (0.5*m1 + m2) * L1 * g;
	double f2 = 0.5 * m2 * L2 * g;

	// th0  = y(1);
	// th1  = y(2);
	// th2  = y(3);
	// th0d = y(4);
	// th1d = y(5);
	// th2d = y(6);

	SX th0  = state["th0"];
	SX th1  = state["th1"];
	SX th2  = state["th2"];
	SX th0d = state["th0d"];
	SX th1d = state["th1d"];
	SX th2d = state["th2d"];

	// D = [          d1,     d2*cos(th1),     d3*cos(th2);
	// 	     d2*cos(th1),              d4, d5*cos(th1-th2);
	// 	     d3*cos(th2), d5*cos(th1-th2),              d6;];

	SX D( zerosSX(3,3) );
	makeDense(D);
	D[0,0] =          d1;   D[0,1] =     d2*cos(th1);   D[0,2] =     d3*cos(th2);
	D[1,0] = d2*cos(th1);   D[1,1] =              d4;   D[1,2] = d5*cos(th1-th2);
	D[2,0] = d3*cos(th2);   D[2,1] = d5*cos(th1-th2);   D[2,2] =              d6;

	// C = [0,     -d2*sin(th1)*th1d,    -d3*sin(th2)*th2d;
	// 	    0,                     0, d5*sin(th1-th2)*th2d;
	// 	    0, -d5*sin(th1-th2)*th1d,                    0;];
	SX C( zerosSX(3,3) );
	makeDense(C);
	C[0,0] = 0;   C[0,1] =      -d2*sin(th1)*th1d;   C[0,2] =     -d3*sin(th2)*th2d;
	C[1,0] = 0;   C[1,1] =                      0;   C[1,2] =  d5*sin(th1-th2)*th2d;
	C[2,0] = 0;   C[2,1] =  -d5*sin(th1-th2)*th1d;   C[2,2] =                     0;

	// G = [0; -f1*sin(th1); -f2*sin(th2);];
	SX G( zerosSX(3,1) );
	makeDense(G);
	G.at(0) = 0;
	G.at(1) = -f1*sin(th1);
	G.at(2) = -f2*sin(th2);

	// H = [1;0;0;];
	SX H( zerosSX(3,1) );
	makeDense(H);
	H.at(0) = 1;
	H.at(1) = 0;
	H.at(2) = 0;

	// dy(1:3) = y(4:6);
	xDot["th0"] = th0d;
	xDot["th1"] = th1d;
	xDot["th2"] = th2d;

	// dy(4:6) = D\( - C*y(4:6) - G + H*u );
	SX vel( zerosSX(3,1) );
	makeDense(vel);
	vel.at(0) = th0d;
	vel.at(1) = th1d;
	vel.at(2) = th2d;
	SX accel = mul( inv(D), - mul( C, vel ) - G + mul( H, SX(action["u"]) ) );

	simplify(accel.at(0));
	simplify(accel.at(1));
	simplify(accel.at(2));

	xDot["th0d"] = accel.at(0);
	xDot["th1d"] = accel.at(1);
	xDot["th2d"] = accel.at(2);

	// cout << th0 << endl;
	// cout << th1 << endl;
	// cout << th2 << endl;

	// cout << th0d << endl;
	// cout << th1d << endl;
	// cout << th2d << endl;

	// cout << accel.at(0) << endl;
	// cout << accel.at(1) << endl;
	// cout << accel.at(2) << endl;

//.........这里部分代码省略.........