se3.hpp 4.89 KB
Newer Older
1
//
2
// Copyright (c) 2015-2016 CNRS
3
// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
//
// This file is part of Pinocchio
// Pinocchio is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// Pinocchio is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// Pinocchio If not, see
// <http://www.gnu.org/licenses/>.

19
20
21
22
23
#ifndef __se3_python_se3_hpp__
#define __se3_python_se3_hpp__

#include <eigenpy/exception.hpp>
#include <eigenpy/eigenpy.hpp>
24
25
#include <eigenpy/memory.hpp>

26
#include "pinocchio/spatial/se3.hpp"
27
#include "pinocchio/bindings/python/utils/copyable.hpp"
28

29
EIGENPY_DEFINE_STRUCT_ALLOCATOR_SPECIALIZATION(se3::SE3)
30
31
32
33
34
35
36
37
38
39
40

namespace se3
{
  namespace python
  {
    namespace bp = boost::python;

    template<typename SE3>
    struct SE3PythonVisitor
      : public boost::python::def_visitor< SE3PythonVisitor<SE3> >
    {
41
      typedef typename SE3::Scalar Scalar;
42
43
44
45
46
      typedef typename SE3::Matrix3 Matrix3;
      typedef typename SE3::Matrix6 Matrix6;
      typedef typename SE3::Matrix4 Matrix4;
      typedef typename SE3::Vector6 Vector6;
      typedef typename SE3::Vector3 Vector3;
47
      
48
49
50
51
52
    public:

      template<class PyClass>
      void visit(PyClass& cl) const 
      {
53
        cl
54
        .def(bp::init<Matrix3,Vector3>
55
56
57
             ((bp::arg("Rotation"),bp::arg("translation")),
              "Initialize from rotation and translation."))
        .def(bp::init<int>((bp::arg("trivial arg (should be 1)")),"Init to identity."))
58
        .def(bp::init<SE3> ((bp::arg("other")), "Copy constructor."))
59
60
61

	  .add_property("rotation",&SE3PythonVisitor::getRotation,&SE3PythonVisitor::setRotation)
	  .add_property("translation",&SE3PythonVisitor::getTranslation,&SE3PythonVisitor::setTranslation)
62
63
	  .add_property("homogeneous",&SE3::toHomogeneousMatrix)
	  .add_property("action",&SE3::toActionMatrix)
64
65
66
        
    .def("setIdentity",&SE3PythonVisitor::setIdentity)
    .def("setRandom",&SE3PythonVisitor::setRandom)
67

68
	  .def("inverse", &SE3::inverse)
69
70
71
72
	  .def("act_point", &SE3PythonVisitor::act_point)
	  .def("actInv_point", &SE3PythonVisitor::actInv_point)
	  .def("act_se3", &SE3PythonVisitor::act_se3)
	  .def("actInv_se3", &SE3PythonVisitor::actInv_se3)
73
        
74
75
        .def("isApprox",(bool (SE3::*)(const SE3 & other, const Scalar & prec)) &SE3::isApprox,bp::args("other","prec"),"Returns true if *this is approximately equal to other, within the precision given by prec.")
        .def("isApprox",(bool (SE3::*)(const SE3 & other)) &SE3::isApprox,bp::args("other"),"Returns true if *this is approximately equal to other.")
76
        
77
78
        .def("isIdentity",(bool (SE3::*)(const Scalar & prec)) &SE3::isIdentity,bp::args("prec"),"Returns true if *this is approximately equal to the identity placement, within the precision given by prec.")
        .def("isIdentity",(bool (SE3::*)(void)) &SE3::isIdentity,"Returns true if *this is approximately equal to the identity placement.")
79
80
	  
	  .def("__str__",&SE3PythonVisitor::toString)
81
	  .def("__invert__",&SE3::inverse)
82
	  .def(bp::self * bp::self)
83
	  .add_property("np",&SE3::toActionMatrix)
84

85
	  .def("Identity",&SE3::Identity)
86
	  .staticmethod("Identity")
87
	  .def("Random",&SE3::Random)
88
89
90
91
	  .staticmethod("Random")
	  ;
	  }

92
93
94
95
      static Matrix3 getRotation( const SE3 & self ) { return self.rotation(); }
      static void setRotation( SE3 & self, const Matrix3 & R ) { self.rotation(R); }
      static Vector3 getTranslation( const SE3 & self ) { return self.translation(); }
      static void setTranslation( SE3 & self, const Vector3 & p ) { self.translation(p); }
96
      
97
98
99
100
101
102
103
104
      static void setIdentity(SE3 & self) { self.setIdentity(); }
      static void setRandom(SE3 & self) { self.setRandom(); }

      static Vector3 act_point( const SE3 & self, const Vector3 & pt ) { return self.act(pt); }
      static Vector3 actInv_point( const SE3 & self, const Vector3 & pt ) { return self.actInv(pt); }
      static SE3 act_se3( const SE3 & self, const SE3 & x ) { return self.act(x); }
      static SE3 actInv_se3( const SE3 & self, const SE3 & x ) { return self.actInv(x); }
      static std::string toString(const SE3& m) 
105
106
107
108
      {	  std::ostringstream s; s << m; return s.str();       }

      static void expose()
      {
109
	bp::class_<SE3>("SE3",
110
111
112
113
			   "SE3 transformations, composing rotation and translation.\n\n"
			   "Supported operations ...",
			   bp::init<>())
	  .def(SE3PythonVisitor<SE3>())
114
        .def(CopyableVisitor<SE3>())
115
116
117
118
119
120
121
122
123
124
125
126
127
	;
    
    }


    };
    


  }} // namespace se3::python

#endif // ifndef __se3_python_se3_hpp__