Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actions/contact-invdyn.hxx	Lines:	203	231	87.9 %
Date:	2024-02-13 11:12:33	Branches:	184	530	34.7 %


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

#include <pinocchio/algorithm/center-of-mass.hpp>
#include <pinocchio/algorithm/centroidal.hpp>
#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"
#include "crocoddyl/core/utils/exception.hpp"
#include "crocoddyl/core/utils/math.hpp"
#include "crocoddyl/multibody/residuals/contact-force.hpp"

namespace crocoddyl {

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

template <typename Scalar>
DifferentialActionModelContactInvDynamicsTpl<Scalar>::
    DifferentialActionModelContactInvDynamicsTpl(
        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)
    : Base(state, state->get_nv() + contacts->get_nc_total(), costs->get_nr(),
           constraints->get_ng(),
           state->get_nv() - actuation->get_nu() + contacts->get_nc_total() +
               constraints->get_nh()),
      actuation_(actuation),
      contacts_(contacts),
      costs_(costs),
      constraints_(constraints),
      pinocchio_(*state->get_pinocchio().get()) {
  init(state);
}

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

template <typename Scalar>
void DifferentialActionModelContactInvDynamicsTpl<Scalar>::init(
    const boost::shared_ptr<StateMultibody>& state) {
  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_) + ")");
  }
  const std::size_t nu = actuation_->get_nu();
  const std::size_t nc = contacts_->get_nc_total();
  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);
  contacts_->setComputeAllContacts(true);

  if (state_->get_nv() - actuation_->get_nu() > 0) {
    constraints_->addConstraint(
        "tau",
        boost::make_shared<ConstraintModelResidual>(
            state_, boost::make_shared<
                        typename DifferentialActionModelContactInvDynamicsTpl<
                            Scalar>::ResidualModelActuation>(state, nu, nc)));
  }
  if (contacts_->get_nc_total() != 0) {
    typename ContactModelMultiple::ContactModelContainer contact_list;
    contact_list = contacts_->get_contacts();
    typename ContactModelMultiple::ContactModelContainer::iterator it_m, end_m;
    for (it_m = contact_list.begin(), end_m = contact_list.end(); it_m != end_m;
         ++it_m) {
      const boost::shared_ptr<ContactItem>& contact = it_m->second;
      const std::string name = contact->name;
      const pinocchio::FrameIndex id = contact->contact->get_id();
      const std::size_t nc_i = contact->contact->get_nc();
      const bool active = contact->active;
      constraints_->addConstraint(
          name + "_acc",
          boost::make_shared<ConstraintModelResidual>(
              state_,
              boost::make_shared<
                  typename DifferentialActionModelContactInvDynamicsTpl<
                      Scalar>::ResidualModelContact>(state, id, nc_i, nc)),
          active);
      constraints_->addConstraint(
          name + "_force",
          boost::make_shared<ConstraintModelResidual>(
              state_, boost::make_shared<ResidualModelContactForceTpl<Scalar> >(
                          state, id, pinocchio::ForceTpl<Scalar>::Zero(), nc_i,
                          nu_, false)),
          !active);
    }
  }
}

template <typename Scalar>
void DifferentialActionModelContactInvDynamicsTpl<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 std::size_t nc = contacts_->get_nc_total();
  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);
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
      u.head(nv);
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
      f_ext = u.tail(nc);

  d->xout = a;
  pinocchio::forwardKinematics(pinocchio_, d->pinocchio, q, v, a);
  pinocchio::computeJointJacobians(pinocchio_, d->pinocchio);
  contacts_->calc(d->multibody.contacts, x);
  contacts_->updateForce(d->multibody.contacts, f_ext);
  pinocchio::rnea(pinocchio_, d->pinocchio, q, v, a,
                  d->multibody.contacts->fext);
  pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
  pinocchio::centerOfMass(pinocchio_, d->pinocchio, q, v, a);
  actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
  d->multibody.joint->a = a;
  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;
  for (std::string name : contacts_->get_active_set()) {
    constraints_->changeConstraintStatus(name + "_acc", true);
    constraints_->changeConstraintStatus(name + "_force", false);
  }
  for (std::string name : contacts_->get_inactive_set()) {
    constraints_->changeConstraintStatus(name + "_acc", false);
    constraints_->changeConstraintStatus(name + "_force", true);
  }
  d->constraints->resize(this, d);
  constraints_->calc(d->constraints, x, u);
}

