Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actions/contact-fwddyn.hxx	Lines:	171	210	81.4 %
Date:	2024-02-13 11:12:33	Branches:	178	564	31.6 %


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

#include <pinocchio/algorithm/centroidal.hpp>
#include <pinocchio/algorithm/compute-all-terms.hpp>
#include <pinocchio/algorithm/contact-dynamics.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/kinematics-derivatives.hpp>
#include <pinocchio/algorithm/rnea-derivatives.hpp>
#include <pinocchio/algorithm/rnea.hpp>

#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/core/utils/math.hpp"
#include "crocoddyl/multibody/actions/contact-fwddyn.hpp"

namespace crocoddyl {

template <typename Scalar>
DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
    DifferentialActionModelContactFwdDynamicsTpl(
        boost::shared_ptr<StateMultibody> state,
        boost::shared_ptr<ActuationModelAbstract> actuation,
        boost::shared_ptr<ContactModelMultiple> contacts,
        boost::shared_ptr<CostModelSum> costs, const Scalar JMinvJt_damping,
        const bool enable_force)
    : Base(state, actuation->get_nu(), costs->get_nr(), 0, 0),
      actuation_(actuation),
      contacts_(contacts),
      costs_(costs),
      constraints_(nullptr),
      pinocchio_(*state->get_pinocchio().get()),
      with_armature_(true),
      armature_(VectorXs::Zero(state->get_nv())),
      JMinvJt_damping_(fabs(JMinvJt_damping)),
      enable_force_(enable_force) {
  init();
}

template <typename Scalar>
DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
    DifferentialActionModelContactFwdDynamicsTpl(
        boost::shared_ptr<StateMultibody> state,
        boost::shared_ptr<ActuationModelAbstract> actuation,
        boost::shared_ptr<ContactModelMultiple> contacts,
        boost::shared_ptr<CostModelSum> costs,
        boost::shared_ptr<ConstraintModelManager> constraints,
        const Scalar JMinvJt_damping, const bool enable_force)
    : Base(state, actuation->get_nu(), costs->get_nr(), constraints->get_ng(),
           constraints->get_nh()),
      actuation_(actuation),
      contacts_(contacts),
      costs_(costs),
      constraints_(constraints),
      pinocchio_(*state->get_pinocchio().get()),
      with_armature_(true),
      armature_(VectorXs::Zero(state->get_nv())),
      JMinvJt_damping_(fabs(JMinvJt_damping)),
      enable_force_(enable_force) {
  init();
}

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

template <typename Scalar>
void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::init() {
  if (JMinvJt_damping_ < Scalar(0.)) {
    JMinvJt_damping_ = Scalar(0.);
    throw_pretty("Invalid argument: "
                 << "The damping factor has to be positive, set to 0");
  }
  if (contacts_->get_nu() != nu_) {
    throw_pretty(
        "Invalid argument: "
        << "Contacts doesn't have the same control dimension (it should be " +
               std::to_string(nu_) + ")");
  }
  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>
void DifferentialActionModelContactFwdDynamicsTpl<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_) + ")");
  }

  const std::size_t nc = contacts_->get_nc();
  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());

  // Computing the forward dynamics with the holonomic constraints defined by
  // the contact model
  pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
  pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);

  if (!with_armature_) {
    d->pinocchio.M.diagonal() += armature_;
  }
  actuation_->calc(d->multibody.actuation, x, u);
  contacts_->calc(d->multibody.contacts, x);

#ifndef NDEBUG
  Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.contacts->Jc.topRows(nc));

  if (Jc_lu.rank() < d->multibody.contacts->Jc.topRows(nc).rows() &&
      JMinvJt_damping_ == Scalar(0.)) {
    throw_pretty(
        "A damping factor is needed as the contact Jacobian is not full-rank");
  }
