Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/core/solvers/box-fddp.cpp	Lines:	60	121	49.6 %
Date:	2024-02-13 11:12:33	Branches:	37	240	15.4 %


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

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

#include <iostream>

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

namespace crocoddyl {

SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
    : SolverFDDP(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;
}

SolverBoxFDDP::~SolverBoxFDDP() {}

void SolverBoxFDDP::resizeData() {
  START_PROFILER("SolverBoxFDDP::resizeData");
  SolverFDDP::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("SolverBoxFDDP::resizeData");
}

void SolverBoxFDDP::allocateData() {
  SolverFDDP::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 SolverBoxFDDP::computeGains(const std::size_t t) {
  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
      SolverFDDP::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];

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

    // Compute controls
    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);
      }
    }
    K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
    k_[t] = -boxqp_sol.x;

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

void SolverBoxFDDP::forwardPass(const double steplength) {
  if (steplength > 1. || steplength < 0.) {
    throw_pretty("Invalid argument: "
                 << "invalid step length, value is between 0. to 1.");
  }
  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();
  if ((is_feasible_) || (steplength == 1)) {
    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_)) {
        throw_pretty("forward_error");
      }
      if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
        throw_pretty("forward_error");
      }
    }

    const boost::shared_ptr<ActionModelAbstract>& m =
        problem_->get_terminalModel();
    const boost::shared_ptr<ActionDataAbstract>& d =
        problem_->get_terminalData();
    xs_try_.back() = xnext_;
    m->calc(d, xs_try_.back());
    cost_try_ += d->cost;

    if (raiseIfNaN(cost_try_)) {
      throw_pretty("forward_error");
    }
  } else {
    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();
      m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
      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_)) {
        throw_pretty("forward_error");
      }
      if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
        throw_pretty("forward_error");
      }
    }

    const boost::shared_ptr<ActionModelAbstract>& m =
        problem_->get_terminalModel();
    const boost::shared_ptr<ActionDataAbstract>& d =
        problem_->get_terminalData();
    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_)) {
      throw_pretty("forward_error");
    }
  }
}

