template <typename Robot> bool DDP<Robot>::init(State const & x0_, std::vector<U> const & us_, std::vector<Ref> const & xrefs_, Params const & params_, U const & umin_, U const & umax_, int const & num_)
{
if(xrefs_.size()<num_+1)
{
std::cout<<"ERROR: Not enough references."<<std::endl;
return false;
}
if(us_.size()<num)
{
std::cout<<"ERROR: Not enough control inputs."<<std::endl;
return false;
}
params=params_;
num=num_;
x0=x0_;
us.reserve(num);
xs.reserve(num+1);
xrefs.reserve(num+1);
us.resize(num,VecN::Zero());
xs.resize(num+1,x0);
xrefs.resize(num+1,x0);
umin=umin_;
umax=umax_;
for(int i=0;i<num;i++)
{
us[i]=clamp<N>(us_[i],umin,umax);
xrefs[i]=xrefs_[i];
}
xrefs[num]=xrefs_[num];
xs[0]=x0;
VecM error;
for(int i=0;i<num;i++)
{
error=Robot::State::diff(xs[i], xrefs[i]);
J0+=Robot::L(params.Q,params.R,error,us[i])*dt;
DState dstate(sys,xs[i],us[i]);
xs[i+1]=xs[i].update(dstate,dt);
}
error=Robot::State::diff(xs[num], xrefs[num]);
J0+=Robot::L(params.Qf,MatNN::Zero(),error,VecN::Zero());
// std::cout<<"=========================================="<<std::endl;
// std::cout<<"J0: "<<J0<<std::endl;
// std::cout<<"=========================================="<<std::endl;
return true;
}
void CircuitStep(CircuitState& state, CircuitState& next, double dt)
{
fullMatrix A(state.cir->stateLen, state.cir->stateLen);
fullMatrix RHS(state.cir->stateLen);
fullMatrix dstate(state.cir->stateLen);
uint i;
for(i=0; i < state.cir->Devices.size(); i++)
state.cir->Devices[i]->writeEqn(state, dt, A, RHS);
for(i=0; i < state.cir->Nodes.size(); i++)
state.cir->Nodes[i]->writeEqn(state, dt, A, RHS);
solve(A, dstate, RHS);
next.soln = state.soln + dstate;
next.time = state.time + dt;
}
DState*
nextstate(DState *d, int c)
{
step(&d->l, c, &l1);
return d->next[c] = dstate(&l1);
}
DState*
startdstate(State *start)
{
return dstate(startlist(start, &l1));
}
