Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/state-base.hxx	Lines:	57	87	65.5 %
Date:	2024-02-13 11:12:33	Branches:	36	132	27.3 %


///////////////////////////////////////////////////////////////////////////////
// BSD 3-Clause License
//
// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.
///////////////////////////////////////////////////////////////////////////////
#include "crocoddyl/core/mathbase.hpp"

namespace crocoddyl {
template <typename Scalar>
StateAbstractTpl<Scalar>::StateAbstractTpl(const std::size_t nx,
                                           const std::size_t ndx)
    : nx_(nx),
      ndx_(ndx),
      lb_(VectorXs::Constant(nx_, -std::numeric_limits<Scalar>::infinity())),
      ub_(VectorXs::Constant(nx_, std::numeric_limits<Scalar>::infinity())),
      has_limits_(false) {
  nv_ = ndx / 2;
  nq_ = nx_ - nv_;
}

template <typename Scalar>
StateAbstractTpl<Scalar>::StateAbstractTpl()
    : nx_(0),
      ndx_(0),
      lb_(MathBase::VectorXs::Constant(
          nx_, -std::numeric_limits<Scalar>::infinity())),
      ub_(MathBase::VectorXs::Constant(
          nx_, std::numeric_limits<Scalar>::infinity())),
      has_limits_(false) {}

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

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::diff_dx(
    const Eigen::Ref<const VectorXs>& x0,
    const Eigen::Ref<const VectorXs>& x1) {
  VectorXs dxout = VectorXs::Zero(ndx_);
  diff(x0, x1, dxout);
  return dxout;
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::integrate_x(
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx) {
  VectorXs xout = VectorXs::Zero(nx_);
  integrate(x, dx, xout);
  return xout;
}

template <typename Scalar>
std::vector<typename MathBaseTpl<Scalar>::MatrixXs>
StateAbstractTpl<Scalar>::Jdiff_Js(const Eigen::Ref<const VectorXs>& x0,
                                   const Eigen::Ref<const VectorXs>& x1,
                                   Jcomponent firstsecond) {
  MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
  Jfirst.setZero();
  Jsecond.setZero();
  std::vector<MatrixXs> Jacs;
  Jdiff(x0, x1, Jfirst, Jsecond, firstsecond);
  switch (firstsecond) {
    case both:
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
      break;
    case first:
      Jacs.push_back(Jfirst);
      break;
    case second:
      Jacs.push_back(Jsecond);
      break;
    default:
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
      break;
  }
  return Jacs;
}

template <typename Scalar>
std::vector<typename MathBaseTpl<Scalar>::MatrixXs>
StateAbstractTpl<Scalar>::Jintegrate_Js(const Eigen::Ref<const VectorXs>& x,
                                        const Eigen::Ref<const VectorXs>& dx,
                                        const Jcomponent firstsecond) {
  MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
  Jfirst.setZero();
  Jsecond.setZero();
  std::vector<MatrixXs> Jacs;
  Jintegrate(x, dx, Jfirst, Jsecond, firstsecond, setto);
  switch (firstsecond) {
    case both:
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
      break;
    case first:
      Jacs.push_back(Jfirst);
      break;
    case second:
      Jacs.push_back(Jsecond);
      break;
    default:
      Jacs.push_back(Jfirst);
      Jacs.push_back(Jsecond);
      break;
  }
  return Jacs;
}

template <typename Scalar>
std::size_t StateAbstractTpl<Scalar>::get_nx() const {
  return nx_;
}

template <typename Scalar>
std::size_t StateAbstractTpl<Scalar>::get_ndx() const {
  return ndx_;
}

template <typename Scalar>
std::size_t StateAbstractTpl<Scalar>::get_nq() const {
  return nq_;
}

template <typename Scalar>
std::size_t StateAbstractTpl<Scalar>::get_nv() const {
  return nv_;
}

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

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

template <typename Scalar>
bool StateAbstractTpl<Scalar>::get_has_limits() const {
  return has_limits_;
}

template <typename Scalar>
void StateAbstractTpl<Scalar>::set_lb(const VectorXs& lb) {
  if (static_cast<std::size_t>(lb.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "lower bound has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  lb_ = lb;
  update_has_limits();
}

template <typename Scalar>
void StateAbstractTpl<Scalar>::set_ub(const VectorXs& ub) {
  if (static_cast<std::size_t>(ub.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "upper bound has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  ub_ = ub;
  update_has_limits();
}

template <typename Scalar>
void StateAbstractTpl<Scalar>::update_has_limits() {
  has_limits_ = isfinite(lb_.array()).any() || isfinite(ub_.array()).any();
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2020, LAAS-CNRS, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8			#include "crocoddyl/core/mathbase.hpp"
9
10			namespace crocoddyl {
11			template <typename Scalar>
12		11360	StateAbstractTpl<Scalar>::StateAbstractTpl(const std::size_t nx,
13			const std::size_t ndx)
14			: nx_(nx),
15			ndx_(ndx),
16		22720	lb_(VectorXs::Constant(nx_, -std::numeric_limits<Scalar>::infinity())),
17			ub_(VectorXs::Constant(nx_, std::numeric_limits<Scalar>::infinity())),
18	✓✗✓✗ ✓✗✓✗	11360	has_limits_(false) {
19		11360	nv_ = ndx / 2;
20		11360	nq_ = nx_ - nv_;
21		11360	}
22
23			template <typename Scalar>
24			StateAbstractTpl<Scalar>::StateAbstractTpl()
25			: nx_(0),
26			ndx_(0),
27			lb_(MathBase::VectorXs::Constant(
28			nx_, -std::numeric_limits<Scalar>::infinity())),
29			ub_(MathBase::VectorXs::Constant(
30			nx_, std::numeric_limits<Scalar>::infinity())),
31			has_limits_(false) {}
32
33			template <typename Scalar>
34		22732	StateAbstractTpl<Scalar>::~StateAbstractTpl() {}
35
36			template <typename Scalar>
37		853	typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::diff_dx(
38			const Eigen::Ref<const VectorXs>& x0,
39			const Eigen::Ref<const VectorXs>& x1) {
40	✓✗	853	VectorXs dxout = VectorXs::Zero(ndx_);
41	✓✗✓✗	853	diff(x0, x1, dxout);
42		853	return dxout;
43			}
44
45			template <typename Scalar>
46		42	typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::integrate_x(
47			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx) {
48	✓✗	42	VectorXs xout = VectorXs::Zero(nx_);
49	✓✗✓✗	42	integrate(x, dx, xout);
50		42	return xout;
51			}
52
53			template <typename Scalar>
54			std::vector<typename MathBaseTpl<Scalar>::MatrixXs>
55		6	StateAbstractTpl<Scalar>::Jdiff_Js(const Eigen::Ref<const VectorXs>& x0,
56			const Eigen::Ref<const VectorXs>& x1,
57			Jcomponent firstsecond) {
58	✓✗✓✗	12	MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
59	✓✗	6	Jfirst.setZero();
60	✓✗	6	Jsecond.setZero();
61		6	std::vector<MatrixXs> Jacs;
62	✓✗✓✗ ✓✗	6	Jdiff(x0, x1, Jfirst, Jsecond, firstsecond);
63	✓✗✓✗	6	switch (firstsecond) {
64		4	case both:
65	✓✗	4	Jacs.push_back(Jfirst);
66	✓✗	4	Jacs.push_back(Jsecond);
67		4	break;
68			case first:
69			Jacs.push_back(Jfirst);
70			break;
71		2	case second:
72	✓✗	2	Jacs.push_back(Jsecond);
73		2	break;
74			default:
75			Jacs.push_back(Jfirst);
76			Jacs.push_back(Jsecond);
77			break;
78			}
79		12	return Jacs;
80			}
81
82			template <typename Scalar>
83			std::vector<typename MathBaseTpl<Scalar>::MatrixXs>
84		18	StateAbstractTpl<Scalar>::Jintegrate_Js(const Eigen::Ref<const VectorXs>& x,
85			const Eigen::Ref<const VectorXs>& dx,
86			const Jcomponent firstsecond) {
87	✓✗✓✗	36	MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
88	✓✗	18	Jfirst.setZero();
89	✓✗	18	Jsecond.setZero();
90		18	std::vector<MatrixXs> Jacs;
91	✓✗✓✗ ✓✗	18	Jintegrate(x, dx, Jfirst, Jsecond, firstsecond, setto);
92	✓✗✗✗	18	switch (firstsecond) {
93		18	case both:
94	✓✗	18	Jacs.push_back(Jfirst);
95	✓✗	18	Jacs.push_back(Jsecond);
96		18	break;
97			case first:
98			Jacs.push_back(Jfirst);
99			break;
100			case second:
101			Jacs.push_back(Jsecond);
102			break;
103			default:
104			Jacs.push_back(Jfirst);
105			Jacs.push_back(Jsecond);
106			break;
107			}
108		36	return Jacs;
109			}
110
111			template <typename Scalar>
112		637586	std::size_t StateAbstractTpl<Scalar>::get_nx() const {
113		637586	return nx_;
114			}
115
116			template <typename Scalar>
117		7816013	std::size_t StateAbstractTpl<Scalar>::get_ndx() const {
118		7816013	return ndx_;
119			}
120
121			template <typename Scalar>
122		156008	std::size_t StateAbstractTpl<Scalar>::get_nq() const {
123		156008	return nq_;
124			}
125
126			template <typename Scalar>
127		4070917	std::size_t StateAbstractTpl<Scalar>::get_nv() const {
128		4070917	return nv_;
129			}
130
131			template <typename Scalar>
132		7	const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_lb()
133			const {
134		7	return lb_;
135			}
136
137			template <typename Scalar>
138		7	const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_ub()
139			const {
140		7	return ub_;
141			}
142
143			template <typename Scalar>
144			bool StateAbstractTpl<Scalar>::get_has_limits() const {
145			return has_limits_;
146			}
147
148			template <typename Scalar>
149			void StateAbstractTpl<Scalar>::set_lb(const VectorXs& lb) {
150			if (static_cast<std::size_t>(lb.size()) != nx_) {
151			throw_pretty("Invalid argument: "
152			<< "lower bound has wrong dimension (it should be " +
153			std::to_string(nx_) + ")");
154			}
155			lb_ = lb;
156			update_has_limits();
157			}
158
159			template <typename Scalar>
160			void StateAbstractTpl<Scalar>::set_ub(const VectorXs& ub) {
161			if (static_cast<std::size_t>(ub.size()) != nx_) {
162			throw_pretty("Invalid argument: "
163			<< "upper bound has wrong dimension (it should be " +
164			std::to_string(nx_) + ")");
165			}
166			ub_ = ub;
167			update_has_limits();
168			}
169
170			template <typename Scalar>
171		11084	void StateAbstractTpl<Scalar>::update_has_limits() {
172	✓✗✓✗ ✓✗✗✓ ✗✗✗✗ ✗✗✗✗	11084	has_limits_ = isfinite(lb_.array()).any() \|\| isfinite(ub_.array()).any();
173		11084	}
174
175			} // namespace crocoddyl