From 8ef99688f0d83f9c2bc67ced26ac773e2426f7b4 Mon Sep 17 00:00:00 2001 From: Tin Lai Date: Wed, 18 Sep 2024 22:32:35 +1000 Subject: [PATCH] use mesh loader to support more format Signed-off-by: Tin Lai --- crates/rapier3d-urdf/Cargo.toml | 5 +- crates/rapier3d-urdf/src/lib.rs | 98 +++++++++++++++++---------------- examples3d/Cargo.toml | 1 - 3 files changed, 53 insertions(+), 51 deletions(-) diff --git a/crates/rapier3d-urdf/Cargo.toml b/crates/rapier3d-urdf/Cargo.toml index ef53da981..16675faff 100644 --- a/crates/rapier3d-urdf/Cargo.toml +++ b/crates/rapier3d-urdf/Cargo.toml @@ -12,9 +12,6 @@ keywords = ["physics", "joints", "multibody", "robotics", "urdf"] license = "Apache-2.0" edition = "2021" -[features] -stl = ["dep:rapier3d-stl"] - [dependencies] log = "0.4" anyhow = "1" @@ -23,4 +20,4 @@ bitflags = "2" xurdf = "0.2" rapier3d = { version = "0.22", path = "../rapier3d" } -rapier3d-stl = { version = "0.3.0", path = "../rapier3d-stl", optional = true } +mesh-loader = "0.1.12" diff --git a/crates/rapier3d-urdf/src/lib.rs b/crates/rapier3d-urdf/src/lib.rs index 1f913dae6..3810a617d 100644 --- a/crates/rapier3d-urdf/src/lib.rs +++ b/crates/rapier3d-urdf/src/lib.rs @@ -32,7 +32,10 @@ use rapier3d::{ JointAxis, MassProperties, MultibodyJointHandle, MultibodyJointSet, RigidBody, RigidBodyBuilder, RigidBodyHandle, RigidBodySet, RigidBodyType, }, - geometry::{Collider, ColliderBuilder, ColliderHandle, ColliderSet, SharedShape, TriMeshFlags}, + geometry::{ + Collider, ColliderBuilder, ColliderHandle, ColliderSet, MeshConverter, SharedShape, + TriMeshFlags, + }, math::{Isometry, Point, Real, Vector}, na, }; @@ -310,13 +313,13 @@ impl UrdfRobot { name_to_link_id.insert(&link.name, id); let mut colliders = vec![]; if options.create_colliders_from_collision_shapes { - colliders.extend(link.collisions.iter().filter_map(|co| { - urdf_to_collider(&options, mesh_dir, &co.geometry, &co.origin) + colliders.extend(link.collisions.iter().flat_map(|co| { + urdf_to_colliders(&options, mesh_dir, &co.geometry, &co.origin) })) } if options.create_colliders_from_visual_shapes { - colliders.extend(link.visuals.iter().filter_map(|vis| { - urdf_to_collider(&options, mesh_dir, &vis.geometry, &vis.origin) + colliders.extend(link.visuals.iter().flat_map(|vis| { + urdf_to_colliders(&options, mesh_dir, &vis.geometry, &vis.origin) })) } let mut body = urdf_to_rigid_body(&options, &link.inertial); @@ -488,66 +491,69 @@ fn urdf_to_rigid_body(options: &UrdfLoaderOptions, inertial: &Inertial) -> Rigid builder.build() } -fn urdf_to_collider( +fn urdf_to_colliders( options: &UrdfLoaderOptions, - _mesh_dir: &Path, // NOTO: this isn’t used if there is no external mesh feature enabled (like stl). + mesh_dir: &Path, geometry: &Geometry, origin: &Pose, -) -> Option { - let mut builder = options.collider_blueprint.clone(); +) -> Vec { let mut shape_transform = Isometry::identity(); - let shape = match &geometry { - Geometry::Box { size } => SharedShape::cuboid( - size[0] as Real / 2.0, - size[1] as Real / 2.0, - size[2] as Real / 2.0, - ), + + let mut colliders = Vec::new(); + match &geometry { + Geometry::Box { size } => { + colliders.push(SharedShape::cuboid( + size[0] as Real / 2.0, + size[1] as Real / 2.0, + size[2] as Real / 2.0, + )); + } Geometry::Cylinder { radius, length } => { // This rotation will make the cylinder Z-up as per the URDF spec, // instead of rapier’s default Y-up. shape_transform = Isometry::rotation(Vector::x() * Real::frac_pi_2()); - SharedShape::cylinder(*length as Real / 2.0, *radius as Real) + colliders.push(SharedShape::cylinder( + *length as Real / 2.0, + *radius as Real, + )); + } + Geometry::Sphere { radius } => { + colliders.push(SharedShape::ball(*radius as Real)); } - Geometry::Sphere { radius } => SharedShape::ball(*radius as Real), Geometry::Mesh { filename, scale } => { - let path: &Path = filename.as_ref(); let _scale = scale .map(|s| Vector::new(s.x as Real, s.y as Real, s.z as Real)) .unwrap_or_else(|| Vector::::repeat(1.0)); - match path.extension().and_then(|ext| ext.to_str()) { - #[cfg(feature = "stl")] - Some("stl") | Some("STL") => { - use rapier3d::geometry::MeshConverter; - let full_path = _mesh_dir.join(filename); - match rapier3d_stl::load_from_path( - full_path, - MeshConverter::TriMeshWithFlags(options.trimesh_flags), - _scale, - ) { - Ok(stl_shape) => { - shape_transform = stl_shape.pose; - stl_shape.shape - } - Err(e) => { - log::error!("failed to load STL file {filename}: {e}"); - return None; - } + + let loader = mesh_loader::Loader::default(); + let full_path = mesh_dir.join(filename); + if let Ok(scene) = loader.load(full_path) { + for (raw_mesh, _) in scene.meshes.into_iter().zip(scene.materials) { + let vertices: Vec<_> = raw_mesh + .vertices + .iter() + .map(|xyz| Point::new(xyz[0], xyz[1], xyz[2])) + .collect(); + let indices: Vec<_> = raw_mesh.faces; + let converter = MeshConverter::TriMeshWithFlags(options.trimesh_flags); + if let Ok((shape, _)) = converter.convert(vertices, indices) { + colliders.push(shape) } } - _ => { - log::error!("failed to load file with unknown type {filename}"); - return None; - } } } }; - builder.shape = shape; - Some( - builder - .position(urdf_to_isometry(origin) * shape_transform) - .build(), - ) + colliders + .drain(..) + .map(move |shape| { + let mut builder = options.collider_blueprint.clone(); + builder.shape = shape; + builder + .position(urdf_to_isometry(origin) * shape_transform) + .build() + }) + .collect() } fn urdf_to_isometry(pose: &Pose) -> Isometry { diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 4f09fad43..a6b0666c6 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -29,7 +29,6 @@ path = "../crates/rapier3d" [dependencies.rapier3d-urdf] path = "../crates/rapier3d-urdf" -features = ["stl"] [[bin]] name = "all_examples3"