expose-kinematics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2021 CNRS INRIA
3 //
4 
6 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
7 #include "pinocchio/algorithm/center-of-mass-derivatives.hpp"
8 
9 #include <boost/python/tuple.hpp>
10 
11 namespace pinocchio
12 {
13  namespace python
14  {
15  namespace bp = boost::python;
16 
18  Data & data,
19  const Model::JointIndex jointId,
20  ReferenceFrame rf)
21  {
22  typedef Data::Matrix6x Matrix6x;
23 
24  Matrix6x partial_dq(Matrix6x::Zero(6,model.nv));
25  Matrix6x partial_dv(Matrix6x::Zero(6,model.nv));
26 
27  getJointVelocityDerivatives(model,data,jointId,rf,
28  partial_dq,partial_dv);
29 
30  return bp::make_tuple(partial_dq,partial_dv);
31  }
32 
34  Data & data,
35  const Model::JointIndex jointId,
36  ReferenceFrame rf)
37  {
38  typedef Data::Matrix6x Matrix6x;
39 
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));
44 
45  getJointAccelerationDerivatives(model,data,jointId,rf,
46  v_partial_dq,a_partial_dq,
47  a_partial_dv,a_partial_da);
48 
49  return bp::make_tuple(v_partial_dq,a_partial_dq,a_partial_dv,a_partial_da);
50  }
51 
53  Data & data)
54  {
55  typedef Data::Matrix3x Matrix3x;
56  Matrix3x partial_dq(Matrix3x::Zero(3,model.nv));
57  getCenterOfMassVelocityDerivatives(model,data,partial_dq);
58  return partial_dq;
59  }
60 
61 
63  {
64  using namespace Eigen;
65 
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"
72  "Parameters:\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");
78 
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"
86  "Parameters:\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");
91 
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"
99  "Parameters:\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");
104 
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"
112  "Parameters:\n"
113  "\tmodel: model of the kinematic tree\n"
114  "\tdata: data related to the model\n");
115 
116  }
117 
118 
119 
120  } // namespace python
121 } // namespace pinocchio
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
Definition: joint-mimic.cpp:15
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.
Definition: timings.cpp:28
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)


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:30