Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/constraints/residual.hxx	Lines:	70	85	82.4 %
Date:	2024-02-13 11:12:33	Branches:	68	144	47.2 %


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

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

namespace crocoddyl {

using std::isfinite;

template <typename Scalar>
ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
    boost::shared_ptr<typename Base::StateAbstract> state,
    boost::shared_ptr<ResidualModelAbstract> residual, const VectorXs& lower,
    const VectorXs& upper)
    : Base(state, residual, residual->get_nr(), 0) {
  lb_ = lower;
  ub_ = upper;
  for (std::size_t i = 0; i < residual_->get_nr(); ++i) {
    if (isfinite(lb_(i)) && isfinite(ub_(i))) {
      if (lb_(i) - ub_(i) > 0) {
        throw_pretty(
            "Invalid argument: the upper bound is not equal to / 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/max value");
  }
}

template <typename Scalar>
ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
    boost::shared_ptr<typename Base::StateAbstract> state,
    boost::shared_ptr<ResidualModelAbstract> residual)
    : Base(state, residual, 0, residual->get_nr()) {}

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

template <typename Scalar>
void ConstraintModelResidualTpl<Scalar>::calc(
    const boost::shared_ptr<ConstraintDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  // Compute the constraint residual
  residual_->calc(data->residual, x, u);

  // Fill the residual values for its corresponding type of constraint
  updateCalc(data);
}

template <typename Scalar>
void ConstraintModelResidualTpl<Scalar>::calc(
    const boost::shared_ptr<ConstraintDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  // Compute the constraint residual
  residual_->calc(data->residual, x);

  // Fill the residual values for its corresponding type of constraint
  updateCalc(data);
}

template <typename Scalar>
void ConstraintModelResidualTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ConstraintDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  // Compute the derivatives of the residual function
  residual_->calcDiff(data->residual, x, u);

  // Fill the residual values for its corresponding type of constraint
  updateCalcDiff(data);
}

template <typename Scalar>
void ConstraintModelResidualTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ConstraintDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  // Compute the derivatives of the residual function
  residual_->calcDiff(data->residual, x);

  // Fill the residual values for its corresponding type of constraint
  updateCalcDiff(data);
}

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

template <typename Scalar>
void ConstraintModelResidualTpl<Scalar>::updateCalc(
    const boost::shared_ptr<ConstraintDataAbstract>& data) {
  switch (type_) {
    case ConstraintType::Inequality:
      data->g = data->residual->r;
      break;
    case ConstraintType::Equality:
      data->h = data->residual->r;
      break;
    case ConstraintType::Both:  // this condition is not supported and possible
      break;
  }
}

template <typename Scalar>
void ConstraintModelResidualTpl<Scalar>::updateCalcDiff(
    const boost::shared_ptr<ConstraintDataAbstract>& data) {
  const bool is_rq = residual_->get_q_dependent();
  const bool is_rv = residual_->get_v_dependent();
  const bool is_ru = residual_->get_u_dependent() || nu_ == 0;
  switch (type_) {
    case ConstraintType::Inequality:
      if (is_rq && is_rv) {
        data->Gx = data->residual->Rx;
      } else if (is_rq) {
        const std::size_t nv = state_->get_nv();
        data->Gx.leftCols(nv) = data->residual->Rx.leftCols(nv);
        data->Gx.rightCols(nv).setZero();
      } else if (is_rv) {
        const std::size_t nv = state_->get_nv();
        data->Gx.leftCols(nv).setZero();
        data->Gx.rightCols(nv) = data->residual->Rx.rightCols(nv);
      }
      if (is_ru) {
        data->Gu = data->residual->Ru;
      }
      break;
    case ConstraintType::Equality:
      if (is_rq && is_rv) {
        data->Hx = data->residual->Rx;
      } else if (is_rq) {
        const std::size_t nv = state_->get_nv();
        data->Hx.leftCols(nv) = data->residual->Rx.leftCols(nv);
        data->Hx.rightCols(nv).setZero();
      } else if (is_rv) {
        const std::size_t nv = state_->get_nv();
        data->Hx.leftCols(nv).setZero();
        data->Hx.rightCols(nv) = data->residual->Rx.rightCols(nv);
      }
      if (is_ru) {
        data->Hu = data->residual->Ru;
      }
      break;
    case ConstraintType::Both:  // this condition is not supported and possible
      break;
  }
}

