bindings/python/spatial/se3.hpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 // Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5 
6 #ifndef __pinocchio_python_spatial_se3_hpp__
7 #define __pinocchio_python_spatial_se3_hpp__
8 
9 #include <eigenpy/memory.hpp>
11 #include <boost/python/tuple.hpp>
12 
13 #include "pinocchio/spatial/se3.hpp"
14 #include "pinocchio/spatial/motion.hpp"
15 #include "pinocchio/spatial/force.hpp"
16 #include "pinocchio/spatial/inertia.hpp"
17 #include "pinocchio/spatial/explog.hpp"
18 
21 
23 
24 namespace pinocchio
25 {
26  namespace python
27  {
28  namespace bp = boost::python;
29 
30  BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(isIdentity_overload,SE3::isIdentity,0,1)
31 
32  template<typename T> struct call;
33 
34  template<typename Scalar, int Options>
35  struct call< SE3Tpl<Scalar,Options> >
36  {
38 
39  static bool isApprox(const SE3 & self, const SE3 & other,
40  const Scalar & prec = Eigen::NumTraits<Scalar>::dummy_precision())
41  {
42  return self.isApprox(other,prec);
43  }
44  };
45 
47 
48  template<typename SE3>
50  : public boost::python::def_visitor< SE3PythonVisitor<SE3> >
51  {
52  typedef typename SE3::Scalar Scalar;
53  typedef typename SE3::Matrix3 Matrix3;
54  typedef typename SE3::Vector3 Vector3;
55  typedef typename SE3::Matrix4 Matrix4;
56  typedef typename SE3::Quaternion Quaternion;
57 
58  public:
59 
60  template<class PyClass>
61  void visit(PyClass& cl) const
62  {
63  cl
64  .def(bp::init<Matrix3,Vector3>
65  ((bp::arg("self"),bp::arg("rotation"),bp::arg("translation")),
66  "Initialize from a rotation matrix and a translation vector."))
67  .def(bp::init<Quaternion,Vector3>
68  ((bp::arg("self"),bp::arg("quat"),bp::arg("translation")),
69  "Initialize from a quaternion and a translation vector."))
70  .def(bp::init<int>((bp::arg("self"),bp::arg("int")),"Init to identity."))
71  .def(bp::init<SE3>((bp::arg("self"),bp::arg("other")), "Copy constructor."))
72  .def(bp::init<Matrix4>
73  ((bp::arg("self"),bp::arg("array")),
74  "Initialize from an homogeneous matrix."))
75 
76  .add_property("rotation",
77  bp::make_function((typename SE3::AngularRef (SE3::*)()) &SE3::rotation,bp::return_internal_reference<>()),
78  (void (SE3::*)(const Matrix3 &)) &SE3::rotation,
79  "The rotation part of the transformation.")
80  .add_property("translation",
81  bp::make_function((typename SE3::LinearRef (SE3::*)()) &SE3::translation,bp::return_internal_reference<>()),
82  (void (SE3::*)(const Vector3 &)) &SE3::translation,
83  "The translation part of the transformation.")
84 
85  .add_property("homogeneous",&SE3::toHomogeneousMatrix,
86  "Returns the equivalent homegeneous matrix (acting on SE3).")
87  .add_property("action",&SE3::toActionMatrix,
88  "Returns the related action matrix (acting on Motion).")
89  .def("toActionMatrix",&SE3::toActionMatrix,bp::arg("self"),
90  "Returns the related action matrix (acting on Motion).")
91  .add_property("actionInverse",&SE3::toActionMatrixInverse,
92  "Returns the inverse of the action matrix (acting on Motion).\n"
93  "This is equivalent to do m.inverse().action")
94  .def("toActionMatrixInverse",&SE3::toActionMatrixInverse,bp::arg("self"),
95  "Returns the inverse of the action matrix (acting on Motion).\n"
96  "This is equivalent to do m.inverse().toActionMatrix()")
97  .add_property("dualAction",&SE3::toDualActionMatrix,
98  "Returns the related dual action matrix (acting on Force).")
99  .def("toDualActionMatrix",&SE3::toDualActionMatrix,bp::arg("self"),
100  "Returns the related dual action matrix (acting on Force).")
101 
102  .def("setIdentity",&SE3PythonVisitor::setIdentity,bp::arg("self"),
103  "Set *this to the identity placement.")
104  .def("setRandom",&SE3PythonVisitor::setRandom,bp::arg("self"),
105  "Set *this to a random placement.")
106 
107  .def("inverse", &SE3::inverse, bp::arg("self"),
108  "Returns the inverse transform")
109 
110  .def("act", (Vector3 (SE3::*)(const Vector3 &) const) &SE3::act,
111  bp::args("self","point"),
112  "Returns a point which is the result of the entry point transforms by *this.")
113  .def("actInv", (Vector3 (SE3::*)(const Vector3 &) const) &SE3::actInv,
114  bp::args("self","point"),
115  "Returns a point which is the result of the entry point by the inverse of *this.")
116 
117  .def("act", (SE3 (SE3::*)(const SE3 & other) const) &SE3::act,
118  bp::args("self","M"), "Returns the result of *this * M.")
119  .def("actInv", (SE3 (SE3::*)(const SE3 & other) const) &SE3::actInv,
120  bp::args("self","M"), "Returns the result of the inverse of *this times M.")
121 
122  .def("act", (Motion (SE3::*)(const Motion &) const) &SE3::act,
123  bp::args("self","motion"), "Returns the result action of *this onto a Motion.")
124  .def("actInv", (Motion (SE3::*)(const Motion &) const) &SE3::actInv,
125  bp::args("self","motion"), "Returns the result of the inverse of *this onto a Motion.")
126 
127  .def("act", (Force (SE3::*)(const Force &) const) &SE3::act,
128  bp::args("self","force"), "Returns the result of *this onto a Force.")
129  .def("actInv", (Force (SE3::*)(const Force &) const) &SE3::actInv,
130  bp::args("self","force"), "Returns the result of the inverse of *this onto an Inertia.")
131 
132  .def("act", (Inertia (SE3::*)(const Inertia &) const) &SE3::act,
133  bp::args("self","inertia"), "Returns the result of *this onto a Force.")
134  .def("actInv", (Inertia (SE3::*)(const Inertia &) const) &SE3::actInv,
135  bp::args("self","inertia"), "Returns the result of the inverse of *this onto an Inertia.")
136 
137  .def("isApprox",
139  isApproxSE3_overload(bp::args("self","other","prec"),
140  "Returns true if *this is approximately equal to other, within the precision given by prec."))
141 
142  .def("isIdentity",
143  &SE3::isIdentity,
144  isIdentity_overload(bp::args("self","prec"),
145  "Returns true if *this is approximately equal to the identity placement, within the precision given by prec."))
146 
147  .def("__invert__",&SE3::inverse,"Returns the inverse of *this.")
148  .def(bp::self * bp::self)
149  .def("__mul__",&__mul__<Motion>)
150  .def("__mul__",&__mul__<Force>)
151  .def("__mul__",&__mul__<Inertia>)
152  .def("__mul__",&__mul__<Vector3>)
153  .add_property("np",&SE3::toHomogeneousMatrix)
154 
155  .def(bp::self == bp::self)
156  .def(bp::self != bp::self)
157 
158  .def("Identity",&SE3::Identity,"Returns the identity transformation.")
159  .staticmethod("Identity")
160  .def("Random",&SE3::Random,"Returns a random transformation.")
161  .staticmethod("Random")
162  .def("Interpolate",&SE3::template Interpolate<double>,
163  bp::args("A","B","alpha"),
164  "Linear interpolation on the SE3 manifold.\n\n"
165  "This method computes the linear interpolation between A and B, such that the result C = A + (B-A)*t if it would be applied on classic Euclidian space.\n"
166  "This operation is very similar to the SLERP operation on Rotations.\n"
167  "Parameters:\n"
168  "\tA: Initial transformation\n"
169  "\tB: Target transformation\n"
170  "\talpha: Interpolation factor")
171  .staticmethod("Interpolate")
172 
173  .def("__array__",&SE3::toHomogeneousMatrix)
174 
175  .def_pickle(Pickle())
176  ;
177  }
178 
179  static void expose()
180  {
181  bp::class_<SE3>("SE3",
182  "SE3 transformation defined by a 3d vector and a rotation matrix.",
183  bp::init<>(bp::arg("self"),"Default constructor."))
184  .def(SE3PythonVisitor<SE3>())
185  .def(CopyableVisitor<SE3>())
186  .def(PrintableVisitor<SE3>())
187  ;
188 
189  }
190  private:
191 
192  struct Pickle : bp::pickle_suite
193  {
194  static
195  boost::python::tuple
196  getinitargs(const SE3 & M)
197  { return bp::make_tuple((Matrix3)M.rotation(),(Vector3)M.translation()); }
198  };
199 
200  static void setIdentity(SE3 & self) { self.setIdentity(); }
201  static void setRandom(SE3 & self) { self.setRandom(); }
202 
203  template<typename Spatial>
204  static Spatial __mul__(const SE3 & self, const Spatial & other)
205  { return self.act(other); }
206  };
207 
208  } // namespace python
209 } // namespace pinocchio
210 
211 #endif // ifndef __pinocchio_python_spatial_se3_hpp__
static boost::python::tuple getinitargs(const SE3 &M)
Set the Python method str and repr to use the overloading operator<<.
Definition: printable.hpp:21
BOOST_PYTHON_FUNCTION_OVERLOADS(computeKKTContactDynamicMatrixInverse_overload, computeKKTContactDynamicMatrixInverse_proxy, 4, 5) static const Eigen
static Spatial __mul__(const SE3 &self, const Spatial &other)
static void act(const Eigen::MatrixBase< Mat > &iV, const ForceDense< ForceDerived > &f, Eigen::MatrixBase< MatRet > const &jF)
Action of a motion set on a force object. The input motion set is represented by a 6xN matrix whose e...
SE3::Scalar Scalar
Definition: conversions.cpp:13
static bool isApprox(const SE3 &self, const SE3 &other, const Scalar &prec=Eigen::NumTraits< Scalar >::dummy_precision())
void inverse(const Eigen::MatrixBase< MatrixIn > &m_in, const Eigen::MatrixBase< MatrixOut > &dest)
Eigen::Quaternion< Scalar, Options > Quaternion
traits< SE3Tpl >::Matrix4 Matrix4
ConstLinearRef translation() const
Definition: se3-base.hpp:38
Add the Python method copy to allow a copy of this by calling the copy constructor.
Definition: copyable.hpp:21
traits< SE3Tpl >::Vector3 Vector3
Main pinocchio namespace.
Definition: timings.cpp:30
#define EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(...)
traits< SE3Tpl >::Matrix3 Matrix3
ConstAngularRef rotation() const
Definition: se3-base.hpp:37


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