Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/core/states/euclidean.hxx	Lines:	57	81	70.4 %
Date:	2024-02-13 11:12:33	Branches:	63	368	17.1 %


///////////////////////////////////////////////////////////////////////////////
// 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.
///////////////////////////////////////////////////////////////////////////////

namespace crocoddyl {

template <typename Scalar>
StateVectorTpl<Scalar>::StateVectorTpl(const std::size_t nx)
    : StateAbstractTpl<Scalar>(nx, nx) {}

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

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::zero() const {
  return VectorXs::Zero(nx_);
}

template <typename Scalar>
typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::rand() const {
  return VectorXs::Random(nx_);
}

template <typename Scalar>
void StateVectorTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
                                  const Eigen::Ref<const VectorXs>& x1,
                                  Eigen::Ref<VectorXs> dxout) const {
  if (static_cast<std::size_t>(x0.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x0 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(x1.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x1 has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(dxout.size()) != ndx_) {
    throw_pretty("Invalid argument: "
                 << "dxout has wrong dimension (it should be " +
                        std::to_string(ndx_) + ")");
  }
  dxout = x1 - x0;
}

template <typename Scalar>
void StateVectorTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
                                       const Eigen::Ref<const VectorXs>& dx,
                                       Eigen::Ref<VectorXs> xout) const {
  if (static_cast<std::size_t>(x.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  if (static_cast<std::size_t>(dx.size()) != ndx_) {
    throw_pretty("Invalid argument: "
                 << "dx has wrong dimension (it should be " +
                        std::to_string(ndx_) + ")");
  }
  if (static_cast<std::size_t>(xout.size()) != nx_) {
    throw_pretty("Invalid argument: "
                 << "xout has wrong dimension (it should be " +
                        std::to_string(nx_) + ")");
  }
  xout = x + dx;
}

template <typename Scalar>
void StateVectorTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>&,
                                   const Eigen::Ref<const VectorXs>&,
                                   Eigen::Ref<MatrixXs> Jfirst,
                                   Eigen::Ref<MatrixXs> Jsecond,
                                   const Jcomponent firstsecond) const {
  assert_pretty(
      is_a_Jcomponent(firstsecond),
      ("firstsecond must be one of the Jcomponent {both, first, second}"));
  if (firstsecond == first || firstsecond == both) {
    if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
        static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jfirst has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    Jfirst.setZero();
    Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, -1.);
  }
  if (firstsecond == second || firstsecond == both) {
    if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
        static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jsecond has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    Jsecond.setZero();
    Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
  }
}

template <typename Scalar>
void StateVectorTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>&,
                                        const Eigen::Ref<const VectorXs>&,
                                        Eigen::Ref<MatrixXs> Jfirst,
                                        Eigen::Ref<MatrixXs> Jsecond,
                                        const Jcomponent firstsecond,
                                        const AssignmentOp op) const {
  assert_pretty(
      is_a_Jcomponent(firstsecond),
      ("firstsecond must be one of the Jcomponent {both, first, second}"));
  assert_pretty(is_a_AssignmentOp(op),
                ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
  if (firstsecond == first || firstsecond == both) {
    if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ ||
        static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jfirst has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    switch (op) {
      case setto:
        Jfirst.diagonal().array() = Scalar(1.);
        break;
      case addto:
        Jfirst.diagonal().array() += Scalar(1.);
        break;
      case rmfrom:
        Jfirst.diagonal().array() -= Scalar(1.);
        break;
      default:
        throw_pretty(
            "Invalid argument: allowed operators: setto, addto, rmfrom");
        break;
    }
  }
  if (firstsecond == second || firstsecond == both) {
    if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ ||
        static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
      throw_pretty("Invalid argument: "
                   << "Jsecond has wrong dimension (it should be " +
                          std::to_string(ndx_) + "," + std::to_string(ndx_) +
                          ")");
    }
    switch (op) {
      case setto:
        Jsecond.diagonal().array() = Scalar(1.);
        break;
      case addto:
        Jsecond.diagonal().array() += Scalar(1.);
        break;
      case rmfrom:
        Jsecond.diagonal().array() -= Scalar(1.);
        break;
      default:
        throw_pretty(
            "Invalid argument: allowed operators: setto, addto, rmfrom");
        break;
    }
  }
}

