Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actions/free-invdyn.hxx	Lines:	115	134	85.8 %
Date:	2024-02-13 11:12:33	Branches:	78	286	27.3 %


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

#include <pinocchio/algorithm/compute-all-terms.hpp>
#include <pinocchio/algorithm/jacobian.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <pinocchio/algorithm/rnea-derivatives.hpp>
#include <pinocchio/algorithm/rnea.hpp>

#include "crocoddyl/core/constraints/residual.hpp"

namespace crocoddyl {

template <typename Scalar>
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
    DifferentialActionModelFreeInvDynamicsTpl(
        boost::shared_ptr<StateMultibody> state,
        boost::shared_ptr<ActuationModelAbstract> actuation,
        boost::shared_ptr<CostModelSum> costs)
    : Base(state, state->get_nv(), costs->get_nr(), 0,
           state->get_nv() - actuation->get_nu()),
      actuation_(actuation),
      costs_(costs),
      constraints_(
          boost::make_shared<ConstraintModelManager>(state, state->get_nv())),
      pinocchio_(*state->get_pinocchio().get()) {
  init(state);
}

template <typename Scalar>
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
    DifferentialActionModelFreeInvDynamicsTpl(
        boost::shared_ptr<StateMultibody> state,
        boost::shared_ptr<ActuationModelAbstract> actuation,
        boost::shared_ptr<CostModelSum> costs,
        boost::shared_ptr<ConstraintModelManager> constraints)
    : Base(state, state->get_nv(), costs->get_nr(), constraints->get_ng(),
           constraints->get_nh() + state->get_nv() - actuation->get_nu()),
      actuation_(actuation),
      costs_(costs),
      constraints_(constraints),
      pinocchio_(*state->get_pinocchio().get()) {
  init(state);
}

template <typename Scalar>
void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::init(
    const boost::shared_ptr<StateMultibody>& state) {
  if (costs_->get_nu() != nu_) {
    throw_pretty(
        "Invalid argument: "
        << "Costs doesn't have the same control dimension (it should be " +
               std::to_string(nu_) + ")");
  }
  if (constraints_->get_nu() != nu_) {
    throw_pretty("Invalid argument: "
                 << "Constraints doesn't have the same control dimension (it "
                    "should be " +
                        std::to_string(nu_) + ")");
  }
  VectorXs lb =
      VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
  VectorXs ub =
      VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
  Base::set_u_lb(lb);
  Base::set_u_ub(ub);

  if (state->get_nv() - actuation_->get_nu() > 0) {
    constraints_->addConstraint(
        "tau",
        boost::make_shared<ConstraintModelResidual>(
            state_, boost::make_shared<
                        typename DifferentialActionModelFreeInvDynamicsTpl<
                            Scalar>::ResidualModelActuation>(
                        state, actuation_->get_nu())));
  }
}

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

template <typename Scalar>
void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  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 std::size_t nv = state_->get_nv();
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
      x.head(state_->get_nq());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
      x.tail(nv);

  d->xout = u;
  pinocchio::rnea(pinocchio_, d->pinocchio, q, v, u);
  pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
  actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
  d->multibody.joint->a = u;
  d->multibody.joint->tau = d->multibody.actuation->u;
  actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
  costs_->calc(d->costs, x, u);
  d->cost = d->costs->cost;
  d->constraints->resize(this, d);
  constraints_->calc(d->constraints, x, u);
}

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

  Data* d = static_cast<Data*>(data.get());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
      x.head(state_->get_nq());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
      x.tail(state_->get_nv());

  pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);

  costs_->calc(d->costs, x);
  d->cost = d->costs->cost;
  d->constraints->resize(this, d);
  constraints_->calc(d->constraints, x);
}

template <typename Scalar>
void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  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 std::size_t nv = state_->get_nv();
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
      x.head(state_->get_nq());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
      x.tail(nv);

  pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, u);
  d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
      d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
          .transpose();
  actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
  actuation_->torqueTransform(d->multibody.actuation, x,
                              d->multibody.joint->tau);
  d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
      d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
  d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
      d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
  d->multibody.joint->dtau_du.noalias() =
      d->multibody.actuation->Mtau * d->pinocchio.M;
  costs_->calcDiff(d->costs, x, u);
  constraints_->calcDiff(d->constraints, x, u);
}

