1 |
|
|
// |
2 |
|
|
// Copyright (c) 2015-2021 CNRS INRIA |
3 |
|
|
// |
4 |
|
|
|
5 |
|
|
#ifndef __pinocchio_python_joint_variants_hpp__ |
6 |
|
|
#define __pinocchio_python_joint_variants_hpp__ |
7 |
|
|
|
8 |
|
|
#include <boost/python.hpp> |
9 |
|
|
#include <eigenpy/exception.hpp> |
10 |
|
|
|
11 |
|
|
#include "pinocchio/multibody/joint/joint-collection.hpp" |
12 |
|
|
|
13 |
|
|
namespace pinocchio |
14 |
|
|
{ |
15 |
|
|
namespace python |
16 |
|
|
{ |
17 |
|
|
namespace bp = boost::python; |
18 |
|
|
|
19 |
|
|
template<class JointModelDerived> |
20 |
|
|
struct JointModelDerivedPythonVisitor |
21 |
|
|
: public boost::python::def_visitor< JointModelDerivedPythonVisitor<JointModelDerived> > |
22 |
|
|
{ |
23 |
|
|
public: |
24 |
|
|
|
25 |
|
|
template<class PyClass> |
26 |
|
798 |
void visit(PyClass& cl) const |
27 |
|
|
{ |
28 |
✓✗ |
798 |
cl |
29 |
|
|
.def(bp::init<>(bp::arg("self"))) |
30 |
|
|
// All are add_properties cause ReadOnly |
31 |
✓✗✓✗
|
798 |
.add_property("id",&get_id) |
32 |
✓✗ |
798 |
.add_property("idx_q",&get_idx_q) |
33 |
✓✗ |
798 |
.add_property("idx_v",&get_idx_v) |
34 |
✓✗ |
798 |
.add_property("nq",&get_nq) |
35 |
✓✗ |
798 |
.add_property("nv",&get_nv) |
36 |
✓✗ |
798 |
.add_property("hasConfigurationLimit", |
37 |
|
|
&JointModelDerived::hasConfigurationLimit, |
38 |
|
|
"Return vector of boolean if joint has configuration limits.") |
39 |
|
1596 |
.add_property("hasConfigurationLimitInTangent", |
40 |
|
|
&JointModelDerived::hasConfigurationLimitInTangent, |
41 |
|
|
"Return vector of boolean if joint has configuration limits in tangent space.") |
42 |
✓✗ |
798 |
.def("setIndexes", |
43 |
|
|
&JointModelDerived::setIndexes, |
44 |
|
|
bp::args("self","id","idx_q","idx_v")) |
45 |
✓✗✓✗
|
1596 |
.def("hasSameIndexes", |
46 |
|
|
&JointModelDerived::template hasSameIndexes<JointModelDerived>, |
47 |
|
|
bp::args("self","other"), |
48 |
|
|
"Check if this has same indexes than other.") |
49 |
✓✗✓✗
|
1596 |
.def("shortname", |
50 |
|
|
&JointModelDerived::shortname, |
51 |
|
|
bp::arg("self"), |
52 |
|
|
"Returns string indicating the joint type (class name):" |
53 |
|
|
"\n\t- JointModelR[*]: Revolute Joint, with rotation axis [*] ∈ [X,Y,Z]" |
54 |
|
|
"\n\t- JointModelRevoluteUnaligned: Revolute Joint, with rotation axis not aligned with X, Y, nor Z" |
55 |
|
|
"\n\t- JointModelRUB[*]: Unbounded revolute Joint (without position limits), with rotation axis [*] ∈ [X,Y,Z]" |
56 |
|
|
"\n\t- JointModelRevoluteUnboundedUnaligned: Unbounded revolute Joint, with rotation axis not aligned with X, Y, nor Z" |
57 |
|
|
"\n\t- JointModelP[*]: Prismatic Joint, with rotation axis [*] ∈ [X,Y,Z]" |
58 |
|
|
"\n\t- JointModelPlanar: Planar joint" |
59 |
|
|
"\n\t- JointModelPrismaticUnaligned: Prismatic joint, with translation axis not aligned with X, Y, nor Z" |
60 |
|
|
"\n\t- JointModelSphericalZYX: Spherical joint (3D rotation)" |
61 |
|
|
"\n\t- JointModelTranslation: Translation joint (3D translation)" |
62 |
|
|
"\n\t- JointModelFreeFlyer: Joint enabling 3D rotation and translations." |
63 |
|
|
) |
64 |
|
|
|
65 |
✓✗✓✗
|
2394 |
.def(bp::self == bp::self) |
66 |
✓✗✓✗
|
2394 |
.def(bp::self != bp::self) |
67 |
|
|
; |
68 |
|
798 |
} |
69 |
|
|
|
70 |
|
|
static JointIndex get_id(const JointModelDerived & self) |
71 |
|
|
{ return self.id(); } |
72 |
|
|
static int get_idx_q(const JointModelDerived & self) |
73 |
|
|
{ return self.idx_q(); } |
74 |
|
|
static int get_idx_v(const JointModelDerived & self) |
75 |
|
|
{ return self.idx_v(); } |
76 |
|
|
static int get_nq(const JointModelDerived & self) |
77 |
|
|
{ return self.nq(); } |
78 |
|
|
static int get_nv(const JointModelDerived & self) |
79 |
|
|
{ return self.nv(); } |
80 |
|
|
|
81 |
|
|
}; |
82 |
|
|
|
83 |
|
|
template<class JointDataDerived> |
84 |
|
|
struct JointDataDerivedPythonVisitor |
85 |
|
|
: public boost::python::def_visitor< JointDataDerivedPythonVisitor<JointDataDerived> > |
86 |
|
|
{ |
87 |
|
|
public: |
88 |
|
|
|
89 |
|
|
template<class PyClass> |
90 |
|
798 |
void visit(PyClass& cl) const |
91 |
|
|
{ |
92 |
✓✗ |
798 |
cl |
93 |
|
|
// All are add_properties cause ReadOnly |
94 |
|
|
.add_property("S",&get_S) |
95 |
|
798 |
.add_property("M",&get_M) |
96 |
|
798 |
.add_property("v",&get_v) |
97 |
|
798 |
.add_property("c",&get_c) |
98 |
|
798 |
.add_property("U",&get_U) |
99 |
|
798 |
.add_property("Dinv",&get_Dinv) |
100 |
|
798 |
.add_property("UDinv",&get_UDinv) |
101 |
|
798 |
.def("shortname",&JointDataDerived::shortname) |
102 |
|
|
|
103 |
|
798 |
.def(bp::self == bp::self) |
104 |
✓✗ |
1596 |
.def(bp::self != bp::self) |
105 |
|
|
; |
106 |
|
798 |
} |
107 |
|
|
|
108 |
|
|
static typename JointDataDerived::Constraint_t get_S(const JointDataDerived & self) |
109 |
|
|
{ return self.S_accessor(); } |
110 |
|
|
static typename JointDataDerived::Transformation_t get_M(const JointDataDerived & self) |
111 |
|
|
{ return self.M_accessor(); } |
112 |
|
|
static typename JointDataDerived::Motion_t get_v(const JointDataDerived & self) |
113 |
|
|
{ return self.v_accessor(); } |
114 |
|
|
static typename JointDataDerived::Bias_t get_c(const JointDataDerived & self) |
115 |
|
|
{ return self.c_accessor(); } |
116 |
|
|
static typename JointDataDerived::U_t get_U(const JointDataDerived & self) |
117 |
|
|
{ return self.U_accessor(); } |
118 |
|
|
static typename JointDataDerived::D_t get_Dinv(const JointDataDerived & self) |
119 |
|
|
{ return self.Dinv_accessor(); } |
120 |
|
|
static typename JointDataDerived::UD_t get_UDinv(const JointDataDerived & self) |
121 |
|
|
{ return self.UDinv_accessor(); } |
122 |
|
|
|
123 |
|
|
static void expose() |
124 |
|
|
{} |
125 |
|
|
|
126 |
|
|
}; |
127 |
|
|
|
128 |
|
|
}} // namespace pinocchio::python |
129 |
|
|
|
130 |
|
|
#endif // ifndef __pinocchio_python_joint_variants_hpp__ |