change rnea by computeGeneralizedGravity

include/cobotmpc/crocomplements/actuation-human-impedance.hxx

@@ -53,10 +53,10 @@ void ActuationModelHumanImpedanceTpl<Scalar>::calc(const boost::shared_ptr<Actua
     const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
       x.tail(state_->get_nv());
 
-    pinocchio::rnea(*pin_model_.get(), *d->pinocchio, q, VectorXs::Zero(state_->get_nv()) ,VectorXs::Zero(state_->get_nv()));
+    pinocchio::computeGeneralizedGravity(*pin_model_.get(), *d->pinocchio, q);
 
-    d->tau.head(nu_human_) = (Kp_*(xref_.head(nu_human_)-q.head(nu_human_)) + Kd_*(xref_.tail(nu_human_)-v.head(nu_human_)) + d->pinocchio->tau.head(nu_human_));
-    d->tau.tail(nu_) = u + d->pinocchio->tau.tail(nu_);
+    d->tau.head(nu_human_) = (Kp_*(xref_.head(nu_human_)-q.head(nu_human_)) + Kd_*(xref_.tail(nu_human_)-v.head(nu_human_)) + d->pinocchio->g.head(nu_human_));
+    d->tau.tail(nu_) = u + d->pinocchio->g.tail(nu_);
   }
 
 template <typename Scalar>  
@@ -70,16 +70,17 @@ void ActuationModelHumanImpedanceTpl<Scalar>::calcDiff(const boost::shared_ptr<A
       x.head(state_->get_nq());
     // const Eigen::VectorBlock<const Eigen::Ref<const VectorXs>, Eigen::Dynamic> v =
     //   x.tail(state_->get_nv());
+    MatrixXs gravity_partial_dq = MatrixXs::Zero(state_->get_nv(),state_->get_nv());
 
     d->dtau_du.topRows(nu_human_) = MatrixXs::Zero(nu_human_,nu_);
     d->dtau_du.bottomRows(nu_) = MatrixXs::Identity(nu_,nu_);
 
-    pinocchio::computeRNEADerivatives(*pin_model_.get(),*d->pinocchio,q,VectorXs::Zero(state_->get_nv()),VectorXs::Zero(state_->get_nv()));
+    pinocchio::computeGeneralizedGravityDerivatives(*pin_model_.get(),*d->pinocchio,q, gravity_partial_dq);
 
-    d->dtau_dx.leftCols(state_->get_nq())=d->pinocchio->dtau_dq;
-    d->dtau_dx.rightCols(state_->get_nv())=d->pinocchio->dtau_dv;
+    d->dtau_dx.leftCols(state_->get_nv())=gravity_partial_dq;
+    // d->dtau_dx.rightCols(state_->get_nv())=d->pinocchio->dtau_dv;
     d->dtau_dx.topLeftCorner(nu_human_,nu_human_) += -Kp_*MatrixXs::Identity(nu_human_,nu_human_);
-    d->dtau_dx.block(0,state_->get_nq(),nu_human_,nu_human_) += -Kd_*MatrixXs::Identity(nu_human_,nu_human_);
+    d->dtau_dx.block(0,state_->get_nv(),nu_human_,nu_human_) += -Kd_*MatrixXs::Identity(nu_human_,nu_human_);
     
     }