template <typename Scalar>
void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
    throw_pretty("Invalid argument: "
                 << "x has wrong dimension (it should be " +
                        std::to_string(state_->get_nx()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());

  costs_->calcDiff(d->costs, x);
  constraints_->calcDiff(d->constraints, x);
}

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

template <typename Scalar>
bool DifferentialActionModelFreeInvDynamicsTpl<Scalar>::checkData(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data) {
  boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
  if (d != NULL) {
    return true;
  } else {
    return false;
  }
}

template <typename Scalar>
void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::quasiStatic(
    const boost::shared_ptr<DifferentialActionDataAbstract>&,
    Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>&,
    const std::size_t, const Scalar) {
  u.setZero();
}

template <typename Scalar>
void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::print(
    std::ostream& os) const {
  os << "DifferentialActionModelFreeInvDynamics {nx=" << state_->get_nx()
     << ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
}

template <typename Scalar>
std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng() const {
  if (constraints_ != nullptr) {
    return constraints_->get_ng();
  } else {
    return Base::get_ng();
  }
}

template <typename Scalar>
std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh() const {
  if (constraints_ != nullptr) {
    return constraints_->get_nh();
  } else {
    return Base::get_nh();
  }
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_lb() const {
  if (constraints_ != nullptr) {
    return constraints_->get_lb();
  } else {
    return g_lb_;
  }
}

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_ub() const {
  if (constraints_ != nullptr) {
    return constraints_->get_ub();
  } else {
    return g_lb_;
  }
}

template <typename Scalar>
const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_actuation() const {
  return actuation_;
}

template <typename Scalar>
const boost::shared_ptr<CostModelSumTpl<Scalar> >&
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_costs() const {
  return costs_;
}

template <typename Scalar>
const boost::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_constraints() const {
  return constraints_;
}

template <typename Scalar>
pinocchio::ModelTpl<Scalar>&
DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_pinocchio() const {
  return pinocchio_;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2022, Heriot-Watt University, University of Edinburgh
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include <pinocchio/algorithm/compute-all-terms.hpp>
10			#include <pinocchio/algorithm/jacobian.hpp>
11			#include <pinocchio/algorithm/kinematics.hpp>
12			#include <pinocchio/algorithm/rnea-derivatives.hpp>
13			#include <pinocchio/algorithm/rnea.hpp>
14
15			#include "crocoddyl/core/constraints/residual.hpp"
16
17			namespace crocoddyl {
18
19			template <typename Scalar>
20		1	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
21			DifferentialActionModelFreeInvDynamicsTpl(
22			boost::shared_ptr<StateMultibody> state,
23			boost::shared_ptr<ActuationModelAbstract> actuation,
24			boost::shared_ptr<CostModelSum> costs)
25		1	: Base(state, state->get_nv(), costs->get_nr(), 0,
26		1	state->get_nv() - actuation->get_nu()),
27			actuation_(actuation),
28			costs_(costs),
29			constraints_(
30		1	boost::make_shared<ConstraintModelManager>(state, state->get_nv())),
31	✓✗✓✗	4	pinocchio_(*state->get_pinocchio().get()) {
32	✓✗	1	init(state);
33		1	}
34
35			template <typename Scalar>
36		150	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::
37			DifferentialActionModelFreeInvDynamicsTpl(
38			boost::shared_ptr<StateMultibody> state,
39			boost::shared_ptr<ActuationModelAbstract> actuation,
40			boost::shared_ptr<CostModelSum> costs,
41			boost::shared_ptr<ConstraintModelManager> constraints)
42		150	: Base(state, state->get_nv(), costs->get_nr(), constraints->get_ng(),
43		150	constraints->get_nh() + state->get_nv() - actuation->get_nu()),
44			actuation_(actuation),
45			costs_(costs),
46			constraints_(constraints),
47	✓✗	450	pinocchio_(*state->get_pinocchio().get()) {
48	✓✗	150	init(state);
49		150	}
50
51			template <typename Scalar>
52		151	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::init(
53			const boost::shared_ptr<StateMultibody>& state) {
54	✗✓	151	if (costs_->get_nu() != nu_) {
55			throw_pretty(
56			"Invalid argument: "
57			<< "Costs doesn't have the same control dimension (it should be " +
58			std::to_string(nu_) + ")");
59			}
60	✗✓	151	if (constraints_->get_nu() != nu_) {
61			throw_pretty("Invalid argument: "
62			<< "Constraints doesn't have the same control dimension (it "
63			"should be " +
64			std::to_string(nu_) + ")");
65			}
66	✓✗	302	VectorXs lb =
67	✓✗	151	VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
68	✓✗	302	VectorXs ub =
69	✓✗	151	VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
70	✓✗	151	Base::set_u_lb(lb);
71	✓✗	151	Base::set_u_ub(ub);
72
73	✓✓	151	if (state->get_nv() - actuation_->get_nu() > 0) {
74	✓✗✓✗	102	constraints_->addConstraint(
75			"tau",
76			boost::make_shared<ConstraintModelResidual>(
77	✓✗	51	state_, boost::make_shared<
78			typename DifferentialActionModelFreeInvDynamicsTpl<
79	✓✗	102	Scalar>::ResidualModelActuation>(
80			state, actuation_->get_nu())));
81			}
82		151	}
83
84			template <typename Scalar>
85		306	DifferentialActionModelFreeInvDynamicsTpl<
86		306	Scalar>::~DifferentialActionModelFreeInvDynamicsTpl() {}
87
88			template <typename Scalar>
89		7141	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
90			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
91			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
92	✓✗✗✓	7141	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
93			throw_pretty("Invalid argument: "
94			<< "x has wrong dimension (it should be " +
95			std::to_string(state_->get_nx()) + ")");
96			}
97	✓✗✗✓	7141	if (static_cast<std::size_t>(u.size()) != nu_) {
98			throw_pretty("Invalid argument: "
99			<< "u has wrong dimension (it should be " +
100			std::to_string(nu_) + ")");
101			}
102		7141	Data* d = static_cast<Data*>(data.get());
103		7141	const std::size_t nv = state_->get_nv();
104	✓✗	7141	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
105			x.head(state_->get_nq());
106	✓✗	7141	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
107			x.tail(nv);
108
109	✓✗	7141	d->xout = u;
110	✓✗	7141	pinocchio::rnea(pinocchio_, d->pinocchio, q, v, u);
111	✓✗	7141	pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
112	✓✗✓✗	7141	actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
113	✓✗	7141	d->multibody.joint->a = u;
114	✓✗	7141	d->multibody.joint->tau = d->multibody.actuation->u;
115	✓✗✓✗	7141	actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
116	✓✗	7141	costs_->calc(d->costs, x, u);
117		7141	d->cost = d->costs->cost;
118	✓✗	7141	d->constraints->resize(this, d);
119	✓✗	7141	constraints_->calc(d->constraints, x, u);
120		7141	}
121
122			template <typename Scalar>
123		571	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calc(
124			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
125			const Eigen::Ref<const VectorXs>& x) {
126	✓✗✗✓	571	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
127			throw_pretty("Invalid argument: "
128			<< "x has wrong dimension (it should be " +
129			std::to_string(state_->get_nx()) + ")");
130			}
131
132		571	Data* d = static_cast<Data*>(data.get());
133	✓✗	571	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
134			x.head(state_->get_nq());
135	✓✗	571	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
136			x.tail(state_->get_nv());
137
138	✓✗	571	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
139
140	✓✗	571	costs_->calc(d->costs, x);
141		571	d->cost = d->costs->cost;
142	✓✗	571	d->constraints->resize(this, d);
143	✓✗	571	constraints_->calc(d->constraints, x);
144		571	}
145
146			template <typename Scalar>
147		1881	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
148			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
149			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
150	✓✗✗✓	1881	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
151			throw_pretty("Invalid argument: "
152			<< "x has wrong dimension (it should be " +
153			std::to_string(state_->get_nx()) + ")");
154			}
155	✓✗✗✓	1881	if (static_cast<std::size_t>(u.size()) != nu_) {
156			throw_pretty("Invalid argument: "
157			<< "u has wrong dimension (it should be " +
158			std::to_string(nu_) + ")");
159			}
160		1881	Data* d = static_cast<Data*>(data.get());
161		1881	const std::size_t nv = state_->get_nv();
162	✓✗	1881	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
163			x.head(state_->get_nq());
164	✓✗	1881	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
165			x.tail(nv);
166
167	✓✗	1881	pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, u);
168	✓✗✓✗ ✓✗	1881	d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
169		1881	d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
170	✓✗	1881	.transpose();
171	✓✗✓✗	1881	actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
172	✓✗	3762	actuation_->torqueTransform(d->multibody.actuation, x,
173	✓✗	1881	d->multibody.joint->tau);
174	✓✗✓✗ ✓✗	1881	d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
175	✓✗	1881	d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
176	✓✗✓✗ ✓✗	1881	d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
177	✓✗	1881	d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
178	✓✗✓✗	1881	d->multibody.joint->dtau_du.noalias() =
179	✓✗	1881	d->multibody.actuation->Mtau * d->pinocchio.M;
180	✓✗	1881	costs_->calcDiff(d->costs, x, u);
181	✓✗	1881	constraints_->calcDiff(d->constraints, x, u);
182		1881	}
183
184			template <typename Scalar>
185		66	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::calcDiff(
186			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
187			const Eigen::Ref<const VectorXs>& x) {
188	✗✓	66	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
189			throw_pretty("Invalid argument: "
190			<< "x has wrong dimension (it should be " +
191			std::to_string(state_->get_nx()) + ")");
192			}
193		66	Data* d = static_cast<Data*>(data.get());
194
195		66	costs_->calcDiff(d->costs, x);
196		66	constraints_->calcDiff(d->constraints, x);
197		66	}
198
199			template <typename Scalar>
200			boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
201		9894	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::createData() {
202	✓✗	9894	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
203			}
204
205			template <typename Scalar>
206		1971	bool DifferentialActionModelFreeInvDynamicsTpl<Scalar>::checkData(
207			const boost::shared_ptr<DifferentialActionDataAbstract>& data) {
208		3942	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
209	✓✗	1971	if (d != NULL) {
210		1971	return true;
211			} else {
212			return false;
213			}
214			}
215
216			template <typename Scalar>
217		963	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::quasiStatic(
218			const boost::shared_ptr<DifferentialActionDataAbstract>&,
219			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>&,
220			const std::size_t, const Scalar) {
221		963	u.setZero();
222		963	}
223
224			template <typename Scalar>
225		30	void DifferentialActionModelFreeInvDynamicsTpl<Scalar>::print(
226			std::ostream& os) const {
227		30	os << "DifferentialActionModelFreeInvDynamics {nx=" << state_->get_nx()
228		30	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
229		30	}
230
231			template <typename Scalar>
232		49736	std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_ng() const {
233	✓✗	49736	if (constraints_ != nullptr) {
234		49736	return constraints_->get_ng();
235			} else {
236			return Base::get_ng();
237			}
238			}
239
240			template <typename Scalar>
241		49736	std::size_t DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_nh() const {
242	✓✗	49736	if (constraints_ != nullptr) {
243		49736	return constraints_->get_nh();
244			} else {
245			return Base::get_nh();
246			}
247			}
248
249			template <typename Scalar>
250			const typename MathBaseTpl<Scalar>::VectorXs&
251			DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_lb() const {
252			if (constraints_ != nullptr) {
253			return constraints_->get_lb();
254			} else {
255			return g_lb_;
256			}
257			}
258
259			template <typename Scalar>
260			const typename MathBaseTpl<Scalar>::VectorXs&
261			DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_g_ub() const {
262			if (constraints_ != nullptr) {
263			return constraints_->get_ub();
264			} else {
265			return g_lb_;
266			}
267			}
268
269			template <typename Scalar>
270			const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
271		19788	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_actuation() const {
272		19788	return actuation_;
273			}
274
275			template <typename Scalar>
276			const boost::shared_ptr<CostModelSumTpl<Scalar> >&
277		9894	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_costs() const {
278		9894	return costs_;
279			}
280
281			template <typename Scalar>
282			const boost::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
283		9894	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_constraints() const {
284		9894	return constraints_;
285			}
286
287			template <typename Scalar>
288			pinocchio::ModelTpl<Scalar>&
289		9894	DifferentialActionModelFreeInvDynamicsTpl<Scalar>::get_pinocchio() const {
290		9894	return pinocchio_;
291			}
292
293			} // namespace crocoddyl