template <typename Scalar>
void DifferentialActionModelContactInvDynamicsTpl<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;
  d->constraints->resize(this, d);
  constraints_->calc(d->constraints, x);
}

template <typename Scalar>
void DifferentialActionModelContactInvDynamicsTpl<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 std::size_t nc = contacts_->get_nc_total();
  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);
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
      u.head(nv);

  pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, a,
                                    d->multibody.contacts->fext);
  contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
  d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
      d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
          .transpose();
  pinocchio::jacobianCenterOfMass(pinocchio_, d->pinocchio, false);
  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.leftCols(nv).noalias() =
      d->multibody.actuation->Mtau * d->pinocchio.M;
  d->multibody.joint->dtau_du.rightCols(nc).noalias() =
      -d->multibody.actuation->Mtau *
      d->multibody.contacts->Jc.topRows(nc).transpose();
  contacts_->calcDiff(d->multibody.contacts, x);
  costs_->calcDiff(d->costs, x, u);
  for (std::string name : contacts_->get_active_set()) {
    constraints_->changeConstraintStatus(name + "_acc", true);
    constraints_->changeConstraintStatus(name + "_force", false);
  }
  for (std::string name : contacts_->get_inactive_set()) {
    constraints_->changeConstraintStatus(name + "_acc", false);
    constraints_->changeConstraintStatus(name + "_force", true);
  }
  d->constraints->resize(this, d);
  constraints_->calcDiff(d->constraints, x, u);
}

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

template <typename Scalar>
void DifferentialActionModelContactInvDynamicsTpl<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()) + ")");
  }
  Data* d = static_cast<Data*>(data.get());
  const std::size_t nq = state_->get_nq();
  const std::size_t nv = state_->get_nv();
  const std::size_t nu = actuation_->get_nu();
  std::size_t nc = contacts_->get_nc_total();
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
      x.head(nq);
  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,
                   d->tmp_xstatic.tail(nu));
  actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic,
                       d->tmp_xstatic.tail(nu));
  contacts_->setComputeAllContacts(false);
  contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
  contacts_->setComputeAllContacts(true);

  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();
  d->tmp_rstatic.noalias() = pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau;
  if (nc != 0) {
    nc = 0;
    std::size_t nc_r = 0;
    for (typename ContactModelMultiple::ContactModelContainer::const_iterator
             it_m = contacts_->get_contacts().begin();
         it_m != contacts_->get_contacts().end(); ++it_m) {
      const boost::shared_ptr<ContactItem>& m_i = it_m->second;
      const std::size_t nc_i = m_i->contact->get_nc();
      if (m_i->active) {
        u.segment(nv + nc, nc_i) = d->tmp_rstatic.segment(nu + nc_r, nc_i);
        nc_r += nc_i;
      } else {
        u.segment(nv + nc, nc_i).setZero();
      }
      nc += nc_i;
    }
  }
  d->pinocchio.tau.setZero();
}

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

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

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

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

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

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

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

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

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

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

}  // namespace crocoddyl


