Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	include/crocoddyl/multibody/actions/impulse-fwddyn.hxx	Lines:	138	179	77.1 %
Date:	2024-02-13 11:12:33	Branches:	104	312	33.3 %


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

namespace crocoddyl {

template <typename Scalar>
ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl(
    boost::shared_ptr<StateMultibody> state,
    boost::shared_ptr<ImpulseModelMultiple> impulses,
    boost::shared_ptr<CostModelSum> costs, const Scalar r_coeff,
    const Scalar JMinvJt_damping, const bool enable_force)
    : Base(state, 0, costs->get_nr(), 0, 0),
      impulses_(impulses),
      costs_(costs),
      constraints_(nullptr),
      pinocchio_(*state->get_pinocchio().get()),
      with_armature_(true),
      armature_(VectorXs::Zero(state->get_nv())),
      r_coeff_(r_coeff),
      JMinvJt_damping_(JMinvJt_damping),
      enable_force_(enable_force),
      gravity_(state->get_pinocchio()->gravity) {
  init();
}

template <typename Scalar>
ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl(
    boost::shared_ptr<StateMultibody> state,
    boost::shared_ptr<ImpulseModelMultiple> impulses,
    boost::shared_ptr<CostModelSum> costs,
    boost::shared_ptr<ConstraintModelManager> constraints, const Scalar r_coeff,
    const Scalar JMinvJt_damping, const bool enable_force)
    : Base(state, 0, costs->get_nr(), constraints->get_ng(),
           constraints->get_nh()),
      impulses_(impulses),
      costs_(costs),
      constraints_(constraints),
      pinocchio_(*state->get_pinocchio().get()),
      with_armature_(true),
      armature_(VectorXs::Zero(state->get_nv())),
      r_coeff_(r_coeff),
      JMinvJt_damping_(JMinvJt_damping),
      enable_force_(enable_force),
      gravity_(state->get_pinocchio()->gravity) {
  init();
}

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

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<Scalar>::init() {
  if (r_coeff_ < Scalar(0.)) {
    r_coeff_ = Scalar(0.);
    throw_pretty("Invalid argument: "
                 << "The restitution coefficient has to be positive, set to 0");
  }
  if (JMinvJt_damping_ < Scalar(0.)) {
    JMinvJt_damping_ = Scalar(0.);
    throw_pretty("Invalid argument: "
                 << "The damping factor has to be positive, set to 0");
  }
}

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  Data* d = static_cast<Data*>(data.get());

  // Computing impulse dynamics and forces
  initCalc(d, x);

  // Computing the cost and constraints
  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 ActionModelImpulseFwdDynamicsTpl<Scalar>::calc(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  Data* d = static_cast<Data*>(data.get());

  // Computing impulse dynamics and forces
  initCalc(d, x);

  // Computing the cost and constraints
  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 ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
  Data* d = static_cast<Data*>(data.get());

  // Computing derivatives of impulse dynamics and forces
  initCalcDiff(d, x);

  // Computing derivatives of cost and constraints
  costs_->calcDiff(d->costs, x, u);
  if (constraints_ != nullptr) {
    constraints_->calcDiff(d->constraints, x, u);
  }
}

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff(
    const boost::shared_ptr<ActionDataAbstract>& data,
    const Eigen::Ref<const VectorXs>& x) {
  Data* d = static_cast<Data*>(data.get());

  // Computing derivatives of impulse dynamics and forces
  initCalcDiff(d, x);

  // Computing derivatives of cost and constraints
  costs_->calcDiff(d->costs, x);
  if (constraints_ != nullptr) {
    constraints_->calcDiff(d->constraints, x);
  }
}

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

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

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<Scalar>::quasiStatic(
    const boost::shared_ptr<ActionDataAbstract>&, Eigen::Ref<VectorXs>,
    const Eigen::Ref<const VectorXs>&, const std::size_t, const Scalar) {
  // do nothing
}

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalc(
    Data* 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()) + ")");
  }

  const std::size_t nq = state_->get_nq();
  const std::size_t nv = state_->get_nv();
  const std::size_t nc = impulses_->get_nc();
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
      x.head(nq);
  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
      x.tail(nv);

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

  if (!with_armature_) {
    data->pinocchio.M.diagonal() += armature_;
  }
  impulses_->calc(data->multibody.impulses, x);

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

  if (Jc_lu.rank() < data->multibody.impulses->Jc.topRows(nc).rows() &&
      JMinvJt_damping_ == Scalar(0.)) {
    throw_pretty(
        "It is needed a damping factor since the contact Jacobian is not "
        "full-rank");
  }
