Commit 715867fe by Rohan Budhiraja

[algorithm][dhg/dq] add computation of dhg/dq inside computecentroidaldynamicsderivatives

parent 14c5e657
 ... ... @@ -34,6 +34,7 @@ namespace pinocchio /// \param[in] q The joint configuration vector (dim model.nq). /// \param[in] v The joint velocity vector (dim model.nv). /// \param[in] a The joint acceleration vector (dim model.nv). /// \param[out] dh_dq The partial derivative of the centroidal momentum with respect to the configuration vector (dim 6 x model.nv). /// \param[out] dhdot_dq The partial derivative of the centroidal dynamics with respect to the configuration vector (dim 6 x model.nv). /// \param[out] dhdot_dv The partial derivative of the centroidal dynamics with respect to the velocity vector (dim 6 x model.nv). /// \param[out] dhdot_da The partial derivative of the centroidal dynamics with respect to the acceleration vector (dim 6 x model.nv). ... ... @@ -43,13 +44,14 @@ namespace pinocchio /// template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> inline void computeCentroidalDynamicsDerivatives(const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, const Eigen::MatrixBase & a, const Eigen::MatrixBase & dh_dq, const Eigen::MatrixBase & dhdot_dq, const Eigen::MatrixBase & dhdot_dv, const Eigen::MatrixBase & dhdot_da); ... ...
 ... ... @@ -142,6 +142,7 @@ namespace pinocchio ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq); ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); ColsBlock dFda_cols = jmodel.jointCols(data.dFda); ... ... @@ -171,6 +172,9 @@ namespace pinocchio data.doYcrb[parent] += data.doYcrb[i]; data.oh[parent] += data.oh[i]; data.of[parent] += data.of[i]; forceSet::motionActions(J_cols, data.oh[i], dHdq_cols); motionSet::inertiaAction(data.oYcrb[i], dVdq_cols, dHdq_cols); } template ... ... @@ -211,13 +215,14 @@ namespace pinocchio template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> inline void computeCentroidalDynamicsDerivatives(const ModelTpl & model, DataTpl & data, const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, const Eigen::MatrixBase & a, const Eigen::MatrixBase & dh_dq, const Eigen::MatrixBase & dhdot_dq, const Eigen::MatrixBase & dhdot_dv, const Eigen::MatrixBase & dhdot_da) ... ... @@ -225,6 +230,8 @@ namespace pinocchio assert(q.size() == model.nq && "The joint configuration vector is not of right size"); assert(v.size() == model.nv && "The joint velocity vector is not of right size"); assert(a.size() == model.nv && "The joint acceleration vector is not of right size"); assert(dh_dq.cols() == model.nv); assert(dh_dq.rows() == 6); assert(dhdot_dq.cols() == model.nv); assert(dhdot_dq.rows() == 6); assert(dhdot_dv.cols() == model.nv); ... ... @@ -278,6 +285,7 @@ namespace pinocchio data.Ig.inertia() = Ytot.inertia(); // Compute the partial derivatives translateForceSet(data.dHdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike0,dh_dq)); translateForceSet(data.dFdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq)); translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv)); translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da)); ... ...
 ... ... @@ -143,6 +143,9 @@ namespace pinocchio /// \brief The Coriolis matrix (a square matrix of dim model.nv). MatrixXs C; /// \brief Variation of the spatial momenta with respect to the joint configuration. Matrix6x dHdq; /// \brief Variation of the forceset with respect to the joint configuration. Matrix6x dFdq; ... ...
 ... ... @@ -44,6 +44,7 @@ namespace pinocchio , M(model.nv,model.nv) , Minv(model.nv,model.nv) , C(model.nv,model.nv) , dHdq(6,model.nv) , dFdq(6,model.nv) , dFdv(6,model.nv) , dFda(6,model.nv) ... ...
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!