[help wanted] convex obj return wrong collision result

[Alexbeast-CN]

I'm using hpp-fcl version 2.4.0 with ROS humble and ubuntu 22.04.

Both c++ version and python version are installed by apt with apt install ros-humble-hpp-fcl.

I'm tring to test collision detection with a box and a convex shape load from .obj file.

This is my convex shape .obj:

You can download this file here

I firstly wrote a python program, and it works fine. I can move the box with "w, s, a, d, q, e" keys, and it prints "collision detected" when the box is inside the convex shape.

here is my python code

```py import numpy as np import trimesh import rclpy import hppfcl as fcl from pynput import keyboard from rclpy.node import Node from rclpy.qos import QoSProfile, DurabilityPolicy from visualization_msgs.msg import Marker class CollisionTest(Node): def __init__(self): super().__init__('collision_test') # config qos profile qos_profile = QoSProfile(depth=10) qos_profile.durability = DurabilityPolicy.TRANSIENT_LOCAL self.box_publisher = self.create_publisher(Marker, 'box', 10) self.env_publisher = self.create_publisher(Marker, 'env', qos_profile) self.timer = self.create_timer(0.1, self.timer_callback) self.box_position = np.array([0.0, 0.0, 0.0]) self.step = 0.03 self.init_box() self.init_env() # Setup keyboard listener self.listener = keyboard.Listener(on_press=self.on_press) self.listener.start() def init_box(self): # Initialize box collision object box_shape = fcl.Box(0.1, 0.1, 0.1) self.box_obj = fcl.CollisionObject(box_shape) # Initialize box marker self.box_marker = Marker() self.box_marker.header.frame_id = 'map' self.box_marker.ns = 'collision_test' self.box_marker.id = 0 self.box_marker.type = Marker.CUBE self.box_marker.action = Marker.ADD self.box_marker.scale.x = 0.1 self.box_marker.scale.y = 0.1 self.box_marker.scale.z = 0.1 self.box_marker.color.a = 1.0 self.box_marker.color.r = 0.0 self.box_marker.color.g = 1.0 self.box_marker.color.b = 0.0 def init_env(self): file_path = "/path/to/convex_hull.obj" scale = 1.0 # Publish markers env_marker = Marker() env_marker.header.frame_id = 'map' env_marker.ns = 'collision_test' env_marker.id = 0 env_marker.type = Marker.MESH_RESOURCE env_marker.mesh_resource = "file://" + file_path env_marker.action = Marker.ADD env_marker.scale.x = scale env_marker.scale.y = scale env_marker.scale.z = scale env_marker.color.r = 0.1 env_marker.color.g = 0.0 env_marker.color.b = 0.7 env_marker.color.a = 0.3 self.env_publisher.publish(env_marker) # Initialize environment collision object mesh = trimesh.load_mesh(file_path) mesh.apply_scale(scale) verts = fcl.StdVec_Vec3f() faces = fcl.StdVec_Triangle() verts.extend(mesh.vertices) for f in mesh.faces: faces.append(fcl.Triangle(int(f[0]), int(f[1]), int(f[2]))) convex = fcl.Convex(verts, faces) self.env_objs = [fcl.CollisionObject(convex)] def move_box(self): # Move the box marker self.box_marker.pose.position.x = self.box_position[0] self.box_marker.pose.position.y = self.box_position[1] self.box_marker.pose.position.z = self.box_position[2] self.box_publisher.publish(self.box_marker) # Move the box collision object transform = fcl.Transform3f() transform.setTranslation(self.box_position) self.box_obj.setTransform(transform) # Check collision for env_obj in self.env_objs: if self.check_collision(self.box_obj, env_obj): self.get_logger().info('Collision detected!') break def check_collision(self, obj1, obj2): request = fcl.CollisionRequest() result = fcl.CollisionResult() fcl.collide(obj1, obj2, request, result) return result.isCollision() def on_press(self, key): try: if key.char == 'w': self.box_position[1] += self.step elif key.char == 's': self.box_position[1] -= self.step elif key.char == 'a': self.box_position[0] -= self.step elif key.char == 'd': self.box_position[0] += self.step elif key.char == 'q': self.box_position[2] += self.step elif key.char == 'e': self.box_position[2] -= self.step except AttributeError: pass def timer_callback(self): self.move_box() def main(args=None): print(f"hpp-fcl version: {fcl.__version__}") rclpy.init(args=args) node = CollisionTest() rclpy.spin(node) rclpy.shutdown() if __name__ == '__main__': main() ```

However, when I tried to rewrite the code in c++, things are getting wired. It prints "collision detection" in wrong space.

