From dc39421e571bed9b9c1b8d70463bb666c219cec4 Mon Sep 17 00:00:00 2001 From: Xinsong Lin Date: Fri, 8 Mar 2024 11:30:14 -0800 Subject: [PATCH] found and fixed the bug caused by libccd argument --- pybind/collision_detection/fcl/pybind_fcl.cpp | 6 +++--- tests/test_fcl_model.py | 7 ++++++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/pybind/collision_detection/fcl/pybind_fcl.cpp b/pybind/collision_detection/fcl/pybind_fcl.cpp index f0173f5a..1aeb3e9c 100644 --- a/pybind/collision_detection/fcl/pybind_fcl.cpp +++ b/pybind/collision_detection/fcl/pybind_fcl.cpp @@ -230,7 +230,7 @@ void build_pyfcl(py::module &m) { /********** narrowphase *******/ auto PyGJKSolverType = py::enum_(m, "GJKSolverType"); - PyGJKSolverType.value("GST_LIBCCD", GJKSolverType::GST_LIBCCD) + PyGJKSolverType.value("GST_LIBCCD", GJKSolverType::GST_INDEP) .value("GST_INDEP", GJKSolverType::GST_INDEP) .export_values(); @@ -243,7 +243,7 @@ void build_pyfcl(py::module &m) { py::arg("num_max_contacts") = 1, py::arg("enable_contact") = false, py::arg("num_max_cost_sources") = 1, py::arg("enable_cost") = false, py::arg("use_approximate_cost") = true, - py::arg("gjk_solver_type") = GJKSolverType::GST_LIBCCD, + py::arg("gjk_solver_type") = GJKSolverType::GST_INDEP, py::arg("gjk_tolerance") = 1e-6) .def("isSatisfied", &CollisionRequest::isSatisfied, py::arg("result")); @@ -282,7 +282,7 @@ void build_pyfcl(py::module &m) { py::arg("enable_nearest_points") = false, py::arg("enable_signed_distance") = false, py::arg("rel_err") = 0.0, py::arg("abs_err") = 0.0, py::arg("distance_tolerance") = 1e-6, - py::arg("gjk_solver_type") = GJKSolverType::GST_LIBCCD) + py::arg("gjk_solver_type") = GJKSolverType::GST_INDEP) .def("isSatisfied", &DistanceRequest::isSatisfied, py::arg("result")); // DistanceResult diff --git a/tests/test_fcl_model.py b/tests/test_fcl_model.py index 52852268..da918a7e 100644 --- a/tests/test_fcl_model.py +++ b/tests/test_fcl_model.py @@ -2,7 +2,7 @@ import unittest import numpy as np from transforms3d.quaternions import mat2quat -from mplib.pymp.collision_detection.fcl import FCLModel +from mplib.pymp.collision_detection.fcl import FCLModel, Box, CollisionObject, Plane, distance, DistanceRequest from mplib.pymp.kinematics.pinocchio import PinocchioModel FILE_ABS_DIR = os.path.dirname(os.path.abspath(__file__)) @@ -65,5 +65,10 @@ def test_collision(self): is_collision = [collision.is_collision() for collision in collisions] self.assertFalse(any(is_collision)) + def test_distance(self): + ground_obj = CollisionObject(Plane([0, 0, 1], 0), [0, 0, 0], [1, 0, 0, 0]) + box_obj = CollisionObject(Box(1,1,1), [0, 0, 5.5], [1, 0, 0, 0]) + self.assertEqual(distance(ground_obj, box_obj, DistanceRequest()).min_distance, 5) + if __name__ == "__main__": unittest.main()