contact-fwddyn.hxx
Go to the documentation of this file.
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, CTU, INRIA,
5 // University of Oxford Copyright note valid unless otherwise stated in
6 // individual files. All rights reserved.
8 
9 #include <crocoddyl/core/utils/exception.hpp>
10 #include <crocoddyl/core/utils/math.hpp>
11 #include <pinocchio/algorithm/centroidal.hpp>
12 #include <pinocchio/algorithm/compute-all-terms.hpp>
13 #include <pinocchio/algorithm/contact-dynamics.hpp>
14 #include <pinocchio/algorithm/frames.hpp>
15 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
16 #include <pinocchio/algorithm/rnea-derivatives.hpp>
17 #include <pinocchio/algorithm/rnea.hpp>
18 
19 #include "contact-fwddyn.hpp"
20 
21 namespace sobec {
22 namespace newcontacts {
23 
24 template <typename Scalar>
25 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
26  DifferentialActionModelContactFwdDynamicsTpl(
27  boost::shared_ptr<StateMultibody> state,
28  boost::shared_ptr<ActuationModelAbstract> actuation,
29  boost::shared_ptr<crocoContactModelMultiple> contacts,
30  boost::shared_ptr<CostModelSum> costs, const Scalar JMinvJt_damping,
31  const bool enable_force)
32  : Base(state, actuation, contacts, costs, JMinvJt_damping, enable_force),
33  enable_force_(enable_force) {
34  sobec_contacts_ =
35  boost::static_pointer_cast<ContactModelMultipleTpl<Scalar>>(contacts);
36 }
37 
38 template <typename Scalar>
39 DifferentialActionModelContactFwdDynamicsTpl<
40  Scalar>::~DifferentialActionModelContactFwdDynamicsTpl() {}
41 
42 template <typename Scalar>
43 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::calcDiff(
44  const boost::shared_ptr<DifferentialActionDataAbstract>& data,
45  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
46  if (static_cast<std::size_t>(x.size()) != this->get_state()->get_nx()) {
47  throw_pretty("Invalid argument: "
48  << "x has wrong dimension (it should be " +
49  std::to_string(this->get_state()->get_nx()) + ")");
50  }
51  if (static_cast<std::size_t>(u.size()) != this->get_nu()) {
52  throw_pretty("Invalid argument: "
53  << "u has wrong dimension (it should be " +
54  std::to_string(this->get_nu()) + ")");
55  }
56 
57  const std::size_t nv = this->get_state()->get_nv();
58  const std::size_t nc =
59  sobec_contacts_->get_nc(); // this->get_contacts()->get_nc();
60  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q =
61  x.head(this->get_state()->get_nq());
62  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
63  x.tail(nv);
64 
65  Data* d = static_cast<Data*>(data.get());
66 
67  // Computing the dynamics derivatives
68  // We resize the Kinv matrix because Eigen cannot call block operations
69  // recursively: https://eigen.tuxfamily.org/bz/show_bug.cgi?id=408. Therefore,
70  // it is not possible to pass d->Kinv.topLeftCorner(nv + nc, nv + nc)
71  d->Kinv.resize(nv + nc, nv + nc);
72  pinocchio::computeRNEADerivatives(this->get_pinocchio(), d->pinocchio, q, v,
73  d->xout, d->multibody.contacts->fext);
74  pinocchio::getKKTContactDynamicMatrixInverse(
75  this->get_pinocchio(), d->pinocchio,
76  d->multibody.contacts->Jc.topRows(nc), d->Kinv);
77 
78  this->get_actuation()->calcDiff(d->multibody.actuation, x, u);
79  sobec_contacts_->calcDiff(d->multibody.contacts, x);
80 
81  // Add skew term to rnea derivative for contacs expressed in
82  // LOCAL_WORLD_ALIGNED see https://www.overleaf.com/read/tzvrrxxtntwk for
83  // detailed calculations
84  sobec_contacts_->updateRneaDerivatives(d->multibody.contacts, d->pinocchio);
85 
86  const Eigen::Block<MatrixXs> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
87  const Eigen::Block<MatrixXs> a_partial_da = d->Kinv.topRightCorner(nv, nc);
88  const Eigen::Block<MatrixXs> f_partial_dtau =
89  d->Kinv.bottomLeftCorner(nc, nv);
90  const Eigen::Block<MatrixXs> f_partial_da = d->Kinv.bottomRightCorner(nc, nc);
91 
92  d->Fx.leftCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
93  d->Fx.rightCols(nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dv;
94  d->Fx.noalias() -= a_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
95  d->Fx.noalias() += a_partial_dtau * d->multibody.actuation->dtau_dx;
96  d->Fu.noalias() = a_partial_dtau * d->multibody.actuation->dtau_du;
97 
98  // Computing the cost derivatives
99  if (enable_force_) {
100  d->df_dx.topLeftCorner(nc, nv).noalias() =
101  f_partial_dtau * d->pinocchio.dtau_dq;
102  d->df_dx.topRightCorner(nc, nv).noalias() =
103  f_partial_dtau * d->pinocchio.dtau_dv;
104  d->df_dx.topRows(nc).noalias() +=
105  f_partial_da * d->multibody.contacts->da0_dx.topRows(nc);
106  d->df_dx.topRows(nc).noalias() -=
107  f_partial_dtau * d->multibody.actuation->dtau_dx;
108  d->df_du.topRows(nc).noalias() =
109  -f_partial_dtau * d->multibody.actuation->dtau_du;
110  const boost::shared_ptr<MatrixXs> df_dx =
111  boost::make_shared<MatrixXs>(d->df_dx.topRows(nc));
112  const boost::shared_ptr<MatrixXs> df_du =
113  boost::make_shared<MatrixXs>(d->df_du.topRows(nc));
114  sobec_contacts_->updateAccelerationDiff(d->multibody.contacts,
115  d->Fx.bottomRows(nv));
116  sobec_contacts_->updateForceDiff(d->multibody.contacts, df_dx, df_du);
117  }
118  this->get_costs()->calcDiff(d->costs, x, u);
119 }
120 
121 // template <typename Scalar>
122 // void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::quasiStatic(
123 // const boost::shared_ptr<DifferentialActionDataAbstract>& data,
124 // Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x, std::size_t,
125 // Scalar) {
126 // if (static_cast<std::size_t>(u.size()) != this->get_nu()) {
127 // throw_pretty("Invalid argument: "
128 // << "u has wrong dimension (it should be " +
129 // std::to_string(this->get_nu()) + ")");
130 // }
131 // if (static_cast<std::size_t>(x.size()) != this->get_state()->get_nx()) {
132 // throw_pretty("Invalid argument: "
133 // << "x has wrong dimension (it should be " +
134 // std::to_string(this->get_state()->get_nx()) + ")");
135 // }
136 // // Static casting the data
137 // Data* d =
138 // static_cast<Data*>(data.get());
139 // const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic>
140 // q = x.head(this->get_state()->get_nq());
141 
142 // const std::size_t nq = this->get_state()->get_nq();
143 // const std::size_t nv = this->get_state()->get_nv();
144 // const std::size_t nc = sobec_contacts_->get_nc();
145 
146 // // Check the velocity input is zero
147 // assert_pretty(x.tail(nv).isZero(), "The velocity input should be zero for
148 // quasi-static to work.");
149 
150 // d->tmp_xstatic.head(nq) = q;
151 // d->tmp_xstatic.tail(nv).setZero();
152 // u.setZero();
153 
154 // pinocchio::computeAllTerms(this->get_pinocchio(), d->pinocchio, q,
155 // d->tmp_xstatic.tail(nv));
156 // pinocchio::computeJointJacobians(this->get_pinocchio(), d->pinocchio, q);
157 // pinocchio::rnea(this->get_pinocchio(), d->pinocchio, q,
158 // d->tmp_xstatic.tail(nv), d->tmp_xstatic.tail(nv));
159 // this->get_actuation()->calc(d->multibody.actuation, d->tmp_xstatic, u);
160 // this->get_actuation()->calcDiff(d->multibody.actuation, d->tmp_xstatic, u);
161 // sobec_contacts_->calc(d->multibody.contacts, d->tmp_xstatic);
162 
163 // // Allocates memory
164 // d->tmp_Jstatic.resize(nv, this->get_nu() + nc);
165 // d->tmp_Jstatic << d->multibody.actuation->dtau_du,
166 // d->multibody.contacts->Jc.topRows(nc).transpose(); u.noalias() =
167 // (pseudoInverse(d->tmp_Jstatic) * d->pinocchio.tau).head(this->get_nu());
168 // d->pinocchio.tau.setZero();
169 // }
170 
171 } // namespace newcontacts
172 } // namespace sobec
contact-fwddyn.hpp
sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl
Differential action model for contact forward dynamics in multibody systems.
Definition: contact-fwddyn.hpp:85
sobec
Definition: activation-quad-ref.hpp:19
sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl::calcDiff
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the contact dynamics, and cost function.
Definition: contact-fwddyn.hxx:43
sobec::newcontacts::x
@ x
Definition: contact1d.hpp:26
sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl::Base
crocoddyl::DifferentialActionModelContactFwdDynamicsTpl< Scalar > Base
Definition: contact-fwddyn.hpp:91
sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl::Data
crocoddyl::DifferentialActionDataContactFwdDynamicsTpl< Scalar > Data
Definition: contact-fwddyn.hpp:92
sobec::newcontacts::DifferentialActionModelContactFwdDynamicsTpl::DifferentialActionModelContactFwdDynamicsTpl
DifferentialActionModelContactFwdDynamicsTpl(boost::shared_ptr< StateMultibody > state, boost::shared_ptr< ActuationModelAbstract > actuation, boost::shared_ptr< crocoContactModelMultiple > contacts, boost::shared_ptr< CostModelSum > costs, const Scalar JMinvJt_damping=Scalar(0.), const bool enable_force=false)
Initialize the contact forward-dynamics action model.
Definition: contact-fwddyn.hxx:26