Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/residual-base.hxx	Lines:	57	70	81.4 %
Date:	2024-02-13 11:12:33	Branches:	70	168	41.7 %


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

#include <boost/core/demangle.hpp>

namespace crocoddyl {

template <typename Scalar>
ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl(
    boost::shared_ptr<StateAbstract> state, const std::size_t nr,
    const std::size_t nu, const bool q_dependent, const bool v_dependent,
    const bool u_dependent)
    : state_(state),
      nr_(nr),
      nu_(nu),
      unone_(VectorXs::Zero(nu)),
      q_dependent_(q_dependent),
      v_dependent_(v_dependent),
      u_dependent_(u_dependent) {}

template <typename Scalar>
ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl(
    boost::shared_ptr<StateAbstract> state, const std::size_t nr,
    const bool q_dependent, const bool v_dependent, const bool u_dependent)
    : state_(state),
      nr_(nr),
      nu_(state->get_nv()),
      unone_(VectorXs::Zero(state->get_nv())),
      q_dependent_(q_dependent),
      v_dependent_(v_dependent),
      u_dependent_(u_dependent) {}

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

template <typename Scalar>
void ResidualModelAbstractTpl<Scalar>::calc(
    const boost::shared_ptr<ResidualDataAbstract>&,
    const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}

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

template <typename Scalar>
void ResidualModelAbstractTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ResidualDataAbstract>&,
    const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}

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

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

template <typename Scalar>
void ResidualModelAbstractTpl<Scalar>::calcCostDiff(
    const boost::shared_ptr<CostDataAbstract>& cdata,
    const boost::shared_ptr<ResidualDataAbstract>& rdata,
    const boost::shared_ptr<ActivationDataAbstract>& adata,
    const bool update_u) {
  // This function computes the derivatives of the cost function based on a
  // Gauss-Newton approximation
  const bool is_ru = u_dependent_ && nu_ != 0 && update_u;
  const std::size_t nv = state_->get_nv();
  if (is_ru) {
    cdata->Lu.noalias() = rdata->Ru.transpose() * adata->Ar;
    rdata->Arr_Ru.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Ru;
    cdata->Luu.noalias() = rdata->Ru.transpose() * rdata->Arr_Ru;
  }
  if (q_dependent_ && v_dependent_) {
    cdata->Lx.noalias() = rdata->Rx.transpose() * adata->Ar;
    rdata->Arr_Rx.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Rx;
    cdata->Lxx.noalias() = rdata->Rx.transpose() * rdata->Arr_Rx;
    if (is_ru) {
      cdata->Lxu.noalias() = rdata->Rx.transpose() * rdata->Arr_Ru;
    }
  } else if (q_dependent_) {
    Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
        rdata->Rx.leftCols(nv);
    cdata->Lx.head(nv).noalias() = Rq.transpose() * adata->Ar;
    rdata->Arr_Rx.leftCols(nv).noalias() =
        adata->Arr.diagonal().asDiagonal() * Rq;
    cdata->Lxx.topLeftCorner(nv, nv).noalias() =
        Rq.transpose() * rdata->Arr_Rx.leftCols(nv);
    if (is_ru) {
      cdata->Lxu.topRows(nv).noalias() = Rq.transpose() * rdata->Arr_Ru;
    }
  } else if (v_dependent_) {
    Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv =
        rdata->Rx.rightCols(nv);
    cdata->Lx.tail(nv).noalias() = Rv.transpose() * adata->Ar;
    rdata->Arr_Rx.rightCols(nv).noalias() =
        adata->Arr.diagonal().asDiagonal() * Rv;
    cdata->Lxx.bottomRightCorner(nv, nv).noalias() =
        Rv.transpose() * rdata->Arr_Rx.rightCols(nv);
    if (is_ru) {
      cdata->Lxu.bottomRows(nv).noalias() = Rv.transpose() * rdata->Arr_Ru;
    }
  }
}

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

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

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

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

template <typename Scalar>
bool ResidualModelAbstractTpl<Scalar>::get_q_dependent() const {
  return q_dependent_;
}

