free-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/free-fwddyn.hpp"
11 
12 #include <pinocchio/algorithm/aba.hpp>
13 #include <pinocchio/algorithm/aba-derivatives.hpp>
14 #include <pinocchio/algorithm/rnea-derivatives.hpp>
15 #include <pinocchio/algorithm/compute-all-terms.hpp>
16 #include <pinocchio/algorithm/kinematics.hpp>
17 #include <pinocchio/algorithm/jacobian.hpp>
18 #include <pinocchio/algorithm/frames.hpp>
19 #include <pinocchio/algorithm/cholesky.hpp>
20 
21 namespace crocoddyl {
22 
23 template <typename Scalar>
24 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::DifferentialActionModelFreeFwdDynamicsTpl(
25  boost::shared_ptr<StateMultibody> state, boost::shared_ptr<ActuationModelAbstract> actuation,
26  boost::shared_ptr<CostModelSum> costs)
27  : Base(state, actuation->get_nu(), costs->get_nr()),
28  actuation_(actuation),
29  costs_(costs),
30  pinocchio_(*state->get_pinocchio().get()),
31  with_armature_(true),
32  armature_(VectorXs::Zero(state->get_nv())) {
33  if (costs_->get_nu() != nu_) {
34  throw_pretty("Invalid argument: "
35  << "Costs doesn't have the same control dimension (it should be " + std::to_string(nu_) + ")");
36  }
37 }
38 
39 template <typename Scalar>
40 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::~DifferentialActionModelFreeFwdDynamicsTpl() {}
41 
42 template <typename Scalar>
43 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calc(
44  const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
45  const Eigen::Ref<const VectorXs>& u) {
46  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
47  throw_pretty("Invalid argument: "
48  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
49  }
50  if (static_cast<std::size_t>(u.size()) != nu_) {
51  throw_pretty("Invalid argument: "
52  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
53  }
54 
55  DifferentialActionDataFreeFwdDynamicsTpl<Scalar>* d =
56  static_cast<DifferentialActionDataFreeFwdDynamicsTpl<Scalar>*>(data.get());
57  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(state_->get_nq());
58  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(state_->get_nv());
59 
60  actuation_->calc(d->multibody.actuation, x, u);
61 
62  // Computing the dynamics using ABA or manually for armature case
63  if (with_armature_) {
64  d->xout = pinocchio::aba(pinocchio_, d->pinocchio, q, v, d->multibody.actuation->tau);
65  pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
66  } else {
67  pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
68  d->pinocchio.M.diagonal() += armature_;
69  pinocchio::cholesky::decompose(pinocchio_, d->pinocchio);
70  d->Minv.setZero();
71  pinocchio::cholesky::computeMinv(pinocchio_, d->pinocchio, d->Minv);
72  d->u_drift = d->multibody.actuation->tau - d->pinocchio.nle;
73  d->xout.noalias() = d->Minv * d->u_drift;
74  }
75 
76  // Computing the cost value and residuals
77  costs_->calc(d->costs, x, u);
78  d->cost = d->costs->cost;
79 }
80 
81 template <typename Scalar>
82 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::calcDiff(
83  const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
84  const Eigen::Ref<const VectorXs>& u) {
85  if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
86  throw_pretty("Invalid argument: "
87  << "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
88  }
89  if (static_cast<std::size_t>(u.size()) != nu_) {
90  throw_pretty("Invalid argument: "
91  << "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
92  }
93 
94  const std::size_t& nv = state_->get_nv();
95  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(state_->get_nq());
96  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(nv);
97 
98  DifferentialActionDataFreeFwdDynamicsTpl<Scalar>* d =
99  static_cast<DifferentialActionDataFreeFwdDynamicsTpl<Scalar>*>(data.get());
100 
101  actuation_->calcDiff(d->multibody.actuation, x, u);
102 
103  // Computing the dynamics derivatives
104  if (with_armature_) {
105  pinocchio::computeABADerivatives(pinocchio_, d->pinocchio, q, v, d->multibody.actuation->tau, d->Fx.leftCols(nv),
106  d->Fx.rightCols(nv), d->pinocchio.Minv);
107  d->Fx.noalias() += d->pinocchio.Minv * d->multibody.actuation->dtau_dx;
108  d->Fu.noalias() = d->pinocchio.Minv * d->multibody.actuation->dtau_du;
109  } else {
110  pinocchio::computeRNEADerivatives(pinocchio_, d->pinocchio, q, v, d->xout);
111  d->dtau_dx.leftCols(nv) = d->multibody.actuation->dtau_dx.leftCols(nv) - d->pinocchio.dtau_dq;
112  d->dtau_dx.rightCols(nv) = d->multibody.actuation->dtau_dx.rightCols(nv) - d->pinocchio.dtau_dv;
113  d->Fx.noalias() = d->Minv * d->dtau_dx;
114  d->Fu.noalias() = d->Minv * d->multibody.actuation->dtau_du;
115  }
116 
117  // Computing the cost derivatives
118  costs_->calcDiff(d->costs, x, u);
119 }
120 
121 template <typename Scalar>
122 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
123 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::createData() {
124  return boost::make_shared<DifferentialActionDataFreeFwdDynamicsTpl<Scalar> >(this);
125 }
126 
127 template <typename Scalar>
128 pinocchio::ModelTpl<Scalar>& DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_pinocchio() const {
129  return pinocchio_;
130 }
131 
132 template <typename Scalar>
133 const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
134 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_actuation() const {
135  return actuation_;
136 }
137 
138 template <typename Scalar>
139 const boost::shared_ptr<CostModelSumTpl<Scalar> >& DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_costs()
140  const {
141  return costs_;
142 }
143 
144 template <typename Scalar>
145 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_armature() const {
146  return armature_;
147 }
148 
149 template <typename Scalar>
150 void DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::set_armature(const VectorXs& armature) {
151  if (static_cast<std::size_t>(armature.size()) != state_->get_nv()) {
152  throw_pretty("Invalid argument: "
153  << "The armature dimension is wrong (it should be " + std::to_string(state_->get_nv()) + ")");
154  }
155 
156  armature_ = armature;
157  with_armature_ = false;
158 }
159 
160 } // namespace crocoddyl
Definition: action-base.hxx:11