Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/controls/poly-zero.hxx	Lines:	48	69	69.6 %
Date:	2024-02-13 11:12:33	Branches:	24	312	7.7 %


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

namespace crocoddyl {

template <typename Scalar>
ControlParametrizationModelPolyZeroTpl<
    Scalar>::ControlParametrizationModelPolyZeroTpl(const std::size_t nw)
    : Base(nw, nw) {}

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

template <typename Scalar>
void ControlParametrizationModelPolyZeroTpl<Scalar>::calc(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar, const Eigen::Ref<const VectorXs>& u) const {
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  data->w = u;
}

template <typename Scalar>
#ifndef NDEBUG
void ControlParametrizationModelPolyZeroTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar, const Eigen::Ref<const VectorXs>&) const {
#else
void ControlParametrizationModelPolyZeroTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ControlParametrizationDataAbstract>&, const Scalar,
    const Eigen::Ref<const VectorXs>&) const {
#endif
  // The Hessian has constant values which were set in createData.
  assert_pretty(MatrixXs(data->dw_du).isApprox(MatrixXs::Identity(nu_, nu_)),
                "dw_du has wrong value");
}

template <typename Scalar>
boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
ControlParametrizationModelPolyZeroTpl<Scalar>::createData() {
  boost::shared_ptr<Data> data =
      boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
  data->dw_du.setIdentity();
  return data;
}

template <typename Scalar>
void ControlParametrizationModelPolyZeroTpl<Scalar>::params(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar, const Eigen::Ref<const VectorXs>& w) const {
  if (static_cast<std::size_t>(w.size()) != nw_) {
    throw_pretty("Invalid argument: "
                 << "w has wrong dimension (it should be " +
                        std::to_string(nw_) + ")");
  }
  data->u = w;
}

