Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/constraint-base.hxx	Lines:	27	76	35.5 %
Date:	2024-02-13 11:12:33	Branches:	21	180	11.7 %


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

#include <boost/core/demangle.hpp>
#include <limits>

namespace crocoddyl {

template <typename Scalar>
ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
    boost::shared_ptr<StateAbstract> state,
    boost::shared_ptr<ResidualModelAbstract> residual, const std::size_t ng,
    const std::size_t nh)
    : ng_internal_(ng),
      nh_internal_(nh),
      state_(state),
      residual_(residual),
      type_((ng > 0 && nh > 0) ? ConstraintType::Both
                               : (ng > 0 ? ConstraintType::Inequality
                                         : ConstraintType::Equality)),
      lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
      ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
      nu_(residual->get_nu()),
      ng_(ng),
      nh_(nh),
      unone_(VectorXs::Zero(residual->get_nu())) {
  if (nh_ > residual_->get_nr()) {
    throw_pretty("Invalid argument: "
                 << "the number of equality constraints (nh) is wrong as it is "
                    "bigger than the residual dimension.")
  }
  std::size_t max_ng = 2 * (residual_->get_nr() - nh_);
  if (ng_ > max_ng) {
    throw_pretty("Invalid argument: "
                 << "the number of inequality constraints (ng) is wrong as it "
                    "should be in the range [0, " +
                        std::to_string(max_ng) + "]");
  }
}

template <typename Scalar>
ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
    boost::shared_ptr<StateAbstract> state, const std::size_t nu,
    const std::size_t ng, const std::size_t nh)
    : ng_internal_(ng),
      nh_internal_(nh),
      state_(state),
      residual_(boost::make_shared<ResidualModelAbstract>(state, ng + nh, nu)),
      type_((ng > 0 && nh > 0) ? ConstraintType::Both
                               : (ng > 0 ? ConstraintType::Inequality
                                         : ConstraintType::Equality)),
      lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
      ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
      nu_(nu),
      ng_(ng),
      nh_(nh),
      unone_(VectorXs::Zero(nu)) {}

template <typename Scalar>
ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
    boost::shared_ptr<StateAbstract> state, const std::size_t ng,
    const std::size_t nh)
    : ng_internal_(ng),
      nh_internal_(nh),
      state_(state),
      residual_(boost::make_shared<ResidualModelAbstract>(state, ng + nh)),
      type_((ng > 0 && nh > 0) ? ConstraintType::Both
                               : (ng > 0 ? ConstraintType::Inequality
                                         : ConstraintType::Equality)),
      lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
      ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
      nu_(state->get_nv()),
      ng_(ng),
      nh_(nh),
      unone_(VectorXs::Zero(state->get_nv())) {}

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

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

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

template <typename Scalar>
boost::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
ConstraintModelAbstractTpl<Scalar>::createData(
    DataCollectorAbstract* const data) {
  return boost::allocate_shared<ConstraintDataAbstract>(
      Eigen::aligned_allocator<ConstraintDataAbstract>(), this, data);
}

template <typename Scalar>
void ConstraintModelAbstractTpl<Scalar>::update_bounds(const VectorXs& lower,
                                                       const VectorXs& upper) {
  if (static_cast<std::size_t>(upper.size()) != ng_internal_ ||
      static_cast<std::size_t>(lower.size()) != ng_internal_) {
    throw_pretty(
        "Invalid argument: the dimension of the lower/upper bound is not the "
        "same to ng.")
  }
  if (((upper - lower).array() <= 0.).any()) {
    throw_pretty(
        "Invalid argument: the upper bound is not higher than the lower bound.")
  }
  if ((lb_.array() == std::numeric_limits<Scalar>::infinity()).any() ||
      (lb_.array() == std::numeric_limits<Scalar>::max()).any()) {
    throw_pretty(
        "Invalid argument: the lower bound cannot contain a positive "
        "infinity/max value");
  }
  if ((ub_.array() == -std::numeric_limits<Scalar>::infinity()).any() ||
      (ub_.array() == -std::numeric_limits<Scalar>::infinity()).any()) {
    throw_pretty(
        "Invalid argument: the lower bound cannot contain a negative "
        "infinity/min value");
  }
  ng_ = ng_internal_;
  nh_ = nh_internal_;
  lb_ = lower;
  ub_ = upper;
  if (nh_ == 0) {
    type_ = ConstraintType::Inequality;
  } else {
    type_ = ConstraintType::Both;
  }
}

