6 #include "pinocchio/algorithm/frames.hpp" 77 using namespace Eigen;
79 bp::def(
"updateFramePlacements",
80 &updateFramePlacements<double,0,JointCollectionDefaultTpl>,
81 bp::args(
"model",
"data"),
82 "Computes the placements of all the operational frames according to the current joint placement stored in data" 83 "and puts the results in data.");
85 bp::def(
"updateFramePlacement",
86 &updateFramePlacement<double,0,JointCollectionDefaultTpl>,
87 bp::args(
"model",
"data",
"frame_id"),
88 "Computes the placement of the given operational frame (frame_id) according to the current joint placement stored in data, stores the results in data and returns it.",
89 bp::return_value_policy<bp::return_by_value>());
91 bp::def(
"getFrameVelocity",
92 &getFrameVelocity<double,0,JointCollectionDefaultTpl>,
93 getFrameVelocity_overload(
94 bp::args(
"model",
"data",
"frame_id",
"reference_frame"),
95 "Returns the spatial velocity of the frame expressed in the coordinate system given by reference_frame.\n" 96 "forwardKinematics(model,data,q,v[,a]) should be called first to compute the joint spatial velocity stored in data.v"));
98 bp::def(
"getFrameAcceleration",
99 &getFrameAcceleration<double,0,JointCollectionDefaultTpl>,
100 getFrameAcceleration_overload(
101 bp::args(
"model",
"data",
"frame_id",
"reference_frame"),
102 "Returns the spatial acceleration of the frame expressed in the coordinate system given by reference_frame.\n" 103 "forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
105 bp::def(
"getFrameClassicalAcceleration",
106 &getFrameClassicalAcceleration<double,0,JointCollectionDefaultTpl>,
107 getFrameClassicalAcceleration_overload(
108 bp::args(
"model",
"data",
"frame_id",
"reference_frame"),
109 "Returns the \"classical\" acceleration of the frame expressed in the coordinate system given by reference_frame.\n" 110 "forwardKinematics(model,data,q,v,a) should be called first to compute the joint spatial acceleration stored in data.a ."));
112 bp::def(
"framesForwardKinematics",
113 &framesForwardKinematics<double,0,JointCollectionDefaultTpl,VectorXd>,
114 bp::args(
"model",
"data",
"q"),
115 "Calls first the forwardKinematics(model,data,q) and then update the Frame placement quantities (data.oMf).");
117 bp::def(
"computeFrameJacobian",
119 bp::args(
"model",
"data",
"q",
"frame_id",
"reference_frame"),
120 "Computes the Jacobian of the frame given by its frame_id in the coordinate system given by reference_frame.\n");
122 bp::def(
"computeFrameJacobian",
124 bp::args(
"model",
"data",
"q",
"frame_id"),
125 "Computes the Jacobian of the frame given by its frame_id.\n" 126 "The columns of the Jacobian are expressed in the coordinates system of the Frame itself.\n" 127 "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v," 128 "where v is the joint velocity.");
130 bp::def(
"getFrameJacobian",
132 bp::args(
"model",
"data",
"frame_id",
"reference_frame"),
133 "Computes the Jacobian of the frame given by its ID either in the local or the world frames.\n" 134 "The columns of the Jacobian are expressed in the LOCAL frame coordinates system.\n" 135 "In other words, the velocity of the frame vF expressed in the local coordinate is given by J*v," 136 "where v is the joint velocity.\n" 137 "computeJointJacobians(model,data,q) and updateFramePlacements(model,data) must have been called first.");
140 bp::args(
"model",
"data",
"q",
"v",
"frame_id",
"reference_frame"),
141 "Computes the Jacobian Time Variation of the frame given by its frame_id either in the reference frame provided by reference_frame.\n");
144 bp::args(
"model",
"data",
"frame_id",
"reference_frame"),
145 "Returns the Jacobian time variation of the frame given by its frame_id either in the reference frame provided by reference_frame.\n" 146 "You have to call computeJointJacobiansTimeVariation(model,data,q,v) and updateFramePlacements(model,data) first.");
148 bp::def(
"computeSupportedInertiaByFrame",
149 &computeSupportedInertiaByFrame<double,0,JointCollectionDefaultTpl>,
150 bp::args(
"model",
"data",
"frame_id",
"with_subtree"),
151 "Computes the supported inertia by the frame (given by frame_id) and returns it.\n" 152 "The supported inertia corresponds to the sum of the inertias of all the child frames (that belongs to the same joint body) and the child joints, if with_subtree=True.\n" 153 "You must first call pinocchio::forwardKinematics to update placement values in data structure.");
155 bp::def(
"computeSupportedForceByFrame",
156 &computeSupportedForceByFrame<double,0,JointCollectionDefaultTpl>,
157 bp::args(
"model",
"data",
"frame_id"),
158 "Computes the supported force of the frame (given by frame_id) and returns it.\n" 159 "The supported force corresponds to the sum of all the forces experienced after the given frame.\n" 160 "You must first call pinocchio::rnea to update placement values in data structure.");
static Data::Matrix6x get_frame_jacobian_time_variation_proxy(const Model &model, Data &data, Model::FrameIndex jointId, ReferenceFrame rf)
void computeFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const FrameIndex frameId, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6xLike > &J)
Computes the Jacobian of a specific Frame expressed in the desired reference_frame given as argument...
ReferenceFrame
List of Reference Frames supported by Pinocchio.
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
pinocchio::FrameIndex FrameIndex
static Data::Matrix6x get_frame_jacobian_proxy(const Model &model, Data &data, const Model::FrameIndex frame_id, ReferenceFrame rf)
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobiansTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depen...
static Data::Matrix6x compute_frame_jacobian_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, Model::FrameIndex frame_id)
Main pinocchio namespace.
int nv
Dimension of the velocity vector space.
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
void getFrameJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &dJ)
Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the ...
static Data::Matrix6x frame_jacobian_time_variation_proxy(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Model::FrameIndex frame_id, const ReferenceFrame rf)
void getFrameJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const FrameIndex frame_id, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix6xLike > &J)
Returns the jacobian of the frame expressed either expressed in the LOCAL frame coordinate system or ...
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
JointCollectionTpl & model