Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actions/free-fwddyn.hxx	Lines:	129	151	85.4 %
Date:	2024-02-13 11:12:33	Branches:	125	398	31.4 %


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

#include <pinocchio/algorithm/aba-derivatives.hpp>
#include <pinocchio/algorithm/aba.hpp>
#include <pinocchio/algorithm/cholesky.hpp>
#include <pinocchio/algorithm/compute-all-terms.hpp>
#include <pinocchio/algorithm/frames.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/utils/exception.hpp"
#include "crocoddyl/multibody/actions/free-fwddyn.hpp"

namespace crocoddyl {

template <typename Scalar>
DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::
    DifferentialActionModelFreeFwdDynamicsTpl(
        boost::shared_ptr<StateMultibody> state,
        boost::shared_ptr<ActuationModelAbstract> actuation,
        boost::shared_ptr<CostModelSum> costs,
        boost::shared_ptr<ConstraintModelManager> constraints)
    : Base(state, actuation->get_nu(), costs->get_nr()),
      actuation_(actuation),
      costs_(costs),
      constraints_(constraints),
      pinocchio_(*state->get_pinocchio().get()),
      without_armature_(true),
      armature_(VectorXs::Zero(state->get_nv())) {
  if (costs_->get_nu() != nu_) {
    throw_pretty(
        "Invalid argument: "
        << "Costs doesn't have the same control dimension (it should be " +
               std::to_string(nu_) + ")");
  }
  Base::set_u_lb(Scalar(-1.) * pinocchio_.effortLimit.tail(nu_));
  Base::set_u_ub(Scalar(+1.) * pinocchio_.effortLimit.tail(nu_));
}

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

template <typename Scalar>
void DifferentialActionModelFreeFwdDynamicsTpl<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 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());

  actuation_->calc(d->multibody.actuation, x, u);

  // Computing the dynamics using ABA or manually for armature case
  if (without_armature_) {
    d->xout = pinocchio::aba(pinocchio_, d->pinocchio, q, v,
                             d->multibody.actuation->tau);
    pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
  } else {
    pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
    d->pinocchio.M.diagonal() += armature_;
    pinocchio::cholesky::decompose(pinocchio_, d->pinocchio);
    d->Minv.setZero();
    pinocchio::cholesky::computeMinv(pinocchio_, d->pinocchio, d->Minv);
    d->u_drift = d->multibody.actuation->tau - d->pinocchio.nle;
    d->xout.noalias() = d->Minv * d->u_drift;
  }
  d->multibody.joint->a = d->xout;
  d->multibody.joint->tau = u;
  costs_->calc(d->costs, x, u);
  d->cost = d->costs->cost;
  if (constraints_ != nullptr) {
    d->constraints->resize(this, d);
    constraints_->calc(d->constraints, x, u);
  }
}

template <typename Scalar>
void DifferentialActionModelFreeFwdDynamicsTpl<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;
  if (constraints_ != nullptr) {
    d->constraints->resize(this, d);
    constraints_->calc(d->constraints, x);
  }
}

template <typename Scalar>
void DifferentialActionModelFreeFwdDynamicsTpl<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_) + ")");
  }

  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);

  Data* d = static_cast<Data*>(data.get());

  actuation_->calcDiff(d->multibody.actuation, x, u);

  // Computing the dynamics derivatives
  if (without_armature_) {
    pinocchio::computeABADerivatives(
        pinocchio_, d->pinocchio, q, v, d->multibody.actuation->tau,
        d->Fx.leftCols(nv), d->Fx.rightCols(nv), d->pinocchio.Minv);
    d->Fx.noalias() += d->pinocchio.Minv * d->multibody.actuation->dtau_dx;
    d->Fu.noalias() = d->pinocchio.Minv * d->multibody.actuation->dtau_du;
  } else {
    pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout);
    d->dtau_dx.leftCols(nv) =
        d->multibody.actuation->dtau_dx.leftCols(nv) - d->pinocchio.dtau_dq;
    d->dtau_dx.rightCols(nv) =
        d->multibody.actuation->dtau_dx.rightCols(nv) - d->pinocchio.dtau_dv;
    d->Fx.noalias() = d->Minv * d->dtau_dx;
    d->Fu.noalias() = d->Minv * d->multibody.actuation->dtau_du;
  }
  d->multibody.joint->da_dx = d->Fx;
  d->multibody.joint->da_du = d->Fu;
  costs_->calcDiff(d->costs, x, u);
  if (constraints_ != nullptr) {
    constraints_->calcDiff(d->constraints, x, u);
  }
}