template <typename Scalar>
bool ResidualModelAbstractTpl<Scalar>::get_v_dependent() const {
  return v_dependent_;
}

template <typename Scalar>
bool ResidualModelAbstractTpl<Scalar>::get_u_dependent() const {
  return u_dependent_;
}

template <typename Scalar>
std::ostream& operator<<(std::ostream& os,
                         const ResidualModelAbstractTpl<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) 2021-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
11			namespace crocoddyl {
12
13			template <typename Scalar>
14		11354	ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl(
15			boost::shared_ptr<StateAbstract> state, const std::size_t nr,
16			const std::size_t nu, const bool q_dependent, const bool v_dependent,
17			const bool u_dependent)
18			: state_(state),
19			nr_(nr),
20			nu_(nu),
21			unone_(VectorXs::Zero(nu)),
22			q_dependent_(q_dependent),
23			v_dependent_(v_dependent),
24	✓✗✓✗	11354	u_dependent_(u_dependent) {}
25
26			template <typename Scalar>
27		45	ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl(
28			boost::shared_ptr<StateAbstract> state, const std::size_t nr,
29			const bool q_dependent, const bool v_dependent, const bool u_dependent)
30			: state_(state),
31			nr_(nr),
32			nu_(state->get_nv()),
33		45	unone_(VectorXs::Zero(state->get_nv())),
34			q_dependent_(q_dependent),
35			v_dependent_(v_dependent),
36	✓✗✓✗	90	u_dependent_(u_dependent) {}
37
38			template <typename Scalar>
39		22862	ResidualModelAbstractTpl<Scalar>::~ResidualModelAbstractTpl() {}
40
41			template <typename Scalar>
42			void ResidualModelAbstractTpl<Scalar>::calc(
43			const boost::shared_ptr<ResidualDataAbstract>&,
44			const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
45
46			template <typename Scalar>
47		33262	void ResidualModelAbstractTpl<Scalar>::calc(
48			const boost::shared_ptr<ResidualDataAbstract>& data,
49			const Eigen::Ref<const VectorXs>& x) {
50	✓✗	33262	calc(data, x, unone_);
51		33262	}
52
53			template <typename Scalar>
54			void ResidualModelAbstractTpl<Scalar>::calcDiff(
55			const boost::shared_ptr<ResidualDataAbstract>&,
56			const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
57
58			template <typename Scalar>
59		1609	void ResidualModelAbstractTpl<Scalar>::calcDiff(
60			const boost::shared_ptr<ResidualDataAbstract>& data,
61			const Eigen::Ref<const VectorXs>& x) {
62	✓✗	1609	calcDiff(data, x, unone_);
63		1609	}
64
65			template <typename Scalar>
66			boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
67		98996	ResidualModelAbstractTpl<Scalar>::createData(
68			DataCollectorAbstract* const data) {
69			return boost::allocate_shared<ResidualDataAbstract>(
70	✓✗	98996	Eigen::aligned_allocator<ResidualDataAbstract>(), this, data);
71			}
72
73			template <typename Scalar>
74		34813	void ResidualModelAbstractTpl<Scalar>::calcCostDiff(
75			const boost::shared_ptr<CostDataAbstract>& cdata,
76			const boost::shared_ptr<ResidualDataAbstract>& rdata,
77			const boost::shared_ptr<ActivationDataAbstract>& adata,
78			const bool update_u) {
79			// This function computes the derivatives of the cost function based on a
80			// Gauss-Newton approximation
81	✓✓✓✓ ✓✓	34813	const bool is_ru = u_dependent_ && nu_ != 0 && update_u;
82		34813	const std::size_t nv = state_->get_nv();
83	✓✓	34813	if (is_ru) {
84	✓✗✓✗ ✓✗	27168	cdata->Lu.noalias() = rdata->Ru.transpose() * adata->Ar;
85	✓✗✓✗ ✓✗	27168	rdata->Arr_Ru.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Ru;
86	✓✗✓✗ ✓✗	27168	cdata->Luu.noalias() = rdata->Ru.transpose() * rdata->Arr_Ru;
87			}
88	✓✓✓✓	34813	if (q_dependent_ && v_dependent_) {
89	✓✗✓✗ ✓✗	13706	cdata->Lx.noalias() = rdata->Rx.transpose() * adata->Ar;
90	✓✗✓✗ ✓✗	13706	rdata->Arr_Rx.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Rx;
91	✓✗✓✗ ✓✗	13706	cdata->Lxx.noalias() = rdata->Rx.transpose() * rdata->Arr_Rx;
92	✓✓	13706	if (is_ru) {
93	✓✗✓✗ ✓✗	11382	cdata->Lxu.noalias() = rdata->Rx.transpose() * rdata->Arr_Ru;
94			}
95	✓✓	21107	} else if (q_dependent_) {
96		5384	Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
97	✓✗	5384	rdata->Rx.leftCols(nv);
98	✓✗✓✗ ✓✗✓✗ ✓✗	5384	cdata->Lx.head(nv).noalias() = Rq.transpose() * adata->Ar;
99	✓✗✓✗ ✓✗	5384	rdata->Arr_Rx.leftCols(nv).noalias() =
100	✓✗✓✗	5384	adata->Arr.diagonal().asDiagonal() * Rq;
101	✓✗✓✗ ✓✗	5384	cdata->Lxx.topLeftCorner(nv, nv).noalias() =
102	✓✗✓✗ ✓✗	5384	Rq.transpose() * rdata->Arr_Rx.leftCols(nv);
103	✓✓	5384	if (is_ru) {
104	✓✗✓✗ ✓✗✓✗ ✓✗	63	cdata->Lxu.topRows(nv).noalias() = Rq.transpose() * rdata->Arr_Ru;
105			}
106	✗✓	15723	} else if (v_dependent_) {
107			Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv =
108			rdata->Rx.rightCols(nv);
109			cdata->Lx.tail(nv).noalias() = Rv.transpose() * adata->Ar;
110			rdata->Arr_Rx.rightCols(nv).noalias() =
111			adata->Arr.diagonal().asDiagonal() * Rv;
112			cdata->Lxx.bottomRightCorner(nv, nv).noalias() =
113			Rv.transpose() * rdata->Arr_Rx.rightCols(nv);
114			if (is_ru) {
115			cdata->Lxu.bottomRows(nv).noalias() = Rv.transpose() * rdata->Arr_Ru;
116			}
117			}
118		34813	}
119
120			template <typename Scalar>
121		20	void ResidualModelAbstractTpl<Scalar>::print(std::ostream& os) const {
122	✓✗	20	os << boost::core::demangle(typeid(*this).name());
123		20	}
124
125			template <typename Scalar>
126			const boost::shared_ptr<StateAbstractTpl<Scalar> >&
127		1911220	ResidualModelAbstractTpl<Scalar>::get_state() const {
128		1911220	return state_;
129			}
130
131			template <typename Scalar>
132		3676603	std::size_t ResidualModelAbstractTpl<Scalar>::get_nr() const {
133		3676603	return nr_;
134			}
135
136			template <typename Scalar>
137		1515498	std::size_t ResidualModelAbstractTpl<Scalar>::get_nu() const {
138		1515498	return nu_;
139			}
140
141			template <typename Scalar>
142		106192	bool ResidualModelAbstractTpl<Scalar>::get_q_dependent() const {
143		106192	return q_dependent_;
144			}
145
146			template <typename Scalar>
147		106192	bool ResidualModelAbstractTpl<Scalar>::get_v_dependent() const {
148		106192	return v_dependent_;
149			}
150
151			template <typename Scalar>
152		19890	bool ResidualModelAbstractTpl<Scalar>::get_u_dependent() const {
153		19890	return u_dependent_;
154			}
155
156			template <typename Scalar>
157		506	std::ostream& operator<<(std::ostream& os,
158			const ResidualModelAbstractTpl<Scalar>& model) {
159		506	model.print(os);
160		506	return os;
161			}
162
163			} // namespace crocoddyl