#endif

  pinocchio::forwardDynamics(
      pinocchio_, d->pinocchio, d->multibody.actuation->tau,
      d->multibody.contacts->Jc.topRows(nc), d->multibody.contacts->a0.head(nc),
      JMinvJt_damping_);
  d->xout = d->pinocchio.ddq;
  contacts_->updateAcceleration(d->multibody.contacts, d->pinocchio.ddq);
  contacts_->updateForce(d->multibody.contacts, d->pinocchio.lambda_c);
  d->multibody.joint->a = d->pinocchio.ddq;
  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 DifferentialActionModelContactFwdDynamicsTpl<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);
  pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
  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 DifferentialActionModelContactFwdDynamicsTpl<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 std::size_t nc = contacts_->get_nc();
  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());

  // Computing the dynamics derivatives
  // We resize the Kinv matrix because Eigen cannot call block operations
  // recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore,
  // it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc)
  d->Kinv.resize(nv + nc, nv + nc);
  pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout,
                                    d->multibody.contacts->fext);
  contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
  pinocchio::getKKTContactDynamicMatrixInverse(
      pinocchio_, d->pinocchio, d->multibody.contacts->Jc.topRows(nc), d->Kinv);

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

  const Eigen::Block<MatrixXs> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
  const Eigen::Block<MatrixXs> a_partial_da = d->Kinv.topRightCorner(nv, nc);
  const Eigen::Block<MatrixXs> f_partial_dtau =
      d->Kinv.bottomLeftCorner(nc, nv);
  const Eigen::Block<MatrixXs> f_partial_da = d->Kinv.bottomRightCorner(nc, nc);

  d->Fx.leftCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
  d->Fx.rightCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dv;
  d->Fx.noalias() -= a_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
  d->Fx.noalias() += a_partial_dtau * d->multibody.actuation->dtau_dx;
  d->Fu.noalias() = a_partial_dtau * d->multibody.actuation->dtau_du;
  d->multibody.joint->da_dx = d->Fx;
  d->multibody.joint->da_du = d->Fu;

  // Computing the cost derivatives
  if (enable_force_) {
    d->df_dx.topLeftCorner(nc, nv).noalias() =
        f_partial_dtau * d->pinocchio.dtau_dq;
    d->df_dx.topRightCorner(nc, nv).noalias() =
        f_partial_dtau * d->pinocchio.dtau_dv;
    d->df_dx.topRows(nc).noalias() +=
        f_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
    d->df_dx.topRows(nc).noalias() -=
        f_partial_dtau * d->multibody.actuation->dtau_dx;
    d->df_du.topRows(nc).noalias() =
        -f_partial_dtau * d->multibody.actuation->dtau_du;
    contacts_->updateAccelerationDiff(d->multibody.contacts,
                                      d->Fx.bottomRows(nv));
    contacts_->updateForceDiff(d->multibody.contacts, d->df_dx.topRows(nc),
                               d->df_du.topRows(nc));
  }
  costs_->calcDiff(d->costs, x, u);
  if (constraints_ != nullptr) {
    constraints_->calcDiff(d->constraints, x, u);
  }
}

template <typename Scalar>
void DifferentialActionModelContactFwdDynamicsTpl<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> >
DifferentialActionModelContactFwdDynamicsTpl<Scalar>::createData() {
  return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
}

template <typename Scalar>
void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::quasiStatic(
    const boost::shared_ptr<DifferentialActionDataAbstract>& data,
    Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
    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
  DifferentialActionDataContactFwdDynamicsTpl<Scalar>* d =
      static_cast<DifferentialActionDataContactFwdDynamicsTpl<Scalar>*>(
          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();
  const std::size_t nc = contacts_->get_nc();

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

  pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q,
                             d->tmp_xstatic.tail(nv));
  pinocchio::computeJointJacobians(pinocchio_, d->pinocchio, q);
  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);
  contacts_->calc(d->multibody.contacts, d->tmp_xstatic);

  // Allocates memory
  d->tmp_Jstatic.conservativeResize(nv, nu_ + nc);
  d->tmp_Jstatic.leftCols(nu_) = d->multibody.actuation->dtau_du;
  d->tmp_Jstatic.rightCols(nc) =
      d->multibody.contacts->Jc.topRows(nc).transpose();
  u.noalias() = (pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau).head(nu_);
  d->pinocchio.tau.setZero();
}