#endif

  pinocchio::impulseDynamics(pinocchio_, data->pinocchio, v,
                             data->multibody.impulses->Jc.topRows(nc), r_coeff_,
                             JMinvJt_damping_);
  data->xnext.head(nq) = q;
  data->xnext.tail(nv) = data->pinocchio.dq_after;
  impulses_->updateVelocity(data->multibody.impulses, data->pinocchio.dq_after);
  impulses_->updateForce(data->multibody.impulses, data->pinocchio.impulse_c);
}

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalcDiff(
    Data* 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()) + ")");
  }

  const std::size_t nv = state_->get_nv();
  const std::size_t nc = impulses_->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);

  // 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)
  data->Kinv.resize(nv + nc, nv + nc);
  pinocchio::computeRNEADerivatives(pinocchio_, data->pinocchio, q, data->vnone,
                                    data->pinocchio.dq_after - v,
                                    data->multibody.impulses->fext);
  pinocchio::computeGeneralizedGravityDerivatives(pinocchio_, data->pinocchio,
                                                  q, data->dgrav_dq);
  pinocchio::getKKTContactDynamicMatrixInverse(
      pinocchio_, data->pinocchio, data->multibody.impulses->Jc.topRows(nc),
      data->Kinv);

  pinocchio::computeForwardKinematicsDerivatives(
      pinocchio_, data->pinocchio, q, data->pinocchio.dq_after, data->vnone);
  impulses_->calcDiff(data->multibody.impulses, x);
  impulses_->updateRneaDiff(data->multibody.impulses, data->pinocchio);

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

  data->pinocchio.dtau_dq -= data->dgrav_dq;
  data->Fx.topLeftCorner(nv, nv).setIdentity();
  data->Fx.topRightCorner(nv, nv).setZero();
  data->Fx.bottomLeftCorner(nv, nv).noalias() =
      -a_partial_dtau * data->pinocchio.dtau_dq;
  data->Fx.bottomLeftCorner(nv, nv).noalias() -=
      a_partial_da * data->multibody.impulses->dv0_dq.topRows(nc);
  data->Fx.bottomRightCorner(nv, nv).noalias() =
      a_partial_dtau *
      data->pinocchio.M.template selfadjointView<Eigen::Upper>();

  // Computing the cost derivatives
  if (enable_force_) {
    data->df_dx.topLeftCorner(nc, nv).noalias() =
        f_partial_dtau * data->pinocchio.dtau_dq;
    data->df_dx.topLeftCorner(nc, nv).noalias() +=
        f_partial_da * data->multibody.impulses->dv0_dq.topRows(nc);
    data->df_dx.topRightCorner(nc, nv).noalias() =
        f_partial_da * data->multibody.impulses->Jc.topRows(nc);
    impulses_->updateVelocityDiff(data->multibody.impulses,
                                  data->Fx.bottomRows(nv));
    impulses_->updateForceDiff(data->multibody.impulses,
                               data->df_dx.topRows(nc));
  }
}

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

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

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

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

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<Scalar>::print(std::ostream& os) const {
  os << "ActionModelImpulseFwdDynamics {nx=" << state_->get_nx()
     << ", ndx=" << state_->get_ndx() << ", nc=" << impulses_->get_nc() << "}";
}

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

template <typename Scalar>
const boost::shared_ptr<ImpulseModelMultipleTpl<Scalar> >&
ActionModelImpulseFwdDynamicsTpl<Scalar>::get_impulses() const {
  return impulses_;
}

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

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

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

template <typename Scalar>
const Scalar
ActionModelImpulseFwdDynamicsTpl<Scalar>::get_restitution_coefficient() const {
  return r_coeff_;
}

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

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<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 ActionModelImpulseFwdDynamicsTpl<Scalar>::set_restitution_coefficient(
    const Scalar r_coeff) {
  if (r_coeff < 0.) {
    throw_pretty("Invalid argument: "
                 << "The restitution coefficient has to be positive");
  }
  r_coeff_ = r_coeff;
}

