feat: more urdf loader improvements

4 files changed, 60 insertions, 18 deletions

diff --git a/crates/rapier-urdf/Cargo.toml b/crates/rapier-urdf/Cargo.toml
index bd8b0cc..6837950 100644
--- a/crates/rapier-urdf/Cargo.toml
+++ b/crates/rapier-urdf/Cargo.toml
@@ -9,6 +9,7 @@ stl = ["rapier-stl"]
 [dependencies]
 log = "0.4"
 anyhow = "1"
+bitflags = "2"
 # NOTE: we are not using the (more recent) urdf-rs crate because of https://github.com/openrr/urdf-rs/issues/94
 xurdf = "0.2"
 
diff --git a/crates/rapier-urdf/src/lib.rs b/crates/rapier-urdf/src/lib.rs
index f79d94d..6feda78 100644
--- a/crates/rapier-urdf/src/lib.rs
+++ b/crates/rapier-urdf/src/lib.rs
@@ -7,6 +7,7 @@ use rapier3d::{
     },
     geometry::{
         Collider, ColliderBuilder, ColliderHandle, ColliderSet, MeshConverter, SharedShape,
+        TriMeshFlags,
     },
     math::{Isometry, Point, Real, Vector},
     na,
@@ -15,6 +16,15 @@ use std::collections::HashMap;
 use std::path::{Path, PathBuf};
 use xurdf::{Collision, Geometry, Inertial, Joint, Pose, Robot};
 
+bitflags::bitflags! {
+    #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
+    #[derive(Copy, Clone, Debug, PartialEq, Eq, Default)]
+    pub struct UrdfMultibodyOptions: u8 {
+        const JOINTS_ARE_KINEMATIC = 0b0001;
+        const DISABLE_SELF_CONTACTS = 0b0010;
+    }
+}
+
 pub type LinkId = usize;
 
 #[derive(Clone, Debug)]
@@ -24,6 +34,7 @@ pub struct UrdfLoaderOptions {
     pub apply_imported_mass_props: bool,
     pub enable_joint_collisions: bool,
     pub make_roots_fixed: bool,
+    pub trimesh_flags: TriMeshFlags,
     pub shift: Isometry<Real>,
     pub collider_blueprint: ColliderBuilder,
     pub rigid_body_blueprint: RigidBodyBuilder,
@@ -37,6 +48,7 @@ impl Default for UrdfLoaderOptions {
             apply_imported_mass_props: true,
             enable_joint_collisions: false,
             make_roots_fixed: false,
+            trimesh_flags: TriMeshFlags::all(),
             shift: Isometry::identity(),
             collider_blueprint: ColliderBuilder::ball(0.0).density(0.0),
             rigid_body_blueprint: RigidBodyBuilder::dynamic(),
@@ -77,6 +89,15 @@ pub struct UrdfRobot {
     pub joints: Vec<UrdfJoint>,
 }
 
+impl UrdfRobot {
+    pub fn append_transform(&mut self, transform: &Isometry<Real>) {
+        for link in &mut self.links {
+            link.body
+                .set_position(transform * link.body.position(), true);
+        }
+    }
+}
+
 pub struct UrdfJointHandle<JointHandle> {
     pub joint: JointHandle,
     pub link1: RigidBodyHandle,
@@ -237,6 +258,7 @@ impl UrdfRobot {
         rigid_body_set: &mut RigidBodySet,
         collider_set: &mut ColliderSet,
         joint_set: &mut MultibodyJointSet,
+        multibody_options: UrdfMultibodyOptions,
     ) -> UrdfRobotHandles<Option<MultibodyJointHandle>> {
         let links: Vec<_> = self
             .links
@@ -257,7 +279,20 @@ impl UrdfRobot {
             .map(|joint| {
                 let link1 = links[joint.link1].body;
                 let link2 = links[joint.link2].body;
-                let joint = joint_set.insert(link1, link2, joint.joint, false);
+                let joint =
+                    if multibody_options.contains(UrdfMultibodyOptions::JOINTS_ARE_KINEMATIC) {
+                        joint_set.insert_kinematic(link1, link2, joint.joint, false)
+                    } else {
+                        joint_set.insert(link1, link2, joint.joint, false)
+                    };
+
+                if let Some(joint) = joint {
+                    let (multibody, _) = joint_set.get_mut(joint).unwrap_or_else(|| unreachable!());
+                    multibody.set_self_contacts_enabled(
+                        !multibody_options.contains(UrdfMultibodyOptions::DISABLE_SELF_CONTACTS),
+                    );
+                }
+
                 UrdfJointHandle {
                     joint,
                     link1,
@@ -304,7 +339,7 @@ fn urdf_to_collider(
 ) -> Option<Collider> {
     let mut builder = options.collider_blueprint.clone();
     let mut shape_transform = Isometry::identity();
-    let shape = match &geometry {
+    let mut shape = match &geometry {
         Geometry::Box { size } => SharedShape::cuboid(
             size[0] as Real / 2.0,
             size[1] as Real / 2.0,
@@ -326,7 +361,11 @@ fn urdf_to_collider(
                 #[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) {
+                    match rapier_stl::load_from_path(
+                        &full_path,
+                        MeshConverter::TriMeshWithFlags(options.trimesh_flags),
+                        scale,
+                    ) {
                         Ok(stl_shape) => {
                             shape_transform = stl_shape.pose;
                             stl_shape.shape
diff --git a/examples3d/inverse_kinematics3.rs b/examples3d/inverse_kinematics3.rs
index 7905ae0..50cd589 100644
--- a/examples3d/inverse_kinematics3.rs
+++ b/examples3d/inverse_kinematics3.rs
@@ -89,6 +89,7 @@ pub fn init_world(testbed: &mut Testbed) {
                 link_id,
                 &options,
                 &Isometry::from(target_point),
+                |_| true,
                 &mut displacements,
             );
             multibody.apply_displacements(displacements.as_slice());
diff --git a/examples3d/urdf3.rs b/examples3d/urdf3.rs
index 6069171..ea65d8c 100644
--- a/examples3d/urdf3.rs
+++ b/examples3d/urdf3.rs
@@ -1,6 +1,7 @@
 use rapier3d::prelude::*;
 use rapier_testbed3d::Testbed;
-use rapier_urdf::{UrdfLoaderOptions, UrdfRobot};
+use rapier_urdf::{UrdfLoaderOptions, UrdfMultibodyOptions, UrdfRobot};
+use std::path::Path;
 
 pub fn init_world(testbed: &mut Testbed) {
     /*
@@ -17,28 +18,28 @@ 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: false,
         make_roots_fixed: true,
         // 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, _) =
         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());
-        }
-    });
+    // The robot can be inserted using impulse joints.
+    // (We clone because we want to insert the same robot once more afterward.)
+    robot
+        .clone()
+        .insert_using_impulse_joints(&mut bodies, &mut colliders, &mut impulse_joints);
+    // Insert the robot a second time, but using multibody joints this time.
+    robot.append_transform(&Isometry::translation(10.0, 0.0, 0.0));
+    robot.insert_using_multibody_joints(
+        &mut bodies,
+        &mut colliders,
+        &mut multibody_joints,
+        UrdfMultibodyOptions::DISABLE_SELF_CONTACTS,
+    );
 
     /*
      * Set up the testbed.