feat: more urdf parser fixes + stl parser

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]

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).

assets/3d/sample.urdf

@@ -1,132 +0,0 @@
-<robot name="robot">
-  <link name="root">
-    <inertial>
-      <origin xyz="0 0 0.5" rpy="0 0 0"/>
-      <mass value="1"/>
-      <inertia ixx="100"  ixy="0"  ixz="0" iyy="100" iyz="0" izz="100" />
-    </inertial>
-    <visual>
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.2 0.2 0.4" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="1.0 1.0 1.0 1.0"/>
-      </material>
-    </visual>
-    <collision>
-      <origin xyz="0 0 0" rpy="0 0 0"/>
-      <geometry>
-        <cylinder radius="1" length="0.5"/>
-      </geometry>
-    </collision>
-  </link>
-  <link name="shoulder1">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.1 0.1 0.1" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="0 1.0 1.0 1.0"/>
-      </material>
-    </visual>
-  </link>
-  <link name="shoulder2">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.1 0.1 0.1" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="1.0 1.0 0.0 1.0"/>
-      </material>
-    </visual>
-  </link>
-  <link name="shoulder3">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.1 0.1 0.2" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="0.5 0.5 0.2 1.0"/>
-      </material>
-    </visual>
-  </link>
-  <link name="elbow1">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.1 0.1 0.2" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="0.8 0.2 0.2 1.0"/>
-      </material>
-    </visual>
-  </link>
-  <link name="wrist1">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.1 0.15 0.1" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="1.0 0.0 0.0 1.0"/>
-      </material>
-    </visual>
-  </link>
-  <joint name="shoulder_yaw" type="revolute">
-    <origin xyz="0.0 0.2 0.2" />
-    <parent link="root" />
-    <child link="shoulder1" />
-    <axis xyz="0 0 1" />
-    <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
-  </joint>
-  <link name="wrist2">
-    <visual>
-      <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0" />
-      <geometry>
-        <box size="0.05 0.08 0.15" />
-      </geometry>
-      <material name="Cyan">
-        <color rgba="0.0 0.0 1.0 1.0"/>
-      </material>
-    </visual>
-  </link>
-  <joint name="shoulder_pitch" type="revolute">
-    <origin xyz="0.0 0.1 0.0" />
-    <parent link="shoulder1" />
-    <child link="shoulder2" />
-    <axis xyz="0 1 0" />
-    <limit lower="-1" upper="1.0" effort="0" velocity="1.0"/>
-  </joint>
-  <joint name="shoulder_roll" type="revolute">
-    <origin xyz="0.0 0.1 0.0" />
-    <parent link="shoulder2" />
-    <child link="shoulder3" />
-    <axis xyz="1 0 0" />
-    <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
-  </joint>
-  <joint name="elbow_pitch" type="revolute">
-    <origin xyz="0.0 0.0 -0.2" />
-    <parent link="shoulder3" />
-    <child link="elbow1" />
-    <axis xyz="0 1 0" />
-    <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
-  </joint>
-  <joint name="wrist_yaw" type="revolute">
-    <origin xyz="0.0 0.0 -0.2" />
-    <parent link="elbow1" />
-    <child link="wrist1" />
-    <axis xyz="0 0 1" />
-    <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
-  </joint>
-  <joint name="wrist_pitch" type="revolute">
-    <origin xyz="0.0 0.0 -0.2" />
-    <parent link="wrist1" />
-    <child link="wrist2" />
-    <axis xyz="0 1 0" />
-    <limit lower="-2" upper="1.0" effort="0" velocity="1.0"/>
-  </joint>
-</robot>

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" }

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<Real>,
+    /// The raw mesh read from the stl file.
+    pub raw_mesh: IndexedMesh,
+}
+
+pub fn load_from_path(
+    file_path: impl AsRef<Path>,
+    converter: MeshConverter,
+    scale: Vector<Real>,
+) -> Result<StlShape, StlLoaderError> {
+    let mut reader = BufReader::new(File::open(file_path)?);
+    load_from_reader(&mut reader, converter, scale)
+}
+
+pub fn load_from_reader<R: Read + Seek>(
+    read: &mut R,
+    converter: MeshConverter,
+    scale: Vector<Real>,
+) -> Result<StlShape, StlLoaderError> {
+    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<Real>,
+) -> Result<StlShape, MeshConverterError> {
+    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,
+    })
+}

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"
+log = "0.4"
-urdf-rs = "0.8"
+anyhow = "1"
-rapier3d = { version = "0.19", path = "../rapier3d" }
+# 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 }

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},
+    geometry::{
-    math::{Isometry, Point, Vector},
+        Collider, ColliderBuilder, ColliderHandle, ColliderSet, MeshConverter, SharedShape,
+    },
+    math::{Isometry, Point, Real, Vector},
     na,
 };
 use std::collections::HashMap;
