Commit 715867fe authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

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

parent 14c5e657
...@@ -34,6 +34,7 @@ namespace pinocchio ...@@ -34,6 +34,7 @@ namespace pinocchio
/// \param[in] q The joint configuration vector (dim model.nq). /// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] v The joint velocity vector (dim model.nv). /// \param[in] v The joint velocity vector (dim model.nv).
/// \param[in] a The joint acceleration 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_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_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). /// \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 ...@@ -43,13 +44,14 @@ namespace pinocchio
/// ///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3>
inline void inline void
computeCentroidalDynamicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, computeCentroidalDynamicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data, DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q, const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v, const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a, const Eigen::MatrixBase<TangentVectorType2> & a,
const Eigen::MatrixBase<Matrix6xLike0> & dh_dq,
const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq,
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da); const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da);
......
...@@ -142,6 +142,7 @@ namespace pinocchio ...@@ -142,6 +142,7 @@ namespace pinocchio
ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq); ColsBlock dVdq_cols = jmodel.jointCols(data.dVdq);
ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq); ColsBlock dAdq_cols = jmodel.jointCols(data.dAdq);
ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv); ColsBlock dAdv_cols = jmodel.jointCols(data.dAdv);
ColsBlock dHdq_cols = jmodel.jointCols(data.dHdq);
ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq); ColsBlock dFdq_cols = jmodel.jointCols(data.dFdq);
ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv); ColsBlock dFdv_cols = jmodel.jointCols(data.dFdv);
ColsBlock dFda_cols = jmodel.jointCols(data.dFda); ColsBlock dFda_cols = jmodel.jointCols(data.dFda);
...@@ -171,6 +172,9 @@ namespace pinocchio ...@@ -171,6 +172,9 @@ namespace pinocchio
data.doYcrb[parent] += data.doYcrb[i]; data.doYcrb[parent] += data.doYcrb[i];
data.oh[parent] += data.oh[i]; data.oh[parent] += data.oh[i];
data.of[parent] += data.of[i]; data.of[parent] += data.of[i];
forceSet::motionActions(J_cols, data.oh[i], dHdq_cols);
motionSet::inertiaAction<ADDTO>(data.oYcrb[i], dVdq_cols, dHdq_cols);
} }
template<typename Min, typename Mout> template<typename Min, typename Mout>
...@@ -211,13 +215,14 @@ namespace pinocchio ...@@ -211,13 +215,14 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl,
typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2,
typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3> typename Matrix6xLike0, typename Matrix6xLike1, typename Matrix6xLike2, typename Matrix6xLike3>
inline void inline void
computeCentroidalDynamicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, computeCentroidalDynamicsDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data, DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q, const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType1> & v, const Eigen::MatrixBase<TangentVectorType1> & v,
const Eigen::MatrixBase<TangentVectorType2> & a, const Eigen::MatrixBase<TangentVectorType2> & a,
const Eigen::MatrixBase<Matrix6xLike0> & dh_dq,
const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq, const Eigen::MatrixBase<Matrix6xLike1> & dhdot_dq,
const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv, const Eigen::MatrixBase<Matrix6xLike2> & dhdot_dv,
const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da) const Eigen::MatrixBase<Matrix6xLike3> & dhdot_da)
...@@ -225,6 +230,8 @@ namespace pinocchio ...@@ -225,6 +230,8 @@ namespace pinocchio
assert(q.size() == model.nq && "The joint configuration vector is not of right size"); 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(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(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.cols() == model.nv);
assert(dhdot_dq.rows() == 6); assert(dhdot_dq.rows() == 6);
assert(dhdot_dv.cols() == model.nv); assert(dhdot_dv.cols() == model.nv);
...@@ -278,6 +285,7 @@ namespace pinocchio ...@@ -278,6 +285,7 @@ namespace pinocchio
data.Ig.inertia() = Ytot.inertia(); data.Ig.inertia() = Ytot.inertia();
// Compute the partial derivatives // 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.dFdq,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike1,dhdot_dq));
translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv)); translateForceSet(data.dFdv,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike2,dhdot_dv));
translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da)); translateForceSet(data.dFda,com,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike3,dhdot_da));
......
...@@ -143,6 +143,9 @@ namespace pinocchio ...@@ -143,6 +143,9 @@ namespace pinocchio
/// \brief The Coriolis matrix (a square matrix of dim model.nv). /// \brief The Coriolis matrix (a square matrix of dim model.nv).
MatrixXs C; 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. /// \brief Variation of the forceset with respect to the joint configuration.
Matrix6x dFdq; Matrix6x dFdq;
......
...@@ -44,6 +44,7 @@ namespace pinocchio ...@@ -44,6 +44,7 @@ namespace pinocchio
, M(model.nv,model.nv) , M(model.nv,model.nv)
, Minv(model.nv,model.nv) , Minv(model.nv,model.nv)
, C(model.nv,model.nv) , C(model.nv,model.nv)
, dHdq(6,model.nv)
, dFdq(6,model.nv) , dFdq(6,model.nv)
, dFdv(6,model.nv) , dFdv(6,model.nv)
, dFda(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!
Please register or to comment