template <typename Scalar>
void ConstraintModelAbstractTpl<Scalar>::remove_bounds() {
  ng_ = 0;
  nh_ = nh_internal_ + ng_internal_;
  lb_.setConstant(-std::numeric_limits<Scalar>::infinity());
  ub_.setConstant(std::numeric_limits<Scalar>::infinity());
  type_ = ConstraintType::Equality;
}

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

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

template <typename Scalar>
const boost::shared_ptr<ResidualModelAbstractTpl<Scalar> >&
ConstraintModelAbstractTpl<Scalar>::get_residual() const {
  return residual_;
}

template <typename Scalar>
ConstraintType ConstraintModelAbstractTpl<Scalar>::get_type() const {
  return type_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
ConstraintModelAbstractTpl<Scalar>::get_lb() const {
  return lb_;
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
ConstraintModelAbstractTpl<Scalar>::get_ub() const {
  return ub_;
}

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

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

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

template <class Scalar>
std::ostream& operator<<(std::ostream& os,
                         const ConstraintModelAbstractTpl<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) 2020-2023, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include <boost/core/demangle.hpp>
10			#include <limits>
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		2661	ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
16			boost::shared_ptr<StateAbstract> state,
17			boost::shared_ptr<ResidualModelAbstract> residual, const std::size_t ng,
18			const std::size_t nh)
19			: ng_internal_(ng),
20			nh_internal_(nh),
21			state_(state),
22			residual_(residual),
23	✓✗	240	type_((ng > 0 && nh > 0) ? ConstraintType::Both
24			: (ng > 0 ? ConstraintType::Inequality
25			: ConstraintType::Equality)),
26		2661	lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
27			ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
28			nu_(residual->get_nu()),
29			ng_(ng),
30			nh_(nh),
31	✓✓✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	2901	unone_(VectorXs::Zero(residual->get_nu())) {
32	✗✓	2661	if (nh_ > residual_->get_nr()) {
33			throw_pretty("Invalid argument: "
34			<< "the number of equality constraints (nh) is wrong as it is "
35			"bigger than the residual dimension.")
36			}
37		2661	std::size_t max_ng = 2 * (residual_->get_nr() - nh_);
38	✗✓	2661	if (ng_ > max_ng) {
39			throw_pretty("Invalid argument: "
40			<< "the number of inequality constraints (ng) is wrong as it "
41			"should be in the range [0, " +
42			std::to_string(max_ng) + "]");
43			}
44		2661	}
45
46			template <typename Scalar>
47		140	ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
48			boost::shared_ptr<StateAbstract> state, const std::size_t nu,
49			const std::size_t ng, const std::size_t nh)
50			: ng_internal_(ng),
51			nh_internal_(nh),
52			state_(state),
53			residual_(boost::make_shared<ResidualModelAbstract>(state, ng + nh, nu)),
54	✓✗	70	type_((ng > 0 && nh > 0) ? ConstraintType::Both
55			: (ng > 0 ? ConstraintType::Inequality
56			: ConstraintType::Equality)),
57		140	lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
58			ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
59			nu_(nu),
60			ng_(ng),
61			nh_(nh),
62	✓✗✓✓ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗	210	unone_(VectorXs::Zero(nu)) {}
63
64			template <typename Scalar>
65			ConstraintModelAbstractTpl<Scalar>::ConstraintModelAbstractTpl(
66			boost::shared_ptr<StateAbstract> state, const std::size_t ng,
67			const std::size_t nh)
68			: ng_internal_(ng),
69			nh_internal_(nh),
70			state_(state),
71			residual_(boost::make_shared<ResidualModelAbstract>(state, ng + nh)),
72			type_((ng > 0 && nh > 0) ? ConstraintType::Both
73			: (ng > 0 ? ConstraintType::Inequality
74			: ConstraintType::Equality)),
75			lb_(VectorXs::Constant(ng, -std::numeric_limits<Scalar>::infinity())),
76			ub_(VectorXs::Constant(ng, std::numeric_limits<Scalar>::infinity())),
77			nu_(state->get_nv()),
78			ng_(ng),
79			nh_(nh),
80			unone_(VectorXs::Zero(state->get_nv())) {}
81
82			template <typename Scalar>
83		5606	ConstraintModelAbstractTpl<Scalar>::~ConstraintModelAbstractTpl() {}
84
85			template <typename Scalar>
86			void ConstraintModelAbstractTpl<Scalar>::calc(
87			const boost::shared_ptr<ConstraintDataAbstract>& data,
88			const Eigen::Ref<const VectorXs>& x) {
89			calc(data, x, unone_);
90			}
91
92			template <typename Scalar>
93			void ConstraintModelAbstractTpl<Scalar>::calcDiff(
94			const boost::shared_ptr<ConstraintDataAbstract>& data,
95			const Eigen::Ref<const VectorXs>& x) {
96			calcDiff(data, x, unone_);
97			}
98
99			template <typename Scalar>
100			boost::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
101			ConstraintModelAbstractTpl<Scalar>::createData(
102			DataCollectorAbstract* const data) {
103			return boost::allocate_shared<ConstraintDataAbstract>(
104			Eigen::aligned_allocator<ConstraintDataAbstract>(), this, data);
105			}
106
107			template <typename Scalar>
108			void ConstraintModelAbstractTpl<Scalar>::update_bounds(const VectorXs& lower,
109			const VectorXs& upper) {
110			if (static_cast<std::size_t>(upper.size()) != ng_internal_ \|\|
111			static_cast<std::size_t>(lower.size()) != ng_internal_) {
112			throw_pretty(
113			"Invalid argument: the dimension of the lower/upper bound is not the "
114			"same to ng.")
115			}
116			if (((upper - lower).array() <= 0.).any()) {
117			throw_pretty(
118			"Invalid argument: the upper bound is not higher than the lower bound.")
119			}
120			if ((lb_.array() == std::numeric_limits<Scalar>::infinity()).any() \|\|
121			(lb_.array() == std::numeric_limits<Scalar>::max()).any()) {
122			throw_pretty(
123			"Invalid argument: the lower bound cannot contain a positive "
124			"infinity/max value");
125			}
126			if ((ub_.array() == -std::numeric_limits<Scalar>::infinity()).any() \|\|
127			(ub_.array() == -std::numeric_limits<Scalar>::infinity()).any()) {
128			throw_pretty(
129			"Invalid argument: the lower bound cannot contain a negative "
130			"infinity/min value");
131			}
132			ng_ = ng_internal_;
133			nh_ = nh_internal_;
134			lb_ = lower;
135			ub_ = upper;
136			if (nh_ == 0) {
137			type_ = ConstraintType::Inequality;
138			} else {
139			type_ = ConstraintType::Both;
140			}
141			}
142
143			template <typename Scalar>
144			void ConstraintModelAbstractTpl<Scalar>::remove_bounds() {
145			ng_ = 0;
146			nh_ = nh_internal_ + ng_internal_;
147			lb_.setConstant(-std::numeric_limits<Scalar>::infinity());
148			ub_.setConstant(std::numeric_limits<Scalar>::infinity());
149			type_ = ConstraintType::Equality;
150			}
151
152			template <typename Scalar>
153			void ConstraintModelAbstractTpl<Scalar>::print(std::ostream& os) const {
154			os << boost::core::demangle(typeid(*this).name());
155			}
156
157			template <typename Scalar>
158			const boost::shared_ptr<StateAbstractTpl<Scalar> >&
159		467218	ConstraintModelAbstractTpl<Scalar>::get_state() const {
160		467218	return state_;
161			}
162
163			template <typename Scalar>
164			const boost::shared_ptr<ResidualModelAbstractTpl<Scalar> >&
165		229808	ConstraintModelAbstractTpl<Scalar>::get_residual() const {
166		229808	return residual_;
167			}
168
169			template <typename Scalar>
170			ConstraintType ConstraintModelAbstractTpl<Scalar>::get_type() const {
171			return type_;
172			}
173
174			template <typename Scalar>
175			const typename MathBaseTpl<Scalar>::VectorXs&
176		130377	ConstraintModelAbstractTpl<Scalar>::get_lb() const {
177		130377	return lb_;
178			}
179
180			template <typename Scalar>
181			const typename MathBaseTpl<Scalar>::VectorXs&
182		130377	ConstraintModelAbstractTpl<Scalar>::get_ub() const {
183		130377	return ub_;
184			}
185
186			template <typename Scalar>
187		464687	std::size_t ConstraintModelAbstractTpl<Scalar>::get_nu() const {
188		464687	return nu_;
189			}
190
191			template <typename Scalar>
192		1071430	std::size_t ConstraintModelAbstractTpl<Scalar>::get_ng() const {
193		1071430	return ng_;
194			}
195
196			template <typename Scalar>
197		1064123	std::size_t ConstraintModelAbstractTpl<Scalar>::get_nh() const {
198		1064123	return nh_;
199			}
200
201			template <class Scalar>
202			std::ostream& operator<<(std::ostream& os,
203			const ConstraintModelAbstractTpl<Scalar>& model) {
204			model.print(os);
205			return os;
206			}
207
208			} // namespace crocoddyl