Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/controls/poly-two-rk.hxx	Lines:	76	114	66.7 %
Date:	2024-02-13 11:12:33	Branches:	76	454	16.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>
ControlParametrizationModelPolyTwoRKTpl<
    Scalar>::ControlParametrizationModelPolyTwoRKTpl(const std::size_t nw,
                                                     const RKType rktype)
    : Base(nw, 3 * nw), rktype_(rktype) {
  if (rktype_ == RKType::two) {
    std::cerr << "Invalid argument: RK2 parametrization is not supported"
              << std::endl;
  }
}

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

template <typename Scalar>
void ControlParametrizationModelPolyTwoRKTpl<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());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p0 = u.head(nw_);
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p1 =
      u.segment(nw_, nw_);
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p2 = u.tail(nw_);
  d->tmp_t2 = t * t;
  switch (rktype_) {
    case two:
      std::cerr << "Invalid argument: RK2 parametrization is not supported"
                << std::endl;
      break;
    case three:
      d->c[2] = Scalar(4.5) * d->tmp_t2 - Scalar(1.5) * t;
      d->c[1] = -Scalar(9.) * d->tmp_t2 + Scalar(6.) * t;
      d->c[0] = Scalar(4.5) * (d->tmp_t2 - t) + Scalar(1.);
      break;
    case four:
      d->c[2] = Scalar(2.) * d->tmp_t2 - t;
      d->c[1] = -Scalar(2.) * d->c[2] + Scalar(2.) * t;
      d->c[0] = d->c[2] - Scalar(2.) * t + Scalar(1.);
      break;
  }
  d->w = d->c[2] * p2 + d->c[1] * p1 + d->c[0] * p0;
}

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

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

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

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

template <typename Scalar>
void ControlParametrizationModelPolyTwoRKTpl<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.middleCols(nw_, nw_) = d->c[1] * A;
      out.rightCols(nw_) = d->c[2] * A;
      break;
    case addto:
      out.leftCols(nw_) += d->c[0] * A;
      out.middleCols(nw_, nw_) += d->c[1] * A;
      out.rightCols(nw_) += d->c[2] * A;
      break;
    case rmfrom:
      out.leftCols(nw_) -= d->c[0] * A;
      out.middleCols(nw_, nw_) -= d->c[1] * A;
      out.rightCols(nw_) -= d->c[2] * A;
      break;
    default:
      throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
      break;
  }
}

template <typename Scalar>
void ControlParametrizationModelPolyTwoRKTpl<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.middleRows(nw_, nw_) = d->c[1] * A;
      out.bottomRows(nw_) = d->c[2] * A;
      break;
    case addto:
      out.topRows(nw_) += d->c[0] * A;
      out.middleRows(nw_, nw_) += d->c[1] * A;
      out.bottomRows(nw_) += d->c[2] * A;
      break;
    case rmfrom:
      out.topRows(nw_) -= d->c[0] * A;
      out.middleRows(nw_, nw_) -= d->c[1] * A;
      out.bottomRows(nw_) -= d->c[2] * 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		161	ControlParametrizationModelPolyTwoRKTpl<