template <typename Scalar>
void ActionModelImpulseFwdDynamicsTpl<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,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			namespace crocoddyl {
11
12			template <typename Scalar>
13		65	ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl(
14			boost::shared_ptr<StateMultibody> state,
15			boost::shared_ptr<ImpulseModelMultiple> impulses,
16			boost::shared_ptr<CostModelSum> costs, const Scalar r_coeff,
17			const Scalar JMinvJt_damping, const bool enable_force)
18			: Base(state, 0, costs->get_nr(), 0, 0),
19			impulses_(impulses),
20			costs_(costs),
21			constraints_(nullptr),
22		65	pinocchio_(*state->get_pinocchio().get()),
23			with_armature_(true),
24		65	armature_(VectorXs::Zero(state->get_nv())),
25			r_coeff_(r_coeff),
26			JMinvJt_damping_(JMinvJt_damping),
27			enable_force_(enable_force),
28	✓✗✓✗ ✓✗✓✗	195	gravity_(state->get_pinocchio()->gravity) {
29	✓✗	65	init();
30		65	}
31
32			template <typename Scalar>
33		12	ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl(
34			boost::shared_ptr<StateMultibody> state,
35			boost::shared_ptr<ImpulseModelMultiple> impulses,
36			boost::shared_ptr<CostModelSum> costs,
37			boost::shared_ptr<ConstraintModelManager> constraints, const Scalar r_coeff,
38			const Scalar JMinvJt_damping, const bool enable_force)
39			: Base(state, 0, costs->get_nr(), constraints->get_ng(),
40			constraints->get_nh()),
41			impulses_(impulses),
42			costs_(costs),
43			constraints_(constraints),
44		12	pinocchio_(*state->get_pinocchio().get()),
45			with_armature_(true),
46		12	armature_(VectorXs::Zero(state->get_nv())),
47			r_coeff_(r_coeff),
48			JMinvJt_damping_(JMinvJt_damping),
49			enable_force_(enable_force),
50	✓✗✓✗ ✓✗✓✗	36	gravity_(state->get_pinocchio()->gravity) {
51	✓✗	12	init();
52		12	}
53
54			template <typename Scalar>
55		158	ActionModelImpulseFwdDynamicsTpl<Scalar>::~ActionModelImpulseFwdDynamicsTpl() {}
56
57			template <typename Scalar>
58		77	void ActionModelImpulseFwdDynamicsTpl<Scalar>::init() {
59	✗✓	77	if (r_coeff_ < Scalar(0.)) {
60			r_coeff_ = Scalar(0.);
61			throw_pretty("Invalid argument: "
62			<< "The restitution coefficient has to be positive, set to 0");
63			}
64	✗✓	77	if (JMinvJt_damping_ < Scalar(0.)) {
65			JMinvJt_damping_ = Scalar(0.);
66			throw_pretty("Invalid argument: "
67			<< "The damping factor has to be positive, set to 0");
68			}
69		77	}
70
71			template <typename Scalar>
72		4050	void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc(
73			const boost::shared_ptr<ActionDataAbstract>& data,
74			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
75		4050	Data* d = static_cast<Data*>(data.get());
76
77			// Computing impulse dynamics and forces
78		4050	initCalc(d, x);
79
80			// Computing the cost and constraints
81		4050	costs_->calc(d->costs, x, u);
82		4050	d->cost = d->costs->cost;
83	✓✓	4050	if (constraints_ != nullptr) {
84		796	d->constraints->resize(this, d);
85		796	constraints_->calc(d->constraints, x, u);
86			}
87		4050	}
88
89			template <typename Scalar>
90		2930	void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc(
91			const boost::shared_ptr<ActionDataAbstract>& data,
92			const Eigen::Ref<const VectorXs>& x) {
93		2930	Data* d = static_cast<Data*>(data.get());
94
95			// Computing impulse dynamics and forces
96		2930	initCalc(d, x);
97
98			// Computing the cost and constraints
99		2930	costs_->calc(d->costs, x);
100		2930	d->cost = d->costs->cost;
101	✗✓	2930	if (constraints_ != nullptr) {
102			d->constraints->resize(this, d);
103			constraints_->calc(d->constraints, x);
104			}
105		2930	}
106
107			template <typename Scalar>
108		196	void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff(
109			const boost::shared_ptr<ActionDataAbstract>& data,
110			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
111		196	Data* d = static_cast<Data*>(data.get());
112
113			// Computing derivatives of impulse dynamics and forces
114		196	initCalcDiff(d, x);
115
116			// Computing derivatives of cost and constraints
117		196	costs_->calcDiff(d->costs, x, u);
118	✓✓	196	if (constraints_ != nullptr) {
119		12	constraints_->calcDiff(d->constraints, x, u);
120			}
121		196	}
122
123			template <typename Scalar>
124		52	void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff(
125			const boost::shared_ptr<ActionDataAbstract>& data,
126			const Eigen::Ref<const VectorXs>& x) {
127		52	Data* d = static_cast<Data*>(data.get());
128
129			// Computing derivatives of impulse dynamics and forces
130		52	initCalcDiff(d, x);
131
132			// Computing derivatives of cost and constraints
133		52	costs_->calcDiff(d->costs, x);
134	✗✓	52	if (constraints_ != nullptr) {
135			constraints_->calcDiff(d->constraints, x);
136			}
137		52	}
138
139			template <typename Scalar>
140			boost::shared_ptr<ActionDataAbstractTpl<Scalar> >
141		4256	ActionModelImpulseFwdDynamicsTpl<Scalar>::createData() {
142	✓✗	4256	return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
143			}
144
145			template <typename Scalar>
146		128	bool ActionModelImpulseFwdDynamicsTpl<Scalar>::checkData(
147			const boost::shared_ptr<ActionDataAbstract>& data) {
148		256	boost::shared_ptr<Data> d = boost::dynamic_pointer_cast<Data>(data);
149	✓✗	128	if (d != NULL) {
150		128	return true;
151			} else {
152			return false;
153			}
154			}
155
156			template <typename Scalar>
157		160	void ActionModelImpulseFwdDynamicsTpl<Scalar>::quasiStatic(
158			const boost::shared_ptr<ActionDataAbstract>&, Eigen::Ref<VectorXs>,
159			const Eigen::Ref<const VectorXs>&, const std::size_t, const Scalar) {
160			// do nothing
161		160	}
162
163			template <typename Scalar>
164		6980	void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalc(
165			Data* data, const Eigen::Ref<const VectorXs>& x) {
166	✓✗✗✓	6980	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
167			throw_pretty("Invalid argument: "
168			<< "x has wrong dimension (it should be " +
169			std::to_string(state_->get_nx()) + ")");
170			}
171
172		6980	const std::size_t nq = state_->get_nq();
173		6980	const std::size_t nv = state_->get_nv();
174		6980	const std::size_t nc = impulses_->get_nc();
175	✓✗	6980	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
176			x.head(nq);
177	✓✗	6980	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
178			x.tail(nv);
179
180			// Computing the forward dynamics with the holonomic constraints defined by
181			// the contact model
182	✓✗	6980	pinocchio::computeAllTerms(pinocchio_, data->pinocchio, q, v);
183	✓✗	6980	pinocchio::computeCentroidalMomentum(pinocchio_, data->pinocchio);
184
185	✗✓	6980	if (!with_armature_) {
186			data->pinocchio.M.diagonal() += armature_;
187			}
188	✓✗	6980	impulses_->calc(data->multibody.impulses, x);
189
190			#ifndef NDEBUG
191	✓✗✓✗	13960	Eigen::FullPivLU<MatrixXs> Jc_lu(data->multibody.impulses->Jc.topRows(nc));
192
193	✓✗✓✗ ✗✓✗✓	6980	if (Jc_lu.rank() < data->multibody.impulses->Jc.topRows(nc).rows() &&
194			JMinvJt_damping_ == Scalar(0.)) {
195			throw_pretty(
196			"It is needed a damping factor since the contact Jacobian is not "
197			"full-rank");
198			}
199			#endif
200
201	✓✗	6980	pinocchio::impulseDynamics(pinocchio_, data->pinocchio, v,
202	✓✗	6980	data->multibody.impulses->Jc.topRows(nc), r_coeff_,
203			JMinvJt_damping_);
204	✓✗✓✗	6980	data->xnext.head(nq) = q;
205	✓✗✓✗	6980	data->xnext.tail(nv) = data->pinocchio.dq_after;
206	✓✗	6980	impulses_->updateVelocity(data->multibody.impulses, data->pinocchio.dq_after);
207	✓✗	6980	impulses_->updateForce(data->multibody.impulses, data->pinocchio.impulse_c);
208		6980	}
209
210			template <typename Scalar>
211		248	void ActionModelImpulseFwdDynamicsTpl<Scalar>::initCalcDiff(
212			Data* data, const Eigen::Ref<const VectorXs>& x) {
213	✓✗✗✓	248	if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
214			throw_pretty("Invalid argument: "
215			<< "x has wrong dimension (it should be " +
216			std::to_string(state_->get_nx()) + ")");
217			}
218
219		248	const std::size_t nv = state_->get_nv();
220		248	const std::size_t nc = impulses_->get_nc();
221	✓✗	248	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
222			x.head(state_->get_nq());
223	✓✗	248	const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
224			x.tail(nv);
225
226			// Computing the dynamics derivatives
227			// We resize the Kinv matrix because Eigen cannot call block operations
228			// recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore,
229			// it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc)
230	✓✗	248	data->Kinv.resize(nv + nc, nv + nc);
231	✓✗	248	pinocchio::computeRNEADerivatives(pinocchio_, data->pinocchio, q, data->vnone,
232		248	data->pinocchio.dq_after - v,
233	✓✗	248	data->multibody.impulses->fext);
234		248	pinocchio::computeGeneralizedGravityDerivatives(pinocchio_, data->pinocchio,
235	✓✗	248	q, data->dgrav_dq);
236			pinocchio::getKKTContactDynamicMatrixInverse(
237	✓✗✓✗	248	pinocchio_, data->pinocchio, data->multibody.impulses->Jc.topRows(nc),
238		248	data->Kinv);
239
240		248	pinocchio::computeForwardKinematicsDerivatives(
241	✓✗	248	pinocchio_, data->pinocchio, q, data->pinocchio.dq_after, data->vnone);
242	✓✗	248	impulses_->calcDiff(data->multibody.impulses, x);
243	✓✗	248	impulses_->updateRneaDiff(data->multibody.impulses, data->pinocchio);
244
245	✓✗	248	Eigen::Block<MatrixXs> a_partial_dtau = data->Kinv.topLeftCorner(nv, nv);
246	✓✗	248	Eigen::Block<MatrixXs> a_partial_da = data->Kinv.topRightCorner(nv, nc);
247	✓✗	248	Eigen::Block<MatrixXs> f_partial_dtau = data->Kinv.bottomLeftCorner(nc, nv);
248	✓✗	248	Eigen::Block<MatrixXs> f_partial_da = data->Kinv.bottomRightCorner(nc, nc);
249
250	✓✗	248	data->pinocchio.dtau_dq -= data->dgrav_dq;
251	✓✗✓✗	248	data->Fx.topLeftCorner(nv, nv).setIdentity();
252	✓✗✓✗	248	data->Fx.topRightCorner(nv, nv).setZero();
253	✓✗✓✗ ✓✗✓✗	248	data->Fx.bottomLeftCorner(nv, nv).noalias() =
254	✓✗	248	-a_partial_dtau * data->pinocchio.dtau_dq;
255	✓✗✓✗ ✓✗	248	data->Fx.bottomLeftCorner(nv, nv).noalias() -=
256	✓✗✓✗	248	a_partial_da * data->multibody.impulses->dv0_dq.topRows(nc);
257	✓✗✓✗ ✓✗	248	data->Fx.bottomRightCorner(nv, nv).noalias() =
258	✓✗	248	a_partial_dtau *
259	✓✗	248	data->pinocchio.M.template selfadjointView<Eigen::Upper>();
260
261			// Computing the cost derivatives
262	✓✗	248	if (enable_force_) {
263	✓✗✓✗ ✓✗	248	data->df_dx.topLeftCorner(nc, nv).noalias() =
264	✓✗	248	f_partial_dtau * data->pinocchio.dtau_dq;
265	✓✗✓✗ ✓✗	248	data->df_dx.topLeftCorner(nc, nv).noalias() +=
266	✓✗✓✗	248	f_partial_da * data->multibody.impulses->dv0_dq.topRows(nc);
267	✓✗✓✗ ✓✗	248	data->df_dx.topRightCorner(nc, nv).noalias() =
268	✓✗✓✗	248	f_partial_da * data->multibody.impulses->Jc.topRows(nc);
269	✓✗✓✗	496	impulses_->updateVelocityDiff(data->multibody.impulses,
270	✓✗	248	data->Fx.bottomRows(nv));
271	✓✗✓✗	496	impulses_->updateForceDiff(data->multibody.impulses,
272	✓✗	248	data->df_dx.topRows(nc));
273			}
274		248	}
275
276			template <typename Scalar>
277		13720	std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_ng() const {
278	✓✓	13720	if (constraints_ != nullptr) {
279		3208	return constraints_->get_ng();
280			} else {
281		10512	return Base::get_ng();
282			}
283			}
284
285			template <typename Scalar>
286		13720	std::size_t ActionModelImpulseFwdDynamicsTpl<Scalar>::get_nh() const {
287	✓✓	13720	if (constraints_ != nullptr) {
288		3208	return constraints_->get_nh();
289			} else {
290		10512	return Base::get_nh();
291			}
292			}
293
294			template <typename Scalar>
295			const typename MathBaseTpl<Scalar>::VectorXs&
296			ActionModelImpulseFwdDynamicsTpl<Scalar>::get_g_lb() const {
297			if (constraints_ != nullptr) {
298			return constraints_->get_lb();
299			} else {
300			return g_lb_;
301			}
302			}
303
304			template <typename Scalar>
305			const typename MathBaseTpl<Scalar>::VectorXs&
306			ActionModelImpulseFwdDynamicsTpl<Scalar>::get_g_ub() const {
307			if (constraints_ != nullptr) {
308			return constraints_->get_ub();
309			} else {
310			return g_lb_;
311			}
312			}
313
314			template <typename Scalar>
315		44	void ActionModelImpulseFwdDynamicsTpl<Scalar>::print(std::ostream& os) const {
316		44	os << "ActionModelImpulseFwdDynamics {nx=" << state_->get_nx()
317		44	<< ", ndx=" << state_->get_ndx() << ", nc=" << impulses_->get_nc() << "}";
318		44	}
319
320			template <typename Scalar>
321			pinocchio::ModelTpl<Scalar>&
322		4256	ActionModelImpulseFwdDynamicsTpl<Scalar>::get_pinocchio() const {
323		4256	return pinocchio_;
324			}
325
326			template <typename Scalar>
327			const boost::shared_ptr<ImpulseModelMultipleTpl<Scalar> >&
328		17024	ActionModelImpulseFwdDynamicsTpl<Scalar>::get_impulses() const {
329		17024	return impulses_;
330			}
331
332			template <typename Scalar>
333			const boost::shared_ptr<CostModelSumTpl<Scalar> >&
334		4442	ActionModelImpulseFwdDynamicsTpl<Scalar>::get_costs() const {
335		4442	return costs_;
336			}
337
338			template <typename Scalar>
339			const boost::shared_ptr<ConstraintModelManagerTpl<Scalar> >&
340		5052	ActionModelImpulseFwdDynamicsTpl<Scalar>::get_constraints() const {
341		5052	return constraints_;
342			}
343
344			template <typename Scalar>
345			const typename MathBaseTpl<Scalar>::VectorXs&
346			ActionModelImpulseFwdDynamicsTpl<Scalar>::get_armature() const {
347			return armature_;
348			}
349
350			template <typename Scalar>
351			const Scalar
352			ActionModelImpulseFwdDynamicsTpl<Scalar>::get_restitution_coefficient() const {
353			return r_coeff_;
354			}
355
356			template <typename Scalar>
357			const Scalar ActionModelImpulseFwdDynamicsTpl<Scalar>::get_damping_factor()
358			const {
359			return JMinvJt_damping_;
360			}
361
362			template <typename Scalar>
363			void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_armature(
364			const VectorXs& armature) {
365			if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
366			throw_pretty("Invalid argument: "
367			<< "The armature dimension is wrong (it should be " +
368			std::to_string(state_->get_nv()) + ")");
369			}
370			armature_ = armature;
371			with_armature_ = false;
372			}
373
374			template <typename Scalar>
375			void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_restitution_coefficient(
376			const Scalar r_coeff) {
377			if (r_coeff < 0.) {
378			throw_pretty("Invalid argument: "
379			<< "The restitution coefficient has to be positive");
380			}
381			r_coeff_ = r_coeff;
382			}
383
384			template <typename Scalar>
385			void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_damping_factor(
386			const Scalar damping) {
387			if (damping < 0.) {
388			throw_pretty("Invalid argument: "
389			<< "The damping factor has to be positive");
390			}
391			JMinvJt_damping_ = damping;
392			}
393
394			} // namespace crocoddyl