Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/core/solvers/box-ddp.cpp	Lines:	61	108	56.5 %
Date:	2024-02-13 11:12:33	Branches:	43	392	11.0 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2021, CNRS-LAAS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////

#include "crocoddyl/core/solvers/box-ddp.hpp"

#include <iostream>

#include "crocoddyl/core/utils/exception.hpp"

namespace crocoddyl {

SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
    : SolverDDP(problem),
      qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
  allocateData();

  const std::size_t n_alphas = 10;
  alphas_.resize(n_alphas);
  for (std::size_t n = 0; n < n_alphas; ++n) {
    alphas_[n] = 1. / pow(2., static_cast<double>(n));
  }
  // Change the default convergence tolerance since the gradient of the
  // Lagrangian is smaller than an unconstrained OC problem (i.e. gradient = Qu
  // - mu^T * C where mu > 0 and C defines the inequality matrix that bounds the
  // control); and we don't have access to mu from the box QP.
  th_stop_ = 5e-5;
}

SolverBoxDDP::~SolverBoxDDP() {}

void SolverBoxDDP::resizeData() {
  START_PROFILER("SolverBoxDDP::resizeData");
  SolverDDP::resizeData();

  const std::size_t T = problem_->get_T();
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  for (std::size_t t = 0; t < T; ++t) {
    const boost::shared_ptr<ActionModelAbstract>& model = models[t];
    const std::size_t nu = model->get_nu();
    Quu_inv_[t].conservativeResize(nu, nu);
    du_lb_[t].conservativeResize(nu);
    du_ub_[t].conservativeResize(nu);
  }
  STOP_PROFILER("SolverBoxDDP::resizeData");
}

void SolverBoxDDP::allocateData() {
  SolverDDP::allocateData();

  const std::size_t T = problem_->get_T();
  Quu_inv_.resize(T);
  du_lb_.resize(T);
  du_ub_.resize(T);
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  for (std::size_t t = 0; t < T; ++t) {
    const boost::shared_ptr<ActionModelAbstract>& model = models[t];
    const std::size_t nu = model->get_nu();
    Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
    du_lb_[t] = Eigen::VectorXd::Zero(nu);
    du_ub_[t] = Eigen::VectorXd::Zero(nu);
  }
}

void SolverBoxDDP::computeGains(const std::size_t t) {
  START_PROFILER("SolverBoxDDP::computeGains");
  const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
  if (nu > 0) {
    if (!problem_->get_runningModels()[t]->get_has_control_limits() ||
        !is_feasible_) {
      // No control limits on this model: Use vanilla DDP
      SolverDDP::computeGains(t);
      return;
    }

    du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
    du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];

    START_PROFILER("SolverBoxDDP::boxQP");
    const BoxQPSolution& boxqp_sol =
        qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
    START_PROFILER("SolverBoxDDP::boxQP");

    // Compute controls
    START_PROFILER("SolverBoxDDP::Quu_invproj");
    Quu_inv_[t].setZero();
    for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
      for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
        Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) =
            boxqp_sol.Hff_inv(i, j);
      }
    }
    STOP_PROFILER("SolverBoxDDP::Quu_invproj");
    START_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
    K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
    STOP_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
    k_[t] = -boxqp_sol.x;

    // The box-QP clamped the gradient direction; this is important for
    // accounting the algorithm advancement (i.e. stopping criteria)
    START_PROFILER("SolverBoxDDP::Qu_proj");
    for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
      Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
    }
    STOP_PROFILER("SolverBoxDDP::Qu_proj");
  }
  STOP_PROFILER("SolverBoxDDP::computeGains");
}