13			Scalar>::ControlParametrizationModelPolyTwoRKTpl(const std::size_t nw,
14			const RKType rktype)
15		161	: Base(nw, 3 * nw), rktype_(rktype) {
16	✗✓	161	if (rktype_ == RKType::two) {
17			std::cerr << "Invalid argument: RK2 parametrization is not supported"
18			<< std::endl;
19			}
20		161	}
21
22			template <typename Scalar>
23		326	ControlParametrizationModelPolyTwoRKTpl<
24		326	Scalar>::~ControlParametrizationModelPolyTwoRKTpl() {}
25
26			template <typename Scalar>
27		14774	void ControlParametrizationModelPolyTwoRKTpl<Scalar>::calc(
28			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
29			const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
30	✓✗✗✓	14774	if (static_cast<std::size_t>(u.size()) != nu_) {
31			throw_pretty("Invalid argument: "
32			<< "u has wrong dimension (it should be " +
33			std::to_string(nu_) + ")");
34			}
35		14774	Data* d = static_cast<Data*>(data.get());
36	✓✗	14774	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p0 = u.head(nw_);
37		29548	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p1 =
38	✓✗	14774	u.segment(nw_, nw_);
39	✓✗	14774	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs> >& p2 = u.tail(nw_);
40		14774	d->tmp_t2 = t * t;
41	✗✓✓✗	14774	switch (rktype_) {
42			case two:
43			std::cerr << "Invalid argument: RK2 parametrization is not supported"
44			<< std::endl;
45			break;
46		6336	case three:
47	✓✗	6336	d->c[2] = Scalar(4.5) * d->tmp_t2 - Scalar(1.5) * t;
48	✓✗	6336	d->c[1] = -Scalar(9.) * d->tmp_t2 + Scalar(6.) * t;
49	✓✗	6336	d->c[0] = Scalar(4.5) * (d->tmp_t2 - t) + Scalar(1.);
50		6336	break;
51		8438	case four:
52	✓✗	8438	d->c[2] = Scalar(2.) * d->tmp_t2 - t;
53	✓✗✓✗	8438	d->c[1] = -Scalar(2.) * d->c[2] + Scalar(2.) * t;
54	✓✗✓✗	8438	d->c[0] = d->c[2] - Scalar(2.) * t + Scalar(1.);
55		8438	break;
56			}
57	✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗✓✗ ✓✗	14774	d->w = d->c[2] * p2 + d->c[1] * p1 + d->c[0] * p0;
58		14774	}
59
60			template <typename Scalar>
61		6	void ControlParametrizationModelPolyTwoRKTpl<Scalar>::calcDiff(
62			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
63			const Scalar, const Eigen::Ref<const VectorXs>&) const {
64		6	Data* d = static_cast<Data*>(data.get());
65	✓✗✓✗ ✓✗	6	d->dw_du.leftCols(nw_).diagonal().array() = d->c[0];
66	✓✗✓✗ ✓✗	6	d->dw_du.middleCols(nw_, nw_).diagonal().array() = d->c[1];
67	✓✗✓✗ ✓✗	6	d->dw_du.rightCols(nw_).diagonal().array() = d->c[2];
68		6	}
69
70			template <typename Scalar>
71			boost::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
72		14930	ControlParametrizationModelPolyTwoRKTpl<Scalar>::createData() {
73	✓✗	14930	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
74			}
75
76			template <typename Scalar>
77			void ControlParametrizationModelPolyTwoRKTpl<Scalar>::params(
78			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
79			const Scalar, const Eigen::Ref<const VectorXs>& w) const {
80			if (static_cast<std::size_t>(w.size()) != nw_) {
81			throw_pretty("Invalid argument: "
82			<< "w has wrong dimension (it should be " +
83			std::to_string(nw_) + ")");
84			}
85			data->u.head(nw_) = w;
86			data->u.segment(nw_, nw_) = w;
87			data->u.tail(nw_) = w;
88			}
89
90			template <typename Scalar>
91		154	void ControlParametrizationModelPolyTwoRKTpl<Scalar>::convertBounds(
92			const Eigen::Ref<const VectorXs>& w_lb,
93			const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
94			Eigen::Ref<VectorXs> u_ub) const {
95	✗✓	154	if (static_cast<std::size_t>(u_lb.size()) != nu_) {
96			throw_pretty("Invalid argument: "
97			<< "u_lb has wrong dimension (it should be " +
98			std::to_string(nu_) + ")");
99			}
100	✗✓	154	if (static_cast<std::size_t>(u_ub.size()) != nu_) {
101			throw_pretty("Invalid argument: "
102			<< "u_ub has wrong dimension (it should be " +
103			std::to_string(nu_) + ")");
104			}
105	✗✓	154	if (static_cast<std::size_t>(w_lb.size()) != nw_) {
106			throw_pretty("Invalid argument: "
107			<< "w_lb has wrong dimension (it should be " +
108			std::to_string(nw_) + ")");
109			}
110	✗✓	154	if (static_cast<std::size_t>(w_ub.size()) != nw_) {
111			throw_pretty("Invalid argument: "
112			<< "w_ub has wrong dimension (it should be " +
113			std::to_string(nw_) + ")");
114			}
115	✓✗	154	u_lb.head(nw_) = w_lb;
116	✓✗	154	u_lb.segment(nw_, nw_) = w_lb;
117	✓✗	154	u_lb.tail(nw_) = w_lb;
118	✓✗	154	u_ub.head(nw_) = w_ub;
119	✓✗	154	u_ub.segment(nw_, nw_) = w_ub;
120	✓✗	154	u_ub.tail(nw_) = w_ub;
121		154	}
122
123			template <typename Scalar>
124		662	void ControlParametrizationModelPolyTwoRKTpl<Scalar>::multiplyByJacobian(
125			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
126			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
127			const AssignmentOp op) const {
128	✗✓✗✗ ✗✗✗✗ ✗✗	662	assert_pretty(is_a_AssignmentOp(op),
129			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
130	✓✗✓✗ ✗✓	1324	if (A.rows() != out.rows() \|\| static_cast<std::size_t>(A.cols()) != nw_ \|\|
131	✗✓	662	static_cast<std::size_t>(out.cols()) != nu_) {
132			throw_pretty("Invalid argument: "
133			<< "A and out have wrong dimensions (" +
134			std::to_string(A.rows()) + "," +
135			std::to_string(A.cols()) + " and " +
136			std::to_string(out.rows()) + "," +
137			std::to_string(out.cols()) + +")");
138			}
139		662	Data* d = static_cast<Data*>(data.get());
140	✓✓✗✗	662	switch (op) {
141		552	case setto:
142	✓✗✓✗	552	out.leftCols(nw_) = d->c[0] * A;
143	✓✗✓✗	552	out.middleCols(nw_, nw_) = d->c[1] * A;
144	✓✗✓✗	552	out.rightCols(nw_) = d->c[2] * A;
145		552	break;
146		110	case addto:
147	✓✗✓✗	110	out.leftCols(nw_) += d->c[0] * A;
148	✓✗✓✗	110	out.middleCols(nw_, nw_) += d->c[1] * A;
149	✓✗✓✗	110	out.rightCols(nw_) += d->c[2] * A;
150		110	break;
151			case rmfrom:
152			out.leftCols(nw_) -= d->c[0] * A;
153			out.middleCols(nw_, nw_) -= d->c[1] * A;
154			out.rightCols(nw_) -= d->c[2] * A;
155			break;
156			default:
157			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
158			break;
159			}
160		662	}
161
162			template <typename Scalar>
163		310	void ControlParametrizationModelPolyTwoRKTpl<Scalar>::
164			multiplyJacobianTransposeBy(
165			const boost::shared_ptr<ControlParametrizationDataAbstract>& data,
166			const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
167			const AssignmentOp op) const {
168	✗✓✗✗ ✗✗✗✗ ✗✗	310	assert_pretty(is_a_AssignmentOp(op),
169			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
170	✓✗✓✗ ✗✓	620	if (A.cols() != out.cols() \|\| static_cast<std::size_t>(A.rows()) != nw_ \|\|
171	✗✓	310	static_cast<std::size_t>(out.rows()) != nu_) {
172			throw_pretty("Invalid argument: "
173			<< "A and out have wrong dimensions (" +
174			std::to_string(A.rows()) + "," +
175			std::to_string(A.cols()) + " and " +
176			std::to_string(out.rows()) + "," +
177			std::to_string(out.cols()) + ")");
178			}
179		310	Data* d = static_cast<Data*>(data.get());
180	✓✗✗✗	310	switch (op) {
181		310	case setto:
182	✓✗✓✗	310	out.topRows(nw_) = d->c[0] * A;
183	✓✗✓✗	310	out.middleRows(nw_, nw_) = d->c[1] * A;
184	✓✗✓✗	310	out.bottomRows(nw_) = d->c[2] * A;
185		310	break;
186			case addto:
187			out.topRows(nw_) += d->c[0] * A;
188			out.middleRows(nw_, nw_) += d->c[1] * A;
189			out.bottomRows(nw_) += d->c[2] * A;
190			break;
191			case rmfrom:
192			out.topRows(nw_) -= d->c[0] * A;
193			out.middleRows(nw_, nw_) -= d->c[1] * A;
194			out.bottomRows(nw_) -= d->c[2] * A;
195			break;
196			default:
197			throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
198			break;
199			}
200		310	}
201
202			} // namespace crocoddyl