Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/diff-action-base.hxx	Lines:	57	88	64.8 %
Date:	2024-02-13 11:12:33	Branches:	45	204	22.1 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
//                          University of Oxford, 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>
DifferentialActionModelAbstractTpl<Scalar>::DifferentialActionModelAbstractTpl(
    boost::shared_ptr<StateAbstract> state, const std::size_t nu,
    const std::size_t nr, const std::size_t ng, const std::size_t nh)
    : nu_(nu),
      nr_(nr),
      ng_(ng),
      nh_(nh),
      state_(state),
      unone_(VectorXs::Zero(nu)),
      g_lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
      g_ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
      u_lb_(VectorXs::Constant(nu, -std::numeric_limits<Scalar>::infinity())),
      u_ub_(VectorXs::Constant(nu, std::numeric_limits<Scalar>::infinity())),
      has_control_limits_(false) {}

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

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::calc(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  calc(data, x, unone_);
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::calcDiff(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  calcDiff(data, x, unone_);
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::quasiStatic(
    const boost::shared_ptr<DifferentialActionDataAbstract>& 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()) + ")");
  }
  // Check the velocity input is zero
  assert_pretty(x.tail(state_->get_nv()).isZero(),
                "The velocity input should be zero for quasi-static to work.");

  if (nu_ != 0) {
    VectorXs du = VectorXs::Zero(nu_);
    for (std::size_t i = 0; i < maxiter; ++i) {
      calc(data, x, u);
      calcDiff(data, x, u);
      du.noalias() = -pseudoInverse(data->Fu) * data->xout;
      u += du;
      if (du.norm() <= tol) {
        break;
      }
    }
  }
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs
DifferentialActionModelAbstractTpl<Scalar>::quasiStatic_x(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    const VectorXs& x, const std::size_t maxiter, const Scalar tol) {
  VectorXs u(nu_);
  u.setZero();
  quasiStatic(data, u, x, maxiter, tol);
  return u;
}

template <typename Scalar>
boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
DifferentialActionModelAbstractTpl<Scalar>::createData() {
  return boost::allocate_shared<DifferentialActionDataAbstract>(
      Eigen::aligned_allocator<DifferentialActionDataAbstract>(), this);
}

