Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/action-base.hxx	Lines:	61	90	67.8 %
Date:	2024-02-13 11:12:33	Branches:	53	220	24.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>
ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(
    boost::shared_ptr<StateAbstractTpl<Scalar> > 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_(MathBase::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_(MathBase::VectorXs::Constant(
          nu, -std::numeric_limits<Scalar>::infinity())),
      u_ub_(MathBase::VectorXs::Constant(
          nu, std::numeric_limits<Scalar>::infinity())),
      has_control_limits_(false) {}

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

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

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

template <typename Scalar>
void ActionModelAbstractTpl<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()) + ")");
  }
  // 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.");

  const std::size_t ndx = state_->get_ndx();
  VectorXs dx = VectorXs::Zero(ndx);
  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);
      state_->diff(x, data->xnext, dx);
      du.noalias() = -pseudoInverse(data->Fu) * dx;
      u += du;
      if (du.norm() <= tol) {
        break;
      }
    }
  }
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs
ActionModelAbstractTpl<Scalar>::quasiStatic_x(
    const boost::shared_ptr<ActionDataAbstract>& 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<ActionDataAbstractTpl<Scalar> >
ActionModelAbstractTpl<Scalar>::createData() {
  return boost::allocate_shared<ActionDataAbstract>(
      Eigen::aligned_allocator<ActionDataAbstract>(), this);
}

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

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

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

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

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

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

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

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

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

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

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

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

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

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

template <typename Scalar>
void ActionModelAbstractTpl<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 ActionModelAbstractTpl<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		1494	ActionModelAbstractTpl<Scalar>::ActionModelAbstractTpl(
20			boost::shared_ptr<StateAbstractTpl<Scalar> > 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_(MathBase::VectorXs::Zero(nu)),
28		1494	g_lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
29			g_ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
30			u_lb_(MathBase::VectorXs::Constant(
31		1494	nu, -std::numeric_limits<Scalar>::infinity())),
32			u_ub_(MathBase::VectorXs::Constant(
33			nu, std::numeric_limits<Scalar>::infinity())),
34	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	1494	has_control_limits_(false) {}
35
36			template <typename Scalar>
37		3012	ActionModelAbstractTpl<Scalar>::~ActionModelAbstractTpl() {}
38
39			template <typename Scalar>
40		3	void ActionModelAbstractTpl<Scalar>::calc(
41			const boost::shared_ptr<ActionDataAbstract>& data,
42			const Eigen::Ref<const VectorXs>& x) {
43	✓✗	3	calc(data, x, unone_);
44		3	}
45
46			template <typename Scalar>
47		1	void ActionModelAbstractTpl<Scalar>::calcDiff(
48			const boost::shared_ptr<ActionDataAbstract>& data,
49			const Eigen::Ref<const VectorXs>& x) {
50	✓✗	1	calcDiff(data, x, unone_);
51		1	}
52
53			template <typename Scalar>
54		240	void ActionModelAbstractTpl<Scalar>::quasiStatic(
55			const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
56			const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter,
57			const Scalar tol) {
58	✓✗✗✓	240	if (static_cast<std::size_t>(u.size()) != nu_) {
59			throw_pretty("Invalid argument: "
60			<< "u has wrong dimension (it should be " +
61			std::to_string(nu_) + ")");
62			}
63	✓✗✗✓	240	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
64			throw_pretty("Invalid argument: "
65			<< "x has wrong dimension (it should be " +
66			std::to_string(state_->get_nx()) + ")");
67			}
68			// Check the velocity input is zero
69	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗	240	assert_pretty(x.tail(state_->get_nv()).isZero(),
70			"The velocity input should be zero for quasi-static to work.");
71
72		240	const std::size_t ndx = state_->get_ndx();
73	✓✗✓✗	480	VectorXs dx = VectorXs::Zero(ndx);
74	✓✗	240	if (nu_ != 0) {
75	✓✗✓✗	480	VectorXs du = VectorXs::Zero(nu_);
76	✓✗	300	for (std::size_t i = 0; i < maxiter; ++i) {
77	✓✗✓✗	300	calc(data, x, u);
78	✓✗✓✗	300	calcDiff(data, x, u);
79	✓✗✓✗ ✓✗	300	state_->diff(x, data->xnext, dx);
80	✓✗✓✗ ✓✗✓✗ ✓✗	300	du.noalias() = -pseudoInverse(data->Fu) * dx;
81	✓✗	300	u += du;
82	✓✗✓✓	300	if (du.norm() <= tol) {
83		240	break;
84			}
85			}
86			}
87		240	}
88
89			template <typename Scalar>
90			typename MathBaseTpl<Scalar>::VectorXs
91			ActionModelAbstractTpl<Scalar>::quasiStatic_x(
92			const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
93			const std::size_t maxiter, const Scalar tol) {
94			VectorXs u(nu_);
95			u.setZero();
96			quasiStatic(data, u, x, maxiter, tol);
97			return u;
98			}
99
100			template <typename Scalar>
101			boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
102			ActionModelAbstractTpl<Scalar>::createData() {
103			return boost::allocate_shared<ActionDataAbstract>(
104			Eigen::aligned_allocator<ActionDataAbstract>(), this);
105			}
106
107			template <typename Scalar>
108			bool ActionModelAbstractTpl<Scalar>::checkData(
109			const boost::shared_ptr<ActionDataAbstract>&) {
110			return false;
111			}
112
113			template <typename Scalar>
114			void ActionModelAbstractTpl<Scalar>::print(std::ostream& os) const {
115			os << boost::core::demangle(typeid(*this).name());
116			}
117
118			template <typename Scalar>
119		694870	std::size_t ActionModelAbstractTpl<Scalar>::get_nu() const {
120		694870	return nu_;
121			}
122
123			template <typename Scalar>
124		44378	std::size_t ActionModelAbstractTpl<Scalar>::get_nr() const {
125		44378	return nr_;
126			}
127
128			template <typename Scalar>
129		18366	std::size_t ActionModelAbstractTpl<Scalar>::get_ng() const {
130		18366	return ng_;
131			}
132
133			template <typename Scalar>
134		18376	std::size_t ActionModelAbstractTpl<Scalar>::get_nh() const {
135		18376	return nh_;
136			}
137
138			template <typename Scalar>
139			const boost::shared_ptr<StateAbstractTpl<Scalar> >&
140		1166859	ActionModelAbstractTpl<Scalar>::get_state() const {
141		1166859	return state_;
142			}
143
144			template <typename Scalar>
145			const typename MathBaseTpl<Scalar>::VectorXs&
146			ActionModelAbstractTpl<Scalar>::get_g_lb() const {
147			return g_lb_;
148			}
149
150			template <typename Scalar>
151			const typename MathBaseTpl<Scalar>::VectorXs&
152			ActionModelAbstractTpl<Scalar>::get_g_ub() const {
153			return g_ub_;
154			}
155
156			template <typename Scalar>
157			const typename MathBaseTpl<Scalar>::VectorXs&
158		258	ActionModelAbstractTpl<Scalar>::get_u_lb() const {
159		258	return u_lb_;
160			}
161
162			template <typename Scalar>
163			const typename MathBaseTpl<Scalar>::VectorXs&
164		258	ActionModelAbstractTpl<Scalar>::get_u_ub() const {
165		258	return u_ub_;
166			}
167
168			template <typename Scalar>
169		600	bool ActionModelAbstractTpl<Scalar>::get_has_control_limits() const {
170		600	return has_control_limits_;
171			}
172
173			template <typename Scalar>
174			void ActionModelAbstractTpl<Scalar>::set_g_lb(const VectorXs& g_lb) {
175			if (static_cast<std::size_t>(g_lb.size()) != ng_) {
176			throw_pretty(
177			"Invalid argument: "
178			<< "inequality lower bound has wrong dimension (it should be " +
179			std::to_string(ng_) + ")");
180			}
181			g_lb_ = g_lb;
182			}
183
184			template <typename Scalar>
185			void ActionModelAbstractTpl<Scalar>::set_g_ub(const VectorXs& g_ub) {
186			if (static_cast<std::size_t>(g_ub.size()) != ng_) {
187			throw_pretty(
188			"Invalid argument: "
189			<< "inequality upper bound has wrong dimension (it should be " +
190			std::to_string(ng_) + ")");
191			}
192			g_ub_ = g_ub;
193			}
194
195			template <typename Scalar>
196		1302	void ActionModelAbstractTpl<Scalar>::set_u_lb(const VectorXs& u_lb) {
197	✗✓	1302	if (static_cast<std::size_t>(u_lb.size()) != nu_) {
198			throw_pretty("Invalid argument: "
199			<< "control lower bound has wrong dimension (it should be " +
200			std::to_string(nu_) + ")");
201			}
202		1302	u_lb_ = u_lb;
203		1302	update_has_control_limits();
204		1302	}
205
206			template <typename Scalar>
207		1302	void ActionModelAbstractTpl<Scalar>::set_u_ub(const VectorXs& u_ub) {
208	✗✓	1302	if (static_cast<std::size_t>(u_ub.size()) != nu_) {
209			throw_pretty("Invalid argument: "
210			<< "control upper bound has wrong dimension (it should be " +
211			std::to_string(nu_) + ")");
212			}
213		1302	u_ub_ = u_ub;
214		1302	update_has_control_limits();
215		1302	}
216
217			template <typename Scalar>
218		2604	void ActionModelAbstractTpl<Scalar>::update_has_control_limits() {
219		2604	has_control_limits_ =
220	✓✗✓✗ ✓✗✓✓ ✓✗✓✗ ✓✗✓✓	2604	isfinite(u_lb_.array()).any() && isfinite(u_ub_.array()).any();
221		2604	}
222
223			template <typename Scalar>
224		308	std::ostream& operator<<(std::ostream& os,
225			const ActionModelAbstractTpl<Scalar>& model) {
226		308	model.print(os);
227		308	return os;
228			}
229
230			} // namespace crocoddyl