diff --git a/Cargo.toml b/Cargo.toml index 36b49f1..f176685 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,6 +1,6 @@ [workspace] members = ["crates/rapier2d", "crates/rapier2d-f64", "crates/rapier_testbed2d", "crates/rapier_testbed2d-f64", "examples2d", "benchmarks2d", - "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d", "crates/rapier-urdf"] + "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d", "crates/rapier-urdf", "crates/rapier-stl"] resolver = "2" [patch.crates-io] diff --git a/assets/3d/T12/README.md b/assets/3d/T12/README.md new file mode 100644 index 0000000..011d8e5 --- /dev/null +++ b/assets/3d/T12/README.md @@ -0,0 +1,3 @@ +These samples files originate from the repository +[gkjohnson/urdf-loaders](https://github.com/gkjohnson/urdf-loaders/tree/b67f5de98f6222e2d921ce24f46a6725dad9704e/urdf/T12) +(Apache 2.0 license). \ No newline at end of file diff --git a/assets/3d/T12/meshes/Ankle1.STL b/assets/3d/T12/meshes/Ankle1.STL new file mode 100644 index 0000000..0aa9ec5 Binary files /dev/null and b/assets/3d/T12/meshes/Ankle1.STL differ diff --git a/assets/3d/T12/meshes/Ankle2.STL b/assets/3d/T12/meshes/Ankle2.STL new file mode 100644 index 0000000..0aa9ec5 Binary files /dev/null and b/assets/3d/T12/meshes/Ankle2.STL differ diff --git a/assets/3d/T12/meshes/Ankle3.STL b/assets/3d/T12/meshes/Ankle3.STL new file mode 100644 index 0000000..0aa9ec5 Binary files /dev/null and b/assets/3d/T12/meshes/Ankle3.STL differ diff --git a/assets/3d/T12/meshes/Ankle4.STL b/assets/3d/T12/meshes/Ankle4.STL new file mode 100644 index 0000000..0aa9ec5 Binary files /dev/null and b/assets/3d/T12/meshes/Ankle4.STL differ diff --git a/assets/3d/T12/meshes/Ankle5.STL b/assets/3d/T12/meshes/Ankle5.STL new file mode 100644 index 0000000..0aa9ec5 Binary files /dev/null and b/assets/3d/T12/meshes/Ankle5.STL differ diff --git a/assets/3d/T12/meshes/Ankle6.STL b/assets/3d/T12/meshes/Ankle6.STL new file mode 100644 index 0000000..0aa9ec5 Binary files /dev/null and b/assets/3d/T12/meshes/Ankle6.STL differ diff --git a/assets/3d/T12/meshes/Body.STL b/assets/3d/T12/meshes/Body.STL new file mode 100644 index 0000000..3379440 Binary files /dev/null and b/assets/3d/T12/meshes/Body.STL differ diff --git a/assets/3d/T12/meshes/Foot1.STL b/assets/3d/T12/meshes/Foot1.STL new file mode 100644 index 0000000..67da40c Binary files /dev/null and b/assets/3d/T12/meshes/Foot1.STL differ diff --git a/assets/3d/T12/meshes/Foot2.STL b/assets/3d/T12/meshes/Foot2.STL new file mode 100644 index 0000000..67da40c Binary files /dev/null and b/assets/3d/T12/meshes/Foot2.STL differ diff --git a/assets/3d/T12/meshes/Foot3.STL b/assets/3d/T12/meshes/Foot3.STL new file mode 100644 index 0000000..67da40c Binary files /dev/null and b/assets/3d/T12/meshes/Foot3.STL differ diff --git a/assets/3d/T12/meshes/Foot4.STL b/assets/3d/T12/meshes/Foot4.STL new file mode 100644 index 0000000..67da40c Binary files /dev/null and b/assets/3d/T12/meshes/Foot4.STL differ diff --git a/assets/3d/T12/meshes/Foot5.STL b/assets/3d/T12/meshes/Foot5.STL new file mode 100644 index 0000000..67da40c Binary files /dev/null and b/assets/3d/T12/meshes/Foot5.STL differ diff --git a/assets/3d/T12/meshes/Foot6.STL b/assets/3d/T12/meshes/Foot6.STL new file mode 100644 index 0000000..67da40c Binary files /dev/null and b/assets/3d/T12/meshes/Foot6.STL differ diff --git a/assets/3d/T12/meshes/Hip1.STL b/assets/3d/T12/meshes/Hip1.STL new file mode 100644 index 0000000..9a7df1a Binary files /dev/null and b/assets/3d/T12/meshes/Hip1.STL differ diff --git a/assets/3d/T12/meshes/Hip2.STL b/assets/3d/T12/meshes/Hip2.STL new file mode 100644 index 0000000..9a7df1a Binary files /dev/null and b/assets/3d/T12/meshes/Hip2.STL differ diff --git a/assets/3d/T12/meshes/Hip3.STL b/assets/3d/T12/meshes/Hip3.STL new file mode 100644 index 0000000..9a7df1a Binary files /dev/null and b/assets/3d/T12/meshes/Hip3.STL differ diff --git a/assets/3d/T12/meshes/Hip4.STL b/assets/3d/T12/meshes/Hip4.STL new file mode 100644 index 0000000..9a7df1a Binary files /dev/null and b/assets/3d/T12/meshes/Hip4.STL differ diff --git a/assets/3d/T12/meshes/Hip5.STL b/assets/3d/T12/meshes/Hip5.STL new file mode 100644 index 0000000..9a7df1a Binary files /dev/null and b/assets/3d/T12/meshes/Hip5.STL differ diff --git a/assets/3d/T12/meshes/Hip6.STL b/assets/3d/T12/meshes/Hip6.STL new file mode 100644 index 0000000..9a7df1a Binary files /dev/null and b/assets/3d/T12/meshes/Hip6.STL differ diff --git a/assets/3d/T12/meshes/Knee1.STL b/assets/3d/T12/meshes/Knee1.STL new file mode 100644 index 0000000..9057da7 Binary files /dev/null and b/assets/3d/T12/meshes/Knee1.STL differ diff --git a/assets/3d/T12/meshes/Knee2.STL b/assets/3d/T12/meshes/Knee2.STL new file mode 100644 index 0000000..9057da7 Binary files /dev/null and b/assets/3d/T12/meshes/Knee2.STL differ diff --git a/assets/3d/T12/meshes/Knee3.STL b/assets/3d/T12/meshes/Knee3.STL new file mode 100644 index 0000000..9057da7 Binary files /dev/null and b/assets/3d/T12/meshes/Knee3.STL differ diff --git a/assets/3d/T12/meshes/Knee4.STL b/assets/3d/T12/meshes/Knee4.STL new file mode 100644 index 0000000..9057da7 Binary files /dev/null and b/assets/3d/T12/meshes/Knee4.STL differ diff --git a/assets/3d/T12/meshes/Knee5.STL b/assets/3d/T12/meshes/Knee5.STL new file mode 100644 index 0000000..9057da7 Binary files /dev/null and b/assets/3d/T12/meshes/Knee5.STL differ diff --git a/assets/3d/T12/meshes/Knee6.STL b/assets/3d/T12/meshes/Knee6.STL new file mode 100644 index 0000000..9057da7 Binary files /dev/null and b/assets/3d/T12/meshes/Knee6.STL differ diff --git a/assets/3d/T12/meshes/Shin1.STL b/assets/3d/T12/meshes/Shin1.STL new file mode 100644 index 0000000..e515e76 Binary files /dev/null and b/assets/3d/T12/meshes/Shin1.STL differ diff --git a/assets/3d/T12/meshes/Shin2.STL b/assets/3d/T12/meshes/Shin2.STL new file mode 100644 index 0000000..e515e76 Binary files /dev/null and b/assets/3d/T12/meshes/Shin2.STL differ diff --git a/assets/3d/T12/meshes/Shin3.STL b/assets/3d/T12/meshes/Shin3.STL new file mode 100644 index 0000000..e515e76 Binary files /dev/null and b/assets/3d/T12/meshes/Shin3.STL differ diff --git a/assets/3d/T12/meshes/Shin4.STL b/assets/3d/T12/meshes/Shin4.STL new file mode 100644 index 0000000..e515e76 Binary files /dev/null and b/assets/3d/T12/meshes/Shin4.STL differ diff --git a/assets/3d/T12/meshes/Shin5.STL b/assets/3d/T12/meshes/Shin5.STL new file mode 100644 index 0000000..e515e76 Binary files /dev/null and b/assets/3d/T12/meshes/Shin5.STL differ diff --git a/assets/3d/T12/meshes/Shin6.STL b/assets/3d/T12/meshes/Shin6.STL new file mode 100644 index 0000000..e515e76 Binary files /dev/null and b/assets/3d/T12/meshes/Shin6.STL differ diff --git a/assets/3d/T12/meshes/Thigh1.STL b/assets/3d/T12/meshes/Thigh1.STL new file mode 100644 index 0000000..d134267 Binary files /dev/null and b/assets/3d/T12/meshes/Thigh1.STL differ diff --git a/assets/3d/T12/meshes/Thigh2.STL b/assets/3d/T12/meshes/Thigh2.STL new file mode 100644 index 0000000..d134267 Binary files /dev/null and b/assets/3d/T12/meshes/Thigh2.STL differ diff --git a/assets/3d/T12/meshes/Thigh3.STL b/assets/3d/T12/meshes/Thigh3.STL new file mode 100644 index 0000000..d134267 Binary files /dev/null and b/assets/3d/T12/meshes/Thigh3.STL differ diff --git a/assets/3d/T12/meshes/Thigh4.STL b/assets/3d/T12/meshes/Thigh4.STL new file mode 100644 index 0000000..d134267 Binary files /dev/null and b/assets/3d/T12/meshes/Thigh4.STL differ diff --git a/assets/3d/T12/meshes/Thigh5.STL b/assets/3d/T12/meshes/Thigh5.STL new file mode 100644 index 0000000..d134267 Binary files /dev/null and b/assets/3d/T12/meshes/Thigh5.STL differ diff --git a/assets/3d/T12/meshes/Thigh6.STL b/assets/3d/T12/meshes/Thigh6.STL new file mode 100644 index 0000000..d134267 Binary files /dev/null and b/assets/3d/T12/meshes/Thigh6.STL differ diff --git a/assets/3d/T12/urdf/T12.URDF b/assets/3d/T12/urdf/T12.URDF new file mode 100644 index 0000000..364b0cf --- /dev/null +++ b/assets/3d/T12/urdf/T12.URDF @@ -0,0 +1,2131 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/3d/T12/urdf/T12_flipped.URDF b/assets/3d/T12/urdf/T12_flipped.URDF new file mode 100644 index 0000000..fcfe7e8 --- /dev/null +++ b/assets/3d/T12/urdf/T12_flipped.URDF @@ -0,0 +1,2131 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/assets/3d/T12/urdf/T12_wrong_axes.orig.URDF b/assets/3d/T12/urdf/T12_wrong_axes.orig.URDF new file mode 100644 index 0000000..9393ffd --- /dev/null +++ b/assets/3d/T12/urdf/T12_wrong_axes.orig.URDF @@ -0,0 +1,2131 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/assets/3d/sample.urdf b/assets/3d/sample.urdf deleted file mode 100644 index c07d330..0000000 --- a/assets/3d/sample.urdf +++ /dev/null @@ -1,132 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/crates/rapier-stl/Cargo.toml b/crates/rapier-stl/Cargo.toml new file mode 100644 index 0000000..f533170 --- /dev/null +++ b/crates/rapier-stl/Cargo.toml @@ -0,0 +1,10 @@ +[package] +name = "rapier-stl" +version = "0.1.0" +edition = "2021" + +[dependencies] +thiserror = "1.0.61" +stl_io = "0.7" + +rapier3d = { versions = "0.19", path = "../rapier3d" } diff --git a/crates/rapier-stl/src/lib.rs b/crates/rapier-stl/src/lib.rs new file mode 100644 index 0000000..87fafec --- /dev/null +++ b/crates/rapier-stl/src/lib.rs @@ -0,0 +1,72 @@ +use rapier3d::geometry::{ + Collider, ColliderBuilder, Cuboid, MeshConverter, MeshConverterError, SharedShape, TriMesh, +}; +use rapier3d::math::{Isometry, Point, Real, Vector}; +use rapier3d::parry::bounding_volume; +use std::fs::File; +use std::io::{BufReader, Read, Seek}; +use std::path::Path; +use stl_io::IndexedMesh; + +#[derive(thiserror::Error, Debug)] +pub enum StlLoaderError { + #[error(transparent)] + MeshConverter(#[from] MeshConverterError), + #[error(transparent)] + Io(#[from] std::io::Error), +} + +/// The result of loading a shape from an stl mesh. +pub struct StlShape { + /// The shape loaded from the file and converted by the [`MeshConverter`]. + pub shape: SharedShape, + /// The shape’s pose. + pub pose: Isometry, + /// The raw mesh read from the stl file. + pub raw_mesh: IndexedMesh, +} + +pub fn load_from_path( + file_path: impl AsRef, + converter: MeshConverter, + scale: Vector, +) -> Result { + let mut reader = BufReader::new(File::open(file_path)?); + load_from_reader(&mut reader, converter, scale) +} + +pub fn load_from_reader( + read: &mut R, + converter: MeshConverter, + scale: Vector, +) -> Result { + let stl_mesh = stl_io::read_stl(read)?; + Ok(load_from_raw_mesh(stl_mesh, converter, scale)?) +} + +pub fn load_from_raw_mesh( + raw_mesh: IndexedMesh, + converter: MeshConverter, + scale: Vector, +) -> Result { + let mut vertices: Vec<_> = raw_mesh + .vertices + .iter() + .map(|xyz| Point::new(xyz[0] as Real, xyz[1] as Real, xyz[2] as Real)) + .collect(); + vertices + .iter_mut() + .for_each(|pt| pt.coords.component_mul_assign(&scale)); + let indices: Vec<_> = raw_mesh + .faces + .iter() + .map(|f| f.vertices.map(|i| i as u32)) + .collect(); + let (shape, pose) = converter.convert(vertices, indices)?; + + Ok(StlShape { + shape, + pose, + raw_mesh, + }) +} diff --git a/crates/rapier-urdf/Cargo.toml b/crates/rapier-urdf/Cargo.toml index 73957b1..bd8b0cc 100644 --- a/crates/rapier-urdf/Cargo.toml +++ b/crates/rapier-urdf/Cargo.toml @@ -2,11 +2,15 @@ name = "rapier-urdf" version = "0.1.0" edition = "2021" -description = "Load urdf files into rapier" -# See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html +[features] +stl = ["rapier-stl"] [dependencies] -bitflags = "2" -urdf-rs = "0.8" -rapier3d = { version = "0.19", path = "../rapier3d" } \ No newline at end of file +log = "0.4" +anyhow = "1" +# NOTE: we are not using the (more recent) urdf-rs crate because of https://github.com/openrr/urdf-rs/issues/94 +xurdf = "0.2" + +rapier3d = { versions = "0.19", path = "../rapier3d" } +rapier-stl = { version = "0.1.0", path = "../rapier-stl", optional = true } diff --git a/crates/rapier-urdf/src/lib.rs b/crates/rapier-urdf/src/lib.rs index 1376fdf..f79d94d 100644 --- a/crates/rapier-urdf/src/lib.rs +++ b/crates/rapier-urdf/src/lib.rs @@ -1,16 +1,19 @@ +use na::{RealField, UnitQuaternion}; use rapier3d::{ dynamics::{ GenericJoint, GenericJointBuilder, ImpulseJointHandle, ImpulseJointSet, JointAxesMask, JointAxis, MassProperties, MultibodyJointHandle, MultibodyJointSet, RigidBody, RigidBodyBuilder, RigidBodyHandle, RigidBodySet, RigidBodyType, }, - geometry::{Collider, ColliderBuilder, ColliderHandle, ColliderSet, SharedShape}, - math::{Isometry, Point, Vector}, + geometry::{ + Collider, ColliderBuilder, ColliderHandle, ColliderSet, MeshConverter, SharedShape, + }, + math::{Isometry, Point, Real, Vector}, na, }; use std::collections::HashMap; -use std::path::Path; -use urdf_rs::{Collision, Geometry, Inertial, Joint, JointType, Pose, Robot, UrdfError}; +use std::path::{Path, PathBuf}; +use xurdf::{Collision, Geometry, Inertial, Joint, Pose, Robot}; pub type LinkId = usize; @@ -21,6 +24,7 @@ pub struct UrdfLoaderOptions { pub apply_imported_mass_props: bool, pub enable_joint_collisions: bool, pub make_roots_fixed: bool, + pub shift: Isometry, pub collider_blueprint: ColliderBuilder, pub rigid_body_blueprint: RigidBodyBuilder, } @@ -33,6 +37,7 @@ impl Default for UrdfLoaderOptions { apply_imported_mass_props: true, enable_joint_collisions: false, make_roots_fixed: false, + shift: Isometry::identity(), collider_blueprint: ColliderBuilder::ball(0.0).density(0.0), rigid_body_blueprint: RigidBodyBuilder::dynamic(), } @@ -40,73 +45,104 @@ impl Default for UrdfLoaderOptions { } /// An urdf link loaded as a rapier [`RigidBody`] and its [`Collider`]s. -#[derive(Clone)] -pub struct RapierLink { +#[derive(Clone, Debug)] +pub struct UrdfLink { pub body: RigidBody, pub colliders: Vec, } /// An urdf joint loaded as a rapier [`GenericJoint`]. -#[derive(Clone)] -pub struct RapierJoint { +#[derive(Clone, Debug)] +pub struct UrdfJoint { /// The rapier version for the corresponding urdf joint. pub joint: GenericJoint, - /// Index of the rigid-body (from the [`RapierRobot`] array) at the first + /// Index of the rigid-body (from the [`UrdfRobot`] array) at the first /// endpoint of this joint. pub link1: LinkId, - /// Index of the rigid-body (from the [`RapierRobot`] array) at the second + /// Index of the rigid-body (from the [`UrdfRobot`] array) at the second /// endpoint of this joint. pub link2: LinkId, } /// A robot represented as a set of rapier rigid-bodies, colliders, and joints. -#[derive(Clone)] -pub struct RapierRobot { +#[derive(Clone, Debug)] +pub struct UrdfRobot { /// The bodies and colliders loaded from the urdf file. /// /// This vector matches the order of [`Robot::links`]. - pub links: Vec, + pub links: Vec, /// The joints loaded from the urdf file. /// /// This vector matches the order of [`Robot::joints`]. - pub joints: Vec, + pub joints: Vec, } -pub struct RapierJointHandle { +pub struct UrdfJointHandle { pub joint: JointHandle, pub link1: RigidBodyHandle, pub link2: RigidBodyHandle, } -pub struct RapierLinkHandle { +pub struct UrdfLinkHandle { pub body: RigidBodyHandle, pub colliders: Vec, } -pub struct RapierRobotHandles { - pub links: Vec, - pub joints: Vec>, +pub struct UrdfRobotHandles { + pub links: Vec, + pub joints: Vec>, } -impl RapierRobot { +#[derive(Copy, Clone, PartialEq, Eq, Debug, Default)] +enum JointType { + #[default] + Fixed, + Revolute, + Continuous, + Floating, + Planar, + Prismatic, + Spherical, +} + +impl JointType { + fn from_str(str: &str) -> Option { + match str.as_ref() { + "fixed" | "Fixed" => Some(Self::Fixed), + "continuous" | "Continuous" => Some(Self::Continuous), + "revolute" | "Revolute" => Some(Self::Revolute), + "floating" | "Floating" => Some(Self::Floating), + "planar" | "Planar" => Some(Self::Planar), + "prismatic" | "Prismatic" => Some(Self::Prismatic), + "spherical" | "Spherical" => Some(Self::Spherical), + _ => None, + } + } +} + +impl UrdfRobot { pub fn from_file( path: impl AsRef, options: UrdfLoaderOptions, - ) -> urdf_rs::Result<(Self, Robot)> { - let robot = urdf_rs::read_file(path)?; - Ok((Self::from_robot(&robot, options), robot)) + mesh_dir: Option<&Path>, + ) -> anyhow::Result<(Self, Robot)> { + let path = path.as_ref(); + let mesh_dir = mesh_dir.or_else(|| path.parent()); + let robot = xurdf::parse_urdf_from_file(path)?; + Ok((Self::from_robot(&robot, options, mesh_dir), robot)) } - pub fn from_str(str: &str, options: UrdfLoaderOptions) -> urdf_rs::Result<(Self, Robot)> { - let robot = urdf_rs::read_from_string(str)?; - Ok((Self::from_robot(&robot, options), robot)) + pub fn from_str( + str: &str, + options: UrdfLoaderOptions, + mesh_dir: Option<&Path>, + ) -> anyhow::Result<(Self, Robot)> { + let robot = xurdf::parse_urdf_from_string(str)?; + Ok((Self::from_robot(&robot, options, mesh_dir), robot)) } - pub fn from_robot(robot: &Robot, options: UrdfLoaderOptions) -> Self { + pub fn from_robot(robot: &Robot, options: UrdfLoaderOptions, mesh_dir: Option<&Path>) -> Self { let mut name_to_link_id = HashMap::new(); - println!("Num links: {}", robot.links.len()); - println!("Robot: {:?}", robot); - let mut link_is_root = vec![true; robot.links.len()]; let mut links: Vec<_> = robot .links @@ -114,48 +150,45 @@ impl RapierRobot { .enumerate() .map(|(id, link)| { name_to_link_id.insert(&link.name, id); - println!("Num collisions: {}", link.collision.len()); let mut colliders = vec![]; if options.create_colliders_from_collision_shapes { - colliders.extend( - link.collision - .iter() - .map(|co| urdf_to_collider(&options, &co.geometry, &co.origin)), - ) + colliders.extend(link.collisions.iter().filter_map(|co| { + urdf_to_collider(&options, mesh_dir, &co.geometry, &co.origin) + })) } if options.create_colliders_from_visual_shapes { - colliders.extend( - link.visual - .iter() - .map(|vis| urdf_to_collider(&options, &vis.geometry, &vis.origin)), - ) + colliders.extend(link.visuals.iter().filter_map(|vis| { + urdf_to_collider(&options, mesh_dir, &vis.geometry, &vis.origin) + })) } - let body = urdf_to_rigid_body(&options, &link.inertial); - RapierLink { body, colliders } + let mut body = urdf_to_rigid_body(&options, &link.inertial); + body.set_position(options.shift * body.position(), false); + UrdfLink { body, colliders } }) .collect(); let joints: Vec<_> = robot .joints .iter() .map(|joint| { - let link1 = name_to_link_id[&joint.parent.link]; - let link2 = name_to_link_id[&joint.child.link]; - let joint = urdf_to_joint(&options, joint); + let link1 = name_to_link_id[&joint.parent]; + let link2 = name_to_link_id[&joint.child]; + let pose1 = *links[link1].body.position(); + let rb2 = &mut links[link2].body; + let joint = urdf_to_joint(&options, joint, &pose1, rb2); link_is_root[link2] = false; - RapierJoint { + UrdfJoint { joint, link1, link2, } }) .collect(); - println!("{:?}", name_to_link_id); if options.make_roots_fixed { for (link, is_root) in links.iter_mut().zip(link_is_root.into_iter()) { if is_root { - link.body.set_body_type(RigidBodyType::Fixed, true) + link.body.set_body_type(RigidBodyType::Fixed, false) } } } @@ -168,7 +201,7 @@ impl RapierRobot { rigid_body_set: &mut RigidBodySet, collider_set: &mut ColliderSet, joint_set: &mut ImpulseJointSet, - ) -> RapierRobotHandles { + ) -> UrdfRobotHandles { let links: Vec<_> = self .links .into_iter() @@ -179,7 +212,7 @@ impl RapierRobot { .into_iter() .map(|co| collider_set.insert_with_parent(co, body, rigid_body_set)) .collect(); - RapierLinkHandle { body, colliders } + UrdfLinkHandle { body, colliders } }) .collect(); let joints: Vec<_> = self @@ -188,8 +221,8 @@ impl RapierRobot { .map(|joint| { let link1 = links[joint.link1].body; let link2 = links[joint.link2].body; - let joint = joint_set.insert(link1, link2, joint.joint, true); - RapierJointHandle { + let joint = joint_set.insert(link1, link2, joint.joint, false); + UrdfJointHandle { joint, link1, link2, @@ -197,14 +230,14 @@ impl RapierRobot { }) .collect(); - RapierRobotHandles { links, joints } + UrdfRobotHandles { links, joints } } pub fn insert_using_multibody_joints( self, rigid_body_set: &mut RigidBodySet, collider_set: &mut ColliderSet, joint_set: &mut MultibodyJointSet, - ) -> RapierRobotHandles> { + ) -> UrdfRobotHandles> { let links: Vec<_> = self .links .into_iter() @@ -215,7 +248,7 @@ impl RapierRobot { .into_iter() .map(|co| collider_set.insert_with_parent(co, body, rigid_body_set)) .collect(); - RapierLinkHandle { body, colliders } + UrdfLinkHandle { body, colliders } }) .collect(); let joints: Vec<_> = self @@ -224,8 +257,8 @@ impl RapierRobot { .map(|joint| { let link1 = links[joint.link1].body; let link2 = links[joint.link2].body; - let joint = joint_set.insert(link1, link2, joint.joint, true); - RapierJointHandle { + let joint = joint_set.insert(link1, link2, joint.joint, false); + UrdfJointHandle { joint, link1, link2, @@ -233,30 +266,29 @@ impl RapierRobot { }) .collect(); - RapierRobotHandles { links, joints } + UrdfRobotHandles { links, joints } } } fn urdf_to_rigid_body(options: &UrdfLoaderOptions, inertial: &Inertial) -> RigidBody { - let mut builder = options - .rigid_body_blueprint - .clone() - .position(urdf_to_isometry(&inertial.origin)); + let origin = urdf_to_isometry(&inertial.origin); + let mut builder = options.rigid_body_blueprint.clone(); + builder.body_type = RigidBodyType::Dynamic; if options.apply_imported_mass_props { builder = builder.additional_mass_properties(MassProperties::with_inertia_matrix( - Point::origin(), - inertial.mass.value as f32, + origin.translation.vector.into(), + inertial.mass as Real, na::Matrix3::new( - inertial.inertia.ixx as f32, - inertial.inertia.ixy as f32, - inertial.inertia.ixz as f32, - inertial.inertia.ixy as f32, - inertial.inertia.iyy as f32, - inertial.inertia.iyz as f32, - inertial.inertia.ixz as f32, - inertial.inertia.iyz as f32, - inertial.inertia.izz as f32, + inertial.inertia.m11 as Real, + inertial.inertia.m12 as Real, + inertial.inertia.m13 as Real, + inertial.inertia.m21 as Real, + inertial.inertia.m22 as Real, + inertial.inertia.m23 as Real, + inertial.inertia.m31 as Real, + inertial.inertia.m32 as Real, + inertial.inertia.m33 as Real, ), )) } @@ -264,73 +296,133 @@ fn urdf_to_rigid_body(options: &UrdfLoaderOptions, inertial: &Inertial) -> Rigid builder.build() } -fn urdf_to_collider(options: &UrdfLoaderOptions, geometry: &Geometry, origin: &Pose) -> Collider { +fn urdf_to_collider( + options: &UrdfLoaderOptions, + mesh_dir: Option<&Path>, + geometry: &Geometry, + origin: &Pose, +) -> Option { let mut builder = options.collider_blueprint.clone(); + let mut shape_transform = Isometry::identity(); let shape = match &geometry { Geometry::Box { size } => SharedShape::cuboid( - size[0] as f32 / 2.0, - size[1] as f32 / 2.0, - size[2] as f32 / 2.0, + size[0] as Real / 2.0, + size[1] as Real / 2.0, + size[2] as Real / 2.0, ), Geometry::Cylinder { radius, length } => { - SharedShape::cylinder(*length as f32 / 2.0, *radius as f32) + // 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) } - Geometry::Capsule { radius, length } => { - SharedShape::capsule_y(*length as f32 / 2.0, *radius as f32) + 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") => { + let full_path = mesh_dir.map(|dir| dir.join(filename)).unwrap_or_default(); + match rapier_stl::load_from_path(&full_path, MeshConverter::TriMesh, 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; + } + } + } + _ => { + log::error!("failed to load file with unknown type {filename}"); + return None; + } + } } - Geometry::Sphere { radius } => SharedShape::ball(*radius as f32), - Geometry::Mesh { filename, scale } => todo!(), }; builder.shape = shape; - - println!("Collider: {:?}", builder); - - builder.position(urdf_to_isometry(origin)).build() + Some( + builder + .position(urdf_to_isometry(origin) * shape_transform) + .build(), + ) } -fn urdf_to_isometry(pose: &Pose) -> Isometry { +fn urdf_to_isometry(pose: &Pose) -> Isometry { Isometry::from_parts( - Point::new(pose.xyz[0] as f32, pose.xyz[1] as f32, pose.xyz[2] as f32).into(), + Point::new( + pose.xyz[0] as Real, + pose.xyz[1] as Real, + pose.xyz[2] as Real, + ) + .into(), na::UnitQuaternion::from_euler_angles( - pose.rpy[0] as f32, - pose.rpy[1] as f32, - pose.rpy[2] as f32, + pose.rpy[0] as Real, + pose.rpy[1] as Real, + pose.rpy[2] as Real, ), ) } -fn urdf_to_joint(options: &UrdfLoaderOptions, joint: &Joint) -> GenericJoint { - let locked_axes = match joint.joint_type { +fn urdf_to_joint( + options: &UrdfLoaderOptions, + joint: &Joint, + pose1: &Isometry, + link2: &mut RigidBody, +) -> GenericJoint { + let joint_type = JointType::from_str(&joint.joint_type).unwrap_or_default(); + let locked_axes = match joint_type { JointType::Fixed => JointAxesMask::LOCKED_FIXED_AXES, JointType::Continuous | JointType::Revolute => JointAxesMask::LOCKED_REVOLUTE_AXES, JointType::Floating => JointAxesMask::empty(), - JointType::Planar => JointAxesMask::ANG_AXES | JointAxesMask::X, + JointType::Planar => JointAxesMask::ANG_AXES | JointAxesMask::LIN_X, JointType::Prismatic => JointAxesMask::LOCKED_PRISMATIC_AXES, JointType::Spherical => JointAxesMask::LOCKED_SPHERICAL_AXES, }; let joint_to_parent = urdf_to_isometry(&joint.origin); - let joint_axis = na::Unit::new_normalize(Vector::new( - joint.axis.xyz[0] as f32, - joint.axis.xyz[1] as f32, - joint.axis.xyz[2] as f32, - )); + let joint_axis = na::Unit::try_new( + Vector::new( + joint.axis.x as Real, + joint.axis.y as Real, + joint.axis.z as Real, + ), + 1.0e-5, + ); + + link2.set_position(pose1 * joint_to_parent, false); let mut builder = GenericJointBuilder::new(locked_axes) - .local_axis1(joint_to_parent * joint_axis) - .local_axis2(joint_axis) .local_anchor1(joint_to_parent.translation.vector.into()) .contacts_enabled(options.enable_joint_collisions); - match joint.joint_type { - JointType::Revolute | JointType::Prismatic => { + if let Some(joint_axis) = joint_axis { + builder = builder + .local_axis1(joint_to_parent * joint_axis) + .local_axis2(joint_axis); + } + + /* TODO: panics the multibody + match joint_type { + JointType::Prismatic => { builder = builder.limits( - JointAxis::X, - [joint.limit.lower as f32, joint.limit.upper as f32], + JointAxis::LinX, + [joint.limit.lower as Real, joint.limit.upper as Real], + ) + } + JointType::Revolute => { + builder = builder.limits( + JointAxis::AngX, + [joint.limit.lower as Real, joint.limit.upper as Real], ) } _ => {} } + */ // TODO: the following fields are currently ignored: // - Joint::dynamics diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml index 0e20bd2..e8b4356 100644 --- a/examples3d/Cargo.toml +++ b/examples3d/Cargo.toml @@ -29,6 +29,7 @@ path = "../crates/rapier3d" [dependencies.rapier-urdf] path = "../crates/rapier-urdf" +features = ["stl"] [[bin]] name = "all_examples3" diff --git a/examples3d/urdf3.rs b/examples3d/urdf3.rs index e188c86..6069171 100644 --- a/examples3d/urdf3.rs +++ b/examples3d/urdf3.rs @@ -1,6 +1,6 @@ use rapier3d::prelude::*; use rapier_testbed3d::Testbed; -use rapier_urdf::{RapierRobot, UrdfLoaderOptions}; +use rapier_urdf::{UrdfLoaderOptions, UrdfRobot}; pub fn init_world(testbed: &mut Testbed) { /* @@ -17,19 +17,29 @@ pub fn init_world(testbed: &mut Testbed) { let options = UrdfLoaderOptions { create_colliders_from_visual_shapes: true, create_colliders_from_collision_shapes: false, - apply_imported_mass_props: true, + apply_imported_mass_props: false, make_roots_fixed: true, - // rigid_body_blueprint: RigidBodyBuilder::dynamic().gravity_scale(0.0), - collider_blueprint: ColliderBuilder::ball(0.0) - .density(0.0) + // Z-up to Y-up. + shift: Isometry::rotation(Vector::x() * std::f32::consts::FRAC_PI_2), + rigid_body_blueprint: RigidBodyBuilder::default().gravity_scale(1.0), + collider_blueprint: ColliderBuilder::default() + .density(1.0) .active_collision_types(ActiveCollisionTypes::empty()), ..Default::default() }; - let (mut robot, _) = RapierRobot::from_file("assets/3d/sample.urdf", options).unwrap(); + let (mut robot, _) = + UrdfRobot::from_file("assets/3d/T12/urdf/T12.URDF", options, None).unwrap(); + // let (mut robot, _) = UrdfRobot::from_file("assets/3d/sample.urdf", options).unwrap(); // robot.insert_using_impulse_joints(&mut bodies, &mut colliders, &mut impulse_joints); robot.insert_using_multibody_joints(&mut bodies, &mut colliders, &mut multibody_joints); + testbed.add_callback(move |mut graphics, physics, _, state| { + for (_, body) in physics.bodies.iter() { + println!("pose: {:?}", body.position()); + } + }); + /* * Set up the testbed. */