impulse-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 namespace crocoddyl {
10 
11 template <typename Scalar>
12 ActionModelImpulseFwdDynamicsTpl<Scalar>::ActionModelImpulseFwdDynamicsTpl(
13  boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ImpulseModelMultiple> impulses,
14  boost::shared_ptr<CostModelSum> costs, const Scalar& r_coeff, const Scalar& JMinvJt_damping,
15  const bool& enable_force)
16  : Base(state, 0, costs->get_nr()),
17  impulses_(impulses),
18  costs_(costs),
19  pinocchio_(*state->get_pinocchio().get()),
20  with_armature_(true),
21  armature_(VectorXs::Zero(state->get_nv())),
22  r_coeff_(r_coeff),
23  JMinvJt_damping_(JMinvJt_damping),
24  enable_force_(enable_force),
25  gravity_(state->get_pinocchio()->gravity) {
26  if (r_coeff_ < 0.) {
27  r_coeff_ = 0.;
28  throw_pretty("Invalid argument: "
29  << "The restitution coefficient has to be positive, set to 0");
30  }
31  if (JMinvJt_damping_ < 0.) {
32  JMinvJt_damping_ = 0.;
33  throw_pretty("Invalid argument: "
34  << "The damping factor has to be positive, set to 0");
35  }
36 }
37 
38 template <typename Scalar>
39 ActionModelImpulseFwdDynamicsTpl<Scalar>::~ActionModelImpulseFwdDynamicsTpl() {}
40 
41 template <typename Scalar>
42 void ActionModelImpulseFwdDynamicsTpl<Scalar>::calc(const boost::shared_ptr<ActionDataAbstract>& data,
43  const Eigen::Ref<const VectorXs>& x,
44  const Eigen::Ref<const VectorXs>& u) {
45  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
46  throw_pretty("Invalid argument: "
47  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
48  }
49 
50  const std::size_t& nq = state_->get_nq();
51  const std::size_t& nv = state_->get_nv();
52  ActionDataImpulseFwdDynamicsTpl<Scalar>* d = static_cast<ActionDataImpulseFwdDynamicsTpl<Scalar>*>(data.get());
53  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(nq);
54  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(nv);
55 
56  // Computing the forward dynamics with the holonomic constraints defined by the contact model
57  pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
58  pinocchio::updateFramePlacements(pinocchio_, d->pinocchio);
59  pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
60 
61  if (!with_armature_) {
62  d->pinocchio.M.diagonal() += armature_;
63  }
64  impulses_->calc(d->multibody.impulses, x);
65 
66 #ifndef NDEBUG
67  Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.impulses->Jc);
68 
69  if (Jc_lu.rank() < d->multibody.impulses->Jc.rows()) {
70  assert_pretty(JMinvJt_damping_ > 0., "It is needed a damping factor since the contact Jacobian is not full-rank");
71  }
72 #endif
73 
74  pinocchio::impulseDynamics(pinocchio_, d->pinocchio, v, d->multibody.impulses->Jc, r_coeff_, JMinvJt_damping_);
75  d->xnext.head(nq) = q;
76  d->xnext.tail(nv) = d->pinocchio.dq_after;
77  impulses_->updateVelocity(d->multibody.impulses, d->pinocchio.dq_after);
78  impulses_->updateForce(d->multibody.impulses, d->pinocchio.impulse_c);
79 
80  // Computing the cost value and residuals
81  costs_->calc(d->costs, x, u);
82  d->cost = d->costs->cost;
83 }
84 
85 template <typename Scalar>
86 void ActionModelImpulseFwdDynamicsTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
87  const Eigen::Ref<const VectorXs>& x,
88  const Eigen::Ref<const VectorXs>& u) {
89  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
90  throw_pretty("Invalid argument: "
91  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
92  }
93 
94  const std::size_t& nv = state_->get_nv();
95  const std::size_t& ni = impulses_->get_ni();
96  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(state_->get_nq());
97  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(nv);
98 
99  ActionDataImpulseFwdDynamicsTpl<Scalar>* d = static_cast<ActionDataImpulseFwdDynamicsTpl<Scalar>*>(data.get());
100 
101  // Computing the dynamics derivatives
102  pinocchio_.gravity.setZero();
103  pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, d->vnone, d->pinocchio.dq_after - v,
104  d->multibody.impulses->fext);
105  pinocchio_.gravity = gravity_;
106  pinocchio::getKKTContactDynamicMatrixInverse(pinocchio_, d->pinocchio, d->multibody.impulses->Jc, d->Kinv);
107 
108  pinocchio::computeForwardKinematicsDerivatives(pinocchio_, d->pinocchio, q, d->pinocchio.dq_after, d->vnone);
109  impulses_->calcDiff(d->multibody.impulses, x);
110 
111  Eigen::Block<MatrixXs> a_partial_dtau = d->Kinv.topLeftCorner(nv, nv);
112  Eigen::Block<MatrixXs> a_partial_da = d->Kinv.topRightCorner(nv, ni);
113  Eigen::Block<MatrixXs> f_partial_dtau = d->Kinv.bottomLeftCorner(ni, nv);
114  Eigen::Block<MatrixXs> f_partial_da = d->Kinv.bottomRightCorner(ni, ni);
115 
116  d->Fx.topLeftCorner(nv, nv).setIdentity();
117  d->Fx.topRightCorner(nv, nv).setZero();
118  d->Fx.bottomLeftCorner(nv, nv).noalias() = -a_partial_dtau * d->pinocchio.dtau_dq;
119  d->Fx.bottomLeftCorner(nv, nv).noalias() -= a_partial_da * d->multibody.impulses->dv0_dq;
120  d->Fx.bottomRightCorner(nv, nv).noalias() = a_partial_dtau * d->pinocchio.M.template selfadjointView<Eigen::Upper>();
121 
122  // Computing the cost derivatives
123  if (enable_force_) {
124  d->df_dq.noalias() = f_partial_dtau * d->pinocchio.dtau_dq;
125  d->df_dq.noalias() += f_partial_da * d->multibody.impulses->dv0_dq;
126  impulses_->updateVelocityDiff(d->multibody.impulses, d->Fx.bottomRows(nv));
127  impulses_->updateForceDiff(d->multibody.impulses, d->df_dq);
128  }
129  costs_->calcDiff(d->costs, x, u);
130 }
131 
132 template <typename Scalar>
133 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelImpulseFwdDynamicsTpl<Scalar>::createData() {
134  return boost::make_shared<ActionDataImpulseFwdDynamicsTpl<Scalar> >(this);
135 }
136 
137 template <typename Scalar>
138 pinocchio::ModelTpl<Scalar>& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_pinocchio() const {
139  return pinocchio_;
140 }
141 
142 template <typename Scalar>
143 const boost::shared_ptr<ImpulseModelMultipleTpl<Scalar> >& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_impulses()
144  const {
145  return impulses_;
146 }
147 
148 template <typename Scalar>
149 const boost::shared_ptr<CostModelSumTpl<Scalar> >& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_costs() const {
150  return costs_;
151 }
152 
153 template <typename Scalar>
154 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_armature() const {
155  return armature_;
156 }
157 
158 template <typename Scalar>
159 const Scalar& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_restitution_coefficient() const {
160  return r_coeff_;
161 }
162 
163 template <typename Scalar>
164 const Scalar& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_damping_factor() const {
165  return JMinvJt_damping_;
166 }
167 
168 template <typename Scalar>
169 void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_armature(const VectorXs& armature) {
170  if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
171  throw_pretty("Invalid argument: "
172  << "The armature dimension is wrong (it should be " + std::to_string(state_->get_nv()) + ")");
173  }
174  armature_ = armature;
175  with_armature_ = false;
176 }
177 
178 template <typename Scalar>
179 void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_restitution_coefficient(const Scalar& r_coeff) {
180  if (r_coeff < 0.) {
181  throw_pretty("Invalid argument: "
182  << "The restitution coefficient has to be positive");
183  }
184  r_coeff_ = r_coeff;
185 }
186 
187 template <typename Scalar>
188 void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_damping_factor(const Scalar& damping) {
189  if (damping < 0.) {
190  throw_pretty("Invalid argument: "
191  << "The damping factor has to be positive");
192  }
193  JMinvJt_damping_ = damping;
194 }
195 
196 } // namespace crocoddyl
Definition: action-base.hxx:11