Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/controls/poly-one.hxx	Lines:	54	82	65.9 %
Date:	2024-02-13 11:12:33	Branches:	48	376	12.8 %


///////////////////////////////////////////////////////////////////////////////
// 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>
ControlParametrizationModelPolyOneTpl<
    Scalar>::ControlParametrizationModelPolyOneTpl(const std::size_t nw)
    : Base(nw, 2 * nw) {}

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

template <typename Scalar>
void ControlParametrizationModelPolyOneTpl<Scalar>::calc(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar t, 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* d = static_cast<Data*>(data.get());
  d->c[1] = Scalar(2.) * t;
  d->c[0] = Scalar(1.) - d->c[1];
  data->w = d->c[0] * u.head(nw_) + d->c[1] * u.tail(nw_);
}

template <typename Scalar>
void ControlParametrizationModelPolyOneTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    const Scalar, const Eigen::Ref<const VectorXs>&) const {
  Data* d = static_cast<Data*>(data.get());
  data->dw_du.leftCols(nw_).diagonal().array() = d->c[0];
  data->dw_du.rightCols(nw_).diagonal().array() = d->c[1];
}

template <typename Scalar>
boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
ControlParametrizationModelPolyOneTpl<Scalar>::createData() {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
void ControlParametrizationModelPolyOneTpl<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.head(nw_) = w;
  data->u.tail(nw_) = w;
}

template <typename Scalar>
void ControlParametrizationModelPolyOneTpl<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.head(nw_) = w_lb;
  u_lb.tail(nw_) = w_lb;
  u_ub.head(nw_) = w_ub;
  u_ub.tail(nw_) = w_ub;
}

template <typename Scalar>
void ControlParametrizationModelPolyOneTpl<Scalar>::multiplyByJacobian(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    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()) + +")");
  }
  Data* d = static_cast<Data*>(data.get());
  switch (op) {
    case setto:
      out.leftCols(nw_) = d->c[0] * A;
      out.rightCols(nw_) = d->c[1] * A;
      break;
    case addto:
      out.leftCols(nw_) += d->c[0] * A;
      out.rightCols(nw_) += d->c[1] * A;
      break;
    case rmfrom:
      out.leftCols(nw_) -= d->c[0] * A;
      out.rightCols(nw_) -= d->c[1] * A;
      break;
    default:
      throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
      break;
  }
}

