collision.cc 8.78 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
2
3
//
// Software License Agreement (BSD License)
//
4
//  Copyright (c) 2019-2021 CNRS-LAAS, INRIA
Joseph Mirabel's avatar
Joseph Mirabel committed
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
//  Author: Joseph Mirabel
//  All rights reserved.
//
//  Redistribution and use in source and binary forms, with or without
//  modification, are permitted provided that the following conditions
//  are met:
//
//   * Redistributions of source code must retain the above copyright
//     notice, this list of conditions and the following disclaimer.
//   * Redistributions in binary form must reproduce the above
//     copyright notice, this list of conditions and the following
//     disclaimer in the documentation and/or other materials provided
//     with the distribution.
//   * Neither the name of CNRS-LAAS. nor the names of its
//     contributors may be used to endorse or promote products derived
//     from this software without specific prior written permission.
//
//  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
//  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
//  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
//  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
//  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
//  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
//  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
//  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
//  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
//  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
//  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
//  POSSIBILITY OF SUCH DAMAGE.

#include <boost/python.hpp>
36
#include <boost/python/suite/indexing/vector_indexing_suite.hpp>
Joseph Mirabel's avatar
Joseph Mirabel committed
37

38
#include <eigenpy/eigenpy.hpp>
39

Joseph Mirabel's avatar
Joseph Mirabel committed
40
41
42
#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/collision.h>

Joseph Mirabel's avatar
Joseph Mirabel committed
43
44
#include "fcl.hh"

45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
#ifdef HPP_FCL_HAS_DOXYGEN_AUTODOC
#include "doxygen_autodoc/functions.h"
#include "doxygen_autodoc/hpp/fcl/collision_data.h"
#endif

#include "../doc/python/doxygen.hh"
#include "../doc/python/doxygen-boost.hh"