template <typename Scalar>
void ConstraintModelResidualTpl<Scalar>::print(std::ostream& os) const {
  os << "ConstraintModelResidual {" << *residual_ << "}";
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2022, Heriot-Watt University, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/utils/exception.hpp"
10
11			namespace crocoddyl {
12
13			using std::isfinite;
14
15			template <typename Scalar>
16		240	ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
17			boost::shared_ptr<typename Base::StateAbstract> state,
18			boost::shared_ptr<ResidualModelAbstract> residual, const VectorXs& lower,
19			const VectorXs& upper)
20	✓✗	240	: Base(state, residual, residual->get_nr(), 0) {
21	✓✗	240	lb_ = lower;
22	✓✗	240	ub_ = upper;
23	✓✓	3088	for (std::size_t i = 0; i < residual_->get_nr(); ++i) {
24	✓✗✓✓ ✓✗✓✓ ✓✓	2848	if (isfinite(lb_(i)) && isfinite(ub_(i))) {
25	✓✗✓✗ ✗✓	2252	if (lb_(i) - ub_(i) > 0) {
26			throw_pretty(
27			"Invalid argument: the upper bound is not equal to / higher than "
28			"the lower bound.")
29			}
30			}
31			}
32	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	480	if ((lb_.array() == std::numeric_limits<Scalar>::infinity()).any() \|\|
33	✓✗✗✓	480	(lb_.array() == std::numeric_limits<Scalar>::max()).any()) {
34			throw_pretty(
35			"Invalid argument: the lower bound cannot contain a positive "
36			"infinity/max value");
37			}
38	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✗✓	480	if ((ub_.array() == -std::numeric_limits<Scalar>::infinity()).any() \|\|
39	✓✗✗✓	480	(ub_.array() == -std::numeric_limits<Scalar>::infinity()).any()) {
40			throw_pretty(
41			"Invalid argument: the lower bound cannot contain a negative "
42			"infinity/max value");
43			}
44		240	}
45
46			template <typename Scalar>
47		2421	ConstraintModelResidualTpl<Scalar>::ConstraintModelResidualTpl(
48			boost::shared_ptr<typename Base::StateAbstract> state,
49			boost::shared_ptr<ResidualModelAbstract> residual)
50	✓✗	2421	: Base(state, residual, 0, residual->get_nr()) {}
51
52			template <typename Scalar>
53		5326	ConstraintModelResidualTpl<Scalar>::~ConstraintModelResidualTpl() {}
54
55			template <typename Scalar>
56		121688	void ConstraintModelResidualTpl<Scalar>::calc(
57			const boost::shared_ptr<ConstraintDataAbstract>& data,
58			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
59			// Compute the constraint residual
60		121688	residual_->calc(data->residual, x, u);
61
62			// Fill the residual values for its corresponding type of constraint
63		121688	updateCalc(data);
64		121688	}
65
66			template <typename Scalar>
67		16434	void ConstraintModelResidualTpl<Scalar>::calc(
68			const boost::shared_ptr<ConstraintDataAbstract>& data,
69			const Eigen::Ref<const VectorXs>& x) {
70			// Compute the constraint residual
71		16434	residual_->calc(data->residual, x);
72
73			// Fill the residual values for its corresponding type of constraint
74		16434	updateCalc(data);
75		16434	}
76
77			template <typename Scalar>
78		19103	void ConstraintModelResidualTpl<Scalar>::calcDiff(
79			const boost::shared_ptr<ConstraintDataAbstract>& data,
80			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
81			// Compute the derivatives of the residual function
82		19103	residual_->calcDiff(data->residual, x, u);
83
84			// Fill the residual values for its corresponding type of constraint
85		19103	updateCalcDiff(data);
86		19103	}
87
88			template <typename Scalar>
89		787	void ConstraintModelResidualTpl<Scalar>::calcDiff(
90			const boost::shared_ptr<ConstraintDataAbstract>& data,
91			const Eigen::Ref<const VectorXs>& x) {
92			// Compute the derivatives of the residual function
93		787	residual_->calcDiff(data->residual, x);
94
95			// Fill the residual values for its corresponding type of constraint
96		787	updateCalcDiff(data);
97		787	}
98
99			template <typename Scalar>
100			boost::shared_ptr<ConstraintDataAbstractTpl<Scalar> >
101		229668	ConstraintModelResidualTpl<Scalar>::createData(
102			DataCollectorAbstract* const data) {
103			return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
104	✓✗	229668	data);
105			}
106
107			template <typename Scalar>
108		138122	void ConstraintModelResidualTpl<Scalar>::updateCalc(
109			const boost::shared_ptr<ConstraintDataAbstract>& data) {
110	✓✓✗✗	138122	switch (type_) {
111		7801	case ConstraintType::Inequality:
112		7801	data->g = data->residual->r;
113		7801	break;
114		130321	case ConstraintType::Equality:
115		130321	data->h = data->residual->r;
116		130321	break;
117			case ConstraintType::Both: // this condition is not supported and possible
118			break;
119			}
120		138122	}
121
122			template <typename Scalar>
123		19890	void ConstraintModelResidualTpl<Scalar>::updateCalcDiff(
124			const boost::shared_ptr<ConstraintDataAbstract>& data) {
125		19890	const bool is_rq = residual_->get_q_dependent();
126		19890	const bool is_rv = residual_->get_v_dependent();
127	✓✓✓✓	19890	const bool is_ru = residual_->get_u_dependent() \|\| nu_ == 0;
128	✓✓✗✗	19890	switch (type_) {
129		214	case ConstraintType::Inequality:
130	✓✓✓✓	214	if (is_rq && is_rv) {
131		96	data->Gx = data->residual->Rx;
132	✓✓	118	} else if (is_rq) {
133		94	const std::size_t nv = state_->get_nv();
134	✓✗✓✗	94	data->Gx.leftCols(nv) = data->residual->Rx.leftCols(nv);
135	✓✗	94	data->Gx.rightCols(nv).setZero();
136	✗✓	24	} else if (is_rv) {
137			const std::size_t nv = state_->get_nv();
138			data->Gx.leftCols(nv).setZero();
139			data->Gx.rightCols(nv) = data->residual->Rx.rightCols(nv);
140			}
141	✓✓	214	if (is_ru) {
142		78	data->Gu = data->residual->Ru;
143			}
144		214	break;
145		19676	case ConstraintType::Equality:
146	✓✓✓✓	19676	if (is_rq && is_rv) {
147		16307	data->Hx = data->residual->Rx;
148	✓✓	3369	} else if (is_rq) {
149		3337	const std::size_t nv = state_->get_nv();
150	✓✗✓✗	3337	data->Hx.leftCols(nv) = data->residual->Rx.leftCols(nv);
151	✓✗	3337	data->Hx.rightCols(nv).setZero();
152	✗✓	32	} else if (is_rv) {
153			const std::size_t nv = state_->get_nv();
154			data->Hx.leftCols(nv).setZero();
155			data->Hx.rightCols(nv) = data->residual->Rx.rightCols(nv);
156			}
157	✓✓	19676	if (is_ru) {
158		13038	data->Hu = data->residual->Ru;
159			}
160		19676	break;
161			case ConstraintType::Both: // this condition is not supported and possible
162			break;
163			}
164		19890	}
165
166			template <typename Scalar>
167			void ConstraintModelResidualTpl<Scalar>::print(std::ostream& os) const {
168			os << "ConstraintModelResidual {" << *residual_ << "}";
169			}
170
171			} // namespace crocoddyl