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
{}
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
......
......@@ -156,7 +156,7 @@ namespace fcl
if (gjk.hasPenetrationInformation(shape)) {
gjk.getClosestPoints (shape, w0, w1);
distance = gjk.distance;
normal = tf1.getRotation() * (w1 - w0).normalized();
normal = tf1.getRotation() * (w0 - w1).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() * (p2 - p1)).normalized();
normal = (tf1.getRotation() * (p1 - p2)).normalized();
p1 = tf1.transform(p1);
p2 = tf1.transform(p2);
} else {
......
......@@ -141,6 +141,7 @@ INSTALL(TARGETS ${LIBRARY_NAME}
# --- INSTALL SCRIPTS
SET(PYTHON_FILES
__init__.py
viewer.py
)
FOREACH(python ${PYTHON_FILES})
......
......@@ -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",
......
......@@ -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",
......
# 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,
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]);
......
......@@ -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);
}
......
Markdown is supported
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