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()),
19 pinocchio_(*state->get_pinocchio().get()),
21 armature_(VectorXs::Zero(state->get_nv())),
23 JMinvJt_damping_(JMinvJt_damping),
24 enable_force_(enable_force),
25 gravity_(state->get_pinocchio()->gravity) {
28 throw_pretty(
"Invalid argument: " 29 <<
"The restitution coefficient has to be positive, set to 0");
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");
38 template <
typename Scalar>
39 ActionModelImpulseFwdDynamicsTpl<Scalar>::~ActionModelImpulseFwdDynamicsTpl() {}
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()) +
")");
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);
57 pinocchio::computeAllTerms(pinocchio_, d->pinocchio, q, v);
58 pinocchio::updateFramePlacements(pinocchio_, d->pinocchio);
59 pinocchio::computeCentroidalMomentum(pinocchio_, d->pinocchio);
61 if (!with_armature_) {
62 d->pinocchio.M.diagonal() += armature_;
64 impulses_->calc(d->multibody.impulses, x);
67 Eigen::FullPivLU<MatrixXs> Jc_lu(d->multibody.impulses->Jc);
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");
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);
81 costs_->calc(d->costs, x, u);
82 d->cost = d->costs->cost;
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()) +
")");
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);
99 ActionDataImpulseFwdDynamicsTpl<Scalar>* d =
static_cast<ActionDataImpulseFwdDynamicsTpl<Scalar>*
>(data.get());
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);
108 pinocchio::computeForwardKinematicsDerivatives(pinocchio_, d->pinocchio, q, d->pinocchio.dq_after, d->vnone);
109 impulses_->calcDiff(d->multibody.impulses, x);
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);
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>();
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);
129 costs_->calcDiff(d->costs, x, u);
132 template <
typename Scalar>
133 boost::shared_ptr<ActionDataAbstractTpl<Scalar> > ActionModelImpulseFwdDynamicsTpl<Scalar>::createData() {
134 return boost::make_shared<ActionDataImpulseFwdDynamicsTpl<Scalar> >(
this);
137 template <
typename Scalar>
138 pinocchio::ModelTpl<Scalar>& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_pinocchio()
const {
142 template <
typename Scalar>
143 const boost::shared_ptr<ImpulseModelMultipleTpl<Scalar> >& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_impulses()
148 template <
typename Scalar>
149 const boost::shared_ptr<CostModelSumTpl<Scalar> >& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_costs()
const {
153 template <
typename Scalar>
154 const typename MathBaseTpl<Scalar>::VectorXs& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_armature()
const {
158 template <
typename Scalar>
159 const Scalar& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_restitution_coefficient()
const {
163 template <
typename Scalar>
164 const Scalar& ActionModelImpulseFwdDynamicsTpl<Scalar>::get_damping_factor()
const {
165 return JMinvJt_damping_;
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()) +
")");
174 armature_ = armature;
175 with_armature_ =
false;
178 template <
typename Scalar>
179 void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_restitution_coefficient(
const Scalar& r_coeff) {
181 throw_pretty(
"Invalid argument: " 182 <<
"The restitution coefficient has to be positive");
187 template <
typename Scalar>
188 void ActionModelImpulseFwdDynamicsTpl<Scalar>::set_damping_factor(
const Scalar& damping) {
190 throw_pretty(
"Invalid argument: " 191 <<
"The damping factor has to be positive");
193 JMinvJt_damping_ = damping;
Definition: action-base.hxx:11