8 #include "crocoddyl/core/mathbase.hpp" 11 template <
typename Scalar>
12 StateAbstractTpl<Scalar>::StateAbstractTpl(
const std::size_t& nx,
const std::size_t& ndx)
15 lb_(VectorXs::Constant(nx_, -std::numeric_limits<Scalar>::infinity())),
16 ub_(VectorXs::Constant(nx_, std::numeric_limits<Scalar>::infinity())),
22 template <
typename Scalar>
23 StateAbstractTpl<Scalar>::StateAbstractTpl()
26 lb_(MathBase::VectorXs::Constant(nx_, -std::numeric_limits<Scalar>::infinity())),
27 ub_(MathBase::VectorXs::Constant(nx_, std::numeric_limits<Scalar>::infinity())),
30 template <
typename Scalar>
31 StateAbstractTpl<Scalar>::~StateAbstractTpl() {}
33 template <
typename Scalar>
34 typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::diff_dx(
const Eigen::Ref<const VectorXs>& x0,
35 const Eigen::Ref<const VectorXs>& x1) {
36 VectorXs dxout = VectorXs::Zero(ndx_);
41 template <
typename Scalar>
42 typename MathBaseTpl<Scalar>::VectorXs StateAbstractTpl<Scalar>::integrate_x(
const Eigen::Ref<const VectorXs>& x,
43 const Eigen::Ref<const VectorXs>& dx) {
44 VectorXs xout = VectorXs::Zero(nx_);
45 integrate(x, dx, xout);
49 template <
typename Scalar>
50 std::vector<typename MathBaseTpl<Scalar>::MatrixXs> StateAbstractTpl<Scalar>::Jdiff_Js(
51 const Eigen::Ref<const VectorXs>& x0,
const Eigen::Ref<const VectorXs>& x1, Jcomponent firstsecond) {
52 MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
53 std::vector<MatrixXs> Jacs;
54 Jdiff(x0, x1, Jfirst, Jsecond, firstsecond);
55 switch (firstsecond) {
57 Jacs.push_back(Jfirst);
58 Jacs.push_back(Jsecond);
61 Jacs.push_back(Jfirst);
64 Jacs.push_back(Jsecond);
67 Jacs.push_back(Jfirst);
68 Jacs.push_back(Jsecond);
74 template <
typename Scalar>
75 std::vector<typename MathBaseTpl<Scalar>::MatrixXs> StateAbstractTpl<Scalar>::Jintegrate_Js(
76 const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx, Jcomponent firstsecond) {
77 MatrixXs Jfirst(ndx_, ndx_), Jsecond(ndx_, ndx_);
78 std::vector<MatrixXs> Jacs;
79 Jintegrate(x, dx, Jfirst, Jsecond, firstsecond);
80 switch (firstsecond) {
82 Jacs.push_back(Jfirst);
83 Jacs.push_back(Jsecond);
86 Jacs.push_back(Jfirst);
89 Jacs.push_back(Jsecond);
92 Jacs.push_back(Jfirst);
93 Jacs.push_back(Jsecond);
99 template <
typename Scalar>
100 const std::size_t& StateAbstractTpl<Scalar>::get_nx()
const {
104 template <
typename Scalar>
105 const std::size_t& StateAbstractTpl<Scalar>::get_ndx()
const {
109 template <
typename Scalar>
110 const std::size_t& StateAbstractTpl<Scalar>::get_nq()
const {
114 template <
typename Scalar>
115 const std::size_t& StateAbstractTpl<Scalar>::get_nv()
const {
119 template <
typename Scalar>
120 const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_lb()
const {
124 template <
typename Scalar>
125 const typename MathBaseTpl<Scalar>::VectorXs& StateAbstractTpl<Scalar>::get_ub()
const {
129 template <
typename Scalar>
130 bool const& StateAbstractTpl<Scalar>::get_has_limits()
const {
134 template <
typename Scalar>
135 void StateAbstractTpl<Scalar>::set_lb(
const VectorXs& lb) {
136 if (static_cast<std::size_t>(lb.size()) != nx_) {
137 throw_pretty(
"Invalid argument: " 138 <<
"lower bound has wrong dimension (it should be " + std::to_string(nx_) +
")");
144 template <
typename Scalar>
145 void StateAbstractTpl<Scalar>::set_ub(
const VectorXs& ub) {
146 if (static_cast<std::size_t>(ub.size()) != nx_) {
147 throw_pretty(
"Invalid argument: " 148 <<
"upper bound has wrong dimension (it should be " + std::to_string(nx_) +
")");
154 template <
typename Scalar>
155 void StateAbstractTpl<Scalar>::update_has_limits() {
156 has_limits_ = isfinite(lb_.array()).any() && isfinite(ub_.array()).any();
Definition: action-base.hxx:11