$darkmode
pinocchio 4.0.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
jacobian.hpp
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
5 #pragma once
6 
7 // IWYU pragma: begin_keep
8 #include <Eigen/Core>
9 
10 #include <cassert>
11 #include <cstddef>
12 #include <type_traits>
13 #include <vector>
14 
15 #include <boost/fusion/container/vector.hpp>
16 
17 #include "pinocchio/macros.hpp"
18 #include "pinocchio/eigen-common.hpp"
19 #include "pinocchio/fwd.hpp"
20 
21 #include "pinocchio/math.hpp"
22 
23 #include "pinocchio/spatial.hpp"
24 
25 #include "pinocchio/multibody.hpp"
26 #include "pinocchio/multibody/joint.hpp"
27 #include "pinocchio/multibody/visitor.hpp"
28 
29 #include "pinocchio/algorithm/check.hpp"
30 // IWYU pragma: end_keep
31 
32 namespace pinocchio
33 {
53  template<
54  typename Scalar,
55  int Options,
56  template<typename, int> class JointCollectionTpl,
57  typename ConfigVectorType>
58  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians(
59  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
60  DataTpl<Scalar, Options, JointCollectionTpl> & data,
61  const Eigen::MatrixBase<ConfigVectorType> & q);
62 
80  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
81  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x & computeJointJacobians(
82  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
83  DataTpl<Scalar, Options, JointCollectionTpl> & data);
84 
123  template<
124  typename Scalar,
125  int Options,
126  template<typename, int> class JointCollectionTpl,
127  typename Matrix6Like>
128  void getJointJacobian(
129  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
130  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
131  const JointIndex joint_id,
132  const ReferenceFrame reference_frame,
133  const Eigen::MatrixBase<Matrix6Like> & J);
147  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
148  Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> getJointJacobian(
149  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
150  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
151  const JointIndex joint_id,
152  const ReferenceFrame reference_frame)
153  {
154  typedef Eigen::Matrix<Scalar, 6, Eigen::Dynamic, Options> ReturnType;
155  ReturnType res(ReturnType::Zero(6, model.nv));
156 
157  getJointJacobian(model, data, joint_id, reference_frame, res);
158  return res;
159  }
160 
185  template<
186  typename Scalar,
187  int Options,
188  template<typename, int> class JointCollectionTpl,
189  typename ConfigVectorType,
190  typename Matrix6Like>
191  void computeJointJacobian(
192  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
193  DataTpl<Scalar, Options, JointCollectionTpl> & data,
194  const Eigen::MatrixBase<ConfigVectorType> & q,
195  const JointIndex joint_id,
196  const Eigen::MatrixBase<Matrix6Like> & J);
197 
214  template<
215  typename Scalar,
216  int Options,
217  template<typename, int> class JointCollectionTpl,
218  typename ConfigVectorType,
219  typename TangentVectorType>
220  const typename DataTpl<Scalar, Options, JointCollectionTpl>::Matrix6x &
221  computeJointJacobiansTimeVariation(
222  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
223  DataTpl<Scalar, Options, JointCollectionTpl> & data,
224  const Eigen::MatrixBase<ConfigVectorType> & q,
225  const Eigen::MatrixBase<TangentVectorType> & v);
226 
243  template<
244  typename Scalar,
245  int Options,
246  template<typename, int> class JointCollectionTpl,
247  typename Matrix6Like>
248  void getJointJacobianTimeVariation(
249  const ModelTpl<Scalar, Options, JointCollectionTpl> & model,
250  const DataTpl<Scalar, Options, JointCollectionTpl> & data,
251  const JointIndex joint_id,
252  const ReferenceFrame reference_frame,
253  const Eigen::MatrixBase<Matrix6Like> & dJ);
254 
255 } // namespace pinocchio
256 
257 // IWYU pragma: begin_exports
258 #include "pinocchio/src/algorithm/jacobian.hxx"
259 // IWYU pragma: end_exports
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::computeJointJacobians
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame....
pinocchio::computeJointJacobian
void computeJointJacobian(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const JointIndex joint_id, const Eigen::MatrixBase< Matrix6Like > &J)
Computes the Jacobian of a specific joint frame expressed in the local frame of the joint and store t...
pinocchio::computeJointJacobiansTimeVariation
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...
pinocchio::getJointJacobian
void 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 opt...
pinocchio::getJointJacobianTimeVariation
void getJointJacobianTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const JointIndex joint_id, const ReferenceFrame reference_frame, const Eigen::MatrixBase< Matrix6Like > &dJ)
Computes the Jacobian time variation of a specific joint frame expressed either in the world frame (r...