void SolverBoxDDP::forwardPass(double steplength) {
  if (steplength > 1. || steplength < 0.) {
    throw_pretty("Invalid argument: "
                 << "invalid step length, value is between 0. to 1.");
  }
  START_PROFILER("SolverBoxDDP::forwardPass");
  cost_try_ = 0.;
  xnext_ = problem_->get_x0();
  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();
  for (std::size_t t = 0; t < T; ++t) {
    const boost::shared_ptr<ActionModelAbstract>& m = models[t];
    const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
    const std::size_t nu = m->get_nu();

    xs_try_[t] = xnext_;
    m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
    if (nu != 0) {
      us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
      if (m->get_has_control_limits()) {  // clamp control
        us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
      }
      m->calc(d, xs_try_[t], us_try_[t]);
    } else {
      m->calc(d, xs_try_[t]);
    }
    xnext_ = d->xnext;
    cost_try_ += d->cost;

    if (raiseIfNaN(cost_try_)) {
      STOP_PROFILER("SolverBoxDDP::forwardPass");
      throw_pretty("forward_error");
    }
    if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
      STOP_PROFILER("SolverBoxDDP::forwardPass");
      throw_pretty("forward_error");
    }
  }

  const boost::shared_ptr<ActionModelAbstract>& m =
      problem_->get_terminalModel();
  const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
  if ((is_feasible_) || (steplength == 1)) {
    xs_try_.back() = xnext_;
  } else {
    m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
                              xs_try_.back());
  }
  m->calc(d, xs_try_.back());
  cost_try_ += d->cost;

  if (raiseIfNaN(cost_try_)) {
    STOP_PROFILER("SolverBoxDDP::forwardPass");
    throw_pretty("forward_error");
  }
}

