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
14141399
Unverified
Commit
14141399
authored
Aug 14, 2020
by
Justin Carpentier
Committed by
GitHub
Aug 14, 2020
Browse files
Merge pull request #183 from jmirabel/devel
Add operator== for requests + bind StdVec of requests.
parents
a834bd2f
9cba26a7
Changes
8
Hide whitespace changes
Inline
Side-by-side
include/hpp/fcl/collision_data.h
View file @
14141399
...
...
@@ -157,6 +157,14 @@ struct HPP_FCL_DLLAPI QueryRequest
{}
void
updateGuess
(
const
QueryResult
&
result
);
/// @brief whether two QueryRequest are the same or not
inline
bool
operator
==
(
const
QueryRequest
&
other
)
const
{
return
enable_cached_gjk_guess
==
other
.
enable_cached_gjk_guess
&&
cached_gjk_guess
==
other
.
cached_gjk_guess
&&
cached_support_func_guess
==
other
.
cached_support_func_guess
;
}
};
/// @brief base class for all query results
...
...
@@ -234,6 +242,17 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest
}
bool
isSatisfied
(
const
CollisionResult
&
result
)
const
;
/// @brief whether two CollisionRequest are the same or not
inline
bool
operator
==
(
const
CollisionRequest
&
other
)
const
{
return
QueryRequest
::
operator
==
(
other
)
&&
num_max_contacts
==
other
.
num_max_contacts
&&
enable_contact
==
other
.
enable_contact
&&
enable_distance_lower_bound
==
other
.
enable_distance_lower_bound
&&
security_margin
==
other
.
security_margin
&&
break_distance
==
other
.
break_distance
;
}
};
/// @brief collision result
...
...
@@ -354,6 +373,15 @@ struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest
}
bool
isSatisfied
(
const
DistanceResult
&
result
)
const
;
/// @brief whether two DistanceRequest are the same or not
inline
bool
operator
==
(
const
DistanceRequest
&
other
)
const
{
return
QueryRequest
::
operator
==
(
other
)
&&
enable_nearest_points
==
other
.
enable_nearest_points
&&
rel_err
==
other
.
rel_err
&&
abs_err
==
other
.
abs_err
;
}
};
/// @brief distance result
...
...
include/hpp/fcl/narrowphase/narrowphase.h
View file @
14141399
...
...
@@ -156,7 +156,7 @@ namespace fcl
if
(
gjk
.
hasPenetrationInformation
(
shape
))
{
gjk
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
gjk
.
distance
;
normal
=
tf1
.
getRotation
()
*
(
w
1
-
w
0
).
normalized
();
normal
=
tf1
.
getRotation
()
*
(
w
0
-
w
1
).
normalized
();
p1
=
p2
=
tf1
.
transform
((
w0
+
w1
)
/
2
);
}
else
{
details
::
EPA
epa
(
epa_max_face_num
,
epa_max_vertex_num
,
epa_max_iterations
,
epa_tolerance
);
...
...
@@ -168,7 +168,7 @@ namespace fcl
{
epa
.
getClosestPoints
(
shape
,
w0
,
w1
);
distance
=
-
epa
.
depth
;
normal
=
-
epa
.
normal
;
normal
=
tf1
.
getRotation
()
*
epa
.
normal
;
p1
=
p2
=
tf1
.
transform
(
w0
-
epa
.
normal
*
(
epa
.
depth
*
0.5
));
assert
(
distance
<=
1e-6
);
}
else
{
...
...
@@ -261,7 +261,7 @@ namespace fcl
// Return contact points in case of collision
//p1 = tf1.transform (p1);
//p2 = tf1.transform (p2);
normal
=
(
tf1
.
getRotation
()
*
(
p
2
-
p
1
)).
normalized
();
normal
=
(
tf1
.
getRotation
()
*
(
p
1
-
p
2
)).
normalized
();
p1
=
tf1
.
transform
(
p1
);
p2
=
tf1
.
transform
(
p2
);
}
else
{
...
...
python/CMakeLists.txt
View file @
14141399
...
...
@@ -141,6 +141,7 @@ INSTALL(TARGETS ${LIBRARY_NAME}
# --- INSTALL SCRIPTS
SET
(
PYTHON_FILES
__init__.py
viewer.py
)
FOREACH
(
python
${
PYTHON_FILES
}
)
...
...
python/collision.cc
View file @
14141399
...
...
@@ -102,6 +102,13 @@ void exposeCollisionAPI ()
;
}
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
std
::
vector
<
CollisionRequest
>
>
())
{
class_
<
std
::
vector
<
CollisionRequest
>
>
(
"StdVec_CollisionRequest"
)
.
def
(
vector_indexing_suite
<
std
::
vector
<
CollisionRequest
>
>
())
;
}
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
Contact
>
())
{
class_
<
Contact
>
(
"Contact"
,
...
...
python/distance.cc
View file @
14141399
...
...
@@ -89,6 +89,13 @@ void exposeDistanceAPI ()
;
}
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
std
::
vector
<
DistanceRequest
>
>
())
{
class_
<
std
::
vector
<
DistanceRequest
>
>
(
"StdVec_DistanceRequest"
)
.
def
(
vector_indexing_suite
<
std
::
vector
<
DistanceRequest
>
>
())
;
}
if
(
!
eigenpy
::
register_symbolic_link_to_registered_type
<
DistanceResult
>
())
{
class_
<
DistanceResult
,
bases
<
QueryResult
>
>
(
"DistanceResult"
,
...
...
python/hppfcl/viewer.py
0 → 100644
View file @
14141399
# Software License Agreement (BSD License)
#
# Copyright (c) 2019 CNRS
# Author: Joseph Mirabel
import
hppfcl
,
numpy
as
np
from
gepetto
import
Color
def
applyConfiguration
(
gui
,
name
,
tf
):
gui
.
applyConfiguration
(
name
,
tf
.
getTranslation
().
tolist
()
+
tf
.
getQuatRotation
().
coeffs
().
tolist
())
def
displayShape
(
gui
,
name
,
geom
,
color
=
(.
9
,
.
9
,
.
9
,
1.
)):
if
isinstance
(
geom
,
hppfcl
.
Capsule
):
return
gui
.
addCapsule
(
name
,
geom
.
radius
,
2.
*
geom
.
halfLength
,
color
)
elif
isinstance
(
geom
,
hppfcl
.
Cylinder
):
return
gui
.
addCylinder
(
name
,
geom
.
radius
,
2.
*
geom
.
halfLength
,
color
)
elif
isinstance
(
geom
,
hppfcl
.
Box
):
w
,
h
,
d
=
(
2.
*
geom
.
halfSide
).
tolist
()
return
gui
.
addBox
(
name
,
w
,
h
,
d
,
color
)
elif
isinstance
(
geom
,
hppfcl
.
Sphere
):
return
gui
.
addSphere
(
name
,
geom
.
radius
,
color
)
elif
isinstance
(
geom
,
hppfcl
.
Cone
):
return
gui
.
addCone
(
name
,
geom
.
radius
,
2.
*
geom
.
halfLength
,
color
)
elif
isinstance
(
geom
,
hppfcl
.
Convex
):
pts
=
[
geom
.
points
(
geom
.
polygons
(
f
)[
i
]).
tolist
()
for
f
in
range
(
geom
.
num_polygons
)
for
i
in
range
(
3
)
]
gui
.
addCurve
(
name
,
pts
,
color
)
gui
.
setCurveMode
(
name
,
"TRIANGLES"
)
gui
.
setLightingMode
(
name
,
"ON"
)
gui
.
setBoolProperty
(
name
,
"BackfaceDrawing"
,
True
)
return
True
elif
isinstance
(
geom
,
hppfcl
.
ConvexBase
):
pts
=
[
geom
.
points
(
i
).
tolist
()
for
i
in
range
(
geom
.
num_points
)
]
gui
.
addCurve
(
name
,
pts
,
color
)
gui
.
setCurveMode
(
name
,
"POINTS"
)
gui
.
setLightingMode
(
name
,
"OFF"
)
return
True
else
:
msg
=
"Unsupported geometry type for %s (%s)"
%
(
geometry_object
.
name
,
type
(
geom
)
)
warnings
.
warn
(
msg
,
category
=
UserWarning
,
stacklevel
=
2
)
return
False
def
displayDistanceResult
(
gui
,
group_name
,
res
,
closest_points
=
True
,
normal
=
True
):
gui
.
createGroup
(
group_name
)
r
=
0.01
if
closest_points
:
p
=
[
group_name
+
"/p1"
,
group_name
+
"/p2"
]
gui
.
addSphere
(
p
[
0
],
r
,
Color
.
red
)
gui
.
addSphere
(
p
[
1
],
r
,
Color
.
blue
)
qid
=
[
0
,
0
,
0
,
1
]
gui
.
applyConfigurations
(
p
,
[
res
.
getNearestPoint1
().
tolist
()
+
qid
,
res
.
getNearestPoint2
().
tolist
()
+
qid
,
])
if
normal
:
n
=
group_name
+
"/normal"
gui
.
addArrow
(
n
,
r
,
0.1
,
Color
.
green
)
gui
.
applyConfiguration
(
n
,
res
.
getNearestPoint1
().
tolist
()
+
hppfcl
.
Quaternion
.
FromTwoVectors
(
np
.
array
([
1
,
0
,
0
]),
res
.
normal
).
coeffs
().
tolist
())
gui
.
refresh
()
def
displayCollisionResult
(
gui
,
group_name
,
res
,
color
=
Color
.
green
):
if
res
.
isCollision
():
if
gui
.
nodeExists
(
group_name
):
gui
.
setVisibility
(
group_name
,
"ON"
)
else
:
gui
.
createGroup
(
group_name
)
for
i
in
range
(
res
.
numContacts
()):
contact
=
res
.
getContact
(
i
)
n
=
group_name
+
"/contact"
+
str
(
i
)
depth
=
contact
.
penetration_depth
if
gui
.
nodeExists
(
n
):
gui
.
setFloatProperty
(
n
,
'Size'
,
depth
)
gui
.
setFloatProperty
(
n
,
'Radius'
,
0.1
*
depth
)
gui
.
setColor
(
n
,
color
)
else
:
gui
.
addArrow
(
n
,
depth
*
0.1
,
depth
,
color
)
N
=
contact
.
normal
P
=
contact
.
pos
gui
.
applyConfiguration
(
n
,
(
P
-
depth
*
N
/
2
).
tolist
()
+
hppfcl
.
Quaternion
.
FromTwoVectors
(
np
.
array
([
1
,
0
,
0
]),
N
).
coeffs
().
tolist
())
gui
.
refresh
()
elif
gui
.
nodeExists
(
group_name
):
gui
.
setVisibility
(
group_name
,
"OFF"
)
src/narrowphase/gjk.cpp
View file @
14141399
...
...
@@ -575,7 +575,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
alpha
=
std
::
max
(
alpha
,
omega
);
FCL_REAL
diff
(
rl
-
alpha
);
if
(
iterations
==
0
)
diff
=
std
::
abs
(
diff
);
if
(
diff
-
tolerance
*
rl
<=
0
)
// TODO here, we can stop at iteration 0 if this condition is met.
// We stopping at iteration 0, the closest point will not be valid.
// if(diff - tolerance * rl <= 0)
if
(
iterations
>
0
&&
diff
-
tolerance
*
rl
<=
0
)
{
if
(
iterations
>
0
)
removeVertex
(
simplices
[
current
]);
...
...
test/gjk.cpp
View file @
14141399
...
...
@@ -368,7 +368,7 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision,
// Check that guess works as expected
Vec3f
guess
=
gjk
.
getGuessFromSimplex
();
details
::
GJK
gjk2
(
0
,
1e-6
);
details
::
GJK
gjk2
(
3
,
1e-6
);
details
::
GJK
::
Status
status2
=
gjk2
.
evaluate
(
shape
,
guess
);
BOOST_CHECK_EQUAL
(
status2
,
details
::
GJK
::
Valid
);
}
...
...
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