contact-fwddyn.hxx
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #include "crocoddyl/core/utils/exception.hpp"
10 #include "crocoddyl/multibody/actions/contact-fwddyn.hpp"
11 
12 #include <pinocchio/algorithm/compute-all-terms.hpp>
13 #include <pinocchio/algorithm/frames.hpp>
14 #include <pinocchio/algorithm/contact-dynamics.hpp>
15 #include <pinocchio/algorithm/centroidal.hpp>
16 #include <pinocchio/algorithm/rnea-derivatives.hpp>
17 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
18 
19 namespace crocoddyl {
20 
21 template <typename Scalar>
22 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::DifferentialActionModelContactFwdDynamicsTpl(
23  boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActuationModelFloatingBase> actuation,
24  boost::shared_ptr<ContactModelMultiple> contacts, boost::shared_ptr<CostModelSum> costs,
25  const Scalar& JMinvJt_damping, const bool& enable_force)
26  : Base(state, actuation->get_nu(), costs->get_nr()),
27  actuation_(actuation),
28  contacts_(contacts),
29  costs_(costs),
30  pinocchio_(*state->get_pinocchio().get()),
31  with_armature_(true),
32  armature_(VectorXs::Zero(state->get_nv())),
33  JMinvJt_damping_(fabs(JMinvJt_damping)),
34  enable_force_(enable_force) {
35  if (JMinvJt_damping_ < 0.) {
36  JMinvJt_damping_ = 0.;
37  throw_pretty("Invalid argument: "
38  << "The damping factor has to be positive, set to 0");
39  }
40  if (contacts_->get_nu() != nu_) {
41  throw_pretty("Invalid argument: "
42  << "Contacts doesn't have the same control dimension (it should be " + std::to_string(nu_) + ")");
43  }
44  if (costs_->get_nu() != nu_) {
45  throw_pretty("Invalid argument: "
46  << "Costs doesn't have the same control dimension (it should be " + std::to_string(nu_) + ")");
47  }
48 
49  Base::set_u_lb(-1. * pinocchio_.effortLimit.tail(nu_));
50  Base::set_u_ub(+1. * pinocchio_.effortLimit.tail(nu_));
51 }
52 
53 template <typename Scalar>
54 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::~DifferentialActionModelContactFwdDynamicsTpl() {}
55 
56 template <typename Scalar>
57 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calc(
58  const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
59  const Eigen::Ref<const VectorXs>& u) {
60  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
61  throw_pretty("Invalid argument: "
62  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
63  }
64  if (static_cast<std::size_t>(u.size()) != nu_) {
65  throw_pretty("Invalid argument: "
66  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
67  }
68 
69  DifferentialActionDataContactFwdDynamicsTpl<Scalar>* d =
70  static_cast<DifferentialActionDataContactFwdDynamicsTpl<Scalar>*>(data.get());
71  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(state_->get_nq());
72  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(state_->get_nv());
73 
74  // Computing the forward dynamics with the holonomic constraints defined by the contact model
75  pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
76  pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
77 
78  if (!with_armature_) {
79  d->pinocchio.M.diagonal() += armature_;
80  }
81  actuation_->calc(d->multibody.actuation, x, u);
82  contacts_->calc(d->multibody.contacts, x);
83 
84 #ifndef NDEBUG
85  Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.contacts->Jc);
86 
87  if (Jc_lu.rank() < d->multibody.contacts->Jc.rows()) {
88  assert_pretty(JMinvJt_damping_ > 0., "A damping factor is needed as the contact Jacobian is not full-rank");
89  }
90 #endif
91 
92  pinocchio::forwardDynamics(pinocchio_, d->pinocchio, d->multibody.actuation->tau, d->multibody.contacts->Jc,
93  d->multibody.contacts->a0, JMinvJt_damping_);
94  d->xout = d->pinocchio.ddq;
95  contacts_->updateAcceleration(d->multibody.contacts, d->pinocchio.ddq);
96  contacts_->updateForce(d->multibody.contacts, d->pinocchio.lambda_c);
97 
98  // Computing the cost value and residuals
99  costs_->calc(d->costs, x, u);
100  d->cost = d->costs->cost;
101 }
102 
103 template <typename Scalar>
104 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
105  const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
106  const Eigen::Ref<const VectorXs>& u) {
107  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
108  throw_pretty("Invalid argument: "
109  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
110  }
111  if (static_cast<std::size_t>(u.size()) != nu_) {
112  throw_pretty("Invalid argument: "
113  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
114  }
115 
116  const std::size_t& nv = state_->get_nv();
117  const std::size_t& nc = contacts_->get_nc();
118  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(state_->get_nq());
119  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(nv);
120 
121  DifferentialActionDataContactFwdDynamicsTpl<Scalar>* d =
122  static_cast<DifferentialActionDataContactFwdDynamicsTpl<Scalar>*>(data.get());
123 
124  // Computing the dynamics derivatives
125  pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout, d->multibody.contacts->fext);
126  pinocchio::getKKTContactDynamicMatrixInverse(pinocchio_, d->pinocchio, d->multibody.contacts->Jc, d->Kinv);
127 
128  actuation_->calcDiff(d->multibody.actuation, x, u);
129  contacts_->calcDiff(d->multibody.contacts, x);
130 
131  Eigen::Block<MatrixXs> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
132  Eigen::Block<MatrixXs> a_partial_da = d->Kinv.topRightCorner(nv, nc);
133  Eigen::Block<MatrixXs> f_partial_dtau = d->Kinv.bottomLeftCorner(nc, nv);
134  Eigen::Block<MatrixXs> f_partial_da = d->Kinv.bottomRightCorner(nc, nc);
135 
136  d->Fx.leftCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
137  d->Fx.rightCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dv;
138  d->Fx.noalias() -= a_partial_da * d->multibody.contacts->da0_dx;
139  d->Fx.noalias() += a_partial_dtau * d->multibody.actuation->dtau_dx;
140  d->Fu.noalias() = a_partial_dtau * d->multibody.actuation->dtau_du;
141 
142  // Computing the cost derivatives
143  if (enable_force_) {
144  d->df_dx.leftCols(nv).noalias() = f_partial_dtau * d->pinocchio.dtau_dq;
145  d->df_dx.rightCols(nv).noalias() = f_partial_dtau * d->pinocchio.dtau_dv;
146  d->df_dx.noalias() += f_partial_da * d->multibody.contacts->da0_dx;
147  d->df_dx.noalias() -= f_partial_dtau * d->multibody.actuation->dtau_dx;
148  d->df_du.noalias() = -f_partial_dtau * d->multibody.actuation->dtau_du;
149  contacts_->updateAccelerationDiff(d->multibody.contacts, d->Fx.bottomRows(nv));
150  contacts_->updateForceDiff(d->multibody.contacts, d->df_dx, d->df_du);
151  }
152  costs_->calcDiff(d->costs, x, u);
153 }
154 
155 template <typename Scalar>
156 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
157 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::createData() {
158  return boost::make_shared<DifferentialActionDataContactFwdDynamicsTpl<Scalar> >(this);
159 }
160 
161 template <typename Scalar>
162 pinocchio::ModelTpl<Scalar>& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_pinocchio() const {
163  return pinocchio_;
164 }
165 
166 template <typename Scalar>
167 const boost::shared_ptr<ActuationModelFloatingBaseTpl<Scalar> >&
168 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_actuation() const {
169  return actuation_;
170 }
171 
172 template <typename Scalar>
173 const boost::shared_ptr<ContactModelMultipleTpl<Scalar> >&
174 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_contacts() const {
175  return contacts_;
176 }
177 
178 template <typename Scalar>
179 const boost::shared_ptr<CostModelSumTpl<Scalar> >& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_costs()
180  const {
181  return costs_;
182 }
183 
184 template <typename Scalar>
185 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_armature()
186  const {
187  return armature_;
188 }
189 
190 template <typename Scalar>
191 const Scalar& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_damping_factor() const {
192  return JMinvJt_damping_;
193 }
194 
195 template <typename Scalar>
196 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_armature(const VectorXs& armature) {
197  if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
198  throw_pretty("Invalid argument: "
199  << "The armature dimension is wrong (it should be " + std::to_string(state_->get_nv()) + ")");
200  }
201  armature_ = armature;
202  with_armature_ = false;
203 }
204 
205 template <typename Scalar>
206 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_damping_factor(const Scalar& damping) {
207  if (damping < 0.) {
208  throw_pretty("Invalid argument: "
209  << "The damping factor has to be positive");
210  }
211  JMinvJt_damping_ = damping;
212 }
213 
214 } // namespace crocoddyl
Definition: action-base.hxx:11