-use std::path::Path;
+use std::path::{Path, PathBuf};
-use urdf_rs::{Collision, Geometry, Inertial, Joint, JointType, Pose, Robot, UrdfError};
+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<Real>,
     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)]
+#[derive(Clone, Debug)]
-pub struct RapierLink {
+pub struct UrdfLink {
     pub body: RigidBody,
     pub colliders: Vec<Collider>,
 }
 
 /// An urdf joint loaded as a rapier [`GenericJoint`].
-#[derive(Clone)]
+#[derive(Clone, Debug)]
-pub struct RapierJoint {
+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)]
+#[derive(Clone, Debug)]
-pub struct RapierRobot {
+pub struct UrdfRobot {
     /// The bodies and colliders loaded from the urdf file.
     ///
     /// This vector matches the order of [`Robot::links`].
-    pub links: Vec<RapierLink>,
+    pub links: Vec<UrdfLink>,
     /// The joints loaded from the urdf file.
     ///
     /// This vector matches the order of [`Robot::joints`].
-    pub joints: Vec<RapierJoint>,
+    pub joints: Vec<UrdfJoint>,
 }
 
-pub struct RapierJointHandle<JointHandle> {
+pub struct UrdfJointHandle<JointHandle> {
     pub joint: JointHandle,
     pub link1: RigidBodyHandle,
     pub link2: RigidBodyHandle,
 }
 
-pub struct RapierLinkHandle {
+pub struct UrdfLinkHandle {
     pub body: RigidBodyHandle,
     pub colliders: Vec<ColliderHandle>,
 }
 
-pub struct RapierRobotHandles<JointHandle> {
+pub struct UrdfRobotHandles<JointHandle> {
-    pub links: Vec<RapierLinkHandle>,
+    pub links: Vec<UrdfLinkHandle>,
-    pub joints: Vec<RapierJointHandle<JointHandle>>,
+    pub joints: Vec<UrdfJointHandle<JointHandle>>,
 }
 
-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<Self> {
+        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<Path>,
         options: UrdfLoaderOptions,
-    ) -> urdf_rs::Result<(Self, Robot)> {
+        mesh_dir: Option<&Path>,
-        let robot = urdf_rs::read_file(path)?;
+    ) -> anyhow::Result<(Self, Robot)> {
-        Ok((Self::from_robot(&robot, options), 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)> {
+    pub fn from_str(
-        let robot = urdf_rs::read_from_string(str)?;
+        str: &str,
-        Ok((Self::from_robot(&robot, options), robot))
+        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(
+                    colliders.extend(link.collisions.iter().filter_map(|co| {
-                        link.collision
+                        urdf_to_collider(&options, mesh_dir, &co.geometry, &co.origin)
-                            .iter()
+                    }))
-                            .map(|co| urdf_to_collider(&options, &co.geometry, &co.origin)),
-                    )
                 }
                 if options.create_colliders_from_visual_shapes {
-                    colliders.extend(
+                    colliders.extend(link.visuals.iter().filter_map(|vis| {
-                        link.visual
+                        urdf_to_collider(&options, mesh_dir, &vis.geometry, &vis.origin)
-                            .iter()
+                    }))
-                            .map(|vis| urdf_to_collider(&options, &vis.geometry, &vis.origin)),
-                    )
                 }
-                let body = urdf_to_rigid_body(&options, &link.inertial);
+                let mut body = urdf_to_rigid_body(&options, &link.inertial);
-                RapierLink { body, colliders }
+                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 link1 = name_to_link_id[&joint.parent];
-                let link2 = name_to_link_id[&joint.child.link];
+                let link2 = name_to_link_id[&joint.child];
-                let joint = urdf_to_joint(&options, joint);
+                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<ImpulseJointHandle> {
+    ) -> UrdfRobotHandles<ImpulseJointHandle> {
         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);
+                let joint = joint_set.insert(link1, link2, joint.joint, false);
-                RapierJointHandle {
+                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<Option<MultibodyJointHandle>> {
+    ) -> UrdfRobotHandles<Option<MultibodyJointHandle>> {
         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);
+                let joint = joint_set.insert(link1, link2, joint.joint, false);
-                RapierJointHandle {
+                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
+    let origin = urdf_to_isometry(&inertial.origin);
-        .rigid_body_blueprint
+    let mut builder = options.rigid_body_blueprint.clone();
-        .clone()
+    builder.body_type = RigidBodyType::Dynamic;
-        .position(urdf_to_isometry(&inertial.origin));
 
     if options.apply_imported_mass_props {
         builder = builder.additional_mass_properties(MassProperties::with_inertia_matrix(
-            Point::origin(),
+            origin.translation.vector.into(),
-            inertial.mass.value as f32,
+            inertial.mass as Real,
             na::Matrix3::new(
-                inertial.inertia.ixx as f32,
+                inertial.inertia.m11 as Real,
-                inertial.inertia.ixy as f32,
+                inertial.inertia.m12 as Real,
-                inertial.inertia.ixz as f32,
+                inertial.inertia.m13 as Real,
-                inertial.inertia.ixy as f32,
+                inertial.inertia.m21 as Real,
-                inertial.inertia.iyy as f32,
+                inertial.inertia.m22 as Real,
-                inertial.inertia.iyz as f32,
+                inertial.inertia.m23 as Real,
-                inertial.inertia.ixz as f32,
+                inertial.inertia.m31 as Real,
-                inertial.inertia.iyz as f32,
+                inertial.inertia.m32 as Real,
-                inertial.inertia.izz as f32,
+                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<Collider> {
     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[0] as Real / 2.0,
-            size[1] as f32 / 2.0,
+            size[1] as Real / 2.0,
-            size[2] as f32 / 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::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::<Real>::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::Capsule { radius, length } => {
-            SharedShape::capsule_y(*length as f32 / 2.0, *radius as f32)
         }
-        Geometry::Sphere { radius } => SharedShape::ball(*radius as f32),
-        Geometry::Mesh { filename, scale } => todo!(),
     };
 
     builder.shape = shape;
-
+    Some(
-    println!("Collider: {:?}", builder);
+        builder
-
+            .position(urdf_to_isometry(origin) * shape_transform)
-    builder.position(urdf_to_isometry(origin)).build()
+            .build(),
+    )
 }
 
-fn urdf_to_isometry(pose: &Pose) -> Isometry<f32> {
+fn urdf_to_isometry(pose: &Pose) -> Isometry<Real> {
     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[0] as Real,
-            pose.rpy[1] as f32,
+            pose.rpy[1] as Real,
-            pose.rpy[2] as f32,
+            pose.rpy[2] as Real,
         ),
     )
 }
 
-fn urdf_to_joint(options: &UrdfLoaderOptions, joint: &Joint) -> GenericJoint {
+fn urdf_to_joint(
-    let locked_axes = match joint.joint_type {
+    options: &UrdfLoaderOptions,
+    joint: &Joint,
+    pose1: &Isometry<Real>,
+    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(
+    let joint_axis = na::Unit::try_new(
-        joint.axis.xyz[0] as f32,
+        Vector::new(
-        joint.axis.xyz[1] as f32,
+            joint.axis.x as Real,
-        joint.axis.xyz[2] as f32,
+            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 {
+    if let Some(joint_axis) = joint_axis {
-        JointType::Revolute | JointType::Prismatic => {
+        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,
+                JointAxis::LinX,
-                [joint.limit.lower as f32, joint.limit.upper as f32],
+                [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

examples3d/Cargo.toml

@@ -29,6 +29,7 @@ path = "../crates/rapier3d"
 
 [dependencies.rapier-urdf]
 path = "../crates/rapier-urdf"
+features = ["stl"]
 
 [[bin]]
 name = "all_examples3"

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),
+        // Z-up to Y-up.
-        collider_blueprint: ColliderBuilder::ball(0.0)
+        shift: Isometry::rotation(Vector::x() * std::f32::consts::FRAC_PI_2),
-            .density(0.0)
+        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.
      */