impulse-com.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/costs/impulse-com.hpp"
11 
12 #include <pinocchio/algorithm/center-of-mass.hpp>
13 #include <pinocchio/algorithm/center-of-mass-derivatives.hpp>
14 
15 namespace crocoddyl {
16 
17 template <typename Scalar>
18 CostModelImpulseCoMTpl<Scalar>::CostModelImpulseCoMTpl(boost::shared_ptr<StateMultibody> state,
19  boost::shared_ptr<ActivationModelAbstract> activation)
20  : Base(state, activation, 0) {
21  if (activation_->get_nr() != 3) {
22  throw_pretty("Invalid argument: "
23  << "nr is equals to 3");
24  }
25 }
26 
27 template <typename Scalar>
28 CostModelImpulseCoMTpl<Scalar>::CostModelImpulseCoMTpl(boost::shared_ptr<StateMultibody> state) : Base(state, 3, 0) {}
29 
30 template <typename Scalar>
31 CostModelImpulseCoMTpl<Scalar>::~CostModelImpulseCoMTpl() {}
32 
33 template <typename Scalar>
34 void CostModelImpulseCoMTpl<Scalar>::calc(const boost::shared_ptr<CostDataAbstract>& data,
35  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
36  // Compute the cost residual give the reference CoM position
37  CostDataImpulseCoMTpl<Scalar>* d = static_cast<CostDataImpulseCoMTpl<Scalar>*>(data.get());
38  const std::size_t& nq = state_->get_nq();
39  const std::size_t& nv = state_->get_nv();
40  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> q = x.head(nq);
41  const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v = x.tail(nv);
42 
43  pinocchio::centerOfMass(*state_->get_pinocchio().get(), d->pinocchio_internal, q, d->impulses->vnext - v);
44  data->r = d->pinocchio_internal.vcom[0];
45 
46  // Compute the cost
47  activation_->calc(data->activation, data->r);
48  data->cost = data->activation->a_value;
49 }
50 
51 template <typename Scalar>
52 void CostModelImpulseCoMTpl<Scalar>::calcDiff(const boost::shared_ptr<CostDataAbstract>& data,
53  const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {
54  CostDataImpulseCoMTpl<Scalar>* d = static_cast<CostDataImpulseCoMTpl<Scalar>*>(data.get());
55 
56  // Compute the derivatives of the frame placement
57  const std::size_t& nv = state_->get_nv();
58  const std::size_t& ndx = state_->get_ndx();
59  activation_->calcDiff(data->activation, data->r);
60 
61  pinocchio::getCenterOfMassVelocityDerivatives(*state_->get_pinocchio().get(), d->pinocchio_internal, d->dvc_dq);
62  pinocchio::jacobianCenterOfMass(*state_->get_pinocchio().get(), d->pinocchio_internal, false);
63  data->Rx.leftCols(nv) = d->dvc_dq;
64  data->Rx.leftCols(nv).noalias() += d->pinocchio_internal.Jcom * d->impulses->dvnext_dx.leftCols(nv);
65  d->ddv_dv = d->impulses->dvnext_dx.rightCols(ndx - nv);
66  d->ddv_dv.diagonal().array() -= 1;
67  data->Rx.rightCols(ndx - nv).noalias() = d->pinocchio_internal.Jcom * d->ddv_dv;
68 
69  d->Arr_Rx.noalias() = data->activation->Arr * data->Rx;
70  data->Lx.noalias() = data->Rx.transpose() * data->activation->Ar;
71  data->Lxx.noalias() = data->Rx.transpose() * d->Arr_Rx;
72 }
73 
74 template <typename Scalar>
75 boost::shared_ptr<CostDataAbstractTpl<Scalar> > CostModelImpulseCoMTpl<Scalar>::createData(
76  DataCollectorAbstract* const data) {
77  return boost::make_shared<CostDataImpulseCoMTpl<Scalar> >(this, data);
78 }
79 
80 } // namespace crocoddyl
Definition: action-base.hxx:11