Template Function pinocchio::getJointJacobian(const ModelTpl<Scalar, Options, JointCollectionTpl>&, const DataTpl<Scalar, Options, JointCollectionTpl>&, const JointIndex, const ReferenceFrame, const Eigen::MatrixBase<Matrix6Like>&)
Defined in File jacobian.hpp
Function Documentation
-
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl, typename Matrix6Like>
void pinocchio::getJointJacobian(const ModelTpl<Scalar, Options, JointCollectionTpl> &model, const DataTpl<Scalar, Options, JointCollectionTpl> &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase<Matrix6Like> &J) Computes the Jacobian of a specific joint frame expressed in one of the pinocchio::ReferenceFrame options.
For the LOCAL reference frame, the Jacobian
from the joint frame to the world frame is such that , where is the velocity of the origin of the moving joint frame relative to the fixed world frame, projected into the basis of the joint frame. LOCAL_WORLD_ALIGNED is the same velocity but projected into the world frame basis.For the WORLD reference frame, the Jacobian
from the joint frame to the world frame is such that , where is the spatial velocity of the joint frame. The linear component of this spatial velocity is the velocity of a (possibly imaginary) point attached to the moving joint frame j which is traveling through the origin of the world frame at that instant. The angular component is the instantaneous angular velocity of the joint frame as viewed in the world frame.When serialized to a 6D vector, the order of coordinates is: three linear followed by three angular.
For further details regarding the different velocities or the Jacobian see Chapters 2 and 3 respectively in A Mathematical Introduction to Robotic Manipulation by Murray, Li and Sastry.
Note
This jacobian is extracted from data.J. You have to run pinocchio::computeJointJacobians before calling it.
- Template Parameters:
JointCollection – Collection of Joint types.
Matrix6xLike – Type of the matrix containing the joint Jacobian.
- Parameters:
model – [in] The model structure of the rigid body system.
data – [in] The data structure of the rigid body system.
joint_id – [in] The id of the joint.
reference_frame – [in] Reference frame in which the result is expressed.
J – [out] A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.fill(0.).