6 #include "pinocchio/algorithm/centroidal-derivatives.hpp" 8 #include <boost/python/tuple.hpp> 22 Matrix6x partialh_dq(Matrix6x::Zero(6,model.
nv));
23 Matrix6x partial_dq(Matrix6x::Zero(6,model.
nv));
24 Matrix6x partial_dv(Matrix6x::Zero(6,model.
nv));
25 Matrix6x partial_da(Matrix6x::Zero(6,model.
nv));
28 partialh_dq, partial_dq, partial_dv, partial_da);
30 return bp::make_tuple(partialh_dq, partial_dq,partial_dv,partial_da);
38 Matrix6x partialh_dq(Matrix6x::Zero(6,model.
nv));
39 Matrix6x partial_dq(Matrix6x::Zero(6,model.
nv));
40 Matrix6x partial_dv(Matrix6x::Zero(6,model.
nv));
41 Matrix6x partial_da(Matrix6x::Zero(6,model.
nv));
44 return bp::make_tuple(partialh_dq,partial_dq,partial_dv,partial_da);
49 using namespace Eigen;
51 bp::def(
"computeCentroidalDynamicsDerivatives",
53 bp::args(
"Model",
"Data",
"q",
"v",
"a"),
54 "Computes the analytical derivatives of the centroidal dynamics\n" 55 "with respect to the joint configuration vector, velocity and acceleration.");
57 bp::def(
"getCentroidalDynamicsDerivatives",
59 bp::args(
"Model",
"Data"),
60 "Retrive the analytical derivatives of the centroidal dynamics\n" 61 "from the RNEA derivatives.\n" 62 "pinocchio.computeRNEADerivatives should have been called first.");
void computeCentroidalDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Eigen::MatrixBase< Matrix6xLike0 > &dh_dq, const Eigen::MatrixBase< Matrix6xLike1 > &dhdot_dq, const Eigen::MatrixBase< Matrix6xLike2 > &dhdot_dv, const Eigen::MatrixBase< Matrix6xLike3 > &dhdot_da)
Computes the analytical derivatives of the centroidal dynamics with respect to the joint configuratio...
bp::tuple computeCentroidalDynamicsDerivatives_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
void getCentroidalDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Matrix6xLike1 > &dh_dq, const Eigen::MatrixBase< Matrix6xLike1 > &dhdot_dq, const Eigen::MatrixBase< Matrix6xLike2 > &dhdot_dv, const Eigen::MatrixBase< Matrix6xLike3 > &dhdot_da)
Retrive the analytical derivatives of the centroidal dynamics from the RNEA derivatives. pinocchio::computeRNEADerivatives should have been called first.
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
void exposeCentroidalDerivatives()
Main pinocchio namespace.
int nv
Dimension of the velocity vector space.
bp::tuple getCentroidalDynamicsDerivatives_proxy(const Model &model, Data &data)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
JointCollectionTpl & model