Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	src/core/solvers/fddp.cpp	Lines:	137	172	79.7 %
Date:	2024-02-13 11:12:33	Branches:	123	458	26.9 %


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

#ifdef CROCODDYL_WITH_MULTITHREADING
#include <omp.h>
#endif  // CROCODDYL_WITH_MULTITHREADING

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

namespace crocoddyl {

SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem)
    : SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}

SolverFDDP::~SolverFDDP() {}

bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs,
                       const std::vector<Eigen::VectorXd>& init_us,
                       const std::size_t maxiter, const bool is_feasible,
                       const double init_reg) {
  START_PROFILER("SolverFDDP::solve");
  if (problem_->is_updated()) {
    resizeData();
  }
  xs_try_[0] =
      problem_->get_x0();  // it is needed in case that init_xs[0] is infeasible
  setCandidate(init_xs, init_us, is_feasible);

  if (std::isnan(init_reg)) {
    preg_ = reg_min_;
    dreg_ = reg_min_;
  } else {
    preg_ = init_reg;
    dreg_ = init_reg;
  }
  was_feasible_ = false;

  bool recalcDiff = true;
  for (iter_ = 0; iter_ < maxiter; ++iter_) {
    while (true) {
      try {
        computeDirection(recalcDiff);
      } catch (std::exception& e) {
        recalcDiff = false;
        increaseRegularization();
        if (preg_ == reg_max_) {
          return false;
        } else {
          continue;
        }
      }
      break;
    }
    updateExpectedImprovement();

    // We need to recalculate the derivatives when the step length passes
    recalcDiff = false;
    for (std::vector<double>::const_iterator it = alphas_.begin();
         it != alphas_.end(); ++it) {
      steplength_ = *it;

      try {
        dV_ = tryStep(steplength_);
      } catch (std::exception& e) {
        continue;
      }
      expectedImprovement();
      dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);

      if (dVexp_ >= 0) {  // descend direction
        if (std::abs(d_[0]) < th_grad_ || dV_ > th_acceptstep_ * dVexp_) {
          was_feasible_ = is_feasible_;
          setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
          cost_ = cost_try_;
          recalcDiff = true;
          break;
        }
      } else {  // reducing the gaps by allowing a small increment in the cost
                // value
        if (!is_feasible_ && dV_ > th_acceptnegstep_ * dVexp_) {
          was_feasible_ = is_feasible_;
          setCandidate(xs_try_, us_try_, (was_feasible_) || (steplength_ == 1));
          cost_ = cost_try_;
          recalcDiff = true;
          break;
        }
      }
    }

    if (steplength_ > th_stepdec_) {
      decreaseRegularization();
    }
    if (steplength_ <= th_stepinc_) {
      increaseRegularization();
      if (preg_ == reg_max_) {
        STOP_PROFILER("SolverFDDP::solve");
        return false;
      }
    }
    stoppingCriteria();

    const std::size_t n_callbacks = callbacks_.size();
    for (std::size_t c = 0; c < n_callbacks; ++c) {
      CallbackAbstract& callback = *callbacks_[c];
      callback(*this);
    }

    if (was_feasible_ && stop_ < th_stop_) {
      STOP_PROFILER("SolverFDDP::solve");
      return true;
    }
  }
  STOP_PROFILER("SolverFDDP::solve");
  return false;
}

const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
  dv_ = 0;
  const std::size_t T = this->problem_->get_T();
  if (!is_feasible_) {
    // NB: The dimension of vectors xs_try_ and xs_ are T+1, whereas the
    // dimension of dx_ is T. Here, we are re-using the final element of dx_ for
    // the computation of the difference at the terminal node. Using the access
    // iterator back() this re-use of the final element is fine. Cf. the
    // discussion at https://github.com/loco-3d/crocoddyl/issues/1022
    problem_->get_terminalModel()->get_state()->diff(xs_try_.back(), xs_.back(),
                                                     dx_.back());
    fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
    dv_ -= fs_.back().dot(fTVxx_p_);
    const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
        problem_->get_runningModels();

    for (std::size_t t = 0; t < T; ++t) {
      models[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]);
      fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
      dv_ -= fs_[t].dot(fTVxx_p_);
    }
  }
  d_[0] = dg_ + dv_;
  d_[1] = dq_ - 2 * dv_;
  return d_;
}

