Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
hpp-fcl
Commits
f8fe2d8a
Unverified
Commit
f8fe2d8a
authored
Mar 26, 2020
by
Joseph Mirabel
Committed by
GitHub
Mar 26, 2020
Browse files
Merge pull request #146 from jcarpent/topic/devel
Improce code efficiency + use shared memory between Numpy and Eigen
parents
cf5feb8d
3a0cdbbc
Changes
10
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
f8fe2d8a
...
...
@@ -74,7 +74,7 @@ PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
add_required_dependency
(
"eigen3 >= 3.0.0"
)
if
(
BUILD_PYTHON_INTERFACE
)
ADD_COMPILE_DEPENDENCY
(
"eigenpy >= 2.
0
"
)
ADD_COMPILE_DEPENDENCY
(
"eigenpy >= 2.
2
"
)
endif
()
# Add a cache variable to allow not compiling and running tests
...
...
cmake
@
321eb1cc
Compare
93ec987e
...
321eb1cc
Subproject commit
93ec987ecdc016039c8731e203943e5a83eb96d5
Subproject commit
321eb1ccf1d94570eb564f3659b13ef3ef82239e
include/hpp/fcl/narrowphase/narrowphase.h
View file @
f8fe2d8a
...
...
@@ -299,6 +299,8 @@ namespace fcl
mutable
Vec3f
cached_guess
;
};
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wc99-extensions"
/// \name Shape intersection specializations
/// \{
...
...
@@ -403,6 +405,7 @@ namespace fcl
#undef HPP_FCL_DECLARE_SHAPE_DISTANCE_PAIR
/// \}
#pragma GCC diagnostic pop
}
}
// namespace hpp
...
...
package.xml
View file @
f8fe2d8a
<?xml version="1.0"?>
<package
format=
"2"
>
<name>
hpp-fcl
</name>
<version>
1.
3
.0
</version>
<version>
1.
4
.0
</version>
<description>
An extension of the Flexible Collision Library.
</description>
<!-- The maintainer listed here is for the ROS release to receive emails for the buildfarm.
Please check the repository URL for full list of authors and maintainers. -->
...
...
python/collision-geometries.cc
View file @
f8fe2d8a
...
...
@@ -35,6 +35,7 @@
#include
<boost/python.hpp>
#include
<eigenpy/registration.hpp>
#include
<eigenpy/eigen-to-python.hpp>
#include
"fcl.hh"
...
...
@@ -138,10 +139,7 @@ void exposeShapes ()
.
def
(
dv
::
init
<
Box
>
())
.
def
(
dv
::
init
<
Box
,
FCL_REAL
,
FCL_REAL
,
FCL_REAL
>
())
.
def
(
dv
::
init
<
Box
,
const
Vec3f
&>
())
.
add_property
(
"halfSide"
,
make_getter
(
&
Box
::
halfSide
,
return_value_policy
<
return_by_value
>
()),
make_setter
(
&
Box
::
halfSide
,
return_value_policy
<
return_by_value
>
()),
doxygen
::
class_attrib_doc
<
Box
>
(
"halfSide"
))
.
DEF_RW_CLASS_ATTRIB
(
Box
,
halfSide
)
;
class_
<
Capsule
,
bases
<
ShapeBase
>
,
shared_ptr
<
Capsule
>
>
...
...
@@ -263,11 +261,11 @@ void exposeCollisionGeometries ()
class_
<
AABB
>
(
"AABB"
,
"A class describing the AABB collision structure, which is a box in 3D space determined by two diagonal points"
,
no_init
)
.
def
(
init
<>
(
"Default constructor"
))
.
def
(
init
<
Vec3f
>
(
bp
::
arg
(
"v"
),
"Creating an AABB at position v with zero size."
))
.
def
(
init
<
Vec3f
,
Vec3f
>
(
bp
::
args
(
"a"
,
"b"
),
"Creating an AABB with two endpoints a and b."
))
.
def
(
init
<
AABB
,
Vec3f
>
(
bp
::
args
(
"core"
,
"delta"
),
"Creating an AABB centered as core and is of half-dimension delta."
))
.
def
(
init
<
Vec3f
,
Vec3f
,
Vec3f
>
(
bp
::
args
(
"a"
,
"b"
,
"c"
),
"Creating an AABB contains three points."
))
.
def
(
init
<>
(
bp
::
arg
(
"self"
),
"Default constructor"
))
.
def
(
init
<
Vec3f
>
(
bp
::
arg
s
(
"self"
,
"v"
),
"Creating an AABB at position v with zero size."
))
.
def
(
init
<
Vec3f
,
Vec3f
>
(
bp
::
args
(
"
self"
,
"
a"
,
"b"
),
"Creating an AABB with two endpoints a and b."
))
.
def
(
init
<
AABB
,
Vec3f
>
(
bp
::
args
(
"
self"
,
"
core"
,
"delta"
),
"Creating an AABB centered as core and is of half-dimension delta."
))
.
def
(
init
<
Vec3f
,
Vec3f
,
Vec3f
>
(
bp
::
args
(
"
self"
,
"
a"
,
"b"
,
"c"
),
"Creating an AABB contains three points."
))
.
def
(
"contain"
,
(
bool
(
AABB
::*
)(
const
Vec3f
&
)
const
)
&
AABB
::
contain
,
...
...
python/collision.cc
View file @
f8fe2d8a
//
// Software License Agreement (BSD License)
//
// Copyright (c) 2019 CNRS-LAAS INRIA
// Copyright (c) 2019
-2020
CNRS-LAAS INRIA
// Author: Joseph Mirabel
// All rights reserved.
//
...
...
@@ -35,6 +35,7 @@
#include
<boost/python.hpp>
#include
<boost/python/suite/indexing/vector_indexing_suite.hpp>
#include
<eigenpy/eigen-to-python.hpp>
#include
<eigenpy/registration.hpp>
#include
<hpp/fcl/fwd.hh>
...
...
@@ -83,7 +84,6 @@ void exposeCollisionAPI ()
doxygen
::
class_doc
<
CollisionRequest
>
(),
no_init
)
.
def
(
dv
::
init
<
CollisionRequest
>
())
.
def
(
dv
::
init
<
CollisionRequest
,
const
CollisionRequestFlag
,
size_t
>
())
.
DEF_RW_CLASS_ATTRIB
(
CollisionRequest
,
num_max_contacts
)
.
DEF_RW_CLASS_ATTRIB
(
CollisionRequest
,
enable_contact
)
.
DEF_RW_CLASS_ATTRIB
(
CollisionRequest
,
enable_distance_lower_bound
)
...
...
@@ -97,21 +97,15 @@ void exposeCollisionAPI ()
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
Contact
>
())
{
class_
<
Contact
>
(
"Contact"
,
doxygen
::
class_doc
<
Contact
>
(),
init
<>
())
doxygen
::
class_doc
<
Contact
>
(),
init
<>
(
arg
(
"self"
),
"Default constructor"
))
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int>())
//.def(init<CollisionGeometryPtr_t, CollisionGeometryPtr_t, int, int, Vec3f, Vec3f, FCL_REAL>())
.
DEF_RO_CLASS_ATTRIB
(
Contact
,
o1
)
.
DEF_RO_CLASS_ATTRIB
(
Contact
,
o2
)
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
b1
)
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
b2
)
.
add_property
(
"normal"
,
make_getter
(
&
Contact
::
normal
,
return_value_policy
<
return_by_value
>
()),
make_setter
(
&
Contact
::
normal
,
return_value_policy
<
return_by_value
>
()),
doxygen
::
class_attrib_doc
<
Contact
>
(
"normal"
))
.
add_property
(
"pos"
,
make_getter
(
&
Contact
::
pos
,
return_value_policy
<
return_by_value
>
()),
make_setter
(
&
Contact
::
pos
,
return_value_policy
<
return_by_value
>
()),
doxygen
::
class_attrib_doc
<
Contact
>
(
"pos"
))
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
normal
)
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
pos
)
.
DEF_RW_CLASS_ATTRIB
(
Contact
,
penetration_depth
)
.
def
(
self
==
self
)
.
def
(
self
!=
self
)
...
...
python/distance.cc
View file @
f8fe2d8a
...
...
@@ -36,6 +36,7 @@
#include
<boost/python/suite/indexing/vector_indexing_suite.hpp>
#include
<eigenpy/registration.hpp>
#include
<eigenpy/eigen-to-python.hpp>
#include
"fcl.hh"
...
...
@@ -65,6 +66,8 @@ using namespace boost::python;
using
namespace
hpp
::
fcl
;
namespace
dv
=
doxygen
::
visitor
;
struct
DistanceRequestWrapper
{
static
Vec3f
getNearestPoint1
(
const
DistanceResult
&
res
)
{
return
res
.
nearest_points
[
0
];
}
...
...
@@ -77,7 +80,10 @@ void exposeDistanceAPI ()
{
class_
<
DistanceRequest
>
(
"DistanceRequest"
,
doxygen
::
class_doc
<
DistanceRequest
>
(),
init
<
optional
<
bool
,
FCL_REAL
,
FCL_REAL
>
>
())
init
<
optional
<
bool
,
FCL_REAL
,
FCL_REAL
>
>
((
arg
(
"self"
),
arg
(
"enable_nearest_points"
),
arg
(
"rel_err"
),
arg
(
"abs_err"
)),
"Constructor"
))
.
DEF_RW_CLASS_ATTRIB
(
DistanceRequest
,
enable_nearest_points
)
.
DEF_RW_CLASS_ATTRIB
(
DistanceRequest
,
rel_err
)
.
DEF_RW_CLASS_ATTRIB
(
DistanceRequest
,
abs_err
)
...
...
@@ -87,12 +93,11 @@ void exposeDistanceAPI ()
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
DistanceResult
>
())
{
class_
<
DistanceResult
>
(
"DistanceResult"
,
doxygen
::
class_doc
<
DistanceResult
>
(),
init
<>
())
doxygen
::
class_doc
<
DistanceResult
>
(),
no_init
)
.
def
(
dv
::
init
<
DistanceResult
>
())
.
DEF_RW_CLASS_ATTRIB
(
DistanceResult
,
min_distance
)
.
add_property
(
"normal"
,
make_getter
(
&
DistanceResult
::
normal
,
return_value_policy
<
return_by_value
>
()),
make_setter
(
&
DistanceResult
::
normal
,
return_value_policy
<
return_by_value
>
()),
doxygen
::
class_attrib_doc
<
DistanceResult
>
(
"normal"
))
.
DEF_RW_CLASS_ATTRIB
(
DistanceResult
,
normal
)
//.def_readwrite ("nearest_points", &DistanceResult::nearest_points)
.
def
(
"getNearestPoint1"
,
&
DistanceRequestWrapper
::
getNearestPoint1
,
doxygen
::
class_attrib_doc
<
DistanceResult
>
(
"nearest_points"
))
...
...
python/fcl.cc
View file @
f8fe2d8a
...
...
@@ -64,10 +64,10 @@ void exposeMeshLoader ()
{
class_
<
MeshLoader
,
boost
::
shared_ptr
<
MeshLoader
>
>
(
"MeshLoader"
,
doxygen
::
class_doc
<
MeshLoader
>
(),
init
<
optional
<
NODE_TYPE
>
>
(
arg
(
"node_type"
),
init
<
optional
<
NODE_TYPE
>
>
(
(
arg
(
"self"
),
arg
(
"node_type"
)
)
,
doxygen
::
constructor_doc
<
MeshLoader
,
const
NODE_TYPE
&>
()))
.
def
(
"load"
,
&
MeshLoader
::
load
,
load_overloads
(
arg
s
(
"filename"
,
"scale"
),
load_overloads
(
(
arg
(
"self"
),
arg
(
"filename"
),
arg
(
"scale"
)
)
,
doxygen
::
member_func_doc
(
&
MeshLoader
::
load
)))
;
}
...
...
@@ -77,7 +77,7 @@ void exposeMeshLoader ()
class_
<
CachedMeshLoader
,
bases
<
MeshLoader
>
,
boost
::
shared_ptr
<
CachedMeshLoader
>
>
(
"CachedMeshLoader"
,
doxygen
::
class_doc
<
MeshLoader
>
(),
init
<
optional
<
NODE_TYPE
>
>
(
arg
(
"node_type"
),
init
<
optional
<
NODE_TYPE
>
>
(
(
arg
(
"self"
),
arg
(
"node_type"
)
)
,
doxygen
::
constructor_doc
<
CachedMeshLoader
,
const
NODE_TYPE
&>
()))
;
}
...
...
python/math.cc
View file @
f8fe2d8a
...
...
@@ -80,7 +80,7 @@ void exposeMaths ()
eigenpy
::
enableEigenPySpecific
<
Matrix3f
>
();
eigenpy
::
enableEigenPySpecific
<
Vec3f
>
();
class_
<
Transform3f
>
(
"Transform3f"
,
doxygen
::
class_doc
<
Transform3f
>
(),
init
<>
(
doxygen
::
constructor_doc
<
Transform3f
>
()))
class_
<
Transform3f
>
(
"Transform3f"
,
doxygen
::
class_doc
<
Transform3f
>
(),
init
<>
(
arg
(
"self"
),
doxygen
::
constructor_doc
<
Transform3f
>
()))
.
def
(
init
<
Matrix3f
,
Vec3f
>
(
doxygen
::
constructor_doc
<
Transform3f
,
const
Matrix3f
::
MatrixBase
&
,
const
Vec3f
::
MatrixBase
&>
()))
.
def
(
init
<
Quaternion3f
,
Vec3f
>
(
doxygen
::
constructor_doc
<
Transform3f
,
const
Quaternion3f
&
,
const
Vec3f
::
MatrixBase
&>
()))
.
def
(
init
<
Matrix3f
>
(
doxygen
::
constructor_doc
<
Transform3f
,
const
Matrix3f
&
>
()))
...
...
src/narrowphase/details.h
View file @
f8fe2d8a
...
...
@@ -2019,7 +2019,7 @@ namespace fcl {
// FCL_REAL* penetration_depth, Vec3f* normal)
{
static
const
FCL_REAL
eps
(
sqrt
(
std
::
numeric_limits
<
FCL_REAL
>::
epsilon
()));
Plane
new_s2
=
transform
(
s2
,
tf2
);
const
Plane
new_s2
=
transform
(
s2
,
tf2
);
const
Matrix3f
&
R
=
tf1
.
getRotation
();
const
Vec3f
&
T
=
tf1
.
getTranslation
();
...
...
@@ -2027,7 +2027,7 @@ namespace fcl {
const
Vec3f
Q
(
R
.
transpose
()
*
new_s2
.
n
);
const
Vec3f
A
(
Q
.
cwiseProduct
(
s1
.
halfSide
));
FCL_REAL
signed_dist
=
new_s2
.
signedDistance
(
T
);
const
FCL_REAL
signed_dist
=
new_s2
.
signedDistance
(
T
);
distance
=
std
::
abs
(
signed_dist
)
-
A
.
lpNorm
<
1
>
();
if
(
distance
>
0
)
{
// Is the box above or below the plane
...
...
@@ -2048,11 +2048,6 @@ namespace fcl {
return
false
;
}
Vec3f
axis
[
3
];
axis
[
0
]
=
R
.
col
(
0
);
axis
[
1
]
=
R
.
col
(
1
);
axis
[
2
]
=
R
.
col
(
2
);
// find the deepest point
Vec3f
p
=
T
;
...
...
@@ -2065,29 +2060,29 @@ namespace fcl {
std
::
abs
(
Q
[
0
]
+
1
)
<
planeIntersectTolerance
<
FCL_REAL
>
())
{
int
sign2
=
(
A
[
0
]
>
0
)
?
-
sign
:
sign
;
p
+=
R
.
col
(
0
)
*
(
s1
.
halfSide
[
0
]
*
sign2
);
p
.
noalias
()
+=
R
.
col
(
0
)
*
(
s1
.
halfSide
[
0
]
*
sign2
);
}
else
if
(
std
::
abs
(
Q
[
1
]
-
1
)
<
planeIntersectTolerance
<
FCL_REAL
>
()
||
std
::
abs
(
Q
[
1
]
+
1
)
<
planeIntersectTolerance
<
FCL_REAL
>
())
{
int
sign2
=
(
A
[
1
]
>
0
)
?
-
sign
:
sign
;
p
+=
R
.
col
(
1
)
*
(
s1
.
halfSide
[
1
]
*
sign2
);
p
.
noalias
()
+=
R
.
col
(
1
)
*
(
s1
.
halfSide
[
1
]
*
sign2
);
}
else
if
(
std
::
abs
(
Q
[
2
]
-
1
)
<
planeIntersectTolerance
<
FCL_REAL
>
()
||
std
::
abs
(
Q
[
2
]
+
1
)
<
planeIntersectTolerance
<
FCL_REAL
>
())
{
int
sign2
=
(
A
[
2
]
>
0
)
?
-
sign
:
sign
;
p
+=
R
.
col
(
2
)
*
(
s1
.
halfSide
[
2
]
*
sign2
);
p
.
noalias
()
+=
R
.
col
(
2
)
*
(
s1
.
halfSide
[
2
]
*
sign2
);
}
else
{
Vec3f
tmp
(
sign
*
R
*
s1
.
halfSide
);
p
+=
(
A
.
array
()
>
0
).
select
(
-
tmp
,
tmp
);
p
.
noalias
()
+=
(
A
.
array
()
>
0
).
select
(
-
tmp
,
tmp
);
}
// compute the contact point by project the deepest point onto the plane
if
(
signed_dist
>
0
)
normal
=
-
new_s2
.
n
;
else
normal
=
new_s2
.
n
;
p1
=
p2
=
p
-
new_s2
.
n
*
new_s2
.
signedDistance
(
p
);
p1
=
p2
.
noalias
()
=
p
-
new_s2
.
n
*
new_s2
.
signedDistance
(
p
);
return
true
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment