6 #include "pinocchio/algorithm/kinematics-derivatives.hpp" 7 #include "pinocchio/algorithm/center-of-mass-derivatives.hpp" 9 #include <boost/python/tuple.hpp> 24 Matrix6x partial_dq(Matrix6x::Zero(6,model.
nv));
25 Matrix6x partial_dv(Matrix6x::Zero(6,model.
nv));
28 partial_dq,partial_dv);
30 return bp::make_tuple(partial_dq,partial_dv);
40 Matrix6x v_partial_dq(Matrix6x::Zero(6,model.
nv));
41 Matrix6x a_partial_dq(Matrix6x::Zero(6,model.
nv));
42 Matrix6x a_partial_dv(Matrix6x::Zero(6,model.
nv));
43 Matrix6x a_partial_da(Matrix6x::Zero(6,model.
nv));
46 v_partial_dq,a_partial_dq,
47 a_partial_dv,a_partial_da);
49 return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
56 Matrix3x partial_dq(Matrix3x::Zero(3,model.
nv));
64 using namespace Eigen;
66 bp::def(
"computeForwardKinematicsDerivatives",
67 &computeForwardKinematicsDerivatives<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
68 bp::args(
"model",
"data",
"q",
"v",
"a"),
69 "Computes all the terms required to compute the derivatives of the placement, spatial velocity and acceleration\n" 70 "for any joint of the model.\n" 71 "The results are stored in data.\n\n" 73 "\tmodel: model of the kinematic tree\n" 74 "\tdata: data related to the model\n" 75 "\tq: the joint configuration vector (size model.nq)\n" 76 "\tv: the joint velocity vector (size model.nv)\n" 77 "\ta: the joint acceleration vector (size model.nv)\n");
79 bp::def(
"getJointVelocityDerivatives",
81 bp::args(
"model",
"data",
"joint_id",
"reference_frame"),
82 "Computes the partial derivatives of the spatial velocity of a given joint with respect to\n" 83 "the joint configuration and velocity and returns them as a tuple.\n" 84 "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n" 85 "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n" 87 "\tmodel: model of the kinematic tree\n" 88 "\tdata: data related to the model\n" 89 "\tjoint_id: index of the joint\n" 90 "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
92 bp::def(
"getJointAccelerationDerivatives",
94 bp::args(
"model",
"data",
"joint_id",
"reference_frame"),
95 "Computes the partial derivatives of the spatial acceleration of a given joint with respect to\n" 96 "the joint configuration, velocity and acceleration and returns them as a tuple.\n" 97 "The Jacobians can be either expressed in the LOCAL frame of the joint, in the LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of reference_frame.\n" 98 "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n" 100 "\tmodel: model of the kinematic tree\n" 101 "\tdata: data related to the model\n" 102 "\tjoint_id: index of the joint\n" 103 "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
105 bp::def(
"getCenterOfMassVelocityDerivatives",
107 bp::args(
"model",
"data"),
108 "Computes the partial derivaties of the center of mass velocity with respect to\n" 109 "the joint configuration.\n" 110 "You must first call computeAllTerms(model,data,q,v) or centerOfMass(model,data,q,v) " 111 "before calling this function.\n\n" 113 "\tmodel: model of the kinematic tree\n" 114 "\tdata: data related to the model\n");
void exposeKinematicsDerivatives()
void getJointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &v_partial_dv)
Computes the partial derivaties of the spatial velocity of a given with respect to the joint configur...
ReferenceFrame
List of Reference Frames supported by Pinocchio.
void getCenterOfMassVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< Matrix3xOut > &vcom_partial_dq)
Computes the partial derivatie of the center-of-mass velocity with respect to the joint configuration...
bp::tuple getJointAccelerationDerivatives_proxy(const Model &model, Data &data, const Model::JointIndex jointId, ReferenceFrame rf)
pinocchio::JointIndex JointIndex
bp::tuple getJointVelocityDerivatives_proxy(const Model &model, Data &data, const Model::JointIndex jointId, ReferenceFrame rf)
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
void getJointAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex jointId, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xOut1 > &v_partial_dq, const Eigen::MatrixBase< Matrix6xOut2 > &a_partial_dq, const Eigen::MatrixBase< Matrix6xOut3 > &a_partial_dv, const Eigen::MatrixBase< Matrix6xOut4 > &a_partial_da)
Computes the partial derivaties of the spatial acceleration of a given with respect to the joint conf...
Main pinocchio namespace.
int nv
Dimension of the velocity vector space.
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
JointCollectionTpl & model
Data::Matrix3x getCoMVelocityDerivatives_proxy(const Model &model, Data &data)