void SolverFDDP::updateExpectedImprovement() {
  dg_ = 0;
  dq_ = 0;
  const std::size_t T = this->problem_->get_T();
  if (!is_feasible_) {
    dg_ -= Vx_.back().dot(fs_.back());
    fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
    dq_ += fs_.back().dot(fTVxx_p_);
  }
  const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
      problem_->get_runningModels();
  for (std::size_t t = 0; t < T; ++t) {
    const std::size_t nu = models[t]->get_nu();
    if (nu != 0) {
      dg_ += Qu_[t].dot(k_[t]);
      dq_ -= k_[t].dot(Quuk_[t]);
    }
    if (!is_feasible_) {
      dg_ -= Vx_[t].dot(fs_[t]);
      fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
      dq_ += fs_[t].dot(fTVxx_p_);
    }
  }
}

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

double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }

void SolverFDDP::set_th_acceptnegstep(const double th_acceptnegstep) {
  if (0. > th_acceptnegstep) {
    throw_pretty("Invalid argument: "
                 << "th_acceptnegstep value has to be positive.");
  }
  th_acceptnegstep_ = th_acceptnegstep;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifdef CROCODDYL_WITH_MULTITHREADING
11			#include <omp.h>
12			#endif // CROCODDYL_WITH_MULTITHREADING
13
14			#include "crocoddyl/core/solvers/fddp.hpp"
15			#include "crocoddyl/core/utils/exception.hpp"
16
17			namespace crocoddyl {
18
19		21	SolverFDDP::SolverFDDP(boost::shared_ptr<ShootingProblem> problem)
20	✓✗	21	: SolverDDP(problem), dg_(0), dq_(0), dv_(0), th_acceptnegstep_(2) {}
21
22		54	SolverFDDP::~SolverFDDP() {}
23
24		12	bool SolverFDDP::solve(const std::vector<Eigen::VectorXd>& init_xs,
25			const std::vector<Eigen::VectorXd>& init_us,
26			const std::size_t maxiter, const bool is_feasible,
27			const double init_reg) {
28	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	12	START_PROFILER("SolverFDDP::solve");
29	✗✓	12	if (problem_->is_updated()) {
30			resizeData();
31			}
32		12	xs_try_[0] =
33		24	problem_->get_x0(); // it is needed in case that init_xs[0] is infeasible
34		12	setCandidate(init_xs, init_us, is_feasible);
35
36	✓✗	12	if (std::isnan(init_reg)) {
37		12	preg_ = reg_min_;
38		12	dreg_ = reg_min_;
39			} else {
40			preg_ = init_reg;
41			dreg_ = init_reg;
42			}
43		12	was_feasible_ = false;
44
45		12	bool recalcDiff = true;
46	✓✓	49	for (iter_ = 0; iter_ < maxiter; ++iter_) {
47			while (true) {
48			try {
49	✓✗	43	computeDirection(recalcDiff);
50			} catch (std::exception& e) {
51			recalcDiff = false;
52			increaseRegularization();
53			if (preg_ == reg_max_) {
54			return false;
55			} else {
56			continue;
57			}
58			}
59		43	break;
60			}
61		43	updateExpectedImprovement();
62
63			// We need to recalculate the derivatives when the step length passes
64		43	recalcDiff = false;
65		43	for (std::vector<double>::const_iterator it = alphas_.begin();
66	✓✗	43	it != alphas_.end(); ++it) {
67		43	steplength_ = *it;
68
69			try {
70	✓✗	43	dV_ = tryStep(steplength_);
71			} catch (std::exception& e) {
72			continue;
73			}
74	✓✗	43	expectedImprovement();
75	✓✗✓✗	43	dVexp_ = steplength_ * (d_[0] + 0.5 * steplength_ * d_[1]);
76
77	✓✓	43	if (dVexp_ >= 0) { // descend direction
78	✓✗✓✓ ✓✗✓✗	35	if (std::abs(d_[0]) < th_grad_ \|\| dV_ > th_acceptstep_ * dVexp_) {
79		35	was_feasible_ = is_feasible_;
80	✓✓✓✗ ✓✗	35	setCandidate(xs_try_, us_try_, (was_feasible_) \|\| (steplength_ == 1));
81		35	cost_ = cost_try_;
82		35	recalcDiff = true;
83		35	break;
84			}
85			} else { // reducing the gaps by allowing a small increment in the cost
86			// value
87	✓✗✓✗	8	if (!is_feasible_ && dV_ > th_acceptnegstep_ * dVexp_) {
88		8	was_feasible_ = is_feasible_;
89	✓✗✓✗ ✓✗	8	setCandidate(xs_try_, us_try_, (was_feasible_) \|\| (steplength_ == 1));
90		8	cost_ = cost_try_;
91		8	recalcDiff = true;
92		8	break;
93			}
94			}
95			}
96
97	✓✗	43	if (steplength_ > th_stepdec_) {
98		43	decreaseRegularization();
99			}
100	✗✓	43	if (steplength_ <= th_stepinc_) {
101			increaseRegularization();
102			if (preg_ == reg_max_) {
103			STOP_PROFILER("SolverFDDP::solve");
104			return false;
105			}
106			}
107		43	stoppingCriteria();
108
109		43	const std::size_t n_callbacks = callbacks_.size();
110	✓✓	58	for (std::size_t c = 0; c < n_callbacks; ++c) {
111		15	CallbackAbstract& callback = *callbacks_[c];
112		15	callback(*this);
113			}
114
115	✓✓✓✓	43	if (was_feasible_ && stop_ < th_stop_) {
116	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	6	STOP_PROFILER("SolverFDDP::solve");
117		6	return true;
118			}
119			}
120	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	6	STOP_PROFILER("SolverFDDP::solve");
121		6	return false;
122			}
123
124		45	const Eigen::Vector2d& SolverFDDP::expectedImprovement() {
125		45	dv_ = 0;
126		45	const std::size_t T = this->problem_->get_T();
127	✓✓	45	if (!is_feasible_) {
128			// NB: The dimension of vectors xs_try_ and xs_ are T+1, whereas the
129			// dimension of dx_ is T. Here, we are re-using the final element of dx_ for
130			// the computation of the difference at the terminal node. Using the access
131			// iterator back() this re-use of the final element is fine. Cf. the
132			// discussion at https://github.com/loco-3d/crocoddyl/issues/1022
133	✓✗✓✗ ✓✗	24	problem_->get_terminalModel()->get_state()->diff(xs_try_.back(), xs_.back(),
134		12	dx_.back());
135	✓✗✓✗	12	fTVxx_p_.noalias() = Vxx_.back() * dx_.back();
136		12	dv_ -= fs_.back().dot(fTVxx_p_);
137			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
138		12	problem_->get_runningModels();
139
140	✓✓	132	for (std::size_t t = 0; t < T; ++t) {
141	✓✗✓✗ ✓✗	120	models[t]->get_state()->diff(xs_try_[t], xs_[t], dx_[t]);
142	✓✗✓✗	120	fTVxx_p_.noalias() = Vxx_[t] * dx_[t];
143		120	dv_ -= fs_[t].dot(fTVxx_p_);
144			}
145			}
146		45	d_[0] = dg_ + dv_;
147		45	d_[1] = dq_ - 2 * dv_;
148		45	return d_;
149			}
150
151		43	void SolverFDDP::updateExpectedImprovement() {
152		43	dg_ = 0;
153		43	dq_ = 0;
154		43	const std::size_t T = this->problem_->get_T();
155	✓✓	43	if (!is_feasible_) {
156		12	dg_ -= Vx_.back().dot(fs_.back());
157	✓✗✓✗	12	fTVxx_p_.noalias() = Vxx_.back() * fs_.back();
158		12	dq_ += fs_.back().dot(fTVxx_p_);
159			}
160			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
161		43	problem_->get_runningModels();
162	✓✓	505	for (std::size_t t = 0; t < T; ++t) {
163		462	const std::size_t nu = models[t]->get_nu();
164	✓✗	462	if (nu != 0) {
165		462	dg_ += Qu_[t].dot(k_[t]);
166		462	dq_ -= k_[t].dot(Quuk_[t]);
167			}
168	✓✓	462	if (!is_feasible_) {
169		120	dg_ -= Vx_[t].dot(fs_[t]);
170	✓✗✓✗	120	fTVxx_p_.noalias() = Vxx_[t] * fs_[t];
171		120	dq_ += fs_[t].dot(fTVxx_p_);
172			}
173			}
174		43	}
175
176		40	void SolverFDDP::forwardPass(const double steplength) {
177	✓✗✗✓	40	if (steplength > 1. \|\| steplength < 0.) {
178			throw_pretty("Invalid argument: "
179			<< "invalid step length, value is between 0. to 1.");
180			}
181	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	40	START_PROFILER("SolverFDDP::forwardPass");
182		40	cost_try_ = 0.;
183		40	xnext_ = problem_->get_x0();
184		40	const std::size_t T = problem_->get_T();
185			const std::vector<boost::shared_ptr<ActionModelAbstract> >& models =
186		40	problem_->get_runningModels();
187			const std::vector<boost::shared_ptr<ActionDataAbstract> >& datas =
188		40	problem_->get_runningDatas();
189	✓✓✓✓	40	if ((is_feasible_) \|\| (steplength == 1)) {
190	✓✓	435	for (std::size_t t = 0; t < T; ++t) {
191		397	const boost::shared_ptr<ActionModelAbstract>& m = models[t];
192		397	const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
193		397	const std::size_t nu = m->get_nu();
194
195		397	xs_try_[t] = xnext_;
196	✓✗✓✗ ✓✗	397	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
197	✓✗	397	if (nu != 0) {
198	✓✗✓✗ ✓✗✓✗ ✓✗	397	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
199	✓✗✓✗	397	m->calc(d, xs_try_[t], us_try_[t]);
200			} else {
201			m->calc(d, xs_try_[t]);
202			}
203		397	xnext_ = d->xnext;
204		397	cost_try_ += d->cost;
205
206	✗✓	397	if (raiseIfNaN(cost_try_)) {
207			STOP_PROFILER("SolverFDDP::forwardPass");
208			throw_pretty("forward_error");
209			}
210	✗✓	397	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
211			STOP_PROFILER("SolverFDDP::forwardPass");
212			throw_pretty("forward_error");
213			}
214			}
215
216			const boost::shared_ptr<ActionModelAbstract>& m =
217		38	problem_->get_terminalModel();
218			const boost::shared_ptr<ActionDataAbstract>& d =
219		38	problem_->get_terminalData();
220		38	xs_try_.back() = xnext_;
221	✓✗	38	m->calc(d, xs_try_.back());
222		38	cost_try_ += d->cost;
223
224	✗✓	38	if (raiseIfNaN(cost_try_)) {
225			STOP_PROFILER("SolverFDDP::forwardPass");
226			throw_pretty("forward_error");
227		38	}
228			} else {
229	✓✓	7	for (std::size_t t = 0; t < T; ++t) {
230		5	const boost::shared_ptr<ActionModelAbstract>& m = models[t];
231		5	const boost::shared_ptr<ActionDataAbstract>& d = datas[t];
232		5	const std::size_t nu = m->get_nu();
233	✓✗✓✗ ✓✗✓✗	5	m->get_state()->integrate(xnext_, fs_[t] * (steplength - 1), xs_try_[t]);
234	✓✗✓✗ ✓✗	5	m->get_state()->diff(xs_[t], xs_try_[t], dx_[t]);
235	✓✗	5	if (nu != 0) {
236	✓✗✓✗ ✓✗✓✗ ✓✗	5	us_try_[t].noalias() = us_[t] - k_[t] * steplength - K_[t] * dx_[t];
237	✓✗✓✗	5	m->calc(d, xs_try_[t], us_try_[t]);
238			} else {
239			m->calc(d, xs_try_[t]);
240			}
241		5	xnext_ = d->xnext;
242		5	cost_try_ += d->cost;
243
244	✗✓	5	if (raiseIfNaN(cost_try_)) {
245			STOP_PROFILER("SolverFDDP::forwardPass");
246			throw_pretty("forward_error");
247			}
248	✗✓	5	if (raiseIfNaN(xnext_.lpNorm<Eigen::Infinity>())) {
249			STOP_PROFILER("SolverFDDP::forwardPass");
250			throw_pretty("forward_error");
251			}
252			}
253
254			const boost::shared_ptr<ActionModelAbstract>& m =
255		2	problem_->get_terminalModel();
256			const boost::shared_ptr<ActionDataAbstract>& d =
257		2	problem_->get_terminalData();
258	✓✗✓✗ ✓✗✓✗	4	m->get_state()->integrate(xnext_, fs_.back() * (steplength - 1),
259		2	xs_try_.back());
260	✓✗	2	m->calc(d, xs_try_.back());
261		2	cost_try_ += d->cost;
262
263	✗✓	2	if (raiseIfNaN(cost_try_)) {
264			STOP_PROFILER("SolverFDDP::forwardPass");
265			throw_pretty("forward_error");
266			}
267			}
268	✗✓✗✗ ✗✗✗✗ ✗✓✗✓ ✗✗✗✗	40	STOP_PROFILER("SolverFDDP::forwardPass");
269		40	}
270
271			double SolverFDDP::get_th_acceptnegstep() const { return th_acceptnegstep_; }
272
273			void SolverFDDP::set_th_acceptnegstep(const double th_acceptnegstep) {
274			if (0. > th_acceptnegstep) {
275			throw_pretty("Invalid argument: "
276			<< "th_acceptnegstep value has to be positive.");
277			}
278			th_acceptnegstep_ = th_acceptnegstep;
279			}
280
281			} // namespace crocoddyl