template <typename Scalar>
void StateVectorTpl<Scalar>::JintegrateTransport(
    const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&,
    Eigen::Ref<MatrixXs>, const Jcomponent firstsecond) const {
  assert_pretty(is_a_Jcomponent(firstsecond), (""));
  if (firstsecond != first && firstsecond != second) {
    throw_pretty(
        "Invalid argument: firstsecond must be either first or second. both "
        "not supported for this operation.");
  }
}

}  // 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
9			namespace crocoddyl {
10
11			template <typename Scalar>
12		243	StateVectorTpl<Scalar>::StateVectorTpl(const std::size_t nx)
13		243	: StateAbstractTpl<Scalar>(nx, nx) {}
14
15			template <typename Scalar>
16		490	StateVectorTpl<Scalar>::~StateVectorTpl() {}
17
18			template <typename Scalar>
19		1715	typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::zero() const {
20	✓✗	1715	return VectorXs::Zero(nx_);
21			}
22
23			template <typename Scalar>
24		1493	typename MathBaseTpl<Scalar>::VectorXs StateVectorTpl<Scalar>::rand() const {
25	✓✗	1493	return VectorXs::Random(nx_);
26			}
27
28			template <typename Scalar>
29		6158	void StateVectorTpl<Scalar>::diff(const Eigen::Ref<const VectorXs>& x0,
30			const Eigen::Ref<const VectorXs>& x1,
31			Eigen::Ref<VectorXs> dxout) const {
32	✗✓	6158	if (static_cast<std::size_t>(x0.size()) != nx_) {
33			throw_pretty("Invalid argument: "
34			<< "x0 has wrong dimension (it should be " +
35			std::to_string(nx_) + ")");
36			}
37	✗✓	6158	if (static_cast<std::size_t>(x1.size()) != nx_) {
38			throw_pretty("Invalid argument: "
39			<< "x1 has wrong dimension (it should be " +
40			std::to_string(nx_) + ")");
41			}
42	✗✓	6158	if (static_cast<std::size_t>(dxout.size()) != ndx_) {
43			throw_pretty("Invalid argument: "
44			<< "dxout has wrong dimension (it should be " +
45			std::to_string(ndx_) + ")");
46			}
47	✓✗	6158	dxout = x1 - x0;
48		6158	}
49
50			template <typename Scalar>
51		17278	void StateVectorTpl<Scalar>::integrate(const Eigen::Ref<const VectorXs>& x,
52			const Eigen::Ref<const VectorXs>& dx,
53			Eigen::Ref<VectorXs> xout) const {
54	✗✓	17278	if (static_cast<std::size_t>(x.size()) != nx_) {
55			throw_pretty("Invalid argument: "
56			<< "x has wrong dimension (it should be " +
57			std::to_string(nx_) + ")");
58			}
59	✗✓	17278	if (static_cast<std::size_t>(dx.size()) != ndx_) {
60			throw_pretty("Invalid argument: "
61			<< "dx has wrong dimension (it should be " +
62			std::to_string(ndx_) + ")");
63			}
64	✗✓	17278	if (static_cast<std::size_t>(xout.size()) != nx_) {
65			throw_pretty("Invalid argument: "
66			<< "xout has wrong dimension (it should be " +
67			std::to_string(nx_) + ")");
68			}
69	✓✗	17278	xout = x + dx;
70		17278	}
71
72			template <typename Scalar>
73		130	void StateVectorTpl<Scalar>::Jdiff(const Eigen::Ref<const VectorXs>&,
74			const Eigen::Ref<const VectorXs>&,
75			Eigen::Ref<MatrixXs> Jfirst,
76			Eigen::Ref<MatrixXs> Jsecond,
77			const Jcomponent firstsecond) const {
78	✗✓✗✗ ✗✗✗✗ ✗✗	130	assert_pretty(
79			is_a_Jcomponent(firstsecond),
80			("firstsecond must be one of the Jcomponent {both, first, second}"));
81	✓✓✓✓	130	if (firstsecond == first \|\| firstsecond == both) {
82	✓✗✗✓	256	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ \|\|
83	✗✓	128	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
84			throw_pretty("Invalid argument: "
85			<< "Jfirst has wrong dimension (it should be " +
86			std::to_string(ndx_) + "," + std::to_string(ndx_) +
87			")");
88			}
89		128	Jfirst.setZero();
90	✓✗✓✗ ✓✗	128	Jfirst.diagonal() = MathBase::VectorXs::Constant(ndx_, -1.);
91			}
92	✓✓✓✓	130	if (firstsecond == second \|\| firstsecond == both) {
93	✓✗✗✓	234	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ \|\|
94	✗✓	117	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
95			throw_pretty("Invalid argument: "
96			<< "Jsecond has wrong dimension (it should be " +
97			std::to_string(ndx_) + "," + std::to_string(ndx_) +
98			")");
99			}
100		117	Jsecond.setZero();
101	✓✗✓✗ ✓✗	117	Jsecond.diagonal() = MathBase::VectorXs::Constant(ndx_, 1.);
102			}
103		130	}
104
105			template <typename Scalar>
106		1675	void StateVectorTpl<Scalar>::Jintegrate(const Eigen::Ref<const VectorXs>&,
107			const Eigen::Ref<const VectorXs>&,
108			Eigen::Ref<MatrixXs> Jfirst,
109			Eigen::Ref<MatrixXs> Jsecond,
110			const Jcomponent firstsecond,
111			const AssignmentOp op) const {
112	✗✓✗✗ ✗✗✗✗ ✗✗	1675	assert_pretty(
113			is_a_Jcomponent(firstsecond),
114			("firstsecond must be one of the Jcomponent {both, first, second}"));
115	✗✓✗✗ ✗✗✗✗ ✗✗	1675	assert_pretty(is_a_AssignmentOp(op),
116			("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
117	✓✓✓✓	1675	if (firstsecond == first \|\| firstsecond == both) {
118	✓✗✗✓	2534	if (static_cast<std::size_t>(Jfirst.rows()) != ndx_ \|\|
119	✗✓	1267	static_cast<std::size_t>(Jfirst.cols()) != ndx_) {
120			throw_pretty("Invalid argument: "
121			<< "Jfirst has wrong dimension (it should be " +
122			std::to_string(ndx_) + "," + std::to_string(ndx_) +
123			")");
124			}
125	✓✓✗✗	1267	switch (op) {
126		15	case setto:
127	✓✗✓✗ ✓✗	15	Jfirst.diagonal().array() = Scalar(1.);
128		15	break;
129		1252	case addto:
130	✓✗✓✗ ✓✗	1252	Jfirst.diagonal().array() += Scalar(1.);
131		1252	break;
132			case rmfrom:
133			Jfirst.diagonal().array() -= Scalar(1.);
134			break;
135			default:
136			throw_pretty(
137			"Invalid argument: allowed operators: setto, addto, rmfrom");
138			break;
139			}
140			}
141	✓✓✓✓	1675	if (firstsecond == second \|\| firstsecond == both) {
142	✓✗✗✓	844	if (static_cast<std::size_t>(Jsecond.rows()) != ndx_ \|\|
143	✗✓	422	static_cast<std::size_t>(Jsecond.cols()) != ndx_) {
144			throw_pretty("Invalid argument: "
145			<< "Jsecond has wrong dimension (it should be " +
146			std::to_string(ndx_) + "," + std::to_string(ndx_) +
147			")");
148			}
149	✓✗✗✗	422	switch (op) {
150		422	case setto:
151	✓✗✓✗ ✓✗	422	Jsecond.diagonal().array() = Scalar(1.);
152		422	break;
153			case addto:
154			Jsecond.diagonal().array() += Scalar(1.);
155			break;
156			case rmfrom:
157			Jsecond.diagonal().array() -= Scalar(1.);
158			break;
159			default:
160			throw_pretty(
161			"Invalid argument: allowed operators: setto, addto, rmfrom");
162			break;
163			}
164			}
165		1675	}
166
167			template <typename Scalar>
168		2506	void StateVectorTpl<Scalar>::JintegrateTransport(
169			const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&,
170			Eigen::Ref<MatrixXs>, const Jcomponent firstsecond) const {
171	✗✓✗✗ ✗✗✗✗ ✗✗	2506	assert_pretty(is_a_Jcomponent(firstsecond), (""));
172	✓✓✗✓	2506	if (firstsecond != first && firstsecond != second) {
173			throw_pretty(
174			"Invalid argument: firstsecond must be either first or second. both "
175			"not supported for this operation.");
176			}
177		2506	}
178
179			} // namespace crocoddyl