9 #include "crocoddyl/core/utils/exception.hpp" 10 #include "crocoddyl/multibody/costs/impulse-com.hpp" 12 #include <pinocchio/algorithm/center-of-mass.hpp> 13 #include <pinocchio/algorithm/center-of-mass-derivatives.hpp> 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");
27 template <
typename Scalar>
28 CostModelImpulseCoMTpl<Scalar>::CostModelImpulseCoMTpl(boost::shared_ptr<StateMultibody> state) : Base(state, 3, 0) {}
30 template <
typename Scalar>
31 CostModelImpulseCoMTpl<Scalar>::~CostModelImpulseCoMTpl() {}
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>&) {
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);
43 pinocchio::centerOfMass(*state_->get_pinocchio().get(), d->pinocchio_internal, q, d->impulses->vnext - v);
44 data->r = d->pinocchio_internal.vcom[0];
47 activation_->calc(data->activation, data->r);
48 data->cost = data->activation->a_value;
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());
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);
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;
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;
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);
Definition: action-base.hxx:11