const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::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, 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-fddp.hpp"
10
11			#include <iostream>
12
13			#include "crocoddyl/core/utils/exception.hpp"
14
15			namespace crocoddyl {
16
17		4	SolverBoxFDDP::SolverBoxFDDP(boost::shared_ptr<ShootingProblem> problem)
18			: SolverFDDP(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	SolverBoxFDDP::~SolverBoxFDDP() {}
35
36			void SolverBoxFDDP::resizeData() {
37			START_PROFILER("SolverBoxFDDP::resizeData");
38			SolverFDDP::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("SolverBoxFDDP::resizeData");
51			}
52
53		4	void SolverBoxFDDP::allocateData() {
54		4	SolverFDDP::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 SolverBoxFDDP::computeGains(const std::size_t t) {
72		70	const std::size_t nu = problem_->get_runningModels()[t]->get_nu();
73	✓✗	70	if (nu > 0) {
74	✗✓✓✗	70	if (!problem_->get_runningModels()[t]->get_has_control_limits() \|\|
75			!is_feasible_) {
76			// No control limits on this model: Use vanilla DDP
77		70	SolverFDDP::computeGains(t);
78		70	return;
79			}
80
81			du_lb_[t] = problem_->get_runningModels()[t]->get_u_lb() - us_[t];
82			du_ub_[t] = problem_->get_runningModels()[t]->get_u_ub() - us_[t];
83
84			const BoxQPSolution& boxqp_sol =
85			qp_.solve(Quu_[t], Qu_[t], du_lb_[t], du_ub_[t], k_[t]);
86
87			// Compute controls
88			Quu_inv_[t].setZero();
89			for (std::size_t i = 0; i < boxqp_sol.free_idx.size(); ++i) {
90			for (std::size_t j = 0; j < boxqp_sol.free_idx.size(); ++j) {
91			Quu_inv_[t](boxqp_sol.free_idx[i], boxqp_sol.free_idx[j]) =
92			boxqp_sol.Hff_inv(i, j);
93			}
94			}
95			K_[t].noalias() = Quu_inv_[t] * Qxu_[t].transpose();
96			k_[t] = -boxqp_sol.x;
97
98			// The box-QP clamped the gradient direction; this is important for
99			// accounting the algorithm advancement (i.e. stopping criteria)
100			for (std::size_t i = 0; i < boxqp_sol.clamped_idx.size(); ++i) {
101			Qu_[t](boxqp_sol.clamped_idx[i]) = 0.;
102			}
103			}
104			}
105
106		7	void SolverBoxFDDP::forwardPass(const double steplength) {
107	✓✗✗✓	7	if (steplength > 1. \|\| steplength < 0.) {
108			throw_pretty("Invalid argument: "
109			<< "invalid step length, value is between 0. to 1.");
110			}
111		7	cost_try_ = 0.;
112		7	xnext_ = problem_->get_x0();
113		7	const std::size_t T = problem_->get_T();
114			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
115		7	problem_->get_runningModels();
116			const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
117		7	problem_->get_runningDatas();
118	✓✓✓✗	7	if ((is_feasible_) \|\| (steplength == 1)) {
119	✓✓	77	for (std::size_t t = 0; t < T; ++t) {
120		70	const boost::shared_ptr<ActionModelAbstract>& m = models[t];
121		70	const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
122		70	const std::size_t nu = m->get_nu();
123
124		70	xs_try_[t] = xnext_;
125	✓✗✓✗ ✓✗	70	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
126	✓✗	70	if (nu != 0) {
127	✓✗✓✗ ✓✗✓✗ ✓✗	70	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
128	✗✓	70	if (m->get_has_control_limits()) { // clamp control
129			us_try_[t] =
130			us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
131			}
132	✓✗✓✗	70	m->calc(d, xs_try_[t], us_try_[t]);
133			} else {
134			m->calc(d, xs_try_[t]);
135			}
136		70	xnext_ = d->xnext;
137		70	cost_try_ += d->cost;
138
139	✗✓	70	if (raiseIfNaN(cost_try_)) {
140			throw_pretty("forward_error");
141			}
142	✗✓	70	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
143			throw_pretty("forward_error");
144			}
145			}
146
147			const boost::shared_ptr<ActionModelAbstract>& m =
148		7	problem_->get_terminalModel();
149			const boost::shared_ptr<ActionDataAbstract>& d =
150		7	problem_->get_terminalData();
151		7	xs_try_.back() = xnext_;
152	✓✗	7	m->calc(d, xs_try_.back());
153		7	cost_try_ += d->cost;
154
155	✗✓	7	if (raiseIfNaN(cost_try_)) {
156			throw_pretty("forward_error");
157		7	}
158			} else {
159			for (std::size_t t = 0; t < T; ++t) {
160			const boost::shared_ptr<ActionModelAbstract>& m = models[t];
161			const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
162			const std::size_t nu = m->get_nu();
163			m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
164			m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
165			if (nu != 0) {
166			us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
167			if (m->get_has_control_limits()) { // clamp control
168			us_try_[t] =
169			us_try_[t].cwiseMax(m->get_u_lb()).cwiseMin(m->get_u_ub());
170			}
171			m->calc(d, xs_try_[t], us_try_[t]);
172			} else {
173			m->calc(d, xs_try_[t]);
174			}
175			xnext_ = d->xnext;
176			cost_try_ += d->cost;
177
178			if (raiseIfNaN(cost_try_)) {
179			throw_pretty("forward_error");
180			}
181			if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
182			throw_pretty("forward_error");
183			}
184			}
185
186			const boost::shared_ptr<ActionModelAbstract>& m =
187			problem_->get_terminalModel();
188			const boost::shared_ptr<ActionDataAbstract>& d =
189			problem_->get_terminalData();
190			m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
191			xs_try_.back());
192			m->calc(d, xs_try_.back());
193			cost_try_ += d->cost;
194
195			if (raiseIfNaN(cost_try_)) {
196			throw_pretty("forward_error");
197			}
198			}
199		7	}
200
201			const std::vector<Eigen::MatrixXd>& SolverBoxFDDP::get_Quu_inv() const {
202			return Quu_inv_;
203			}
204
205			} // namespace crocoddyl