Skip to content

Commit

Permalink
[solver] Cleaned FDDP code (C++ and Python)
Browse files Browse the repository at this point in the history
  • Loading branch information
@cmastalli
cmastalli committed Oct 22, 2024
1 parent b37eefe commit d2ca7cb
Show file tree
Hide file tree
Showing 2 changed files with 54 additions and 42 deletions.
58 changes: 30 additions & 28 deletions src/core/solvers/fddp.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,9 @@ SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem,
nh_T_(problem->get_terminalModel()->get_nh_T()),
acceptstep_(false),
recalcdir_(true) {
// Allocating the solver's data
allocateData();

// Setting the dynamics solver
switch (dyn_solver) {
case HybridShoot: {
const std::size_t Tshoot =
Expand All @@ -55,7 +56,7 @@ SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem,
set_dynamics_solver(dyn_solver, 0);
break;
}

// Defining the list of step lengths used in the linear search routine
const std::size_t n_alphas = 10;
alphas_.resize(n_alphas);
for (std::size_t n = 0; n < n_alphas; ++n) {
Expand Down Expand Up @@ -414,26 +415,6 @@ void SolverFDDP::calcDir() {
STOP_PROFILER("SolverFDDP::calcDir");
}

void SolverFDDP::linearRollout() {
const std::size_t T = problem_->get_T();
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
dxs_[0] = fs_[0];
for (std::size_t t = 0; t < T; ++t) { // in sequence
const boost::shared_ptr<ActionModelAbstract>& m = models[t];
const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
dxs_[t + 1].noalias() = d->Fx * dxs_[t];
dxs_[t + 1] += fs_[t + 1];
if (m->get_nu() != 0) {
dus_[t] = -k_[t];
dus_[t].noalias() -= K_[t] * dxs_[t];
dxs_[t + 1].noalias() += d->Fu * dus_[t];
}
}
}

void SolverFDDP::backwardPass() {
START_PROFILER("SolverFDDP::backwardPass");
const boost::shared_ptr<ActionDataAbstract>& d_T =
Expand Down Expand Up @@ -506,6 +487,9 @@ void SolverFDDP::updateDir() {
hc_ = d_T->h;
hc_.noalias() += d_T->Hx * dxs_.back();
switch (term_solver_) {
// For the LuNull and QrNull solvers, we compute terminal multiplier using
// nullspace parametrization. Instead of parametrizing Hx, we opt to
// equivalent parametrize dHc. This approach is much efficient.
case LuNull: {
dHc_lu_.compute(dHc_);
dHc_rank_ = dHc_lu_.rank();
Expand Down Expand Up @@ -630,9 +614,7 @@ void SolverFDDP::computePolicy(const std::size_t t) {
void SolverFDDP::computeBatchPolicy(const std::size_t t) {
START_PROFILER("SolverFDDP::computeBatchPolicy");
Kc_[t] = Quc_[t];
const boost::shared_ptr<ActionModelAbstract>& model =
problem_->get_runningModels()[t];
const std::size_t nu = model->get_nu();
const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
if (nu > 0) {
Quu_llt_[t].solveInPlace(Kc_[t]);
}
Expand Down Expand Up @@ -670,6 +652,26 @@ void SolverFDDP::computeBatchValueFunction(const std::size_t t) {
STOP_PROFILER("SolverFDDP::computeBatchValueFunction");
}

void SolverFDDP::linearRollout() {
const std::size_t T = problem_->get_T();
const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
problem_->get_runningModels();
const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
problem_->get_runningDatas();
dxs_[0] = fs_[0];
for (std::size_t t = 0; t < T; ++t) { // in sequence
const boost::shared_ptr<ActionModelAbstract>& m = models[t];
const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
dxs_[t + 1].noalias() = d->Fx * dxs_[t];
dxs_[t + 1] += fs_[t + 1];
if (m->get_nu() != 0) {
dus_[t] = -k_[t];
dus_[t].noalias() -= K_[t] * dxs_[t];
dxs_[t + 1].noalias() += d->Fu * dus_[t];
}
}
}

void SolverFDDP::feasShootForwardPass(const double steplength) {
START_PROFILER("SolverFDDP::feasShootForwardPass");
if (steplength > 1. || steplength < 0.) {
Expand All @@ -691,7 +693,7 @@ void SolverFDDP::feasShootForwardPass(const double steplength) {
const std::size_t nu = m->get_nu();
if (nu != 0) {
m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
us_try_[t] = us_[t] - k_[t] * steplength;
us_try_[t] = us_[t] - steplength * k_[t];
us_try_[t].noalias() -= K_[t] * dx_[t];
m->calc(d, xs_try_[t], us_try_[t]);
} else {
Expand Down Expand Up @@ -799,7 +801,7 @@ void SolverFDDP::hybridShootForwardPass(const double steplength) {
const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
if (m->get_nu() != 0) {
m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
us_try_[t] = us_[t] - k_[t] * steplength;
us_try_[t] = us_[t] - steplength * k_[t];
us_try_[t].noalias() -= K_[t] * dx_[t];
m->calc(d, xs_try_[t], us_try_[t]);
} else {
Expand Down Expand Up @@ -861,7 +863,7 @@ void SolverFDDP::singleShootForwardPass(const double steplength) {
const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
if (m->get_nu() != 0) {
m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
us_try_[t] = us_[t] - k_[t] * steplength;
us_try_[t] = us_[t] - steplength * k_[t];
us_try_[t].noalias() -= K_[t] * dx_[t];
m->calc(d, xs_try_[t], us_try_[t]);
} else {
Expand Down
38 changes: 24 additions & 14 deletions unittest/bindings/factory.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1734,7 +1734,6 @@ def solve(self, init_xs=[], init_us=[], maxiter=100, regInit=None):
self.setCandidate(
init_xs, init_us, False
) # TODO: update Crocoddyl API (let's remove the feasibility boolean)
self.isFeasible = False
self.preg = regInit if regInit is not None else self.reg_min
self.dreg = regInit if regInit is not None else self.reg_min
if self.zero_upsilon:
Expand Down Expand Up @@ -1900,7 +1899,7 @@ def tryStep(self, stepLength=1):
self.dfeas = self.ffeas - self.ffeas_try
self.dfeas += self.gfeas - self.gfeas_try
self.dfeas += self.hfeas - self.hfeas_try
self.dPhiexp = self.dVexp + self.upsilon * stepLength * self.dfeas
self.dPhiexp = self.dVexp + stepLength * self.upsilon * self.dfeas
self.dPhi = self.dV + self.upsilon * self.dfeas
return self.dV

Expand Down Expand Up @@ -2013,17 +2012,14 @@ def updateDir(self):
self.dHcY = np.resize(self.Yc, (nh_T, self.dHc_rank))
self.YdHcY = np.resize(self.Yc, (self.dHc_rank, self.dHc_rank))
self.YdHcY_inv_YHc = np.resize(self.Yhc, self.dHc_rank)
# Compute epsilon using nullspace parametrization. Instead of parametrizing Hx,
# Compute terminal multiplier using nullspace parametrization. Instead of parametrizing Hx,
# we opt to equivalent parametrize dHc. This approach is much efficient.
self.Yc[:, :] = scl.orth(self.dHc)
self.Yhc[:] = self.Yc.T @ self.hc
self.dHcY[:, :] = self.dHc @ self.Yc
self.YdHcY[:, :] = self.Yc.T @ self.dHcY
self.YdHcY_llt = scl.cho_factor(self.YdHcY)
self.YdHcY_inv_YHc[:] = scl.cho_solve(self.YdHcY_llt, self.Yhc)
self.beta_plus[:] = self.Yc @ self.YdHcY_inv_YHc
self.computeNullTerminalMultiplier()
else:
self.YdHcY_llt = scl.cho_factor(self.dHc)
try:
self.YdHcY_llt = scl.cho_factor(self.dHc)
except scl.LinAlgError:
raise ArithmeticError("backward error")
self.beta_plus[:] = scl.cho_solve(self.YdHcY_llt, self.hc)
# Finally, we update the feed-forward term and search direction.
for t in range(self.problem.T): # in parallel
Expand Down Expand Up @@ -2076,7 +2072,9 @@ def computePolicy(self, t):

# This is virtual function
def computeBatchPolicy(self, t):
self.Kc[t][:] = scl.cho_solve(self.Quu_llt[t], self.Quc[t])
nu = self.problem.runningModels[t].nu
if nu > 0:
self.Kc[t][:] = scl.cho_solve(self.Quu_llt[t], self.Quc[t])

# This is virtual function
def computeValueFunction(self, t, model):
Expand Down Expand Up @@ -2195,7 +2193,7 @@ def hybridShootForwardPass(self, stepLength, warning="ignore"):
d = self.problem.runningDatas[t]
if m.nu != 0:
self.dx[t] = m.state.diff(xs[t], xtry[t])
utry[t] = us[t] - self.k[t] * stepLength
utry[t] = us[t] - stepLength * self.k[t]
utry[t] -= self.K[t] @ self.dx[t]
with warnings.catch_warnings():
warnings.simplefilter(warning)
Expand Down Expand Up @@ -2235,7 +2233,7 @@ def singleShootForwardPass(self, stepLength, warning="ignore"):
xtry[t] = xnext.copy()
if m.nu != 0:
self.dx[t] = m.state.diff(xs[t], xtry[t])
utry[t] = us[t] - self.k[t] * stepLength
utry[t] = us[t] - stepLength * self.k[t]
utry[t] -= self.K[t] @ self.dx[t]
with warnings.catch_warnings():
warnings.simplefilter(warning)
Expand All @@ -2255,6 +2253,18 @@ def singleShootForwardPass(self, stepLength, warning="ignore"):
self.cost_try += self.problem.terminalData.cost
raiseIfNan(self.cost_try, ArithmeticError("forward error"))

def computeNullTerminalMultiplier(self):
self.Yc[:, :] = scl.orth(self.dHc)
self.Yhc[:] = self.Yc.T @ self.hc
self.dHcY[:, :] = self.dHc @ self.Yc
self.YdHcY[:, :] = self.Yc.T @ self.dHcY
try:
self.YdHcY_llt = scl.cho_factor(self.YdHcY)
except scl.LinAlgError:
raise ArithmeticError("backward error")
self.YdHcY_inv_YHc[:] = scl.cho_solve(self.YdHcY_llt, self.Yhc)
self.beta_plus[:] = self.Yc @ self.YdHcY_inv_YHc

def increaseRegularization(self):
self.preg *= self.reg_incFactor
if self.preg > self.reg_max:
Expand Down

0 comments on commit d2ca7cb

Please sign in to comment.