Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/integrator/euler.hxx	Lines:	101	110	91.8 %
Date:	2024-02-13 11:12:33	Branches:	92	310	29.7 %


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

#include <boost/core/demangle.hpp>
#include <iostream>
#include <typeinfo>

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

namespace crocoddyl {

template <typename Scalar>
IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
    boost::shared_ptr<DifferentialActionModelAbstract> model,
    boost::shared_ptr<ControlParametrizationModelAbstract> control,
    const Scalar time_step, const bool with_cost_residual)
    : Base(model, control, time_step, with_cost_residual) {}

template <typename Scalar>
IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
    boost::shared_ptr<DifferentialActionModelAbstract> model,
    const Scalar time_step, const bool with_cost_residual)
    : Base(model, time_step, with_cost_residual) {}

template <typename Scalar>
IntegratedActionModelEulerTpl<Scalar>::~IntegratedActionModelEulerTpl() {}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  const std::size_t nv = differential_->get_state()->get_nv();
  Data* d = static_cast<Data*>(data.get());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
      x.tail(nv);

  control_->calc(d->control, Scalar(0.), u);
  differential_->calc(d->differential, x, d->control->w);
  const VectorXs& a = d->differential->xout;
  d->dx.head(nv).noalias() = v * time_step_ + a * time_step2_;
  d->dx.tail(nv).noalias() = a * time_step_;
  differential_->get_state()->integrate(x, d->dx, d->xnext);
  d->cost = time_step_ * d->differential->cost;
  d->g = d->differential->g;
  d->h = d->differential->h;
  if (with_cost_residual_) {
    d->r = d->differential->r;
  }
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  differential_->calc(d->differential, x);
  d->dx.setZero();
  d->xnext = x;
  d->cost = d->differential->cost;
  d->g = d->differential->g;
  d->h = d->differential->h;
  if (with_cost_residual_) {
    d->r = d->differential->r;
  }
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }

  const std::size_t nv = state_->get_nv();
  Data* d = static_cast<Data*>(data.get());

  control_->calc(d->control, Scalar(0.), u);
  differential_->calcDiff(d->differential, x, d->control->w);
  const MatrixXs& da_dx = d->differential->Fx;
  const MatrixXs& da_du = d->differential->Fu;
  control_->multiplyByJacobian(d->control, da_du, d->da_du);
  d->Fx.topRows(nv).noalias() = da_dx * time_step2_;
  d->Fx.bottomRows(nv).noalias() = da_dx * time_step_;
  d->Fx.topRightCorner(nv, nv).diagonal().array() += Scalar(time_step_);
  d->Fu.topRows(nv).noalias() = time_step2_ * d->da_du;
  d->Fu.bottomRows(nv).noalias() = time_step_ * d->da_du;
  state_->JintegrateTransport(x, d->dx, d->Fx, second);
  state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
  state_->JintegrateTransport(x, d->dx, d->Fu, second);

  d->Lx.noalias() = time_step_ * d->differential->Lx;
  control_->multiplyJacobianTransposeBy(d->control, d->differential->Lu, d->Lu);
  d->Lu *= time_step_;
  d->Lxx.noalias() = time_step_ * d->differential->Lxx;
  control_->multiplyByJacobian(d->control, d->differential->Lxu, d->Lxu);
  d->Lxu *= time_step_;
  control_->multiplyByJacobian(d->control, d->differential->Luu, d->Lwu);
  control_->multiplyJacobianTransposeBy(d->control, d->Lwu, d->Luu);
  d->Luu *= time_step_;
  d->Gx = d->differential->Gx;
  d->Hx = d->differential->Hx;
  d->Gu.resize(differential_->get_ng(), nu_);
  d->Hu.resize(differential_->get_nh(), nu_);
  control_->multiplyByJacobian(d->control, d->differential->Gu, d->Gu);
  control_->multiplyByJacobian(d->control, d->differential->Hu, d->Hu);
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  differential_->calcDiff(d->differential, x);
  state_->Jintegrate(x, d->dx, d->Fx, d->Fx);
  d->Lx = d->differential->Lx;
  d->Lxx = d->differential->Lxx;
  d->Gx = d->differential->Gx;
  d->Hx = d->differential->Hx;
}

