9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/actions/free-fwddyn.hpp" 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> 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),
30 pinocchio_(*state->get_pinocchio().get()),
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_) +
")");
39 template <
typename Scalar>
40 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::~DifferentialActionModelFreeFwdDynamicsTpl() {}
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()) +
")");
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_) +
")");
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());
60 actuation_->calc(d->multibody.actuation, x, u);
64 d->xout = pinocchio::aba(pinocchio_, d->pinocchio, q, v, d->multibody.actuation->tau);
65 pinocchio::updateGlobalPlacements(pinocchio_, d->pinocchio);
67 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
68 d->pinocchio.M.diagonal() += armature_;
69 pinocchio::cholesky::decompose(pinocchio_, d->pinocchio);
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;
77 costs_->calc(d->costs, x, u);
78 d->cost = d->costs->cost;
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()) +
")");
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_) +
")");
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);
98 DifferentialActionDataFreeFwdDynamicsTpl<Scalar>* d =
99 static_cast<DifferentialActionDataFreeFwdDynamicsTpl<Scalar>*
>(data.get());
101 actuation_->calcDiff(d->multibody.actuation, x, u);
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;
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;
118 costs_->calcDiff(d->costs, x, u);
121 template <
typename Scalar>
122 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >
123 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::createData() {
124 return boost::make_shared<DifferentialActionDataFreeFwdDynamicsTpl<Scalar> >(
this);
127 template <
typename Scalar>
128 pinocchio::ModelTpl<Scalar>& DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_pinocchio()
const {
132 template <
typename Scalar>
133 const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
134 DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_actuation()
const {
138 template <
typename Scalar>
139 const boost::shared_ptr<CostModelSumTpl<Scalar> >& DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_costs()
144 template <
typename Scalar>
145 const typename MathBaseTpl<Scalar>::VectorXs& DifferentialActionModelFreeFwdDynamicsTpl<Scalar>::get_armature()
const {
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()) +
")");
156 armature_ = armature;
157 with_armature_ =
false;
Definition: action-base.hxx:11