template <typename Scalar>
bool DifferentialActionModelContactFwdDynamicsTpl<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 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::print(
    std::ostream& os) const {
  os << "DifferentialActionModelContactFwdDynamics {nx=" << state_->get_nx()
     << ", ndx=" << state_->get_ndx() << ", nu=" << nu_
     << ", nc=" << contacts_->get_nc() << "}";
}

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

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

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

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

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

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

template <typename Scalar>
const boost::shared_ptr<ContactModelMultipleTpl<Scalar> >&
DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_contacts() const {
  return contacts_;
}

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

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

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

template <typename Scalar>
const Scalar DifferentialActionModelContactFwdDynamicsTpl<
    Scalar>::get_damping_factor() const {
  return JMinvJt_damping_;
}

template <typename Scalar>
void DifferentialActionModelContactFwdDynamicsTpl<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;
  with_armature_ = false;
}

template <typename Scalar>
void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_damping_factor(
    const Scalar damping) {
  if (damping < 0.) {
    throw_pretty("Invalid argument: "
                 << "The damping factor has to be positive");
  }
  JMinvJt_damping_ = damping;
}

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2023, LAAS-CNRS, University of Edinburgh, CTU, INRIA,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include <pinocchio/algorithm/centroidal.hpp>
11			#include <pinocchio/algorithm/compute-all-terms.hpp>
12			#include <pinocchio/algorithm/contact-dynamics.hpp>
13			#include <pinocchio/algorithm/frames.hpp>
14			#include <pinocchio/algorithm/kinematics-derivatives.hpp>
15			#include <pinocchio/algorithm/rnea-derivatives.hpp>
16			#include <pinocchio/algorithm/rnea.hpp>
17
18			#include "crocoddyl/core/utils/exception.hpp"
19			#include "crocoddyl/core/utils/math.hpp"
20			#include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
21
22			namespace crocoddyl {
23
24			template <typename Scalar>
25		449	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
26			DifferentialActionModelContactFwdDynamicsTpl(
27			boost::shared_ptr<StateMultibody> state,
28			boost::shared_ptr<ActuationModelAbstract> actuation,
29			boost::shared_ptr<ContactModelMultiple> contacts,
30			boost::shared_ptr<CostModelSum> costs, const Scalar JMinvJt_damping,
31			const bool enable_force)
32			: Base(state, actuation->get_nu(), costs->get_nr(), 0, 0),
33			actuation_(actuation),
34			contacts_(contacts),
35			costs_(costs),
36			constraints_(nullptr),
37		449	pinocchio_(*state->get_pinocchio().get()),
38			with_armature_(true),
39		449	armature_(VectorXs::Zero(state->get_nv())),
40		449	JMinvJt_damping_(fabs(JMinvJt_damping)),
41	✓✗✓✗ ✓✗	1347	enable_force_(enable_force) {
42	✓✗	449	init();
43		449	}
44
45			template <typename Scalar>
46		13	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
47			DifferentialActionModelContactFwdDynamicsTpl(
48			boost::shared_ptr<StateMultibody> state,
49			boost::shared_ptr<ActuationModelAbstract> actuation,
50			boost::shared_ptr<ContactModelMultiple> contacts,
51			boost::shared_ptr<CostModelSum> costs,
52			boost::shared_ptr<ConstraintModelManager> constraints,
53			const Scalar JMinvJt_damping, const bool enable_force)
54			: Base(state, actuation->get_nu(), costs->get_nr(), constraints->get_ng(),
55			constraints->get_nh()),
56			actuation_(actuation),
57			contacts_(contacts),
58			costs_(costs),
59			constraints_(constraints),
60		13	pinocchio_(*state->get_pinocchio().get()),
61			with_armature_(true),
62		13	armature_(VectorXs::Zero(state->get_nv())),
63		13	JMinvJt_damping_(fabs(JMinvJt_damping)),
64	✓✗✓✗ ✓✗	39	enable_force_(enable_force) {
65	✓✗	13	init();
66		13	}
67
68			template <typename Scalar>
69		928	DifferentialActionModelContactFwdDynamicsTpl<
70		928	Scalar>::~DifferentialActionModelContactFwdDynamicsTpl() {}
71
72			template <typename Scalar>
73		462	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::init() {
74	✗✓	462	if (JMinvJt_damping_ < Scalar(0.)) {
75			JMinvJt_damping_ = Scalar(0.);
76			throw_pretty("Invalid argument: "
77			<< "The damping factor has to be positive, set to 0");
78			}
79	✗✓	462	if (contacts_->get_nu() != nu_) {
80			throw_pretty(
81			"Invalid argument: "
82			<< "Contacts doesn't have the same control dimension (it should be " +
83			std::to_string(nu_) + ")");
84			}
85	✗✓	462	if (costs_->get_nu() != nu_) {
86			throw_pretty(
87			"Invalid argument: "
88			<< "Costs doesn't have the same control dimension (it should be " +
89			std::to_string(nu_) + ")");
90			}
91
92	✓✗✓✗ ✓✗	462	Base::set_u_lb(Scalar(-1.) * pinocchio_.effortLimit.tail(nu_));
93	✓✗✓✗ ✓✗	462	Base::set_u_ub(Scalar(+1.) * pinocchio_.effortLimit.tail(nu_));
94		462	}
95
96			template <typename Scalar>
97		32220	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calc(
98			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
99			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
100	✓✗✗✓	32220	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
101			throw_pretty("Invalid argument: "
102			<< "x has wrong dimension (it should be " +
103			std::to_string(state_->get_nx()) + ")");
104			}
105	✓✗✗✓	32220	if (static_cast<std::size_t>(u.size()) != nu_) {
106			throw_pretty("Invalid argument: "
107			<< "u has wrong dimension (it should be " +
108			std::to_string(nu_) + ")");
109			}
110
111		32220	const std::size_t nc = contacts_->get_nc();
112		32220	Data* d = static_cast<Data*>(data.get());
113	✓✗	32220	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
114			x.head(state_->get_nq());
115	✓✗	32220	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
116			x.tail(state_->get_nv());
117
118			// Computing the forward dynamics with the holonomic constraints defined by
119			// the contact model
120	✓✗	32220	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
121	✓✗	32220	pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
122
123	✗✓	32220	if (!with_armature_) {
124			d->pinocchio.M.diagonal() += armature_;
125			}
126	✓✗	32220	actuation_->calc(d->multibody.actuation, x, u);
127	✓✗	32220	contacts_->calc(d->multibody.contacts, x);
128
129			#ifndef NDEBUG
130	✓✗✓✗	64440	Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.contacts->Jc.topRows(nc));
131
132	✓✗✓✗ ✗✓✗✓ ✗✗	32220	if (Jc_lu.rank() < d->multibody.contacts->Jc.topRows(nc).rows() &&
133			JMinvJt_damping_ == Scalar(0.)) {
134			throw_pretty(
135			"A damping factor is needed as the contact Jacobian is not full-rank");
136			}
137			#endif
138
139			pinocchio::forwardDynamics(
140	✓✗	32220	pinocchio_, d->pinocchio, d->multibody.actuation->tau,
141	✓✗✓✗	32220	d->multibody.contacts->Jc.topRows(nc), d->multibody.contacts->a0.head(nc),
142			JMinvJt_damping_);
143	✓✗	32220	d->xout = d->pinocchio.ddq;
144	✓✗	32220	contacts_->updateAcceleration(d->multibody.contacts, d->pinocchio.ddq);
145	✓✗	32220	contacts_->updateForce(d->multibody.contacts, d->pinocchio.lambda_c);
146	✓✗	32220	d->multibody.joint->a = d->pinocchio.ddq;
147	✓✗	32220	d->multibody.joint->tau = u;
148	✓✗	32220	costs_->calc(d->costs, x, u);
149		32220	d->cost = d->costs->cost;
150	✓✓	32220	if (constraints_ != nullptr) {
151	✓✗	1160	d->constraints->resize(this, d);
152	✓✗	1160	constraints_->calc(d->constraints, x, u);
153			}
154		32220	}
155
156			template <typename Scalar>
157		7114	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calc(
158			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
159			const Eigen::Ref<const VectorXs>& x) {
160	✓✗✗✓	7114	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
161			throw_pretty("Invalid argument: "
162			<< "x has wrong dimension (it should be " +
163			std::to_string(state_->get_nx()) + ")");
164			}
165
166		7114	Data* d = static_cast<Data*>(data.get());
167	✓✗	7114	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
168			x.head(state_->get_nq());
169	✓✗	7114	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
170			x.tail(state_->get_nv());
171
172	✓✗	7114	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
173	✓✗	7114	pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
174	✓✗	7114	costs_->calc(d->costs, x);
175		7114	d->cost = d->costs->cost;
176	✓✓	7114	if (constraints_ != nullptr) {
177	✓✗	834	d->constraints->resize(this, d);
178	✓✗	834	constraints_->calc(d->constraints, x);
179			}
180		7114	}
181
182			template <typename Scalar>
183		5077	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
184			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
185			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
186	✓✗✗✓	5077	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
187			throw_pretty("Invalid argument: "
188			<< "x has wrong dimension (it should be " +
189			std::to_string(state_->get_nx()) + ")");
190			}
191	✓✗✗✓	5077	if (static_cast<std::size_t>(u.size()) != nu_) {
192			throw_pretty("Invalid argument: "
193			<< "u has wrong dimension (it should be " +
194			std::to_string(nu_) + ")");
195			}
196
197		5077	const std::size_t nv = state_->get_nv();
198		5077	const std::size_t nc = contacts_->get_nc();
199	✓✗	5077	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
200			x.head(state_->get_nq());
201	✓✗	5077	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
202			x.tail(nv);
203
204		5077	Data* d = static_cast<Data*>(data.get());
205
206			// Computing the dynamics derivatives
207			// We resize the Kinv matrix because Eigen cannot call block operations
208			// recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore,
209			// it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc)
210	✓✗	5077	d->Kinv.resize(nv + nc, nv + nc);
211		5077	pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout,
212	✓✗	5077	d->multibody.contacts->fext);
213	✓✗	5077	contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
214			pinocchio::getKKTContactDynamicMatrixInverse(
215	✓✗✓✗	5077	pinocchio_, d->pinocchio, d->multibody.contacts->Jc.topRows(nc), d->Kinv);
216
217	✓✗	5077	actuation_->calcDiff(d->multibody.actuation, x, u);
218	✓✗	5077	contacts_->calcDiff(d->multibody.contacts, x);
219
220	✓✗	5077	const Eigen::Block<MatrixXs> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
221	✓✗	5077	const Eigen::Block<MatrixXs> a_partial_da = d->Kinv.topRightCorner(nv, nc);
222		5077	const Eigen::Block<MatrixXs> f_partial_dtau =
223	✓✗	5077	d->Kinv.bottomLeftCorner(nc, nv);
224	✓✗	5077	const Eigen::Block<MatrixXs> f_partial_da = d->Kinv.bottomRightCorner(nc, nc);
225
226	✓✗✓✗ ✓✗✓✗ ✓✗	5077	d->Fx.leftCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
227	✓✗✓✗ ✓✗✓✗ ✓✗	5077	d->Fx.rightCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dv;
228	✓✗✓✗ ✓✗✓✗	5077	d->Fx.noalias() -= a_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
229	✓✗✓✗ ✓✗	5077	d->Fx.noalias() += a_partial_dtau * d->multibody.actuation->dtau_dx;
230	✓✗✓✗ ✓✗	5077	d->Fu.noalias() = a_partial_dtau * d->multibody.actuation->dtau_du;
231	✓✗	5077	d->multibody.joint->da_dx = d->Fx;
232	✓✗	5077	d->multibody.joint->da_du = d->Fu;
233
234			// Computing the cost derivatives
235	✓✗	5077	if (enable_force_) {
236	✓✗✓✗ ✓✗	5077	d->df_dx.topLeftCorner(nc, nv).noalias() =
237	✓✗	5077	f_partial_dtau * d->pinocchio.dtau_dq;
238	✓✗✓✗ ✓✗	5077	d->df_dx.topRightCorner(nc, nv).noalias() =
239	✓✗	5077	f_partial_dtau * d->pinocchio.dtau_dv;
240	✓✗✓✗ ✓✗	5077	d->df_dx.topRows(nc).noalias() +=
241	✓✗✓✗	5077	f_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
242	✓✗✓✗ ✓✗	5077	d->df_dx.topRows(nc).noalias() -=
243	✓✗	5077	f_partial_dtau * d->multibody.actuation->dtau_dx;
244	✓✗✓✗ ✓✗✓✗	5077	d->df_du.topRows(nc).noalias() =
245	✓✗	5077	-f_partial_dtau * d->multibody.actuation->dtau_du;
246	✓✗✓✗	10154	contacts_->updateAccelerationDiff(d->multibody.contacts,
247	✓✗	5077	d->Fx.bottomRows(nv));
248	✓✗✓✗ ✓✗✓✗	10154	contacts_->updateForceDiff(d->multibody.contacts, d->df_dx.topRows(nc),
249	✓✗	5077	d->df_du.topRows(nc));
250			}
251	✓✗	5077	costs_->calcDiff(d->costs, x, u);
252	✓✓	5077	if (constraints_ != nullptr) {
253	✓✗	13	constraints_->calcDiff(d->constraints, x, u);
254			}
255		5077	}
256
257			template <typename Scalar>
258		237	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
259			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
260			const Eigen::Ref<const VectorXs>& x) {
261	✗✓	237	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
262			throw_pretty("Invalid argument: "
263			<< "x has wrong dimension (it should be " +
264			std::to_string(state_->get_nx()) + ")");
265			}
266		237	Data* d = static_cast<Data*>(data.get());
267		237	costs_->calcDiff(d->costs, x);
268	✓✓	237	if (constraints_ != nullptr) {
269		13	constraints_->calcDiff(d->constraints, x);
270			}
271		237	}
272
273			template <typename Scalar>
274			boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
275		39554	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::createData() {
276	✓✗	39554	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
277			}
278
279			template <typename Scalar>
280		2572	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::quasiStatic(
281			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
282			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
283			Scalar) {
284	✓✗✗✓	2572	if (static_cast<std::size_t>(u.size()) != nu_) {
285			throw_pretty("Invalid argument: "
286			<< "u has wrong dimension (it should be " +
287			std::to_string(nu_) + ")");
288			}
289	✓✗✗✓	2572	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
290			throw_pretty("Invalid argument: "
291			<< "x has wrong dimension (it should be " +
292			std::to_string(state_->get_nx()) + ")");
293			}
294			// Static casting the data
295			DifferentialActionDataContactFwdDynamicsTpl<Scalar>* d =
296		2572	static_cast<DifferentialActionDataContactFwdDynamicsTpl<Scalar>*>(
297			data.get());
298	✓✗	2572	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
299			x.head(state_->get_nq());
300
301		2572	const std::size_t nq = state_->get_nq();
302		2572	const std::size_t nv = state_->get_nv();
303		2572	const std::size_t nc = contacts_->get_nc();
304
305	✓✗✓✗	2572	d->tmp_xstatic.head(nq) = q;
306	✓✗✓✗	2572	d->tmp_xstatic.tail(nv).setZero();
307	✓✗	2572	u.setZero();
308
309	✓✗	2572	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q,
310	✓✗	2572	d->tmp_xstatic.tail(nv));
311	✓✗	2572	pinocchio::computeJointJacobians(pinocchio_, d->pinocchio, q);
312	✓✗✓✗	2572	pinocchio::rnea(pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
313	✓✗	2572	d->tmp_xstatic.tail(nv));
314	✓✗✓✗ ✓✗	2572	actuation_->calc(d->multibody.actuation, d->tmp_xstatic, u);
315	✓✗✓✗ ✓✗	2572	actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);
316	✓✗✓✗	2572	contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
317
318			// Allocates memory
319	✓✗	2572	d->tmp_Jstatic.conservativeResize(nv, nu_ + nc);
320	✓✗✓✗	2572	d->tmp_Jstatic.leftCols(nu_) = d->multibody.actuation->dtau_du;
321	✓✗✓✗ ✓✗	2572	d->tmp_Jstatic.rightCols(nc) =
322	✓✗	2572	d->multibody.contacts->Jc.topRows(nc).transpose();
323	✓✗✓✗ ✓✗✓✗ ✓✗	2572	u.noalias() = (pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau).head(nu_);
324	✓✗	2572	d->pinocchio.tau.setZero();
325		2572	}
326
327			template <typename Scalar>
328		5256	bool DifferentialActionModelContactFwdDynamicsTpl<Scalar>::checkData(
329			const boost::shared_ptr<DifferentialActionDataAbstract>& data) {
330		10512	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
331	✓✗	5256	if (d != NULL) {
332		5256	return true;
333			} else {
334			return false;
335			}
336			}
337
338			template <typename Scalar>
339		80	void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::print(
340			std::ostream& os) const {
341		80	os << "DifferentialActionModelContactFwdDynamics {nx=" << state_->get_nx()
342		80	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_
343		80	<< ", nc=" << contacts_->get_nc() << "}";
344		80	}
345
346			template <typename Scalar>
347		161439	std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_ng()
348			const {
349	✓✓	161439	if (constraints_ != nullptr) {
350		5513	return constraints_->get_ng();
351			} else {
352		155926	return Base::get_ng();
353			}
354			}
355
356			template <typename Scalar>
357		161439	std::size_t DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_nh()
358			const {
359	✓✓	161439	if (constraints_ != nullptr) {
360		5513	return constraints_->get_nh();
361			} else {
362		155926	return Base::get_nh();
363			}
364			}
365
366			template <typename Scalar>
367			const typename MathBaseTpl<Scalar>::VectorXs&
368			DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_g_lb() const {
369			if (constraints_ != nullptr) {
370			return constraints_->get_lb();
371			} else {
372			return g_lb_;
373			}
374			}
375
376			template <typename Scalar>
377			const typename MathBaseTpl<Scalar>::VectorXs&
378			DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_g_ub() const {
379			if (constraints_ != nullptr) {
380			return constraints_->get_ub();
381			} else {
382			return g_lb_;
383			}
384			}
385
386			template <typename Scalar>
387			pinocchio::ModelTpl<Scalar>&
388		39554	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_pinocchio() const {
389		39554	return pinocchio_;
390			}
391
392			template <typename Scalar>
393			const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
394		79156	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_actuation() const {
395		79156	return actuation_;
396			}
397
398			template <typename Scalar>
399			const boost::shared_ptr<ContactModelMultipleTpl<Scalar> >&
400		276886	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_contacts() const {
401		276886	return contacts_;
402			}
403
404			template <typename Scalar>
405			const boost::shared_ptr<CostModelSumTpl<Scalar> >&
406		39782	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_costs() const {
407		39782	return costs_;
408			}
409
410			template <typename Scalar>
411			const boost::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
412		40714	DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_constraints() const {
413		40714	return constraints_;
414			}
415
416			template <typename Scalar>
417			const typename MathBaseTpl<Scalar>::VectorXs&
418			DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_armature() const {
419			return armature_;
420			}
421
422			template <typename Scalar>
423			const Scalar DifferentialActionModelContactFwdDynamicsTpl<
424			Scalar>::get_damping_factor() const {
425			return JMinvJt_damping_;
426			}
427
428			template <typename Scalar>
429			void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_armature(
430			const VectorXs& armature) {
431			if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
432			throw_pretty("Invalid argument: "
433			<< "The armature dimension is wrong (it should be " +
434			std::to_string(state_->get_nv()) + ")");
435			}
436			armature_ = armature;
437			with_armature_ = false;
438			}
439
440			template <typename Scalar>
441			void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_damping_factor(
442			const Scalar damping) {
443			if (damping < 0.) {
444			throw_pretty("Invalid argument: "
445			<< "The damping factor has to be positive");
446			}
447			JMinvJt_damping_ = damping;
448			}
449
450			} // namespace crocoddyl