template <typename Scalar>
boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
IntegratedActionModelEulerTpl<Scalar>::createData() {
  if (control_->get_nu() > differential_->get_nu())
    std::cerr << "Warning: It is useless to use an Euler integrator with a "
                 "control parametrization larger than PolyZero"
              << std::endl;
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
bool IntegratedActionModelEulerTpl<Scalar>::checkData(
    const boost::shared_ptr<ActionDataAbstract>& data) {
  boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
  if (data != NULL) {
    return differential_->checkData(d->differential);
  } else {
    return false;
  }
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::quasiStatic(
    const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
    const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
    const Scalar tol) {
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }

  const boost::shared_ptr<Data>& d = boost::static_pointer_cast<Data>(data);

  d->control->w.setZero();
  differential_->quasiStatic(d->differential, d->control->w, x, maxiter, tol);
  control_->params(d->control, Scalar(0.), d->control->w);
  u = d->control->u;
}

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::print(std::ostream& os) const {
  os << "IntegratedActionModelEuler {dt=" << time_step_ << ", "
     << *differential_ << "}";
}

}  // 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			// University of Oxford, University of Pisa,
6			// Heriot-Watt University
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			#include <boost/core/demangle.hpp>
12			#include <iostream>
13			#include <typeinfo>
14
15			#include "crocoddyl/core/utils/exception.hpp"
16
17			namespace crocoddyl {
18
19			template <typename Scalar>
20		154	IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
21			boost::shared_ptr<DifferentialActionModelAbstract> model,
22			boost::shared_ptr<ControlParametrizationModelAbstract> control,
23			const Scalar time_step, const bool with_cost_residual)
24	✓✗	154	: Base(model, control, time_step, with_cost_residual) {}
25
26			template <typename Scalar>
27		94	IntegratedActionModelEulerTpl<Scalar>::IntegratedActionModelEulerTpl(
28			boost::shared_ptr<DifferentialActionModelAbstract> model,
29			const Scalar time_step, const bool with_cost_residual)
30	✓✗	94	: Base(model, time_step, with_cost_residual) {}
31
32			template <typename Scalar>
33		500	IntegratedActionModelEulerTpl<Scalar>::~IntegratedActionModelEulerTpl() {}
34
35			template <typename Scalar>
36		5685	void IntegratedActionModelEulerTpl<Scalar>::calc(
37			const boost::shared_ptr<ActionDataAbstract>& data,
38			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
39	✓✗✗✓	5685	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
40			throw_pretty("Invalid argument: "
41			<< "x has wrong dimension (it should be " +
42			std::to_string(state_->get_nx()) + ")");
43			}
44	✓✗✗✓	5685	if (static_cast<std::size_t>(u.size()) != nu_) {
45			throw_pretty("Invalid argument: "
46			<< "u has wrong dimension (it should be " +
47			std::to_string(nu_) + ")");
48			}
49		5685	const std::size_t nv = differential_->get_state()->get_nv();
50		5685	Data* d = static_cast<Data*>(data.get());
51	✓✗	5685	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
52			x.tail(nv);
53
54	✓✗	5685	control_->calc(d->control, Scalar(0.), u);
55	✓✗✓✗	5685	differential_->calc(d->differential, x, d->control->w);
56		5685	const VectorXs& a = d->differential->xout;
57	✓✗✓✗ ✓✗✓✗ ✓✗✓✗	5685	d->dx.head(nv).noalias() = v * time_step_ + a * time_step2_;
58	✓✗✓✗ ✓✗✓✗	5685	d->dx.tail(nv).noalias() = a * time_step_;
59	✓✗✓✗ ✓✗	5685	differential_->get_state()->integrate(x, d->dx, d->xnext);
60		5685	d->cost = time_step_ * d->differential->cost;
61	✓✗	5685	d->g = d->differential->g;
62	✓✗	5685	d->h = d->differential->h;
63	✓✗	5685	if (with_cost_residual_) {
64	✓✗	5685	d->r = d->differential->r;
65			}
66		5685	}
67
68			template <typename Scalar>
69		1092	void IntegratedActionModelEulerTpl<Scalar>::calc(
70			const boost::shared_ptr<ActionDataAbstract>& data,
71			const Eigen::Ref<const VectorXs>& x) {
72	✗✓	1092	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
73			throw_pretty("Invalid argument: "
74			<< "x has wrong dimension (it should be " +
75			std::to_string(state_->get_nx()) + ")");
76			}
77		1092	Data* d = static_cast<Data*>(data.get());
78
79		1092	differential_->calc(d->differential, x);
80		1092	d->dx.setZero();
81		1092	d->xnext = x;
82		1092	d->cost = d->differential->cost;
83		1092	d->g = d->differential->g;
84		1092	d->h = d->differential->h;
85	✓✗	1092	if (with_cost_residual_) {
86		1092	d->r = d->differential->r;
87			}
88		1092	}
89
90			template <typename Scalar>
91		1873	void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
92			const boost::shared_ptr<ActionDataAbstract>& data,
93			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
94	✗✓	1873	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
95			throw_pretty("Invalid argument: "
96			<< "x has wrong dimension (it should be " +
97			std::to_string(state_->get_nx()) + ")");
98			}
99	✗✓	1873	if (static_cast<std::size_t>(u.size()) != nu_) {
100			throw_pretty("Invalid argument: "
101			<< "u has wrong dimension (it should be " +
102			std::to_string(nu_) + ")");
103			}
104
105		1873	const std::size_t nv = state_->get_nv();
106		1873	Data* d = static_cast<Data*>(data.get());
107
108		1873	control_->calc(d->control, Scalar(0.), u);
109	✓✗	1873	differential_->calcDiff(d->differential, x, d->control->w);
110		1873	const MatrixXs& da_dx = d->differential->Fx;
111		1873	const MatrixXs& da_du = d->differential->Fu;
112	✓✗✓✗	1873	control_->multiplyByJacobian(d->control, da_du, d->da_du);
113	✓✗✓✗ ✓✗	1873	d->Fx.topRows(nv).noalias() = da_dx * time_step2_;
114	✓✗✓✗ ✓✗	1873	d->Fx.bottomRows(nv).noalias() = da_dx * time_step_;
115	✓✗✓✗ ✓✗✓✗	1873	d->Fx.topRightCorner(nv, nv).diagonal().array() += Scalar(time_step_);
116	✓✗✓✗ ✓✗	1873	d->Fu.topRows(nv).noalias() = time_step2_ * d->da_du;
117	✓✗✓✗ ✓✗	1873	d->Fu.bottomRows(nv).noalias() = time_step_ * d->da_du;
118	✓✗✓✗	1873	state_->JintegrateTransport(x, d->dx, d->Fx, second);
119	✓✗✓✗ ✓✗	1873	state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
120	✓✗✓✗	1873	state_->JintegrateTransport(x, d->dx, d->Fu, second);
121
122	✓✗✓✗	1873	d->Lx.noalias() = time_step_ * d->differential->Lx;
123	✓✗✓✗	1873	control_->multiplyJacobianTransposeBy(d->control, d->differential->Lu, d->Lu);
124		1873	d->Lu *= time_step_;
125	✓✗✓✗	1873	d->Lxx.noalias() = time_step_ * d->differential->Lxx;
126	✓✗✓✗	1873	control_->multiplyByJacobian(d->control, d->differential->Lxu, d->Lxu);
127		1873	d->Lxu *= time_step_;
128	✓✗✓✗	1873	control_->multiplyByJacobian(d->control, d->differential->Luu, d->Lwu);
129	✓✗✓✗	1873	control_->multiplyJacobianTransposeBy(d->control, d->Lwu, d->Luu);
130		1873	d->Luu *= time_step_;
131		1873	d->Gx = d->differential->Gx;
132		1873	d->Hx = d->differential->Hx;
133		1873	d->Gu.resize(differential_->get_ng(), nu_);
134		1873	d->Hu.resize(differential_->get_nh(), nu_);
135	✓✗✓✗	1873	control_->multiplyByJacobian(d->control, d->differential->Gu, d->Gu);
136	✓✗✓✗	1873	control_->multiplyByJacobian(d->control, d->differential->Hu, d->Hu);
137		1873	}
138
139			template <typename Scalar>
140		149	void IntegratedActionModelEulerTpl<Scalar>::calcDiff(
141			const boost::shared_ptr<ActionDataAbstract>& data,
142			const Eigen::Ref<const VectorXs>& x) {
143	✗✓	149	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
144			throw_pretty("Invalid argument: "
145			<< "x has wrong dimension (it should be " +
146			std::to_string(state_->get_nx()) + ")");
147			}
148		149	Data* d = static_cast<Data*>(data.get());
149
150		149	differential_->calcDiff(d->differential, x);
151	✓✗✓✗ ✓✗	149	state_->Jintegrate(x, d->dx, d->Fx, d->Fx);
152		149	d->Lx = d->differential->Lx;
153		149	d->Lxx = d->differential->Lxx;
154		149	d->Gx = d->differential->Gx;
155		149	d->Hx = d->differential->Hx;
156		149	}
157
158			template <typename Scalar>
159			boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
160		7410	IntegratedActionModelEulerTpl<Scalar>::createData() {
161	✓✓	7410	if (control_->get_nu() > differential_->get_nu())
162		66	std::cerr << "Warning: It is useless to use an Euler integrator with a "
163			"control parametrization larger than PolyZero"
164		66	<< std::endl;
165	✓✗	7410	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
166			}
167
168			template <typename Scalar>
169		1408	bool IntegratedActionModelEulerTpl<Scalar>::checkData(
170			const boost::shared_ptr<ActionDataAbstract>& data) {
171		2816	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
172	✓✗	1408	if (data != NULL) {
173	✓✗	1408	return differential_->checkData(d->differential);
174			} else {
175			return false;
176			}
177			}
178
179			template <typename Scalar>
180		1760	void IntegratedActionModelEulerTpl<Scalar>::quasiStatic(
181			const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
182			const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
183			const Scalar tol) {
184	✓✗✗✓	1760	if (static_cast<std::size_t>(u.size()) != nu_) {
185			throw_pretty("Invalid argument: "
186			<< "u has wrong dimension (it should be " +
187			std::to_string(nu_) + ")");
188			}
189	✓✗✗✓	1760	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
190			throw_pretty("Invalid argument: "
191			<< "x has wrong dimension (it should be " +
192			std::to_string(state_->get_nx()) + ")");
193			}
194
195		3520	const boost::shared_ptr<Data>& d = boost::static_pointer_cast<Data>(data);
196
197	✓✗	1760	d->control->w.setZero();
198	✓✗✓✗	1760	differential_->quasiStatic(d->differential, d->control->w, x, maxiter, tol);
199	✓✗✓✗	1760	control_->params(d->control, Scalar(0.), d->control->w);
200	✓✗	1760	u = d->control->u;
201		1760	}
202
203			template <typename Scalar>
204		22	void IntegratedActionModelEulerTpl<Scalar>::print(std::ostream& os) const {
205		22	os << "IntegratedActionModelEuler {dt=" << time_step_ << ", "
206		22	<< *differential_ << "}";
207		22	}
208
209			} // namespace crocoddyl