template <typename Scalar>
bool DifferentialActionModelAbstractTpl<Scalar>::checkData(
    const boost::shared_ptr<DifferentialActionDataAbstract>&) {
  return false;
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::print(std::ostream& os) const {
  os << boost::core::demangle(typeid(*this).name());
}

template <typename Scalar>
std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nu() const {
  return nu_;
}

template <typename Scalar>
std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nr() const {
  return nr_;
}

template <typename Scalar>
std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_ng() const {
  return ng_;
}

template <typename Scalar>
std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nh() const {
  return nh_;
}

template <typename Scalar>
const boost::shared_ptr<StateAbstractTpl<Scalar> >&
DifferentialActionModelAbstractTpl<Scalar>::get_state() const {
  return state_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
DifferentialActionModelAbstractTpl<Scalar>::get_g_lb() const {
  return g_lb_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
DifferentialActionModelAbstractTpl<Scalar>::get_g_ub() const {
  return g_ub_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
DifferentialActionModelAbstractTpl<Scalar>::get_u_lb() const {
  return u_lb_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
DifferentialActionModelAbstractTpl<Scalar>::get_u_ub() const {
  return u_ub_;
}

template <typename Scalar>
bool DifferentialActionModelAbstractTpl<Scalar>::get_has_control_limits()
    const {
  return has_control_limits_;
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::set_g_lb(
    const VectorXs& g_lb) {
  if (static_cast<std::size_t>(g_lb.size()) != ng_) {
    throw_pretty(
        "Invalid argument: "
        << "inequality lower bound has wrong dimension (it should be " +
               std::to_string(ng_) + ")");
  }
  g_lb_ = g_lb;
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::set_g_ub(
    const VectorXs& g_ub) {
  if (static_cast<std::size_t>(g_ub.size()) != ng_) {
    throw_pretty(
        "Invalid argument: "
        << "inequality upper bound has wrong dimension (it should be " +
               std::to_string(ng_) + ")");
  }
  g_ub_ = g_ub;
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::set_u_lb(
    const VectorXs& u_lb) {
  if (static_cast<std::size_t>(u_lb.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "lower bound has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  u_lb_ = u_lb;
  update_has_control_limits();
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::set_u_ub(
    const VectorXs& u_ub) {
  if (static_cast<std::size_t>(u_ub.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "upper bound has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  u_ub_ = u_ub;
  update_has_control_limits();
}

template <typename Scalar>
void DifferentialActionModelAbstractTpl<Scalar>::update_has_control_limits() {
  has_control_limits_ =
      isfinite(u_lb_.array()).any() && isfinite(u_ub_.array()).any();
}

template <typename Scalar>
std::ostream& operator<<(
    std::ostream& os, const DifferentialActionModelAbstractTpl<Scalar>& model) {
  model.print(os);
  return os;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include <boost/core/demangle.hpp>
11			#include <iostream>
12			#include <typeinfo>
13
14			#include "crocoddyl/core/utils/exception.hpp"
15
16			namespace crocoddyl {
17
18			template <typename Scalar>
19		1268	DifferentialActionModelAbstractTpl<Scalar>::DifferentialActionModelAbstractTpl(
20			boost::shared_ptr<StateAbstract> state, const std::size_t nu,
21			const std::size_t nr, const std::size_t ng, const std::size_t nh)
22			: nu_(nu),
23			nr_(nr),
24			ng_(ng),
25			nh_(nh),
26			state_(state),
27			unone_(VectorXs::Zero(nu)),
28		1268	g_lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
29			g_ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
30		1268	u_lb_(VectorXs::Constant(nu, -std::numeric_limits<Scalar>::infinity())),
31			u_ub_(VectorXs::Constant(nu, std::numeric_limits<Scalar>::infinity())),
32	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1268	has_control_limits_(false) {}
33
34			template <typename Scalar>
35		2560	DifferentialActionModelAbstractTpl<
36		2560	Scalar>::~DifferentialActionModelAbstractTpl() {}
37
38			template <typename Scalar>
39		3	void DifferentialActionModelAbstractTpl<Scalar>::calc(
40			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
41			const Eigen::Ref<const VectorXs>& x) {
42	✓✗	3	calc(data, x, unone_);
43		3	}
44
45			template <typename Scalar>
46		1	void DifferentialActionModelAbstractTpl<Scalar>::calcDiff(
47			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
48			const Eigen::Ref<const VectorXs>& x) {
49	✓✗	1	calcDiff(data, x, unone_);
50		1	}
51
52			template <typename Scalar>
53		642	void DifferentialActionModelAbstractTpl<Scalar>::quasiStatic(
54			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
55			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
56			const std::size_t maxiter, const Scalar tol) {
57	✗✓	642	if (static_cast<std::size_t>(u.size()) != nu_) {
58			throw_pretty("Invalid argument: "
59			<< "u has wrong dimension (it should be " +
60			std::to_string(nu_) + ")");
61			}
62	✗✓	642	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
63			throw_pretty("Invalid argument: "
64			<< "x has wrong dimension (it should be " +
65			std::to_string(state_->get_nx()) + ")");
66			}
67			// Check the velocity input is zero
68	✓✗✗✓ ✗✗✗✗ ✗✗✗✗	642	assert_pretty(x.tail(state_->get_nv()).isZero(),
69			"The velocity input should be zero for quasi-static to work.");
70
71	✓✗	642	if (nu_ != 0) {
72	✓✗✓✗	1284	VectorXs du = VectorXs::Zero(nu_);
73	✓✗	1284	for (std::size_t i = 0; i < maxiter; ++i) {
74	✓✗✓✗	1284	calc(data, x, u);
75	✓✗✓✗	1284	calcDiff(data, x, u);
76	✓✗✓✗ ✓✗✓✗ ✓✗	1284	du.noalias() = -pseudoInverse(data->Fu) * data->xout;
77	✓✗	1284	u += du;
78	✓✗✓✓	1284	if (du.norm() <= tol) {
79		642	break;
80			}
81			}
82			}
83		642	}
84
85			template <typename Scalar>
86			typename MathBaseTpl<Scalar>::VectorXs
87			DifferentialActionModelAbstractTpl<Scalar>::quasiStatic_x(
88			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
89			const VectorXs& x, const std::size_t maxiter, const Scalar tol) {
90			VectorXs u(nu_);
91			u.setZero();
92			quasiStatic(data, u, x, maxiter, tol);
93			return u;
94			}
95
96			template <typename Scalar>
97			boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
98			DifferentialActionModelAbstractTpl<Scalar>::createData() {
99			return boost::allocate_shared<DifferentialActionDataAbstract>(
100			Eigen::aligned_allocator<DifferentialActionDataAbstract>(), this);
101			}
102
103			template <typename Scalar>
104			bool DifferentialActionModelAbstractTpl<Scalar>::checkData(
105			const boost::shared_ptr<DifferentialActionDataAbstract>&) {
106			return false;
107			}
108
109			template <typename Scalar>
110			void DifferentialActionModelAbstractTpl<Scalar>::print(std::ostream& os) const {
111			os << boost::core::demangle(typeid(*this).name());
112			}
113
114			template <typename Scalar>
115		994292	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nu() const {
116		994292	return nu_;
117			}
118
119			template <typename Scalar>
120		106308	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nr() const {
121		106308	return nr_;
122			}
123
124			template <typename Scalar>
125		229796	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_ng() const {
126		229796	return ng_;
127			}
128
129			template <typename Scalar>
130		229796	std::size_t DifferentialActionModelAbstractTpl<Scalar>::get_nh() const {
131		229796	return nh_;
132			}
133
134			template <typename Scalar>
135			const boost::shared_ptr<StateAbstractTpl<Scalar> >&
136		1644483	DifferentialActionModelAbstractTpl<Scalar>::get_state() const {
137		1644483	return state_;
138			}
139
140			template <typename Scalar>
141			const typename MathBaseTpl<Scalar>::VectorXs&
142			DifferentialActionModelAbstractTpl<Scalar>::get_g_lb() const {
143			return g_lb_;
144			}
145
146			template <typename Scalar>
147			const typename MathBaseTpl<Scalar>::VectorXs&
148			DifferentialActionModelAbstractTpl<Scalar>::get_g_ub() const {
149			return g_ub_;
150			}
151
152			template <typename Scalar>
153			const typename MathBaseTpl<Scalar>::VectorXs&
154		1044	DifferentialActionModelAbstractTpl<Scalar>::get_u_lb() const {
155		1044	return u_lb_;
156			}
157
158			template <typename Scalar>
159			const typename MathBaseTpl<Scalar>::VectorXs&
160		1044	DifferentialActionModelAbstractTpl<Scalar>::get_u_ub() const {
161		1044	return u_ub_;
162			}
163
164			template <typename Scalar>
165			bool DifferentialActionModelAbstractTpl<Scalar>::get_has_control_limits()
166			const {
167			return has_control_limits_;
168			}
169
170			template <typename Scalar>
171			void DifferentialActionModelAbstractTpl<Scalar>::set_g_lb(
172			const VectorXs& g_lb) {
173			if (static_cast<std::size_t>(g_lb.size()) != ng_) {
174			throw_pretty(
175			"Invalid argument: "
176			<< "inequality lower bound has wrong dimension (it should be " +
177			std::to_string(ng_) + ")");
178			}
179			g_lb_ = g_lb;
180			}
181
182			template <typename Scalar>
183			void DifferentialActionModelAbstractTpl<Scalar>::set_g_ub(
184			const VectorXs& g_ub) {
185			if (static_cast<std::size_t>(g_ub.size()) != ng_) {
186			throw_pretty(
187			"Invalid argument: "
188			<< "inequality upper bound has wrong dimension (it should be " +
189			std::to_string(ng_) + ")");
190			}
191			g_ub_ = g_ub;
192			}
193
194			template <typename Scalar>
195		1074	void DifferentialActionModelAbstractTpl<Scalar>::set_u_lb(
196			const VectorXs& u_lb) {
197	✗✓	1074	if (static_cast<std::size_t>(u_lb.size()) != nu_) {
198			throw_pretty("Invalid argument: "
199			<< "lower bound has wrong dimension (it should be " +
200			std::to_string(nu_) + ")");
201			}
202		1074	u_lb_ = u_lb;
203		1074	update_has_control_limits();
204		1074	}
205
206			template <typename Scalar>
207		1074	void DifferentialActionModelAbstractTpl<Scalar>::set_u_ub(
208			const VectorXs& u_ub) {
209	✗✓	1074	if (static_cast<std::size_t>(u_ub.size()) != nu_) {
210			throw_pretty("Invalid argument: "
211			<< "upper bound has wrong dimension (it should be " +
212			std::to_string(nu_) + ")");
213			}
214		1074	u_ub_ = u_ub;
215		1074	update_has_control_limits();
216		1074	}
217
218			template <typename Scalar>
219		2148	void DifferentialActionModelAbstractTpl<Scalar>::update_has_control_limits() {
220		2148	has_control_limits_ =
221	✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✓✓	2148	isfinite(u_lb_.array()).any() && isfinite(u_ub_.array()).any();
222		2148	}
223
224			template <typename Scalar>
225		220	std::ostream& operator<<(
226			std::ostream& os, const DifferentialActionModelAbstractTpl<Scalar>& model) {
227		220	model.print(os);
228		220	return os;
229			}
230
231			} // namespace crocoddyl