template <typename Scalar>
void DifferentialActionModelFreeFwdDynamicsTpl<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);
  if (constraints_ != nullptr) {
    constraints_->calcDiff(d->constraints, x);
  }
}

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

template <typename Scalar>
bool DifferentialActionModelFreeFwdDynamicsTpl<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 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::quasiStatic(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
    const std::size_t, const Scalar) {
  if (static_cast<std::size_t>(u.size()) != nu_) {
    throw_pretty("Invalid argument: "
                 << "u has wrong dimension (it should be " +
                        std::to_string(nu_) + ")");
  }
  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()) + ")");
  }
  // Static casting the data
  Data* d = static_cast<Data*>(data.get());
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
      x.head(state_->get_nq());

  const std::size_t nq = state_->get_nq();
  const std::size_t nv = state_->get_nv();

  d->tmp_xstatic.head(nq) = q;
  d->tmp_xstatic.tail(nv).setZero();
  u.setZero();

  pinocchio::rnea(pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
                  d->tmp_xstatic.tail(nv));
  actuation_->calc(d->multibody.actuation, d->tmp_xstatic, u);
  actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);

  u.noalias() =
      pseudoInverse(d->multibody.actuation->dtau_du) * d->pinocchio.tau;
  d->pinocchio.tau.setZero();
}

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

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

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

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

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

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

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

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

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

template <typename Scalar>
const typename MathBaseTpl<Scalar>::VectorXs&
DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_armature() const {
  return armature_;
}

template <typename Scalar>
void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::set_armature(
    const VectorXs& armature) {
  if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
    throw_pretty("Invalid argument: "
                 << "The armature dimension is wrong (it should be " +
                        std::to_string(state_->get_nv()) + ")");
  }

  armature_ = armature;
  without_armature_ = false;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2022, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include <pinocchio/algorithm/aba-derivatives.hpp>
