9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/actions/contact-fwddyn.hpp" 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> 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),
30 pinocchio_(*state->get_pinocchio().get()),
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");
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_) +
")");
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_) +
")");
49 Base::set_u_lb(-1. * pinocchio_.effortLimit.tail(nu_));
50 Base::set_u_ub(+1. * pinocchio_.effortLimit.tail(nu_));
53 template <
typename Scalar>
54 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::~DifferentialActionModelContactFwdDynamicsTpl() {}
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()) +
")");
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_) +
")");
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());
75 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
76 pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
78 if (!with_armature_) {
79 d->pinocchio.M.diagonal() += armature_;
81 actuation_->calc(d->multibody.actuation, x, u);
82 contacts_->calc(d->multibody.contacts, x);
85 Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.contacts->Jc);
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");
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);
99 costs_->calc(d->costs, x, u);
100 d->cost = d->costs->cost;
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()) +
")");
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_) +
")");
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);
121 DifferentialActionDataContactFwdDynamicsTpl<Scalar>* d =
122 static_cast<DifferentialActionDataContactFwdDynamicsTpl<Scalar>*
>(data.get());
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);
128 actuation_->calcDiff(d->multibody.actuation, x, u);
129 contacts_->calcDiff(d->multibody.contacts, x);
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);
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;
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);
152 costs_->calcDiff(d->costs, x, u);
155 template <
typename Scalar>
156 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
157 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::createData() {
158 return boost::make_shared<DifferentialActionDataContactFwdDynamicsTpl<Scalar> >(
this);
161 template <
typename Scalar>
162 pinocchio::ModelTpl<Scalar>& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_pinocchio()
const {
166 template <
typename Scalar>
167 const boost::shared_ptr<ActuationModelFloatingBaseTpl<Scalar> >&
168 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_actuation()
const {
172 template <
typename Scalar>
173 const boost::shared_ptr<ContactModelMultipleTpl<Scalar> >&
174 DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_contacts()
const {
178 template <
typename Scalar>
179 const boost::shared_ptr<CostModelSumTpl<Scalar> >& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_costs()
184 template <
typename Scalar>
185 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_armature()
190 template <
typename Scalar>
191 const Scalar& DifferentialActionModelContactFwdDynamicsTpl<Scalar>::get_damping_factor()
const {
192 return JMinvJt_damping_;
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()) +
")");
201 armature_ = armature;
202 with_armature_ =
false;
205 template <
typename Scalar>
206 void DifferentialActionModelContactFwdDynamicsTpl<Scalar>::set_damping_factor(
const Scalar& damping) {
208 throw_pretty(
"Invalid argument: " 209 <<
"The damping factor has to be positive");
211 JMinvJt_damping_ = damping;
Definition: action-base.hxx:11