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
Humanoid Path Planner
hpp-fcl
Commits
58c08c80
Unverified
Commit
58c08c80
authored
Jul 13, 2020
by
Joseph Mirabel
Committed by
GitHub
Jul 13, 2020
Browse files
Merge pull request #182 from jmirabel/devel
Fix calculation of normal and nearest points in GJKSolver::shapeDistance
parents
cdd0b106
68e14b69
Changes
8
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/narrowphase/narrowphase.h
View file @
58c08c80
...
...
@@ -234,7 +234,8 @@ namespace fcl
Vec3f
w0
,
w1
;
gjk
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
0
;
p1
=
p2
=
tf1
.
transform
(
.5
*
(
w0
+
w1
));
p1
=
tf1
.
transform
(
w0
);
p2
=
tf1
.
transform
(
w1
);
normal
=
Vec3f
(
0
,
0
,
0
);
return
false
;
}
...
...
@@ -261,7 +262,8 @@ namespace fcl
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal
=
(
tf1
.
getRotation
()
*
(
p2
-
p1
)).
normalized
();
p1
=
p2
=
tf1
.
transform
(
p1
);
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
}
else
{
details
::
EPA
epa
(
epa_max_face_num
,
epa_max_vertex_num
,
epa_max_iterations
,
epa_tolerance
);
...
...
@@ -275,15 +277,15 @@ namespace fcl
epa
.
getClosestPoints
(
shape
,
w0
,
w1
);
assert
(
epa
.
depth
>=
-
eps
);
distance
=
(
std
::
min
)
(
0.
,
-
epa
.
depth
);
// TODO should be
// normal = tf1.getRotation() * epa.normal;
normal
=
tf2
.
getRotation
()
*
epa
.
normal
;
p1
=
p2
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
normal
=
tf1
.
getRotation
()
*
epa
.
normal
;
p1
=
tf1
.
transform
(
w0
);
p2
=
tf1
.
transform
(
w1
);
return
false
;
}
distance
=
-
(
std
::
numeric_limits
<
FCL_REAL
>::
max
)();
gjk
.
getClosestPoints
(
shape
,
p1
,
p2
);
p1
=
p2
=
tf1
.
transform
(
p1
);
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
}
return
false
;
}
...
...
python/CMakeLists.txt
View file @
58c08c80
...
...
@@ -42,7 +42,11 @@ SET(${LIBRARY_NAME}_HEADERS
fcl.hh
)
IF
(
NOT DOXYGEN_FOUND
OR
(
APPLE AND PYTHON_VERSION_MAJOR LESS 3
))
SET
(
ENABLE_PYTHON_DOXYGEN_AUTODOC TRUE CACHE BOOL
"Enable automatic documentation of Python bindings from Doxygen documentation"
)
IF
(
NOT ENABLE_PYTHON_DOXYGEN_AUTODOC
OR NOT DOXYGEN_FOUND
OR
(
APPLE AND PYTHON_VERSION_MAJOR LESS 3
))
# Disable documentation on APPLE for python 2.
SET
(
ENABLE_DOXYGEN_AUTODOC FALSE
)
ELSE
()
SET
(
ENABLE_DOXYGEN_AUTODOC TRUE
)
...
...
python/collision-geometries.cc
View file @
58c08c80
...
...
@@ -34,8 +34,7 @@
#include
<boost/python.hpp>
#include
<eigenpy/registration.hpp>
#include
<eigenpy/eigen-to-python.hpp>
#include
<eigenpy/eigenpy.hpp>
#include
"fcl.hh"
...
...
python/collision.cc
View file @
58c08c80
...
...
@@ -35,8 +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
<eigenpy/eigenpy.hpp>
#include
<hpp/fcl/fwd.hh>
#include
<hpp/fcl/collision.h>
...
...
python/distance.cc
View file @
58c08c80
...
...
@@ -35,8 +35,7 @@
#include
<boost/python.hpp>
#include
<boost/python/suite/indexing/vector_indexing_suite.hpp>
#include
<eigenpy/registration.hpp>
#include
<eigenpy/eigen-to-python.hpp>
#include
<eigenpy/eigenpy.hpp>
#include
"fcl.hh"
...
...
python/fcl.cc
View file @
58c08c80
...
...
@@ -34,7 +34,7 @@
#include
<boost/python.hpp>
#include
<eigenpy/
registration
.hpp>
#include
<eigenpy/
eigenpy
.hpp>
#include
"fcl.hh"
...
...
python/gjk.cc
View file @
58c08c80
...
...
@@ -35,7 +35,7 @@
#include
<boost/python.hpp>
#include
<boost/python/suite/indexing/vector_indexing_suite.hpp>
#include
<eigenpy/
registration
.hpp>
#include
<eigenpy/
eigenpy
.hpp>
#include
"fcl.hh"
...
...
src/collision_func_matrix.cpp
View file @
58c08c80
...
...
@@ -85,10 +85,10 @@ std::size_t ShapeShapeCollide(const CollisionGeometry* o1, const Transform3f& tf
if
(
distance
<=
0
)
{
if
(
result
.
numContacts
()
<
request
.
num_max_contacts
)
{
const
Vec3f
&
p1
=
distanceResult
.
nearest_points
[
0
];
assert
(
!
request
.
enable_contact
||
p1
=
=
distanceResult
.
nearest_points
[
1
]
)
;
const
Vec3f
&
p2
=
distanceResult
.
nearest_points
[
1
];
Contact
contact
(
o1
,
o2
,
distanceResult
.
b1
,
distanceResult
.
b2
,
p1
,
(
p1
+
p2
)
/
2
,
distanceResult
.
normal
,
-
distance
+
request
.
security_margin
);
...
...
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