11			#include <pinocchio/algorithm/aba.hpp>
12			#include <pinocchio/algorithm/cholesky.hpp>
13			#include <pinocchio/algorithm/compute-all-terms.hpp>
14			#include <pinocchio/algorithm/frames.hpp>
15			#include <pinocchio/algorithm/jacobian.hpp>
16			#include <pinocchio/algorithm/kinematics.hpp>
17			#include <pinocchio/algorithm/rnea-derivatives.hpp>
18			#include <pinocchio/algorithm/rnea.hpp>
19
20			#include "crocoddyl/core/utils/exception.hpp"
21			#include "crocoddyl/multibody/actions/free-fwddyn.hpp"
22
23			namespace crocoddyl {
24
25			template <typename Scalar>
26		160	DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::
27			DifferentialActionModelFreeFwdDynamicsTpl(
28			boost::shared_ptr<StateMultibody> state,
29			boost::shared_ptr<ActuationModelAbstract> actuation,
30			boost::shared_ptr<CostModelSum> costs,
31			boost::shared_ptr<ConstraintModelManager> constraints)
32			: Base(state, actuation->get_nu(), costs->get_nr()),
33			actuation_(actuation),
34			costs_(costs),
35			constraints_(constraints),
36		160	pinocchio_(*state->get_pinocchio().get()),
37			without_armature_(true),
38	✓✗✓✗ ✓✗	320	armature_(VectorXs::Zero(state->get_nv())) {
39	✗✓	160	if (costs_->get_nu() != nu_) {
40			throw_pretty(
41			"Invalid argument: "
42			<< "Costs doesn't have the same control dimension (it should be " +
43			std::to_string(nu_) + ")");
44			}
45	✓✗✓✗ ✓✗✓✗	160	Base::set_u_lb(Scalar(-1.) * pinocchio_.effortLimit.tail(nu_));
46	✓✗✓✗ ✓✗✓✗	160	Base::set_u_ub(Scalar(+1.) * pinocchio_.effortLimit.tail(nu_));
47		160	}
48
49			template <typename Scalar>
50		324	DifferentialActionModelFreeFwdDynamicsTpl<
51		324	Scalar>::~DifferentialActionModelFreeFwdDynamicsTpl() {}
52
53			template <typename Scalar>
54		7780	void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calc(
55			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
56			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
57	✓✗✗✓	7780	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
58			throw_pretty("Invalid argument: "
59			<< "x has wrong dimension (it should be " +
60			std::to_string(state_->get_nx()) + ")");
61			}
62	✓✗✗✓	7780	if (static_cast<std::size_t>(u.size()) != nu_) {
63			throw_pretty("Invalid argument: "
64			<< "u has wrong dimension (it should be " +
65			std::to_string(nu_) + ")");
66			}
67
68		7780	Data* d = static_cast<Data*>(data.get());
69	✓✗	7780	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
70			x.head(state_->get_nq());
71	✓✗	7780	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
72			x.tail(state_->get_nv());
73
74	✓✗	7780	actuation_->calc(d->multibody.actuation, x, u);
75
76			// Computing the dynamics using ABA or manually for armature case
77	✓✓	7780	if (without_armature_) {
78	✓✗	7778	d->xout = pinocchio::aba(pinocchio_, d->pinocchio, q, v,
79	✓✗	7778	d->multibody.actuation->tau);
80	✓✗	7778	pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
81			} else {
82	✓✗	2	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
83	✓✗✓✗	2	d->pinocchio.M.diagonal() += armature_;
84	✓✗	2	pinocchio::cholesky::decompose(pinocchio_, d->pinocchio);
85	✓✗	2	d->Minv.setZero();
86	✓✗	2	pinocchio::cholesky::computeMinv(pinocchio_, d->pinocchio, d->Minv);
87	✓✗✓✗	2	d->u_drift = d->multibody.actuation->tau - d->pinocchio.nle;
88	✓✗✓✗ ✓✗	2	d->xout.noalias() = d->Minv * d->u_drift;
89			}
90	✓✗	7780	d->multibody.joint->a = d->xout;
91	✓✗	7780	d->multibody.joint->tau = u;
92	✓✗	7780	costs_->calc(d->costs, x, u);
93		7780	d->cost = d->costs->cost;
94	✓✓	7780	if (constraints_ != nullptr) {
95	✓✗	4829	d->constraints->resize(this, d);
96	✓✗	4829	constraints_->calc(d->constraints, x, u);
97			}
98		7780	}
99
100			template <typename Scalar>
101		688	void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calc(
102			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
103			const Eigen::Ref<const VectorXs>& x) {
104	✓✗✗✓	688	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
105			throw_pretty("Invalid argument: "
106			<< "x has wrong dimension (it should be " +
107			std::to_string(state_->get_nx()) + ")");
108			}
109
110		688	Data* d = static_cast<Data*>(data.get());
111	✓✗	688	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
112			x.head(state_->get_nq());
113	✓✗	688	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
114			x.tail(state_->get_nv());
115
116	✓✗	688	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
117
118	✓✗	688	costs_->calc(d->costs, x);
119		688	d->cost = d->costs->cost;
120	✓✓	688	if (constraints_ != nullptr) {
121	✓✗	394	d->constraints->resize(this, d);
122	✓✗	394	constraints_->calc(d->constraints, x);
123			}
124		688	}
125
126			template <typename Scalar>
127		2346	void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calcDiff(
128			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
129			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
130	✓✗✗✓	2346	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
131			throw_pretty("Invalid argument: "
132			<< "x has wrong dimension (it should be " +
133			std::to_string(state_->get_nx()) + ")");
134			}
135	✓✗✗✓	2346	if (static_cast<std::size_t>(u.size()) != nu_) {
136			throw_pretty("Invalid argument: "
137			<< "u has wrong dimension (it should be " +
138			std::to_string(nu_) + ")");
139			}
140
141		2346	const std::size_t nv = state_->get_nv();
142	✓✗	2346	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
143			x.head(state_->get_nq());
144	✓✗	2346	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
145			x.tail(nv);
146
147		2346	Data* d = static_cast<Data*>(data.get());
148
149	✓✗	2346	actuation_->calcDiff(d->multibody.actuation, x, u);
150
151			// Computing the dynamics derivatives
152	✓✓	2346	if (without_armature_) {
153			pinocchio::computeABADerivatives(
154	✓✗	2345	pinocchio_, d->pinocchio, q, v, d->multibody.actuation->tau,
155	✓✗✓✗	2345	d->Fx.leftCols(nv), d->Fx.rightCols(nv), d->pinocchio.Minv);
156	✓✗✓✗ ✓✗	2345	d->Fx.noalias() += d->pinocchio.Minv * d->multibody.actuation->dtau_dx;
157	✓✗✓✗ ✓✗	2345	d->Fu.noalias() = d->pinocchio.Minv * d->multibody.actuation->dtau_du;
158			} else {
159	✓✗	1	pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout);
160	✓✗✓✗ ✓✗	1	d->dtau_dx.leftCols(nv) =
161	✓✗	1	d->multibody.actuation->dtau_dx.leftCols(nv) - d->pinocchio.dtau_dq;
162	✓✗✓✗ ✓✗	1	d->dtau_dx.rightCols(nv) =
163	✓✗	1	d->multibody.actuation->dtau_dx.rightCols(nv) - d->pinocchio.dtau_dv;
164	✓✗✓✗ ✓✗	1	d->Fx.noalias() = d->Minv * d->dtau_dx;
165	✓✗✓✗ ✓✗	1	d->Fu.noalias() = d->Minv * d->multibody.actuation->dtau_du;
166			}
167	✓✗	2346	d->multibody.joint->da_dx = d->Fx;
168	✓✗	2346	d->multibody.joint->da_du = d->Fu;
169	✓✗	2346	costs_->calcDiff(d->costs, x, u);
170	✓✓	2346	if (constraints_ != nullptr) {
171	✓✗	1254	constraints_->calcDiff(d->constraints, x, u);
172			}
173		2346	}
174
175			template <typename Scalar>
176		136	void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calcDiff(
177			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
178			const Eigen::Ref<const VectorXs>& x) {
179	✗✓	136	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
180			throw_pretty("Invalid argument: "
181			<< "x has wrong dimension (it should be " +
182			std::to_string(state_->get_nx()) + ")");
183			}
184		136	Data* d = static_cast<Data*>(data.get());
185
186		136	costs_->calcDiff(d->costs, x);
187	✓✓	136	if (constraints_ != nullptr) {
188		44	constraints_->calcDiff(d->constraints, x);
189			}
190		136	}
191
192			template <typename Scalar>
193			boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
194		10259	DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::createData() {
195	✓✗	10259	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
196			}
197
198			template <typename Scalar>
199		1971	bool DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::checkData(
200			const boost::shared_ptr<DifferentialActionDataAbstract>& data) {
201		3942	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
202	✓✗	1971	if (d != NULL) {
203		1971	return true;
204			} else {
205			return false;
206			}
207			}
208
209			template <typename Scalar>
210		962	void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::quasiStatic(
211			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
212			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
213			const std::size_t, const Scalar) {
214	✓✗✗✓	962	if (static_cast<std::size_t>(u.size()) != nu_) {
215			throw_pretty("Invalid argument: "
216			<< "u has wrong dimension (it should be " +
217			std::to_string(nu_) + ")");
218			}
219	✓✗✗✓	962	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
220			throw_pretty("Invalid argument: "
221			<< "x has wrong dimension (it should be " +
222			std::to_string(state_->get_nx()) + ")");
223			}
224			// Static casting the data
225		962	Data* d = static_cast<Data*>(data.get());
226	✓✗	962	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
227			x.head(state_->get_nq());
228
229		962	const std::size_t nq = state_->get_nq();
230		962	const std::size_t nv = state_->get_nv();
231
232	✓✗✓✗	962	d->tmp_xstatic.head(nq) = q;
233	✓✗✓✗	962	d->tmp_xstatic.tail(nv).setZero();
234	✓✗	962	u.setZero();
235
236	✓✗✓✗	962	pinocchio::rnea(pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
237	✓✗	962	d->tmp_xstatic.tail(nv));
238	✓✗✓✗ ✓✗	962	actuation_->calc(d->multibody.actuation, d->tmp_xstatic, u);
239	✓✗✓✗ ✓✗	962	actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);
240
241	✓✗✓✗	962	u.noalias() =
242	✓✗✓✗	1924	pseudoInverse(d->multibody.actuation->dtau_du) * d->pinocchio.tau;
243	✓✗	962	d->pinocchio.tau.setZero();
244		962	}
245
246			template <typename Scalar>
247		49991	std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_ng() const {
248	✓✓	49991	if (constraints_ != nullptr) {
249		33508	return constraints_->get_ng();
250			} else {
251		16483	return Base::get_ng();
252			}
253			}
254
255			template <typename Scalar>
256		49991	std::size_t DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_nh() const {
257	✓✓	49991	if (constraints_ != nullptr) {
258		33508	return constraints_->get_nh();
259			} else {
260		16483	return Base::get_nh();
261			}
262			}
263
264			template <typename Scalar>
265			const typename MathBaseTpl<Scalar>::VectorXs&
266			DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_g_lb() const {
267			if (constraints_ != nullptr) {
268			return constraints_->get_lb();
269			} else {
270			return g_lb_;
271			}
272			}
273
274			template <typename Scalar>
275			const typename MathBaseTpl<Scalar>::VectorXs&
276			DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_g_ub() const {
277			if (constraints_ != nullptr) {
278			return constraints_->get_ub();
279			} else {
280			return g_lb_;
281			}
282			}
283
284			template <typename Scalar>
285		30	void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::print(
286			std::ostream& os) const {
287		30	os << "DifferentialActionModelFreeFwdDynamics {nx=" << state_->get_nx()
288		30	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_ << "}";
289		30	}
290
291			template <typename Scalar>
292			pinocchio::ModelTpl<Scalar>&
293		10259	DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_pinocchio() const {
294		10259	return pinocchio_;
295			}
296
297			template <typename Scalar>
298			const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
299		20518	DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_actuation() const {
300		20518	return actuation_;
301			}
302
303			template <typename Scalar>
304			const boost::shared_ptr<CostModelSumTpl<Scalar> >&
305		10259	DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_costs() const {
306		10259	return costs_;
307			}
308
309			template <typename Scalar>
310			const boost::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
311		16922	DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_constraints() const {
312		16922	return constraints_;
313			}
314
315			template <typename Scalar>
316			const typename MathBaseTpl<Scalar>::VectorXs&
317			DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_armature() const {
318			return armature_;
319			}
320
321			template <typename Scalar>
322		1	void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::set_armature(
323			const VectorXs& armature) {
324	✗✓	1	if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
325			throw_pretty("Invalid argument: "
326			<< "The armature dimension is wrong (it should be " +
327			std::to_string(state_->get_nv()) + ")");
328			}
329
330		1	armature_ = armature;
331		1	without_armature_ = false;
332		1	}
333
334			} // namespace crocoddyl