expose-kinematics-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2021 CNRS INRIA
3 //
4 
8 
9 #include <boost/python/tuple.hpp>
10 
11 namespace pinocchio
12 {
13  namespace python
14  {
15  namespace bp = boost::python;
16 
18  const context::Model & model,
20  const JointIndex jointId,
21  ReferenceFrame rf)
22  {
24 
25  Matrix6x partial_dq(Matrix6x::Zero(6, model.nv));
26  Matrix6x partial_dv(Matrix6x::Zero(6, model.nv));
27 
28  getJointVelocityDerivatives(model, data, jointId, rf, partial_dq, partial_dv);
29 
30  return bp::make_tuple(partial_dq, partial_dv);
31  }
32 
34  const context::Model & model,
36  const JointIndex joint_id,
37  const context::SE3 & placement,
38  ReferenceFrame rf)
39  {
41 
42  Matrix3x v_partial_dq(Matrix3x::Zero(3, model.nv));
43  Matrix3x v_partial_dv(Matrix3x::Zero(3, model.nv));
44 
45  getPointVelocityDerivatives(model, data, joint_id, placement, rf, v_partial_dq, v_partial_dv);
46 
47  return bp::make_tuple(v_partial_dq, v_partial_dv);
48  }
49 
51  const context::Model & model,
53  const JointIndex jointId,
54  ReferenceFrame rf)
55  {
57 
58  Matrix6x v_partial_dq(Matrix6x::Zero(6, model.nv));
59  Matrix6x a_partial_dq(Matrix6x::Zero(6, model.nv));
60  Matrix6x a_partial_dv(Matrix6x::Zero(6, model.nv));
61  Matrix6x a_partial_da(Matrix6x::Zero(6, model.nv));
62 
64  model, data, jointId, rf, v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
65 
66  return bp::make_tuple(v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
67  }
68 
70  const context::Model & model,
72  const JointIndex joint_id,
73  const context::SE3 & placement,
74  ReferenceFrame rf)
75  {
77 
78  Matrix3x v_partial_dq(Matrix3x::Zero(3, model.nv));
79  Matrix3x a_partial_dq(Matrix3x::Zero(3, model.nv));
80  Matrix3x a_partial_dv(Matrix3x::Zero(3, model.nv));
81  Matrix3x a_partial_da(Matrix3x::Zero(3, model.nv));
82 
84  model, data, joint_id, placement, rf, v_partial_dq, a_partial_dq, a_partial_dv,
85  a_partial_da);
86 
87  return bp::make_tuple(v_partial_dq, a_partial_dq, a_partial_dv, a_partial_da);
88  }
89 
92  {
94  Matrix3x partial_dq(Matrix3x::Zero(3, model.nv));
96  return partial_dq;
97  }
98 
100  {
101  typedef context::Scalar Scalar;
102  typedef context::VectorXs VectorXs;
103  enum
104  {
106  };
107 
108  bp::def(
109  "computeForwardKinematicsDerivatives",
112  bp::args("model", "data", "q", "v", "a"),
113  "Computes all the terms required to compute the derivatives of the placement, "
114  "spatial velocity and acceleration\n"
115  "for any joint of the model.\n"
116  "The results are stored in data.\n\n"
117  "Parameters:\n"
118  "\tmodel: model of the kinematic tree\n"
119  "\tdata: data related to the model\n"
120  "\tq: the joint configuration vector (size model.nq)\n"
121  "\tv: the joint velocity vector (size model.nv)\n"
122  "\ta: the joint acceleration vector (size model.nv)\n");
123 
124  bp::def(
125  "getJointVelocityDerivatives", getJointVelocityDerivatives_proxy,
126  bp::args("model", "data", "joint_id", "reference_frame"),
127  "Computes the partial derivatives of the spatial velocity of a given joint with respect "
128  "to\n"
129  "the joint configuration and velocity and returns them as a tuple.\n"
130  "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the "
131  "LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of "
132  "reference_frame.\n"
133  "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
134  "Parameters:\n"
135  "\tmodel: model of the kinematic tree\n"
136  "\tdata: data related to the model\n"
137  "\tjoint_id: index of the joint\n"
138  "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
139 
140  bp::def(
141  "getPointVelocityDerivatives", getPointVelocityDerivatives_proxy,
142  bp::args("model", "data", "joint_id", "placement", "reference_frame"),
143  "Computes the partial derivatives of the velocity of a point given by its placement "
144  "information w.r.t. the joint frame and returns them as a tuple.\n"
145  "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the "
146  "LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of "
147  "reference_frame.\n"
148  "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
149  "Parameters:\n"
150  "\tmodel: model of the kinematic tree\n"
151  "\tdata: data related to the model\n"
152  "\tjoint_id: index of the joint\n"
153  "\tplacement: relative placement of the point w.r.t. the joint frame\n"
154  "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
155 
156  bp::def(
157  "getPointClassicAccelerationDerivatives", getPointClassicAccelerationDerivatives_proxy,
158  bp::args("model", "data", "joint_id", "placement", "reference_frame"),
159  "Computes the partial derivatives of the classic acceleration of a point given by its "
160  "placement information w.r.t. the joint frame and returns them as a tuple.\n"
161  "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the "
162  "LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of "
163  "reference_frame.\n"
164  "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
165  "Parameters:\n"
166  "\tmodel: model of the kinematic tree\n"
167  "\tdata: data related to the model\n"
168  "\tjoint_id: index of the joint\n"
169  "\tplacement: relative placement of the point w.r.t. the joint frame\n"
170  "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
171 
172  bp::def(
173  "getJointAccelerationDerivatives", getJointAccelerationDerivatives_proxy,
174  bp::args("model", "data", "joint_id", "reference_frame"),
175  "Computes the partial derivatives of the spatial acceleration of a given joint with "
176  "respect to\n"
177  "the joint configuration, velocity and acceleration and returns them as a tuple.\n"
178  "The partial derivatives can be either expressed in the LOCAL frame of the joint, in the "
179  "LOCAL_WORLD_ALIGNED frame or in the WORLD coordinate frame depending on the value of "
180  "reference_frame.\n"
181  "You must first call computeForwardKinematicsDerivatives before calling this function.\n\n"
182  "Parameters:\n"
183  "\tmodel: model of the kinematic tree\n"
184  "\tdata: data related to the model\n"
185  "\tjoint_id: index of the joint\n"
186  "\treference_frame: reference frame in which the resulting derivatives are expressed\n");
187 
188  bp::def(
189  "getCenterOfMassVelocityDerivatives", getCoMVelocityDerivatives_proxy,
190  bp::args("model", "data"),
191  "Computes the partial derivaties of the center of mass velocity with respect to\n"
192  "the joint configuration.\n"
193  "You must first call computeAllTerms(model,data,q,v) or centerOfMass(model,data,q,v) "
194  "before calling this function.\n\n"
195  "Parameters:\n"
196  "\tmodel: model of the kinematic tree\n"
197  "\tdata: data related to the model\n");
198  }
199 
200  } // namespace python
201 } // namespace pinocchio
boost::python
pinocchio::DataTpl
Definition: context/generic.hpp:25
pinocchio::ReferenceFrame
ReferenceFrame
Various conventions to express the velocity of a moving frame.
Definition: multibody/fwd.hpp:46
pinocchio::SE3Tpl< Scalar, Options >
pinocchio::getPointClassicAccelerationDerivatives
void getPointClassicAccelerationDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix3xOut1 > &v_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut2 > &a_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut3 > &a_point_partial_dv, const Eigen::MatrixBase< Matrix3xOut4 > &a_point_partial_da)
Computes the partial derivatives of the classic acceleration of a point given by its placement inform...
setup.data
data
Definition: cmake/cython/setup.in.py:48
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
pinocchio::python::getCoMVelocityDerivatives_proxy
context::Data::Matrix3x getCoMVelocityDerivatives_proxy(const context::Model &model, context::Data &data)
Definition: expose-kinematics-derivatives.cpp:91
pinocchio::python::getPointClassicAccelerationDerivatives_proxy
bp::tuple getPointClassicAccelerationDerivatives_proxy(const context::Model &model, context::Data &data, const JointIndex joint_id, const context::SE3 &placement, ReferenceFrame rf)
Definition: expose-kinematics-derivatives.cpp:69
pinocchio::python::VectorXs
context::VectorXs VectorXs
Definition: admm-solver.cpp:30
pinocchio::getJointAccelerationDerivatives
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...
pinocchio::python::Options
@ Options
Definition: expose-contact-inverse-dynamics.cpp:22
pinocchio::computeForwardKinematicsDerivatives
void computeForwardKinematicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
Computes all the terms required to compute the derivatives of the placement, spatial velocity and acc...
pinocchio::context::VectorXs
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
Definition: context/generic.hpp:47
pinocchio::placement
const MotionDense< Motion2 > const SE3Tpl< SE3Scalar, SE3Options > & placement
Definition: spatial/classic-acceleration.hpp:122
pinocchio::DataTpl::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
The 3d jacobian type (temporary)
Definition: multibody/data.hpp:94
algorithms.hpp
pinocchio::getJointVelocityDerivatives
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...
pinocchio::python::getPointVelocityDerivatives_proxy
bp::tuple getPointVelocityDerivatives_proxy(const context::Model &model, context::Data &data, const JointIndex joint_id, const context::SE3 &placement, ReferenceFrame rf)
Definition: expose-kinematics-derivatives.cpp:33
python
pinocchio::getCenterOfMassVelocityDerivatives
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...
pinocchio::python::getJointVelocityDerivatives_proxy
bp::tuple getJointVelocityDerivatives_proxy(const context::Model &model, context::Data &data, const JointIndex jointId, ReferenceFrame rf)
Definition: expose-kinematics-derivatives.cpp:17
center-of-mass-derivatives.hpp
pinocchio::python::context::Options
@ Options
Definition: bindings/python/context/generic.hpp:40
Matrix6x
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6x
Definition: joint-mimic.cpp:15
kinematics-derivatives.hpp
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::DataTpl::Matrix6x
Eigen::Matrix< Scalar, 6, Eigen::Dynamic, Options > Matrix6x
The 6d jacobian type (temporary)
Definition: multibody/data.hpp:92
pinocchio::getPointVelocityDerivatives
void getPointVelocityDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const DataTpl< Scalar, Options, JointCollectionTpl > &data, const Model::JointIndex joint_id, const SE3Tpl< Scalar, Options > &placement, const ReferenceFrame rf, const Eigen::MatrixBase< Matrix3xOut1 > &v_point_partial_dq, const Eigen::MatrixBase< Matrix3xOut2 > &v_point_partial_dv)
Computes the partial derivatives of the velocity of a point given by its placement information w....
pinocchio::JointCollectionDefaultTpl
Definition: context/generic.hpp:15
append-urdf-model-with-another-model.joint_id
joint_id
Definition: append-urdf-model-with-another-model.py:34
pinocchio::ModelTpl
Definition: context/generic.hpp:20
pinocchio::context::Matrix3x
Eigen::Matrix< Scalar, 3, Eigen::Dynamic, Options > Matrix3x
Definition: context/generic.hpp:52
pinocchio::python::exposeKinematicsDerivatives
void exposeKinematicsDerivatives()
Definition: expose-kinematics-derivatives.cpp:99
pinocchio::python::getJointAccelerationDerivatives_proxy
bp::tuple getJointAccelerationDerivatives_proxy(const context::Model &model, context::Data &data, const JointIndex jointId, ReferenceFrame rf)
Definition: expose-kinematics-derivatives.cpp:50
pinocchio::python::context::Scalar
PINOCCHIO_PYTHON_SCALAR_TYPE Scalar
Definition: bindings/python/context/generic.hpp:37
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:27


pinocchio
Author(s):
autogenerated on Sun Dec 22 2024 03:41:09