template <typename Scalar>
void ControlParametrizationModelPolyZeroTpl<Scalar>::convertBounds(
    const Eigen::Ref<const VectorXs>& w_lb,
    const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
    Eigen::Ref<VectorXs> u_ub) const {
  if (static_cast<std::size_t>(u_lb.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u_lb has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  if (static_cast<std::size_t>(u_ub.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u_ub has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  if (static_cast<std::size_t>(w_lb.size()) != nw_) {
    throw_pretty("Invalid argument: "
                 << "w_lb has wrong dimension (it should be " +
                        std::to_string(nw_) + ")");
  }
  if (static_cast<std::size_t>(w_ub.size()) != nw_) {
    throw_pretty("Invalid argument: "
                 << "w_ub has wrong dimension (it should be " +
                        std::to_string(nw_) + ")");
  }
  u_lb = w_lb;
  u_ub = w_ub;
}

template <typename Scalar>
void ControlParametrizationModelPolyZeroTpl<Scalar>::multiplyByJacobian(
    const boost::shared_ptr<ControlParametrizationDataAbstract>&,
    const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
    const AssignmentOp op) const {
  assert_pretty(is_a_AssignmentOp(op),
                ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
  if (A.rows() != out.rows() || static_cast<std::size_t>(A.cols()) != nw_ ||
      static_cast<std::size_t>(out.cols()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "A and out have wrong dimensions (" +
                        std::to_string(A.rows()) + "," +
                        std::to_string(A.cols()) + " and " +
                        std::to_string(out.rows()) + "," +
                        std::to_string(out.cols()) + ")");
  }
  switch (op) {
    case setto:
      out = A;
      break;
    case addto:
      out += A;
      break;
    case rmfrom:
      out -= A;
      break;
    default:
      throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
      break;
  }
}

template <typename Scalar>
void ControlParametrizationModelPolyZeroTpl<Scalar>::
    multiplyJacobianTransposeBy(
        const boost::shared_ptr<ControlParametrizationDataAbstract>&,
        const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
        const AssignmentOp op) const {
  assert_pretty(is_a_AssignmentOp(op),
                ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
  if (A.cols() != out.cols() || static_cast<std::size_t>(A.rows()) != nw_ ||
      static_cast<std::size_t>(out.rows()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "A and out have wrong dimensions (" +
                        std::to_string(A.rows()) + "," +
                        std::to_string(A.cols()) + " and " +
                        std::to_string(out.rows()) + "," +
                        std::to_string(out.cols()) + ")");
  }
  switch (op) {
    case setto:
      out = A;
      break;
    case addto:
      out += A;
      break;
    case rmfrom:
      out -= A;
      break;
    default:
      throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
      break;
  }
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021, University of Edinburgh, University of Trento
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			namespace crocoddyl {
10
11			template <typename Scalar>
12		652	ControlParametrizationModelPolyZeroTpl<
13			Scalar>::ControlParametrizationModelPolyZeroTpl(const std::size_t nw)
14		652	: Base(nw, nw) {}
15
16			template <typename Scalar>
17		2032	ControlParametrizationModelPolyZeroTpl<
18		2032	Scalar>::~ControlParametrizationModelPolyZeroTpl() {}
19
20			template <typename Scalar>
21		50500	void ControlParametrizationModelPolyZeroTpl<Scalar>::calc(
22			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
23			const Scalar, const Eigen::Ref<const VectorXs>& u) const {
24	✗✓	50500	if (static_cast<std::size_t>(u.size()) != nu_) {
25			throw_pretty("Invalid argument: "
26			<< "u has wrong dimension (it should be " +
27			std::to_string(nu_) + ")");
28			}
29		50500	data->w = u;
30		50500	}
31
32			template <typename Scalar>
33			#ifndef NDEBUG
34		3	void ControlParametrizationModelPolyZeroTpl<Scalar>::calcDiff(
35			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
36			const Scalar, const Eigen::Ref<const VectorXs>&) const {
37			#else
38			void ControlParametrizationModelPolyZeroTpl<Scalar>::calcDiff(
39			const boost::shared_ptr<ControlParametrizationDataAbstract>&, const Scalar,
40			const Eigen::Ref<const VectorXs>&) const {
41			#endif
42			// The Hessian has constant values which were set in createData.
43	✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗	3	assert_pretty(MatrixXs(data->dw_du).isApprox(MatrixXs::Identity(nu_, nu_)),
44			"dw_du has wrong value");
45		3	}
46
47			template <typename Scalar>
48			boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
49		68202	ControlParametrizationModelPolyZeroTpl<Scalar>::createData() {
50	✓✗	68202	boost::shared_ptr<Data> data =
51			boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
52	✓✗	68202	data->dw_du.setIdentity();
53		68202	return data;
54			}
55
56			template <typename Scalar>
57		7040	void ControlParametrizationModelPolyZeroTpl<Scalar>::params(
58			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
59			const Scalar, const Eigen::Ref<const VectorXs>& w) const {
60	✗✓	7040	if (static_cast<std::size_t>(w.size()) != nw_) {
61			throw_pretty("Invalid argument: "
62			<< "w has wrong dimension (it should be " +
63			std::to_string(nw_) + ")");
64			}
65		7040	data->u = w;
66		7040	}
67
68			template <typename Scalar>
69		648	void ControlParametrizationModelPolyZeroTpl<Scalar>::convertBounds(
70			const Eigen::Ref<const VectorXs>& w_lb,
71			const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
72			Eigen::Ref<VectorXs> u_ub) const {
73	✗✓	648	if (static_cast<std::size_t>(u_lb.size()) != nu_) {
74			throw_pretty("Invalid argument: "
75			<< "u_lb has wrong dimension (it should be " +
76			std::to_string(nu_) + ")");
77			}
78	✗✓	648	if (static_cast<std::size_t>(u_ub.size()) != nu_) {
79			throw_pretty("Invalid argument: "
80			<< "u_ub has wrong dimension (it should be " +
81			std::to_string(nu_) + ")");
82			}
83	✗✓	648	if (static_cast<std::size_t>(w_lb.size()) != nw_) {
84			throw_pretty("Invalid argument: "
85			<< "w_lb has wrong dimension (it should be " +
86			std::to_string(nw_) + ")");
87			}
88	✗✓	648	if (static_cast<std::size_t>(w_ub.size()) != nw_) {
89			throw_pretty("Invalid argument: "
90			<< "w_ub has wrong dimension (it should be " +
91			std::to_string(nw_) + ")");
92			}
93		648	u_lb = w_lb;
94		648	u_ub = w_ub;
95		648	}
96
97			template <typename Scalar>
98		61755	void ControlParametrizationModelPolyZeroTpl<Scalar>::multiplyByJacobian(
99			const boost::shared_ptr<ControlParametrizationDataAbstract>&,
100			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
101			const AssignmentOp op) const {
102	✗✓✗✗ ✗✗✗✗ ✗✗	61755	assert_pretty(is_a_AssignmentOp(op),
103			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
104	✓✗✓✗ ✗✓	123510	if (A.rows() != out.rows() \|\| static_cast<std::size_t>(A.cols()) != nw_ \|\|
105	✗✓	61755	static_cast<std::size_t>(out.cols()) != nu_) {
106			throw_pretty("Invalid argument: "
107			<< "A and out have wrong dimensions (" +
108			std::to_string(A.rows()) + "," +
109			std::to_string(A.cols()) + " and " +
110			std::to_string(out.rows()) + "," +
111			std::to_string(out.cols()) + ")");
112			}
113	✓✓✗✗	61755	switch (op) {
114		53694	case setto:
115		53694	out = A;
116		53694	break;
117		8061	case addto:
118		8061	out += A;
119		8061	break;
120			case rmfrom:
121			out -= A;
122			break;
123			default:
124			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
125			break;
126			}
127		61755	}
128
129			template <typename Scalar>
130		27927	void ControlParametrizationModelPolyZeroTpl<Scalar>::
131			multiplyJacobianTransposeBy(
132			const boost::shared_ptr<ControlParametrizationDataAbstract>&,
133			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
134			const AssignmentOp op) const {
135	✗✓✗✗ ✗✗✗✗ ✗✗	27927	assert_pretty(is_a_AssignmentOp(op),
136			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
137	✓✗✓✗ ✗✓	55854	if (A.cols() != out.cols() \|\| static_cast<std::size_t>(A.rows()) != nw_ \|\|
138	✗✓	27927	static_cast<std::size_t>(out.rows()) != nu_) {
139			throw_pretty("Invalid argument: "
140			<< "A and out have wrong dimensions (" +
141			std::to_string(A.rows()) + "," +
142			std::to_string(A.cols()) + " and " +
143			std::to_string(out.rows()) + "," +
144			std::to_string(out.cols()) + ")");
145			}
146	✓✗✗✗	27927	switch (op) {
147		27927	case setto:
148		27927	out = A;
149		27927	break;
150			case addto:
151			out += A;
152			break;
153			case rmfrom:
154			out -= A;
155			break;
156			default:
157			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
158			break;
159			}
160		27927	}
161
162			} // namespace crocoddyl