9 #include <pinocchio/algorithm/centroidal.hpp>
10 #include <pinocchio/algorithm/compute-all-terms.hpp>
11 #include <pinocchio/algorithm/contact-dynamics.hpp>
12 #include <pinocchio/algorithm/frames.hpp>
13 #include <pinocchio/algorithm/kinematics-derivatives.hpp>
14 #include <pinocchio/algorithm/rnea-derivatives.hpp>
15 #include <pinocchio/algorithm/rnea.hpp>
17 #include "contact-fwddyn.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
19 #include "crocoddyl/core/utils/math.hpp"
23 template <
typename Scalar>
24 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::
25 DifferentialActionModelContactFwdDynamicsTpl(
26 boost::shared_ptr<StateMultibody> state,
27 boost::shared_ptr<ActuationModelAbstract> actuation,
28 boost::shared_ptr<crocoContactModelMultiple> contacts,
29 boost::shared_ptr<CostModelSum> costs,
const Scalar JMinvJt_damping,
30 const bool enable_force)
31 : Base(state, actuation, contacts, costs, JMinvJt_damping, enable_force),
32 enable_force_(enable_force) {
34 boost::static_pointer_cast<sobec::ContactModelMultipleTpl<Scalar>>(
38 template <
typename Scalar>
39 DifferentialActionModelContactFwdDynamicsTpl<
40 Scalar>::~DifferentialActionModelContactFwdDynamicsTpl() {}
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()) +
")");
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()) +
")");
57 const std::size_t nv = this->get_state()->get_nv();
58 const std::size_t nc =
59 sobec_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 =
65 Data* d =
static_cast<Data*
>(data.get());
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);
78 this->get_actuation()->calcDiff(d->multibody.actuation, x, u);
79 sobec_contacts_->calcDiff(d->multibody.contacts, x);
84 sobec_contacts_->updateRneaDerivatives(d->multibody.contacts, d->pinocchio);
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);
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;
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);
118 this->get_costs()->calcDiff(d->costs, x, u);