const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv() const {
  return Quu_inv_;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2021, CNRS-LAAS, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/solvers/box-ddp.hpp"
10
11			#include <iostream>
12
13			#include "crocoddyl/core/utils/exception.hpp"
14
15			namespace crocoddyl {
16
17		4	SolverBoxDDP::SolverBoxDDP(boost::shared_ptr<ShootingProblem> problem)
18			: SolverDDP(problem),
19	✓✗✓✗	4	qp_(problem->get_runningModels()[0]->get_nu(), 100, 0.1, 1e-5, 0.) {
20	✓✗	4	allocateData();
21
22		4	const std::size_t n_alphas = 10;
23	✓✗	4	alphas_.resize(n_alphas);
24	✓✓	44	for (std::size_t n = 0; n < n_alphas; ++n) {
25		40	alphas_[n] = 1. / pow(2., static_cast<double>(n));
26			}
27			// Change the default convergence tolerance since the gradient of the
28			// Lagrangian is smaller than an unconstrained OC problem (i.e. gradient = Qu
29			// - mu^T * C where mu > 0 and C defines the inequality matrix that bounds the
30			// control); and we don't have access to mu from the box QP.
31		4	th_stop_ = 5e-5;
32		4	}
33
34		12	SolverBoxDDP::~SolverBoxDDP() {}
35
36			void SolverBoxDDP::resizeData() {
37			START_PROFILER("SolverBoxDDP::resizeData");
38			SolverDDP::resizeData();
39
40			const std::size_t T = problem_->get_T();
41			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
42			problem_->get_runningModels();
43			for (std::size_t t = 0; t < T; ++t) {
44			const boost::shared_ptr<ActionModelAbstract>& model = models[t];
45			const std::size_t nu = model->get_nu();
46			Quu_inv_[t].conservativeResize(nu, nu);
47			du_lb_[t].conservativeResize(nu);
48			du_ub_[t].conservativeResize(nu);
49			}
50			STOP_PROFILER("SolverBoxDDP::resizeData");
51			}
52
53		4	void SolverBoxDDP::allocateData() {
54		4	SolverDDP::allocateData();
55
56		4	const std::size_t T = problem_->get_T();
57		4	Quu_inv_.resize(T);
58		4	du_lb_.resize(T);
59		4	du_ub_.resize(T);
60			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
61		4	problem_->get_runningModels();
62	✓✓	44	for (std::size_t t = 0; t < T; ++t) {
63		40	const boost::shared_ptr<ActionModelAbstract>& model = models[t];
64		40	const std::size_t nu = model->get_nu();
65	✓✗	40	Quu_inv_[t] = Eigen::MatrixXd::Zero(nu, nu);
66	✓✗	40	du_lb_[t] = Eigen::VectorXd::Zero(nu);
67	✓✗	40	du_ub_[t] = Eigen::VectorXd::Zero(nu);
68			}
69		4	}
70
71		70	void SolverBoxDDP::computeGains(const std::size_t t) {
72	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	70	START_PROFILER("SolverBoxDDP::computeGains");
73		70	const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
74	✓✗	70	if (nu > 0) {
75	✗✓✓✗	70	if (!problem_->get_runningModels()[t]->get_has_control_limits() \|\|
76			!is_feasible_) {
77			// No control limits on this model: Use vanilla DDP
78		70	SolverDDP::computeGains(t);
79		70	return;
80			}
81
82			du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
83			du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
84
85			START_PROFILER("SolverBoxDDP::boxQP");
86			const BoxQPSolution& boxqp_sol =
87			qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
88			START_PROFILER("SolverBoxDDP::boxQP");
89
90			// Compute controls
91			START_PROFILER("SolverBoxDDP::Quu_invproj");
92			Quu_inv_[t].setZero();
93			for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
94			for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
95			Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) =
96			boxqp_sol.Hff_inv(i, j);
97			}
98			}
99			STOP_PROFILER("SolverBoxDDP::Quu_invproj");
100			START_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
101			K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
102			STOP_PROFILER("SolverBoxDDP::Quu_invproj_Qxu");
103			k_[t] = -boxqp_sol.x;
104
105			// The box-QP clamped the gradient direction; this is important for
106			// accounting the algorithm advancement (i.e. stopping criteria)
107			START_PROFILER("SolverBoxDDP::Qu_proj");
108			for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
109			Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
110			}
111			STOP_PROFILER("SolverBoxDDP::Qu_proj");
112			}
113			STOP_PROFILER("SolverBoxDDP::computeGains");
114			}
115
116		7	void SolverBoxDDP::forwardPass(double steplength) {
117	✓✗✗✓	7	if (steplength > 1. \|\| steplength < 0.) {
118			throw_pretty("Invalid argument: "
119			<< "invalid step length, value is between 0. to 1.");
120			}
121	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	7	START_PROFILER("SolverBoxDDP::forwardPass");
122		7	cost_try_ = 0.;
123		7	xnext_ = problem_->get_x0();
124		7	const std::size_t T = problem_->get_T();
125			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
126		7	problem_->get_runningModels();
127			const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
128		7	problem_->get_runningDatas();
129	✓✓	77	for (std::size_t t = 0; t < T; ++t) {
130		70	const boost::shared_ptr<ActionModelAbstract>& m = models[t];
131		70	const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
132		70	const std::size_t nu = m->get_nu();
133
134		70	xs_try_[t] = xnext_;
135	✓✗✓✗ ✓✗	70	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
136	✓✗	70	if (nu != 0) {
137	✓✗✓✗ ✓✗✓✗ ✓✗	70	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
138	✗✓	70	if (m->get_has_control_limits()) { // clamp control
139			us_try_[t] = us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
140			}
141	✓✗✓✗	70	m->calc(d, xs_try_[t], us_try_[t]);
142			} else {
143			m->calc(d, xs_try_[t]);
144			}
145		70	xnext_ = d->xnext;
146		70	cost_try_ += d->cost;
147
148	✗✓	70	if (raiseIfNaN(cost_try_)) {
149			STOP_PROFILER("SolverBoxDDP::forwardPass");
150			throw_pretty("forward_error");
151			}
152	✗✓	70	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
153			STOP_PROFILER("SolverBoxDDP::forwardPass");
154			throw_pretty("forward_error");
155			}
156			}
157
158			const boost::shared_ptr<ActionModelAbstract>& m =
159		7	problem_->get_terminalModel();
160		7	const boost::shared_ptr<ActionDataAbstract>& d = problem_->get_terminalData();
161	✓✓✓✗	7	if ((is_feasible_) \|\| (steplength == 1)) {
162		7	xs_try_.back() = xnext_;
163			} else {
164			m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
165			xs_try_.back());
166			}
167	✓✗	7	m->calc(d, xs_try_.back());
168		7	cost_try_ += d->cost;
169
170	✗✓	7	if (raiseIfNaN(cost_try_)) {
171			STOP_PROFILER("SolverBoxDDP::forwardPass");
172			throw_pretty("forward_error");
173			}
174		7	}
175
176			const std::vector<Eigen::MatrixXd>& SolverBoxDDP::get_Quu_inv() const {
177			return Quu_inv_;
178			}
179
180			} // namespace crocoddyl