template <typename Scalar>
void ControlParametrizationModelPolyOneTpl<Scalar>::multiplyJacobianTransposeBy(
    const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
    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()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());
  switch (op) {
    case setto:
      out.topRows(nw_) = d->c[0] * A;
      out.bottomRows(nw_) = d->c[1] * A;
      break;
    case addto:
      out.topRows(nw_) += d->c[0] * A;
      out.bottomRows(nw_) += d->c[1] * A;
      break;
    case rmfrom:
      out.topRows(nw_) -= d->c[0] * A;
      out.bottomRows(nw_) -= d->c[1] * 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		246	ControlParametrizationModelPolyOneTpl<
13			Scalar>::ControlParametrizationModelPolyOneTpl(const std::size_t nw)
14		246	: Base(nw, 2 * nw) {}
15
16			template <typename Scalar>
17		496	ControlParametrizationModelPolyOneTpl<
18		496	Scalar>::~ControlParametrizationModelPolyOneTpl() {}
19
20			template <typename Scalar>
21		15104	void ControlParametrizationModelPolyOneTpl<Scalar>::calc(
22			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
23			const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
24	✗✓	15104	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		15104	Data* d = static_cast<Data*>(data.get());
30		15104	d->c[1] = Scalar(2.) * t;
31		15104	d->c[0] = Scalar(1.) - d->c[1];
32	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	15104	data->w = d->c[0] * u.head(nw_) + d->c[1] * u.tail(nw_);
33		15104	}
34
35			template <typename Scalar>
36		3	void ControlParametrizationModelPolyOneTpl<Scalar>::calcDiff(
37			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
38			const Scalar, const Eigen::Ref<const VectorXs>&) const {
39		3	Data* d = static_cast<Data*>(data.get());
40	✓✗✓✗ ✓✗	3	data->dw_du.leftCols(nw_).diagonal().array() = d->c[0];
41	✓✗✓✗ ✓✗	3	data->dw_du.rightCols(nw_).diagonal().array() = d->c[1];
42		3	}
43
44			template <typename Scalar>
45			boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
46		15304	ControlParametrizationModelPolyOneTpl<Scalar>::createData() {
47	✓✗	15304	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
48			}
49
50			template <typename Scalar>
51			void ControlParametrizationModelPolyOneTpl<Scalar>::params(
52			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
53			const Scalar, const Eigen::Ref<const VectorXs>& w) const {
54			if (static_cast<std::size_t>(w.size()) != nw_) {
55			throw_pretty("Invalid argument: "
56			<< "w has wrong dimension (it should be " +
57			std::to_string(nw_) + ")");
58			}
59			data->u.head(nw_) = w;
60			data->u.tail(nw_) = w;
61			}
62
63			template <typename Scalar>
64		242	void ControlParametrizationModelPolyOneTpl<Scalar>::convertBounds(
65			const Eigen::Ref<const VectorXs>& w_lb,
66			const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
67			Eigen::Ref<VectorXs> u_ub) const {
68	✗✓	242	if (static_cast<std::size_t>(u_lb.size()) != nu_) {
69			throw_pretty("Invalid argument: "
70			<< "u_lb has wrong dimension (it should be " +
71			std::to_string(nu_) + ")");
72			}
73	✗✓	242	if (static_cast<std::size_t>(u_ub.size()) != nu_) {
74			throw_pretty("Invalid argument: "
75			<< "u_ub has wrong dimension (it should be " +
76			std::to_string(nu_) + ")");
77			}
78	✗✓	242	if (static_cast<std::size_t>(w_lb.size()) != nw_) {
79			throw_pretty("Invalid argument: "
80			<< "w_lb has wrong dimension (it should be " +
81			std::to_string(nw_) + ")");
82			}
83	✗✓	242	if (static_cast<std::size_t>(w_ub.size()) != nw_) {
84			throw_pretty("Invalid argument: "
85			<< "w_ub has wrong dimension (it should be " +
86			std::to_string(nw_) + ")");
87			}
88	✓✗	242	u_lb.head(nw_) = w_lb;
89	✓✗	242	u_lb.tail(nw_) = w_lb;
90	✓✗	242	u_ub.head(nw_) = w_ub;
91	✓✗	242	u_ub.tail(nw_) = w_ub;
92		242	}
93
94			template <typename Scalar>
95		859	void ControlParametrizationModelPolyOneTpl<Scalar>::multiplyByJacobian(
96			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
97			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
98			const AssignmentOp op) const {
99	✗✓✗✗ ✗✗✗✗ ✗✗	859	assert_pretty(is_a_AssignmentOp(op),
100			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
101	✓✗✓✗ ✗✓	1718	if (A.rows() != out.rows() \|\| static_cast<std::size_t>(A.cols()) != nw_ \|\|
102	✗✓	859	static_cast<std::size_t>(out.cols()) != nu_) {
103			throw_pretty("Invalid argument: "
104			<< "A and out have wrong dimensions (" +
105			std::to_string(A.rows()) + "," +
106			std::to_string(A.cols()) + " and " +
107			std::to_string(out.rows()) + "," +
108			std::to_string(out.cols()) + +")");
109			}
110		859	Data* d = static_cast<Data*>(data.get());
111	✓✓✗✗	859	switch (op) {
112		727	case setto:
113	✓✗✓✗	727	out.leftCols(nw_) = d->c[0] * A;
114	✓✗✓✗	727	out.rightCols(nw_) = d->c[1] * A;
115		727	break;
116		132	case addto:
117	✓✗✓✗	132	out.leftCols(nw_) += d->c[0] * A;
118	✓✗✓✗	132	out.rightCols(nw_) += d->c[1] * A;
119		132	break;
120			case rmfrom:
121			out.leftCols(nw_) -= d->c[0] * A;
122			out.rightCols(nw_) -= d->c[1] * A;
123			break;
124			default:
125			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
126			break;
127			}
128		859	}
129
130			template <typename Scalar>
131		397	void ControlParametrizationModelPolyOneTpl<Scalar>::multiplyJacobianTransposeBy(
132			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
133			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
134			const AssignmentOp op) const {
135	✗✓✗✗ ✗✗✗✗ ✗✗	397	assert_pretty(is_a_AssignmentOp(op),
136			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
137	✓✗✓✗ ✗✓	794	if (A.cols() != out.cols() \|\| static_cast<std::size_t>(A.rows()) != nw_ \|\|
138	✗✓	397	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		397	Data* d = static_cast<Data*>(data.get());
147	✓✗✗✗	397	switch (op) {
148		397	case setto:
149	✓✗✓✗	397	out.topRows(nw_) = d->c[0] * A;
150	✓✗✓✗	397	out.bottomRows(nw_) = d->c[1] * A;
151		397	break;
152			case addto:
153			out.topRows(nw_) += d->c[0] * A;
154			out.bottomRows(nw_) += d->c[1] * A;
155			break;
156			case rmfrom:
157			out.topRows(nw_) -= d->c[0] * A;
158			out.bottomRows(nw_) -= d->c[1] * A;
159			break;
160			default:
161			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
162			break;
163			}
164		397	}
165
166			} // namespace crocoddyl