Unverified Commit 14141399 authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #183 from jmirabel/devel

Add operator== for requests + bind StdVec of requests.
parents a834bd2f 9cba26a7
...@@ -157,6 +157,14 @@ struct HPP_FCL_DLLAPI QueryRequest ...@@ -157,6 +157,14 @@ struct HPP_FCL_DLLAPI QueryRequest
{} {}
void updateGuess(const QueryResult& result); 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 /// @brief base class for all query results
...@@ -234,6 +242,17 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest ...@@ -234,6 +242,17 @@ struct HPP_FCL_DLLAPI CollisionRequest : QueryRequest
} }
bool isSatisfied(const CollisionResult& result) const; 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 /// @brief collision result
...@@ -354,6 +373,15 @@ struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest ...@@ -354,6 +373,15 @@ struct HPP_FCL_DLLAPI DistanceRequest : QueryRequest
} }
bool isSatisfied(const DistanceResult& result) const; 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 /// @brief distance result
......
...@@ -156,7 +156,7 @@ namespace fcl ...@@ -156,7 +156,7 @@ namespace fcl
if (gjk.hasPenetrationInformation(shape)) { if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1); gjk.getClosestPoints (shape, w0, w1);
distance = gjk.distance; distance = gjk.distance;
normal = tf1.getRotation() * (w1 - w0).normalized(); normal = tf1.getRotation() * (w0 - w1).normalized();
p1 = p2 = tf1.transform((w0 + w1) / 2); p1 = p2 = tf1.transform((w0 + w1) / 2);
} else { } else {
details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance); details::EPA epa(epa_max_face_num, epa_max_vertex_num, epa_max_iterations, epa_tolerance);
...@@ -168,7 +168,7 @@ namespace fcl ...@@ -168,7 +168,7 @@ namespace fcl
{ {
epa.getClosestPoints (shape, w0, w1); epa.getClosestPoints (shape, w0, w1);
distance = -epa.depth; distance = -epa.depth;
normal = -epa.normal; normal = tf1.getRotation() * epa.normal;
p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5)); p1 = p2 = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
assert (distance <= 1e-6); assert (distance <= 1e-6);
} else { } else {
...@@ -261,7 +261,7 @@ namespace fcl ...@@ -261,7 +261,7 @@ namespace fcl
// Return contact points in case of collision // Return contact points in case of collision
//p1 = tf1.transform (p1); //p1 = tf1.transform (p1);
//p2 = tf1.transform (p2); //p2 = tf1.transform (p2);
normal = (tf1.getRotation() * (p2 - p1)).normalized(); normal = (tf1.getRotation() * (p1 - p2)).normalized();
p1 = tf1.transform(p1); p1 = tf1.transform(p1);
p2 = tf1.transform(p2); p2 = tf1.transform(p2);
} else { } else {
......
...@@ -141,6 +141,7 @@ INSTALL(TARGETS ${LIBRARY_NAME} ...@@ -141,6 +141,7 @@ INSTALL(TARGETS ${LIBRARY_NAME}
# --- INSTALL SCRIPTS # --- INSTALL SCRIPTS
SET(PYTHON_FILES SET(PYTHON_FILES
__init__.py __init__.py
viewer.py
) )
FOREACH(python ${PYTHON_FILES}) FOREACH(python ${PYTHON_FILES})
......
...@@ -102,6 +102,13 @@ void exposeCollisionAPI () ...@@ -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>()) if(!eigenpy::register_symbolic_link_to_registered_type<Contact>())
{ {
class_ <Contact> ("Contact", class_ <Contact> ("Contact",
......
...@@ -89,6 +89,13 @@ void exposeDistanceAPI () ...@@ -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>()) if(!eigenpy::register_symbolic_link_to_registered_type<DistanceResult>())
{ {
class_ <DistanceResult, bases<QueryResult> > ("DistanceResult", class_ <DistanceResult, bases<QueryResult> > ("DistanceResult",
......
# 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")
...@@ -575,7 +575,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, ...@@ -575,7 +575,10 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess,
alpha = std::max(alpha, omega); alpha = std::max(alpha, omega);
FCL_REAL diff (rl - alpha); FCL_REAL diff (rl - alpha);
if (iterations == 0) diff = std::abs(diff); 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) if (iterations > 0)
removeVertex(simplices[current]); removeVertex(simplices[current]);
......
...@@ -368,7 +368,7 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision, ...@@ -368,7 +368,7 @@ void test_gjk_triangle_capsule (Vec3f T, bool expect_collision,
// Check that guess works as expected // Check that guess works as expected
Vec3f guess = gjk.getGuessFromSimplex(); Vec3f guess = gjk.getGuessFromSimplex();
details::GJK gjk2 (0, 1e-6); details::GJK gjk2 (3, 1e-6);
details::GJK::Status status2 = gjk2.evaluate(shape, guess); details::GJK::Status status2 = gjk2.evaluate(shape, guess);
BOOST_CHECK_EQUAL(status2, details::GJK::Valid); BOOST_CHECK_EQUAL(status2, details::GJK::Valid);
} }
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment