algorithm/expose-joints.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 //
4 
7 
8 namespace pinocchio
9 {
10  namespace python
11  {
12 
13  static context::VectorXs
15  {
17  normalize(model, q);
18  return q;
19  }
20 
22  {
23  return randomConfiguration(model);
24  }
25 
26  bp::tuple dIntegrate_proxy(
27  const context::Model & model, const context::VectorXs & q, const context::VectorXs & v)
28  {
29  context::MatrixXs J0(context::MatrixXs::Zero(model.nv, model.nv));
30  context::MatrixXs J1(context::MatrixXs::Zero(model.nv, model.nv));
31 
32  dIntegrate(model, q, v, J0, ARG0);
33  dIntegrate(model, q, v, J1, ARG1);
34 
35  return bp::make_tuple(J0, J1);
36  }
37 
39  const context::Model & model,
40  const context::VectorXs & q,
41  const context::VectorXs & v,
42  const ArgumentPosition arg)
43  {
44  context::MatrixXs J(context::MatrixXs::Zero(model.nv, model.nv));
45 
46  dIntegrate(model, q, v, J, arg, SETTO);
47 
48  return J;
49  }
50 
52  const context::Model & model,
53  const context::VectorXs & q,
54  const context::VectorXs & v,
55  const context::MatrixXs & Jin,
56  const ArgumentPosition arg)
57  {
58  int ncols = Jin.cols();
59  context::MatrixXs Jout(context::MatrixXs::Zero(model.nv, ncols));
60  dIntegrateTransport(model, q, v, Jin, Jout, arg);
61  return Jout;
62  }
63 
64  bp::tuple dDifference_proxy(
65  const context::Model & model, const context::VectorXs & q1, const context::VectorXs & q2)
66  {
67  context::MatrixXs J0(context::MatrixXs::Zero(model.nv, model.nv));
68  context::MatrixXs J1(context::MatrixXs::Zero(model.nv, model.nv));
69 
70  dDifference(model, q1, q2, J0, ARG0);
71  dDifference(model, q1, q2, J1, ARG1);
72 
73  return bp::make_tuple(J0, J1);
74  }
75 
77  const context::Model & model,
78  const context::VectorXs & q1,
79  const context::VectorXs & q2,
80  const ArgumentPosition arg)
81  {
82  context::MatrixXs J(context::MatrixXs::Zero(model.nv, model.nv));
83 
84  dDifference(model, q1, q2, J, arg);
85 
86  return J;
87  }
88 
90  {
91  typedef context::Scalar Scalar;
93  enum
94  {
96  };
97 
98  bp::def(
99  "integrate", &integrate<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>,
100  bp::args("model", "q", "v"),
101  "Integrate the joint configuration vector q with a tangent vector v during one unit time.\n"
102  "This is the canonical integrator of a Configuration Space composed of Lie groups, such as "
103  "most robots.\n\n"
104  "Parameters:\n"
105  "\tmodel: model of the kinematic tree\n"
106  "\tq: the joint configuration vector (size model.nq)\n"
107  "\tv: the joint velocity vector (size model.nv)\n");
108 
109  bp::def(
110  "dIntegrate", &dIntegrate_proxy, bp::args("model", "q", "v"),
111  "Computes the partial derivatives of the integrate function with respect to the first "
112  "and the second argument, and returns the two Jacobians as a tuple.\n\n"
113  "Parameters:\n"
114  "\tmodel: model of the kinematic tree\n"
115  "\tq: the joint configuration vector (size model.nq)\n"
116  "\tv: the joint velocity vector (size model.nv)\n");
117 
118  bp::def(
119  "dIntegrate", &dIntegrate_arg_proxy, bp::args("model", "q", "v", "argument_position"),
120  "Computes the partial derivatives of the integrate function with respect to the "
121  "first (arg == ARG0) "
122  "or the second argument (arg == ARG1).\n\n"
123  "Parameters:\n"
124  "\tmodel: model of the kinematic tree\n"
125  "\tq: the joint configuration vector (size model.nq)\n"
126  "\tv: the joint velocity vector (size model.nv)\n"
127  "\targument_position: either pinocchio.ArgumentPosition.ARG0 or "
128  "pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n");
129 
130  bp::def(
131  "dIntegrateTransport", &dIntegrateTransport_proxy,
132  bp::args("model", "q", "v", "Jin", "argument_position"),
133  "Takes a matrix expressed at q (+) v and uses parallel transport to express it in "
134  "the tangent space at q."
135  "\tThis operation does the product of the matrix by the Jacobian of the integration "
136  "operation, but more efficiently."
137  "Parameters:\n"
138  "\tmodel: model of the kinematic tree\n"
139  "\tq: the joint configuration vector (size model.nq)\n"
140  "\tv: the joint velocity vector (size model.nv)\n"
141  "\tJin: the input matrix (row size model.nv)"
142  "\targument_position: either pinocchio.ArgumentPosition.ARG0 (q) or "
143  "pinocchio.ArgumentPosition.ARG1 (v), depending on the desired Jacobian value.\n");
144 
145  bp::def(
146  "interpolate", &interpolate<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>,
147  bp::args("model", "q1", "q2", "alpha"),
148  "Interpolate between two given joint configuration vectors q1 and q2.\n\n"
149  "Parameters:\n"
150  "\tmodel: model of the kinematic tree\n"
151  "\tq1: the initial joint configuration vector (size model.nq)\n"
152  "\tq2: the terminal joint configuration vector (size model.nq)\n"
153  "\talpha: the interpolation coefficient in [0,1]\n");
154 
155  bp::def(
156  "difference", &difference<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>,
157  bp::args("model", "q1", "q2"),
158  "Difference between two joint configuration vectors, i.e. the tangent vector that "
159  "must be integrated during one unit time"
160  "to go from q1 to q2.\n\n"
161  "Parameters:\n"
162  "\tmodel: model of the kinematic tree\n"
163  "\tq1: the initial joint configuration vector (size model.nq)\n"
164  "\tq2: the terminal joint configuration vector (size model.nq)\n");
165 
166  bp::def(
167  "squaredDistance",
168  &squaredDistance<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>,
169  bp::args("model", "q1", "q2"),
170  "Squared distance vector between two joint configuration vectors.\n\n"
171  "Parameters:\n"
172  "\tmodel: model of the kinematic tree\n"
173  "\tq1: the initial joint configuration vector (size model.nq)\n"
174  "\tq2: the terminal joint configuration vector (size model.nq)\n");
175 
176  bp::def(
177  "distance", &distance<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>,
178  bp::args("model", "q1", "q2"),
179  "Distance between two joint configuration vectors.\n\n"
180  "Parameters:\n"
181  "\tmodel: model of the kinematic tree\n"
182  "\tq1: the initial joint configuration vector (size model.nq)\n"
183  "\tq2: the terminal joint configuration vector (size model.nq)\n");
184 
185  bp::def(
186  "dDifference", &dDifference_proxy, bp::args("model", "q1", "q2"),
187  "Computes the partial derivatives of the difference function with respect to the first "
188  "and the second argument, and returns the two Jacobians as a tuple.\n\n"
189  "Parameters:\n"
190  "\tmodel: model of the kinematic tree\n"
191  "\tq1: the initial joint configuration vector (size model.nq)\n"
192  "\tq2: the terminal joint configuration vector (size model.nq)\n");
193 
194  bp::def(
195  "dDifference", &dDifference_arg_proxy, bp::args("model", "q1", "q2", "argument_position"),
196  "Computes the partial derivatives of the difference function with respect to the "
197  "first (arg == ARG0) "
198  "or the second argument (arg == ARG1).\n\n"
199  "Parameters:\n"
200  "\tmodel: model of the kinematic tree\n"
201  "\tq1: the initial joint configuration vector (size model.nq)\n"
202  "\tq2: the terminal joint configuration vector (size model.nq)\n"
203  "\targument_position: either pinocchio.ArgumentPosition.ARG0 or "
204  "pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n");
205 
206  bp::def(
207  "randomConfiguration", &randomConfiguration_proxy, bp::arg("model"),
208  "Generate a random configuration in the bounds given by the lower and upper limits "
209  "contained in model.\n\n"
210  "Parameters:\n"
211  "\tmodel: model of the kinematic tree\n");
212 
213  bp::def(
214  "randomConfiguration",
215  &randomConfiguration<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>,
216  bp::args("model", "lower_bound", "upper_bound"),
217  "Generate a random configuration in the bounds given by the Joint lower and upper limits "
218  "arguments.\n\n"
219  "Parameters:\n"
220  "\tmodel: model of the kinematic tree\n"
221  "\tlower_bound: the lower bound on the joint configuration vectors (size model.nq)\n"
222  "\tupper_bound: the upper bound on the joint configuration vectors (size model.nq)\n");
223 
224  bp::def(
225  "neutral", &neutral<Scalar, Options, JointCollectionDefaultTpl>, bp::arg("model"),
226  "Returns the neutral configuration vector associated to the model.\n\n"
227  "Parameters:\n"
228  "\tmodel: model of the kinematic tree\n");
229 
230  bp::def(
231  "normalize", normalize_proxy, bp::args("model", "q"),
232  "Returns the configuration normalized.\n"
233  "For instance, when the configuration vectors contains some quaternion values, it must be "
234  "required to renormalize these components to keep orthonormal rotation values.\n\n"
235  "Parameters:\n"
236  "\tmodel: model of the kinematic tree\n"
237  "\tq: a joint configuration vector to normalize (size model.nq)\n");
238 
239 #ifndef PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS
240 
241  static const Scalar dummy_precision = Eigen::NumTraits<Scalar>::dummy_precision();
242  bp::def(
243  "isSameConfiguration",
244  &isSameConfiguration<Scalar, Options, JointCollectionDefaultTpl, VectorXs, VectorXs>,
245  bp::args("model", "q1", "q2", "prec"),
246  "Return true if two configurations are equivalent within the given precision "
247  "provided by prec.\n\n"
248  "Parameters:\n"
249  "\tmodel: model of the kinematic tree\n"
250  "\tq1: a joint configuration vector (size model.nq)\n"
251  "\tq2: a joint configuration vector (size model.nq)\n"
252  "\tprec: requested accuracy for the comparison\n");
253 
254  bp::def(
255  "isNormalized", &isNormalized<Scalar, Options, JointCollectionDefaultTpl, VectorXs>,
256  (bp::arg("model"), bp::arg("q"), bp::arg("prec") = dummy_precision),
257  "Check whether a configuration vector is normalized within the given precision "
258  "provided by prec.\n\n"
259  "Parameters:\n"
260  "\tmodel: model of the kinematic tree\n"
261  "\tq: a joint configuration vector (size model.nq)\n"
262  "\tprec: requested accuracy for the check\n");
263 #endif
264  }
265 
266  } // namespace python
267 } // namespace pinocchio
pinocchio::python::dDifference_proxy
bp::tuple dDifference_proxy(const context::Model &model, const context::VectorXs &q1, const context::VectorXs &q2)
Definition: algorithm/expose-joints.cpp:64
pinocchio::SETTO
@ SETTO
Definition: fwd.hpp:132
pinocchio::dIntegrate
void dIntegrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType > &J, const ArgumentPosition arg, const AssignmentOperatorType op=SETTO)
Computes the Jacobian of a small variation of the configuration vector or the tangent vector into the...
Definition: joint-configuration.hpp:487
q2
q2
pinocchio::python::Scalar
context::Scalar Scalar
Definition: admm-solver.cpp:29
inverse-kinematics-3d.J
J
Definition: inverse-kinematics-3d.py:28
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:121
pinocchio::dIntegrateTransport
void dIntegrateTransport(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< JacobianMatrixType1 > &Jin, const Eigen::MatrixBase< JacobianMatrixType2 > &Jout, const ArgumentPosition arg)
Transport a matrix from the terminal to the initial tangent space of the integrate operation,...
Definition: joint-configuration.hpp:576
pinocchio::ARG0
@ ARG0
Definition: fwd.hpp:123
pinocchio::python::VectorXs
context::VectorXs VectorXs
Definition: admm-solver.cpp:30
pinocchio::randomConfiguration
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
Definition: joint-configuration.hpp:315
pinocchio::python::exposeJointsAlgo
void exposeJointsAlgo()
Definition: algorithm/expose-joints.cpp:89
pinocchio::python::dDifference_arg_proxy
context::MatrixXs dDifference_arg_proxy(const context::Model &model, const context::VectorXs &q1, const context::VectorXs &q2, const ArgumentPosition arg)
Definition: algorithm/expose-joints.cpp:76
pinocchio::python::Options
@ Options
Definition: expose-contact-inverse-dynamics.cpp:22
joint-configuration.hpp
pinocchio::context::VectorXs
Eigen::Matrix< Scalar, Eigen::Dynamic, 1, Options > VectorXs
Definition: context/generic.hpp:47
algorithms.hpp
pinocchio::context::MatrixXs
Eigen::Matrix< Scalar, Eigen::Dynamic, Eigen::Dynamic, Options > MatrixXs
Definition: context/generic.hpp:49
python
pinocchio::python::normalize_proxy
static context::VectorXs normalize_proxy(const context::Model &model, const context::VectorXs &config)
Definition: algorithm/expose-joints.cpp:14
pinocchio::python::v
const Vector3Like & v
Definition: bindings/python/spatial/explog.hpp:66
pinocchio::python::dIntegrate_proxy
bp::tuple dIntegrate_proxy(const context::Model &model, const context::VectorXs &q, const context::VectorXs &v)
Definition: algorithm/expose-joints.cpp:26
pinocchio::ARG1
@ ARG1
Definition: fwd.hpp:124
pinocchio::q
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorType > & q
Definition: joint-configuration.hpp:1083
pinocchio::python::randomConfiguration_proxy
static context::VectorXs randomConfiguration_proxy(const context::Model &model)
Definition: algorithm/expose-joints.cpp:21
pinocchio::python::context::Options
@ Options
Definition: bindings/python/context/generic.hpp:40
pinocchio::python::dIntegrateTransport_proxy
context::MatrixXs dIntegrateTransport_proxy(const context::Model &model, const context::VectorXs &q, const context::VectorXs &v, const context::MatrixXs &Jin, const ArgumentPosition arg)
Definition: algorithm/expose-joints.cpp:51
pinocchio::python::dIntegrate_arg_proxy
context::MatrixXs dIntegrate_arg_proxy(const context::Model &model, const context::VectorXs &q, const context::VectorXs &v, const ArgumentPosition arg)
Definition: algorithm/expose-joints.cpp:38
pinocchio::q1
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > & q1
Definition: joint-configuration.hpp:1138
pinocchio::dDifference
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
Definition: joint-configuration.hpp:742
pinocchio::ModelTpl
Definition: context/generic.hpp:20
setup.config
config
Definition: cmake/cython/setup.in.py:103
pinocchio::python::context::Scalar
PINOCCHIO_PYTHON_SCALAR_TYPE Scalar
Definition: bindings/python/context/generic.hpp:37
pinocchio::normalize
void normalize(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &qout)
Normalize a configuration vector.
Definition: joint-configuration.hpp:887
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