expose-aba-derivatives.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018-2021 CNRS INRIA
3 //
4 
6 #include "pinocchio/algorithm/aba-derivatives.hpp"
8 
10 
11 namespace pinocchio
12 {
13  namespace python
14  {
15 
16  namespace bp = boost::python;
17  bp::tuple computeABADerivativesDefault(const Model & model, Data & data,
18  const Eigen::VectorXd & q,
19  const Eigen::VectorXd & v,
20  const Eigen::VectorXd & tau)
21  {
22  computeABADerivatives(model,data,q,v,tau);
23  make_symmetric(data.Minv);
24  return bp::make_tuple(make_ref(data.ddq_dq),
25  make_ref(data.ddq_dv),
26  make_ref(data.Minv));
27  }
28 
29  typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector;
30 
31  bp::tuple computeABADerivatives_fext(const Model & model, Data & data,
32  const Eigen::VectorXd & q,
33  const Eigen::VectorXd & v,
34  const Eigen::VectorXd & tau,
35  const ForceAlignedVector & fext)
36  {
37  computeABADerivatives(model,data,q,v,tau,fext);
38  make_symmetric(data.Minv);
39  return bp::make_tuple(make_ref(data.ddq_dq),
40  make_ref(data.ddq_dv),
41  make_ref(data.Minv));
42  }
43 
45  {
46  using namespace Eigen;
47 
48  bp::def("computeABADerivatives",
50  bp::args("model","data","q","v","tau"),
51  "Computes the ABA derivatives, store the result in data.ddq_dq, data.ddq_dv and data.Minv (aka ddq_dtau)\n"
52  "which correspond to the partial derivatives of the joint acceleration vector output with respect to the joint configuration,\n"
53  "velocity and torque vectors.\n\n"
54  "Parameters:\n"
55  "\tmodel: model of the kinematic tree\n"
56  "\tdata: data related to the model\n"
57  "\tq: the joint configuration vector (size model.nq)\n"
58  "\tv: the joint velocity vector (size model.nv)\n"
59  "\ttau: the joint torque vector (size model.nv)\n\n"
60  "Returns: (ddq_dq, ddq_dv, ddq_da)");
61 
62  bp::def("computeABADerivatives",
64  bp::args("model","data","q","v","tau","fext"),
65  "Computes the ABA derivatives with external contact foces,\n"
66  "store the result in data.ddq_dq, data.ddq_dv and data.Minv (aka ddq_dtau)\n"
67  "which correspond to the partial derivatives of the acceleration output with respect to the joint configuration,\n"
68  "velocity and torque vectors.\n\n"
69  "Parameters:\n"
70  "\tmodel: model of the kinematic tree\n"
71  "\tdata: data related to the model\n"
72  "\tq: the joint configuration vector (size model.nq)\n"
73  "\tv: the joint velocity vector (size model.nv)\n"
74  "\ttau: the joint torque vector (size model.nv)\n"
75  "\tfext: list of external forces expressed in the local frame of the joints (size model.njoints)\n\n"
76  "Returns: (ddq_dq, ddq_dv, ddq_da)");
77  }
78  } // namespace python
79 } // namespace pinocchio
bp::tuple computeABADerivatives_fext(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau, const ForceAlignedVector &fext)
void make_symmetric(Eigen::MatrixBase< Matrix > &mat)
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
bp::tuple computeABADerivativesDefault(const Model &model, Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau)
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
MatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
typedef PINOCCHIO_ALIGNED_STD_VECTOR(Force) ForceAlignedVector
MatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Main pinocchio namespace.
Definition: timings.cpp:30
void computeABADerivatives(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 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.
Eigen::Ref< Matrix > make_ref(Eigen::PlainObjectBase< Matrix > &mat)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
JointCollectionTpl & model


pinocchio
Author(s):
autogenerated on Tue Jun 1 2021 02:45:02