expose-rpy.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2019-2021 CNRS INRIA
3 //
4 
7 #include "pinocchio/math/rpy.hpp"
8 
9 namespace pinocchio
10 {
11  namespace python
12  {
13  namespace bp = boost::python;
14 
15  BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobian_overload, rpy::computeRpyJacobian, 1, 2)
16  BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianInverse_overload, rpy::computeRpyJacobianInverse, 1, 2)
17  BOOST_PYTHON_FUNCTION_OVERLOADS(computeRpyJacobianTimeDerivative_overload, rpy::computeRpyJacobianTimeDerivative, 2, 3)
18 
19  Eigen::Matrix3d rotate(const std::string & axis, const double ang)
20  {
21  if(axis.length() != 1U)
22  throw std::invalid_argument(std::string("Invalid axis: ").append(axis));
23  Eigen::Vector3d u;
24  u.setZero();
25  const char axis_ = axis[0];
26  switch(axis_)
27  {
28  case 'x': u[0] = 1.; break;
29  case 'y': u[1] = 1.; break;
30  case 'z': u[2] = 1.; break;
31  default: throw std::invalid_argument(std::string("Invalid axis: ").append(1U,axis_));
32  }
33 
34  return Eigen::AngleAxisd(ang, u).matrix();
35  }
36 
37  void exposeRpy()
38  {
39  using namespace Eigen;
40  using namespace pinocchio::rpy;
41 
42  {
43  // using the rpy scope
44  bp::scope current_scope = getOrCreatePythonNamespace("rpy");
45 
46  bp::def("rpyToMatrix",
47  static_cast<Matrix3d (*)(const double&, const double&, const double&)>(&rpyToMatrix),
48  bp::args("roll", "pitch", "yaw"),
49  "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
50  " where R_a(theta) denotes the rotation of theta radians axis a");
51 
52  bp::def("rpyToMatrix",
53  static_cast<Matrix3d (*)(const MatrixBase<Vector3d>&)>(&rpyToMatrix),
54  bp::arg("rpy"),
55  "Given (r, p, y), the rotation is given as R = R_z(y)R_y(p)R_x(r),"
56  " where R_a(theta) denotes the rotation of theta radians axis a");
57 
58  bp::def("matrixToRpy",
59  &matrixToRpy<Matrix3d>,
60  bp::arg("R"),
61  "Given a rotation matrix R, the angles (r, p, y) are given so that R = R_z(y)R_y(p)R_x(r),"
62  " where R_a(theta) denotes the rotation of theta radians axis a."
63  " The angles are guaranteed to be in the ranges: r in [-pi,pi],"
64  " p in[-pi/2,pi/2], y in [-pi,pi]");
65 
66  bp::def("rotate",
67  &rotate,
68  bp::args("axis", "ang"),
69  "Rotation matrix corresponding to a rotation about x, y or z"
70  " e.g. R = rot('x', pi / 4): rotate pi/4 rad about x axis");
71 
72  bp::def("computeRpyJacobian",
73  &computeRpyJacobian<Vector3d>,
74  computeRpyJacobian_overload(
75  bp::args("rpy","reference_frame"),
76  "Compute the Jacobian of the Roll-Pitch-Yaw conversion"
77  " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
78  " and reference frame F (either LOCAL or WORLD),"
79  " the Jacobian is such that omega_F = J_F(phi)phidot,"
80  " where omega_F is the angular velocity expressed in frame F"
81  " and J_F is the Jacobian computed with reference frame F"
82  "\nParameters:\n"
83  "\trpy Roll-Pitch-Yaw vector"
84  "\treference_frame Reference frame in which the angular velocity is expressed."
85  " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
86  )
87  );
88 
89  bp::def("computeRpyJacobianInverse",
90  &computeRpyJacobianInverse<Vector3d>,
91  computeRpyJacobianInverse_overload(
92  bp::args("rpy","reference_frame"),
93  "Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion"
94  " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
95  " and reference frame F (either LOCAL or WORLD),"
96  " the Jacobian is such that omega_F = J_F(phi)phidot,"
97  " where omega_F is the angular velocity expressed in frame F"
98  " and J_F is the Jacobian computed with reference frame F"
99  "\nParameters:\n"
100  "\trpy Roll-Pitch-Yaw vector"
101  "\treference_frame Reference frame in which the angular velocity is expressed."
102  " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
103  )
104  );
105 
106  bp::def("computeRpyJacobianTimeDerivative",
107  &computeRpyJacobianTimeDerivative<Vector3d, Vector3d>,
108  computeRpyJacobianTimeDerivative_overload(
109  bp::args("rpy", "rpydot", "reference_frame"),
110  "Compute the time derivative of the Jacobian of the Roll-Pitch-Yaw conversion"
111  " Given phi = (r, p, y) such that that R = R_z(y)R_y(p)R_x(r)"
112  " and reference frame F (either LOCAL or WORLD),"
113  " the Jacobian is such that omega_F = J_F(phi)phidot,"
114  " where omega_F is the angular velocity expressed in frame F"
115  " and J_F is the Jacobian computed with reference frame F"
116  "\nParameters:\n"
117  "\trpy Roll-Pitch-Yaw vector"
118  "\treference_frame Reference frame in which the angular velocity is expressed."
119  " Notice LOCAL_WORLD_ALIGNED is equivalent to WORLD"
120  )
121  );
122  }
123 
124  }
125 
126  } // namespace python
127 } // namespace pinocchio
result_of::push_front< V const, T >::type append(T const &t, V const &v)
Append the element T at the front of boost fusion vector V.
Definition: fusion.hpp:24
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobian(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the Jacobian of the Roll-Pitch-Yaw conversion.
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
Eigen::Matrix3d rotate(const std::string &axis, const double ang)
Definition: expose-rpy.cpp:19
JointCollectionTpl const Eigen::MatrixBase< ConfigVectorIn1 > const Eigen::MatrixBase< ConfigVectorIn2 > const Scalar & u
boost::python::object getOrCreatePythonNamespace(const std::string &submodule_name)
Helper to create or simply return an existing namespace in Python.
Definition: namespace.hpp:22
U
Definition: ocp.py:61
Main pinocchio namespace.
Definition: timings.cpp:28
Eigen::Matrix< typename Vector3Like::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like)::Options > computeRpyJacobianInverse(const Eigen::MatrixBase< Vector3Like > &rpy, const ReferenceFrame rf=LOCAL)
Compute the inverse Jacobian of the Roll-Pitch-Yaw conversion.
Eigen::Matrix< Scalar, 3, 3 > rpyToMatrix(const Scalar &r, const Scalar &p, const Scalar &y)
Convert from Roll, Pitch, Yaw to rotation Matrix.
Definition: rpy.py:1
Eigen::Matrix< typename Vector3Like0::Scalar, 3, 3, PINOCCHIO_EIGEN_PLAIN_TYPE(Vector3Like0)::Options > computeRpyJacobianTimeDerivative(const Eigen::MatrixBase< Vector3Like0 > &rpy, const Eigen::MatrixBase< Vector3Like1 > &rpydot, const ReferenceFrame rf=LOCAL)
Compute the time derivative Jacobian of the Roll-Pitch-Yaw conversion.
Roll-pitch-yaw operations.


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