Here is a short video to show the problem:

https://github.com/humanoid-path-planner/hpp-fcl/assets/69784031/efc4783c-fbb8-4476-acb7-0e45e3930c60

here is my c++ code

```cpp #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include namespace fcl = hpp::fcl; using Convex_t = fcl::Convex; class CollisionTest : public rclcpp::Node { private: rclcpp::Publisher::SharedPtr box_publisher_; rclcpp::Publisher::SharedPtr env_publisher_; rclcpp::TimerBase::SharedPtr timer_; Eigen::Vector3d box_position_ = Eigen::Vector3d::Zero(); double step = 0.03; visualization_msgs::msg::Marker box_marker_; fcl::CollisionObject box_obj_{ nullptr }; std::vector env_objs_{}; void init_collision_objs(aiNode* node, const aiScene* scene, visualization_msgs::msg::Marker& marker) { double scale_factor = 1; for (u_int i = 0; i < node->mNumMeshes; i++) { aiMesh* mesh = scene->mMeshes[node->mMeshes[i]]; fcl::Vec3f* vertices = new fcl::Vec3f[mesh->mNumVertices]; for (u_int j = 0; j < mesh->mNumVertices; j++) { double x = mesh->mVertices[j].x * scale_factor; double y = mesh->mVertices[j].y * scale_factor; double z = mesh->mVertices[j].z * scale_factor; vertices[j] = fcl::Vec3f(x, y, z); } fcl::Triangle* triangles = new fcl::Triangle[mesh->mNumFaces]; for (u_int j = 0; j < mesh->mNumFaces; j++) { aiFace face = mesh->mFaces[j]; triangles[j] = fcl::Triangle(face.mIndices[0], face.mIndices[1], face.mIndices[2]); // add points to marker for (int k = 0; k < 3; k++) { geometry_msgs::msg::Point p; p.x = vertices[face.mIndices[k]].x(); p.y = vertices[face.mIndices[k]].y(); p.z = vertices[face.mIndices[k]].z(); marker.points.push_back(p); } } // auto convex = // std::make_shared(true, vertices, mesh->mNumVertices, triangles, mesh->mNumFaces); auto convex = std::shared_ptr( new Convex_t(true, vertices, mesh->mNumVertices, triangles, mesh->mNumFaces)); // auto tf = fcl::Transform3f::Identity(); fcl::CollisionObject obj(convex); env_objs_.emplace_back(obj); } for (unsigned int i = 0; i < node->mNumChildren; i++) { init_collision_objs(node->mChildren[i], scene, marker); } } void init_env() { std::string file_path = "/home/developer/dd6ax/src/dd6ax_utils/test/dragon0_convex_hull.obj"; // double angle_in_degrees = 90.0; // Eigen::AngleAxisd rotation_axis(angle_in_degrees * M_PI / 180.0, Eigen::Vector3d::UnitX()); // Eigen::Quaterniond rotation_quaternion = Eigen::Quaterniond(rotation_axis); // rotation_quaternion.normalize(); Assimp::Importer importer; const aiScene* scene = importer.ReadFile(file_path, aiProcess_Triangulate | aiProcess_FlipUVs); if (!scene || scene->mFlags & AI_SCENE_FLAGS_INCOMPLETE || !scene->mRootNode) { std::cerr << "Error::Assimp:: " << importer.GetErrorString() << std::endl; return; } visualization_msgs::msg::Marker marker; marker.header.frame_id = "map"; marker.header.stamp = this->now(); marker.ns = "collision_test"; marker.id = 0; marker.type = visualization_msgs::msg::Marker::TRIANGLE_LIST; // marker.type = visualization_msgs::msg::Marker::MESH_RESOURCE; // marker.mesh_resource = "file://" + file_path; marker.action = visualization_msgs::msg::Marker::ADD; marker.scale.x = 1.0; marker.scale.y = 1.0; marker.scale.z = 1.0; marker.color.r = 0.1; marker.color.g = 0.0; marker.color.b = 0.7; marker.color.a = 0.3; init_collision_objs(scene->mRootNode, scene, marker); RCLCPP_INFO(this->get_logger(), "Publishing marker with %zu points", marker.points.size()); env_publisher_->publish(std::move(marker)); } void init_box() { // init box collision object box_obj_ = fcl::CollisionObject(std::make_shared(0.1, 0.1, 0.1), fcl::Transform3f::Identity()); // init box marker box_marker_.header.frame_id = "map"; box_marker_.header.stamp = this->now(); box_marker_.ns = "collision_test"; box_marker_.id = 0; box_marker_.type = visualization_msgs::msg::Marker::CUBE; box_marker_.action = visualization_msgs::msg::Marker::ADD; box_marker_.pose.orientation.x = 0.0; box_marker_.pose.orientation.y = 0.0; box_marker_.pose.orientation.z = 0.0; box_marker_.pose.orientation.w = 1.0; box_marker_.scale.x = 0.1; box_marker_.scale.y = 0.1; box_marker_.scale.z = 0.1; box_marker_.color.a = 1.0; box_marker_.color.r = 0.0; box_marker_.color.g = 1.0; box_marker_.color.b = 0.0; } void get_box_position() { fd_set readfds; FD_ZERO(&readfds); FD_SET(STDIN_FILENO, &readfds); // Wait up to 20 milliseconds for input struct timeval timeout = { 0, 20 * 1000 }; int result = select(STDIN_FILENO + 1, &readfds, NULL, NULL, &timeout); if (result <= 0) { return; } if (!FD_ISSET(STDIN_FILENO, &readfds)) { return; } char c; if (read(STDIN_FILENO, &c, 1) <= 0) { return; } switch (c) { case 'w': box_position_.y() += step; break; case 's': box_position_.y() -= step; break; case 'a': box_position_.x() -= step; break; case 'd': box_position_.x() += step; break; case 'q': box_position_.z() += step; break; case 'e': box_position_.z() -= step; break; default: break; } } void move_box() { // move the box marker box_marker_.pose.position.x = box_position_.x(); box_marker_.pose.position.y = box_position_.y(); box_marker_.pose.position.z = box_position_.z(); box_publisher_->publish(box_marker_); // move the box collision object auto fcl_tf = fcl::Transform3f::Identity(); fcl_tf.setTranslation(box_position_); box_obj_.setTransform(fcl_tf); // check collision for (const auto& env_obj : env_objs_) { if (check_collision(box_obj_, env_obj)) { RCLCPP_INFO(this->get_logger(), "Collision detected!"); } break; } // double distance = get_distance(box_obj_, env_obj); // RCLCPP_INFO(this->get_logger(), "Distance: %f", distance); } bool check_collision(fcl::CollisionObject obj1, fcl::CollisionObject obj2) { fcl::CollisionRequest request; fcl::CollisionResult result; fcl::collide(&obj1, &obj2, request, result); return result.isCollision(); } double get_distance(fcl::CollisionObject obj1, fcl::CollisionObject obj2) { fcl::DistanceRequest request; fcl::DistanceResult result; fcl::distance(&obj1, &obj2, request, result); return result.min_distance; } void timer_callback() { get_box_position(); move_box(); } public: CollisionTest() : Node("collision_test") { // set the terminal to unbuffered input struct termios tty; memset(&tty, 0, sizeof tty); if (tcgetattr(STDIN_FILENO, &tty) != 0) { RCLCPP_ERROR(this->get_logger(), "Error from tcgetattr"); } tty.c_lflag &= ~ICANON; tty.c_lflag &= ~ECHO; tty.c_cc[VMIN] = 1; // Minimum of one character for getc tty.c_cc[VTIME] = 0; // Wait indefinetly if (tcsetattr(STDIN_FILENO, TCSANOW, &tty) != 0) { RCLCPP_ERROR(this->get_logger(), "Error from tcsetattr"); } // init publishers and timer box_publisher_ = create_publisher("box", 10); init_box(); timer_ = create_wall_timer(std::chrono::milliseconds(100), std::bind(&CollisionTest::timer_callback, this)); env_publisher_ = create_publisher( "env", rclcpp::QoS(rclcpp::KeepLast(10)).transient_local()); init_env(); } ~CollisionTest() { // reset the terminal to buffered input struct termios tty; tcgetattr(STDIN_FILENO, &tty); tty.c_lflag |= ICANON; tty.c_lflag |= ECHO; tcsetattr(STDIN_FILENO, TCSANOW, &tty); }; }; int main(int argc, char* argv[]) { std::cout << "HPP-FCL version is: " << HPP_FCL_VERSION << std::endl; rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } ```

The reseason why I write the init_collision_objs() function as a recursive function is because I will deal with a group of geometries in one .obj file in the future. The vertices of the convex shape are correct because the TRIANGLE_LIST visualization markers are working correctly.

Thanks to anyone who can give a hand.

[Alexbeast-CN]

I found out that it was actually an issue with assimp. When it loads an OBJ file, it triples the number of points to match the number of triangles. But in reality, many points should be shared by triangles. Therefore, after manually processing the loading of the OBJ file with fstream, there were no problems.