#define DEF_RW_CLASS_ATTRIB(CLASS, ATTRIB)                                     \
  def_readwrite (#ATTRIB, &CLASS::ATTRIB,                                      \
      doxygen::class_attrib_doc<CLASS>(#ATTRIB))
#define DEF_RO_CLASS_ATTRIB(CLASS, ATTRIB)                                     \
  def_readonly (#ATTRIB, &CLASS::ATTRIB,                                       \
      doxygen::class_attrib_doc<CLASS>(#ATTRIB))
#define DEF_CLASS_FUNC(CLASS, ATTRIB)                                          \
Joseph Mirabel's avatar
Joseph Mirabel committed
60
  def (dv::member_func(#ATTRIB, &CLASS::ATTRIB))
61
62
63
#define DEF_CLASS_FUNC2(CLASS, ATTRIB,policy)                                  \
  def (#ATTRIB, &CLASS::ATTRIB, doxygen::member_func_doc(&CLASS::ATTRIB),policy)

Joseph Mirabel's avatar
Joseph Mirabel committed
64
65
66
using namespace boost::python;
using namespace hpp::fcl;

Joseph Mirabel's avatar
Joseph Mirabel committed
67
68
namespace dv = doxygen::visitor;

69
70
71
template<int index> const CollisionGeometry* geto(const Contact& c)
{ return index == 1 ? c.o1 : c.o2; }

Joseph Mirabel's avatar
Joseph Mirabel committed
72
73
void exposeCollisionAPI ()
{
74
75
76
77
78
79
  if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequestFlag>())
  {
    enum_ <CollisionRequestFlag> ("CollisionRequestFlag")
      .value ("CONTACT", CONTACT)
      .value ("DISTANCE_LOWER_BOUND", DISTANCE_LOWER_BOUND)
      .value ("NO_REQUEST", NO_REQUEST)
80
      .export_values()
81
82
      ;
  }
83
  
Justin Carpentier's avatar
Justin Carpentier committed
84
  if(!eigenpy::register_symbolic_link_to_registered_type<CPUTimes>())
85
  {
Justin Carpentier's avatar
Justin Carpentier committed
86
87
88
89
90
    class_<CPUTimes>("CPUTimes",no_init)
    .def_readonly("wall",&CPUTimes::wall,"wall time in micro seconds (us)")
    .def_readonly("user",&CPUTimes::user,"user time in micro seconds (us)")
    .def_readonly("system",&CPUTimes::system,"system time in micro seconds (us)")
    .def("clear",&CPUTimes::clear,arg("self"),"Reset the time values.")
91
92
    ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
93

94
95
96
97
98
99
100
101
102
103
104
  if(!eigenpy::register_symbolic_link_to_registered_type<QueryRequest>())
  {
    class_ <QueryRequest> ("QueryRequest",
        doxygen::class_doc<QueryRequest>(), no_init)
      .DEF_RW_CLASS_ATTRIB (QueryRequest, enable_cached_gjk_guess    )
      .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_gjk_guess           )
      .DEF_RW_CLASS_ATTRIB (QueryRequest, cached_support_func_guess  )
      .DEF_CLASS_FUNC (QueryRequest, updateGuess)
      ;
  }

105
106
  if(!eigenpy::register_symbolic_link_to_registered_type<CollisionRequest>())
  {
107
    class_ <CollisionRequest, bases<QueryRequest> > ("CollisionRequest",
Joseph Mirabel's avatar
Joseph Mirabel committed
108
        doxygen::class_doc<CollisionRequest>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
109
110
      .def (dv::init<CollisionRequest>())
      .def (dv::init<CollisionRequest, const CollisionRequestFlag, size_t>())
111
112
113
114
115
      .DEF_RW_CLASS_ATTRIB (CollisionRequest, num_max_contacts           )
      .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_contact             )
      .DEF_RW_CLASS_ATTRIB (CollisionRequest, enable_distance_lower_bound)
      .DEF_RW_CLASS_ATTRIB (CollisionRequest, security_margin            )
      .DEF_RW_CLASS_ATTRIB (CollisionRequest, break_distance             )
116
      .DEF_RW_CLASS_ATTRIB (CollisionRequest, distance_upper_bound       )
117
118
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
119

120
121
122
123
124
125
126
  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<CollisionRequest> >())
  {
    class_< std::vector<CollisionRequest> >("StdVec_CollisionRequest")
      .def(vector_indexing_suite< std::vector<CollisionRequest> >())
      ;
  }

127
128
  if(!eigenpy::register_symbolic_link_to_registered_type<Contact>())
  {
129
    class_ <Contact> ("Contact",
130
        doxygen::class_doc<Contact>(), init<>(arg("self"),"Default constructor"))
131
132
      //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>())
      //.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>())
133
134
135
136
137
138
      .add_property ("o1",
          make_function(&geto<1>, return_value_policy<reference_existing_object>()),
          doxygen::class_attrib_doc<Contact>("o1"))
      .add_property ("o2",
          make_function(&geto<2>, return_value_policy<reference_existing_object>()),
          doxygen::class_attrib_doc<Contact>("o2"))
139
140
      .DEF_RW_CLASS_ATTRIB (Contact, b1)
      .DEF_RW_CLASS_ATTRIB (Contact, b2)
141
142
      .DEF_RW_CLASS_ATTRIB (Contact, normal)
      .DEF_RW_CLASS_ATTRIB (Contact, pos)
143
      .DEF_RW_CLASS_ATTRIB (Contact, penetration_depth)
144
145
146
147
      .def (self == self)
      .def (self != self)
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
148

149
150
151
152
153
154
  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<Contact> >())
  {
    class_< std::vector<Contact> >("StdVec_Contact")
      .def(vector_indexing_suite< std::vector<Contact> >())
      ;
  }
155

156
157
158
159
160
161
  if(!eigenpy::register_symbolic_link_to_registered_type<QueryResult>())
  {
    class_ <QueryResult> ("QueryResult",
        doxygen::class_doc<QueryResult>(), no_init)
      .DEF_RW_CLASS_ATTRIB (QueryResult, cached_gjk_guess         )
      .DEF_RW_CLASS_ATTRIB (QueryResult, cached_support_func_guess)
162
      .DEF_RW_CLASS_ATTRIB (QueryResult, timings)
163
164
165
      ;
  }

166
  if(!eigenpy::register_symbolic_link_to_registered_type< CollisionResult >())
167
  {
168
    class_ <CollisionResult, bases<QueryResult> > ("CollisionResult",
Joseph Mirabel's avatar
Joseph Mirabel committed
169
        doxygen::class_doc<CollisionResult>(), no_init)
Justin Carpentier's avatar
Justin Carpentier committed
170
      .def (dv::init<CollisionResult>())
171
172
      .DEF_CLASS_FUNC (CollisionResult, isCollision)
      .DEF_CLASS_FUNC (CollisionResult, numContacts)
Joseph Mirabel's avatar
Joseph Mirabel committed
173
      .DEF_CLASS_FUNC (CollisionResult, addContact)
174
175
      .DEF_CLASS_FUNC (CollisionResult, clear)
      .DEF_CLASS_FUNC2 (CollisionResult, getContact , return_value_policy<copy_const_reference>())
176
177
178
179
      .def(dv::member_func("getContacts",static_cast<void (CollisionResult::*)(std::vector<Contact> &) const>(&CollisionResult::getContacts)))
      .def("getContacts",static_cast<const std::vector<Contact> & (CollisionResult::*)() const>(&CollisionResult::getContacts),
           doxygen::member_func_doc(static_cast<const std::vector<Contact> & (CollisionResult::*)() const>(&CollisionResult::getContacts)),
           return_internal_reference<>())
180
181

      .DEF_RW_CLASS_ATTRIB(CollisionResult, distance_lower_bound)
182
183
      ;
  }
Joseph Mirabel's avatar
Joseph Mirabel committed
184

185
186
  if(!eigenpy::register_symbolic_link_to_registered_type< std::vector<CollisionResult> >())
  {
187
    class_< std::vector<CollisionResult> >("StdVec_CollisionResult")
188
189
190
      .def(vector_indexing_suite< std::vector<CollisionResult> >())
      ;
  }
191

192
  doxygen::def ("collide", static_cast< std::size_t (*)(const CollisionObject*, const CollisionObject*,
Joseph Mirabel's avatar
Joseph Mirabel committed
193
        const CollisionRequest&, CollisionResult&) > (&collide));
194
  doxygen::def ("collide", static_cast< std::size_t (*)(
Joseph Mirabel's avatar
Joseph Mirabel committed
195
196
        const CollisionGeometry*, const Transform3f&,
        const CollisionGeometry*, const Transform3f&,
197
        CollisionRequest&, CollisionResult&) > (&collide));
Joseph Mirabel's avatar
Joseph Mirabel committed
198
199
200
201
202
203

  class_<ComputeCollision> ("ComputeCollision",
      doxygen::class_doc<ComputeCollision>(), no_init)
    .def (dv::init<ComputeCollision, const CollisionGeometry*, const CollisionGeometry*>())
    .def ("__call__", static_cast< std::size_t (ComputeCollision::*)(
        const Transform3f&, const Transform3f&,
204
        CollisionRequest&, CollisionResult&) const> (&ComputeCollision::operator()));
Joseph Mirabel's avatar
Joseph Mirabel committed
205

Joseph Mirabel's avatar
Joseph Mirabel committed
206
}