Generated by: GCOVR (Version 4.2)

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2023, 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/center-of-mass.hpp>
10			#include <pinocchio/algorithm/centroidal.hpp>
11			#include <pinocchio/algorithm/compute-all-terms.hpp>
12			#include <pinocchio/algorithm/jacobian.hpp>
13			#include <pinocchio/algorithm/kinematics.hpp>
14			#include <pinocchio/algorithm/rnea-derivatives.hpp>
15			#include <pinocchio/algorithm/rnea.hpp>
16
17			#include "crocoddyl/core/constraints/residual.hpp"
18			#include "crocoddyl/core/utils/exception.hpp"
19			#include "crocoddyl/core/utils/math.hpp"
20			#include "crocoddyl/multibody/residuals/contact-force.hpp"
21
22			namespace crocoddyl {
23
24			template <typename Scalar>
25		301	DifferentialActionModelContactInvDynamicsTpl<Scalar>::
26			DifferentialActionModelContactInvDynamicsTpl(
27			boost::shared_ptr<StateMultibody> state,
28			boost::shared_ptr<ActuationModelAbstract> actuation,
29			boost::shared_ptr<ContactModelMultiple> contacts,
30			boost::shared_ptr<CostModelSum> costs)
31		301	: Base(state, state->get_nv() + contacts->get_nc_total(), costs->get_nr(),
32		301	0, state->get_nv() - actuation->get_nu() + contacts->get_nc_total()),
33			actuation_(actuation),
34			contacts_(contacts),
35			costs_(costs),
36			constraints_(boost::make_shared<ConstraintModelManager>(
37		301	state, state->get_nv() + contacts->get_nc_total())),
38	✓✗✓✗	1204	pinocchio_(*state->get_pinocchio().get()) {
39	✓✗	301	init(state);
40		301	}
41
42			template <typename Scalar>
43			DifferentialActionModelContactInvDynamicsTpl<Scalar>::
44			DifferentialActionModelContactInvDynamicsTpl(
45			boost::shared_ptr<StateMultibody> state,
46			boost::shared_ptr<ActuationModelAbstract> actuation,
47			boost::shared_ptr<ContactModelMultiple> contacts,
48			boost::shared_ptr<CostModelSum> costs,
49			boost::shared_ptr<ConstraintModelManager> constraints)
50			: Base(state, state->get_nv() + contacts->get_nc_total(), costs->get_nr(),
51			constraints->get_ng(),
52			state->get_nv() - actuation->get_nu() + contacts->get_nc_total() +
53			constraints->get_nh()),
54			actuation_(actuation),
55			contacts_(contacts),
56			costs_(costs),
57			constraints_(constraints),
58			pinocchio_(*state->get_pinocchio().get()) {
59			init(state);
60			}
61
62			template <typename Scalar>
63		606	DifferentialActionModelContactInvDynamicsTpl<
64		606	Scalar>::~DifferentialActionModelContactInvDynamicsTpl() {}
65
66			template <typename Scalar>
67		301	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::init(
68			const boost::shared_ptr<StateMultibody>& state) {
69	✗✓	301	if (contacts_->get_nu() != nu_) {
70			throw_pretty(
71			"Invalid argument: "
72			<< "Contacts doesn't have the same control dimension (it should be " +
73			std::to_string(nu_) + ")");
74			}
75	✗✓	301	if (costs_->get_nu() != nu_) {
76			throw_pretty(
77			"Invalid argument: "
78			<< "Costs doesn't have the same control dimension (it should be " +
79			std::to_string(nu_) + ")");
80			}
81		301	const std::size_t nu = actuation_->get_nu();
82		301	const std::size_t nc = contacts_->get_nc_total();
83	✓✗	602	VectorXs lb =
84	✓✗	301	VectorXs::Constant(nu_, -std::numeric_limits<Scalar>::infinity());
85	✓✗	602	VectorXs ub =
86	✓✗	301	VectorXs::Constant(nu_, std::numeric_limits<Scalar>::infinity());
87	✓✗	301	Base::set_u_lb(lb);
88	✓✗	301	Base::set_u_ub(ub);
89		301	contacts_->setComputeAllContacts(true);
90
91	✓✓	301	if (state_->get_nv() - actuation_->get_nu() > 0) {
92	✓✗✓✗ ✓✗	502	constraints_->addConstraint(
93			"tau",
94			boost::make_shared<ConstraintModelResidual>(
95	✓✗	251	state_, boost::make_shared<
96			typename DifferentialActionModelContactInvDynamicsTpl<
97			Scalar>::ResidualModelActuation>(state, nu, nc)));
98			}
99	✓✓	301	if (contacts_->get_nc_total() != 0) {
100		600	typename ContactModelMultiple::ContactModelContainer contact_list;
101	✓✗	300	contact_list = contacts_->get_contacts();
102		300	typename ContactModelMultiple::ContactModelContainer::iterator it_m, end_m;
103	✓✓	1000	for (it_m = contact_list.begin(), end_m = contact_list.end(); it_m != end_m;
104		700	++it_m) {
105		700	const boost::shared_ptr<ContactItem>& contact = it_m->second;
106	✓✗	700	const std::string name = contact->name;
107		700	const pinocchio::FrameIndex id = contact->contact->get_id();
108		700	const std::size_t nc_i = contact->contact->get_nc();
109		700	const bool active = contact->active;
110	✓✗✓✗ ✓✗	1400	constraints_->addConstraint(
111			name + "_acc",
112			boost::make_shared<ConstraintModelResidual>(
113	✓✗	700	state_,
114			boost::make_shared<
115			typename DifferentialActionModelContactInvDynamicsTpl<
116			Scalar>::ResidualModelContact>(state, id, nc_i, nc)),
117			active);
118	✓✗✓✗ ✓✗	2100	constraints_->addConstraint(
119			name + "_force",
120			boost::make_shared<ConstraintModelResidual>(
121	✓✗	700	state_, boost::make_shared<ResidualModelContactForceTpl<Scalar> >(
122			state, id, pinocchio::ForceTpl<Scalar>::Zero(), nc_i,
123		700	nu_, false)),
124	✓✗	700	!active);
125			}
126			}
127		301	}
128
129			template <typename Scalar>
130		25930	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calc(
131			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
132			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
133	✓✗✗✓	25930	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
134			throw_pretty("Invalid argument: "
135			<< "x has wrong dimension (it should be " +
136			std::to_string(state_->get_nx()) + ")");
137			}
138	✓✗✗✓	25930	if (static_cast<std::size_t>(u.size()) != nu_) {
139			throw_pretty("Invalid argument: "
140			<< "u has wrong dimension (it should be " +
141			std::to_string(nu_) + ")");
142			}
143		25930	Data* d = static_cast<Data*>(data.get());
144		25930	const std::size_t nv = state_->get_nv();
145		25930	const std::size_t nc = contacts_->get_nc_total();
146	✓✗	25930	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
147			x.head(state_->get_nq());
148	✓✗	25930	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
149			x.tail(nv);
150	✓✗	25930	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
151			u.head(nv);
152			const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
153	✓✗	25930	f_ext = u.tail(nc);
154
155	✓✗	25930	d->xout = a;
156	✓✗	25930	pinocchio::forwardKinematics(pinocchio_, d->pinocchio, q, v, a);
157	✓✗	25930	pinocchio::computeJointJacobians(pinocchio_, d->pinocchio);
158	✓✗	25930	contacts_->calc(d->multibody.contacts, x);
159	✓✗✓✗	25930	contacts_->updateForce(d->multibody.contacts, f_ext);
160		25930	pinocchio::rnea(pinocchio_, d->pinocchio, q, v, a,
161	✓✗	25930	d->multibody.contacts->fext);
162	✓✗	25930	pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
163	✓✗	25930	pinocchio::centerOfMass(pinocchio_, d->pinocchio, q, v, a);
164	✓✗✓✗	25930	actuation_->commands(d->multibody.actuation, x, d->pinocchio.tau);
165	✓✗	25930	d->multibody.joint->a = a;
166	✓✗	25930	d->multibody.joint->tau = d->multibody.actuation->u;
167	✓✗✓✗	25930	actuation_->calc(d->multibody.actuation, x, d->multibody.joint->tau);
168	✓✗	25930	costs_->calc(d->costs, x, u);
169		25930	d->cost = d->costs->cost;
170	✓✓✓✗	89296	for (std::string name : contacts_->get_active_set()) {
171	✓✗✓✗	63366	constraints_->changeConstraintStatus(name + "_acc", true);
172	✓✗✓✗	63366	constraints_->changeConstraintStatus(name + "_force", false);
173			}
174	✓✓✓✗	25934	for (std::string name : contacts_->get_inactive_set()) {
175	✓✗✓✗	4	constraints_->changeConstraintStatus(name + "_acc", false);
176	✓✗✓✗	4	constraints_->changeConstraintStatus(name + "_force", true);
177			}
178	✓✗	25930	d->constraints->resize(this, d);
179	✓✗	25930	constraints_->calc(d->constraints, x, u);
180		25930	}
181
182			template <typename Scalar>
183		2862	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calc(
184			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
185			const Eigen::Ref<const VectorXs>& x) {
186	✓✗✗✓	2862	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
192		2862	Data* d = static_cast<Data*>(data.get());
193	✓✗	2862	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
194			x.head(state_->get_nq());
195	✓✗	2862	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
196			x.tail(state_->get_nv());
197
198	✓✗	2862	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
199	✓✗	2862	pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
200	✓✗	2862	costs_->calc(d->costs, x);
201		2862	d->cost = d->costs->cost;
202	✓✗	2862	d->constraints->resize(this, d);
203	✓✗	2862	constraints_->calc(d->constraints, x);
204		2862	}
205
206			template <typename Scalar>
207		3762	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calcDiff(
208			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
209			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
210	✓✗✗✓	3762	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
211			throw_pretty("Invalid argument: "
212			<< "x has wrong dimension (it should be " +
213			std::to_string(state_->get_nx()) + ")");
214			}
215	✓✗✗✓	3762	if (static_cast<std::size_t>(u.size()) != nu_) {
216			throw_pretty("Invalid argument: "
217			<< "u has wrong dimension (it should be " +
218			std::to_string(nu_) + ")");
219			}
220		3762	Data* d = static_cast<Data*>(data.get());
221		3762	const std::size_t nv = state_->get_nv();
222		3762	const std::size_t nc = contacts_->get_nc_total();
223	✓✗	3762	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
224			x.head(state_->get_nq());
225	✓✗	3762	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
226			x.tail(nv);
227	✓✗	3762	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> a =
228			u.head(nv);
229
230		3762	pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, a,
231	✓✗	3762	d->multibody.contacts->fext);
232	✓✗	3762	contacts_->updateRneaDiff(d->multibody.contacts, d->pinocchio);
233	✓✗✓✗ ✓✗	3762	d->pinocchio.M.template triangularView<Eigen::StrictlyLower>() =
234		3762	d->pinocchio.M.template triangularView<Eigen::StrictlyUpper>()
235	✓✗	3762	.transpose();
236	✓✗	3762	pinocchio::jacobianCenterOfMass(pinocchio_, d->pinocchio, false);
237	✓✗✓✗	3762	actuation_->calcDiff(d->multibody.actuation, x, d->multibody.joint->tau);
238	✓✗	7524	actuation_->torqueTransform(d->multibody.actuation, x,
239	✓✗	3762	d->multibody.joint->tau);
240	✓✗✓✗ ✓✗	3762	d->multibody.joint->dtau_dx.leftCols(nv).noalias() =
241	✓✗	3762	d->multibody.actuation->Mtau * d->pinocchio.dtau_dq;
242	✓✗✓✗ ✓✗	3762	d->multibody.joint->dtau_dx.rightCols(nv).noalias() =
243	✓✗	3762	d->multibody.actuation->Mtau * d->pinocchio.dtau_dv;
244	✓✗✓✗ ✓✗	3762	d->multibody.joint->dtau_du.leftCols(nv).noalias() =
245	✓✗	3762	d->multibody.actuation->Mtau * d->pinocchio.M;
246	✓✗✓✗ ✓✗✓✗	3762	d->multibody.joint->dtau_du.rightCols(nc).noalias() =
247	✓✗✓✗	3762	-d->multibody.actuation->Mtau *
248	✓✗	3762	d->multibody.contacts->Jc.topRows(nc).transpose();
249	✓✗	3762	contacts_->calcDiff(d->multibody.contacts, x);
250	✓✗	3762	costs_->calcDiff(d->costs, x, u);
251	✓✓✓✗	12540	for (std::string name : contacts_->get_active_set()) {
252	✓✗✓✗	8778	constraints_->changeConstraintStatus(name + "_acc", true);
253	✓✗✓✗	8778	constraints_->changeConstraintStatus(name + "_force", false);
254			}
255	✗✓✗✗	3762	for (std::string name : contacts_->get_inactive_set()) {
256			constraints_->changeConstraintStatus(name + "_acc", false);
257			constraints_->changeConstraintStatus(name + "_force", true);
258			}
259	✓✗	3762	d->constraints->resize(this, d);
260	✓✗	3762	constraints_->calcDiff(d->constraints, x, u);
261		3762	}
262
263			template <typename Scalar>
264		132	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::calcDiff(
265			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
266			const Eigen::Ref<const VectorXs>& x) {
267	✗✓	132	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
268			throw_pretty("Invalid argument: "
269			<< "x has wrong dimension (it should be " +
270			std::to_string(state_->get_nx()) + ")");
271			}
272		132	Data* d = static_cast<Data*>(data.get());
273		132	costs_->calcDiff(d->costs, x);
274	✓✗	132	if (constraints_ != nullptr) {
275		132	constraints_->calcDiff(d->constraints, x);
276			}
277		132	}
278
279			template <typename Scalar>
280			boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
281		31430	DifferentialActionModelContactInvDynamicsTpl<Scalar>::createData() {
282	✓✗	31430	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
283			}
284
285			template <typename Scalar>
286		1930	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::quasiStatic(
287			const boost::shared_ptr<DifferentialActionDataAbstract>& data,
288			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
289			Scalar) {
290	✓✗✗✓	1930	if (static_cast<std::size_t>(u.size()) != nu_) {
291			throw_pretty("Invalid argument: "
292			<< "u has wrong dimension (it should be " +
293			std::to_string(nu_) + ")");
294			}
295	✓✗✗✓	1930	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
296			throw_pretty("Invalid argument: "
297			<< "x has wrong dimension (it should be " +
298			std::to_string(state_->get_nx()) + ")");
299			}
300		1930	Data* d = static_cast<Data*>(data.get());
301		1930	const std::size_t nq = state_->get_nq();
302		1930	const std::size_t nv = state_->get_nv();
303		1930	const std::size_t nu = actuation_->get_nu();
304		1930	std::size_t nc = contacts_->get_nc_total();
305	✓✗	1930	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
306			x.head(nq);
307	✓✗✓✗	1930	d->tmp_xstatic.head(nq) = q;
308	✓✗✓✗	1930	d->tmp_xstatic.tail(nv).setZero();
309	✓✗	1930	u.setZero();
310
311	✓✗	1930	pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q,
312	✓✗	1930	d->tmp_xstatic.tail(nv));
313	✓✗	1930	pinocchio::computeJointJacobians(pinocchio_, d->pinocchio, q);
314	✓✗✓✗	1930	pinocchio::rnea(pinocchio_, d->pinocchio, q, d->tmp_xstatic.tail(nv),
315	✓✗	1930	d->tmp_xstatic.tail(nv));
316	✓✗✓✗ ✓✗	3860	actuation_->calc(d->multibody.actuation, d->tmp_xstatic,
317	✓✗	1930	d->tmp_xstatic.tail(nu));
318	✓✗✓✗ ✓✗	3860	actuation_->calcDiff(d->multibody.actuation, d->tmp_xstatic,
319	✓✗	1930	d->tmp_xstatic.tail(nu));
320		1930	contacts_->setComputeAllContacts(false);
321	✓✗✓✗	1930	contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
322		1930	contacts_->setComputeAllContacts(true);
323
324	✓✗	1930	d->tmp_Jstatic.conservativeResize(nv, nu + nc);
325	✓✗✓✗	1930	d->tmp_Jstatic.leftCols(nu) = d->multibody.actuation->dtau_du;
326	✓✗✓✗ ✓✗	1930	d->tmp_Jstatic.rightCols(nc) =
327	✓✗	1930	d->multibody.contacts->Jc.topRows(nc).transpose();
328	✓✗✓✗ ✓✗✓✗	1930	d->tmp_rstatic.noalias() = pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau;
329	✓✗	1930	if (nc != 0) {
330		1930	nc = 0;
331		1930	std::size_t nc_r = 0;
332		4506	for (typename ContactModelMultiple::ContactModelContainer::const_iterator
333		1930	it_m = contacts_->get_contacts().begin();
334	✓✓	6436	it_m != contacts_->get_contacts().end(); ++it_m) {
335		4506	const boost::shared_ptr<ContactItem>& m_i = it_m->second;
336		4506	const std::size_t nc_i = m_i->contact->get_nc();
337	✓✓	4506	if (m_i->active) {
338	✓✗✓✗ ✓✗	4502	u.segment(nv + nc, nc_i) = d->tmp_rstatic.segment(nu + nc_r, nc_i);
339		4502	nc_r += nc_i;
340			} else {
341	✓✗✓✗	4	u.segment(nv + nc, nc_i).setZero();
342			}
343		4506	nc += nc_i;
344			}
345			}
346	✓✗	1930	d->pinocchio.tau.setZero();
347		1930	}
348
349			template <typename Scalar>
350		3942	bool DifferentialActionModelContactInvDynamicsTpl<Scalar>::checkData(
351			const boost::shared_ptr<DifferentialActionDataAbstract>& data) {
352		7884	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
353	✓✗	3942	if (d != NULL) {
354		3942	return true;
355			} else {
356			return false;
357			}
358			}
359
360			template <typename Scalar>
361		60	void DifferentialActionModelContactInvDynamicsTpl<Scalar>::print(
362			std::ostream& os) const {
363		60	os << "DifferentialActionModelContactInvDynamics {nx=" << state_->get_nx()
364		60	<< ", ndx=" << state_->get_ndx() << ", nu=" << nu_
365		60	<< ", nc=" << contacts_->get_nc_total() << "}";
366		60	}
367
368			template <typename Scalar>
369		162892	std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_ng()
370			const {
371	✓✗	162892	if (constraints_ != nullptr) {
372		162892	return constraints_->get_ng();
373			} else {
374			return Base::get_ng();
375			}
376			}
377
378			template <typename Scalar>
379		162892	std::size_t DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_nh()
380			const {
381	✓✗	162892	if (constraints_ != nullptr) {
382		162892	return constraints_->get_nh();
383			} else {
384			return Base::get_nh();
385			}
386			}
387
388			template <typename Scalar>
389			const typename MathBaseTpl<Scalar>::VectorXs&
390			DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_g_lb() const {
391			if (constraints_ != nullptr) {
392			return constraints_->get_lb();
393			} else {
394			return g_lb_;
395			}
396			}
397
398			template <typename Scalar>
399			const typename MathBaseTpl<Scalar>::VectorXs&
400			DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_g_ub() const {
401			if (constraints_ != nullptr) {
402			return constraints_->get_ub();
403			} else {
404			return g_lb_;
405			}
406			}
407
408			template <typename Scalar>
409			pinocchio::ModelTpl<Scalar>&
410		31430	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_pinocchio() const {
411		31430	return pinocchio_;
412			}
413
414			template <typename Scalar>
415			const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
416		125720	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_actuation() const {
417		125720	return actuation_;
418			}
419
420			template <typename Scalar>
421			const boost::shared_ptr<ContactModelMultipleTpl<Scalar> >&
422		296206	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_contacts() const {
423		296206	return contacts_;
424			}
425
426			template <typename Scalar>
427			const boost::shared_ptr<CostModelSumTpl<Scalar> >&
428		31430	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_costs() const {
429		31430	return costs_;
430			}
431
432			template <typename Scalar>
433			const boost::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
434		31430	DifferentialActionModelContactInvDynamicsTpl<Scalar>::get_constraints() const {
435		31430	return constraints_;
436			}
437
438			} // namespace crocoddyl