feat: add urdf example

6 files changed, 281 insertions, 43 deletions

diff --git a/assets/3d/sample.urdf b/assets/3d/sample.urdf
new file mode 100644
index 0000000..c07d330
--- /dev/null
+++ b/assets/3d/sample.urdf
@@ -0,0 +1,132 @@
+<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>
diff --git a/crates/rapier-urdf/Cargo.toml b/crates/rapier-urdf/Cargo.toml
index a3d9d8e..73957b1 100644
--- a/crates/rapier-urdf/Cargo.toml
+++ b/crates/rapier-urdf/Cargo.toml
@@ -7,5 +7,6 @@ description = "Load urdf files into rapier"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
+bitflags = "2"
 urdf-rs = "0.8"
-rapier3d = {  version = "0.19", path = "../rapier3d" }
\ No newline at end of file
+rapier3d = { version = "0.19", path = "../rapier3d" }
\ No newline at end of file
diff --git a/crates/rapier-urdf/src/lib.rs b/crates/rapier-urdf/src/lib.rs
index 4be9bf5..1376fdf 100644
--- a/crates/rapier-urdf/src/lib.rs
+++ b/crates/rapier-urdf/src/lib.rs
@@ -2,9 +2,9 @@ use rapier3d::{
     dynamics::{
         GenericJoint, GenericJointBuilder, ImpulseJointHandle, ImpulseJointSet, JointAxesMask,
         JointAxis, MassProperties, MultibodyJointHandle, MultibodyJointSet, RigidBody,
-        RigidBodyBuilder, RigidBodyHandle, RigidBodySet,
+        RigidBodyBuilder, RigidBodyHandle, RigidBodySet, RigidBodyType,
     },
-    geometry::{Collider, ColliderBuilder, ColliderHandle, ColliderSet},
+    geometry::{Collider, ColliderBuilder, ColliderHandle, ColliderSet, SharedShape},
     math::{Isometry, Point, Vector},
     na,
 };
@@ -14,6 +14,31 @@ use urdf_rs::{Collision, Geometry, Inertial, Joint, JointType, Pose, Robot, Urdf
 
 pub type LinkId = usize;
 
+#[derive(Clone, Debug)]
+pub struct UrdfLoaderOptions {
+    pub create_colliders_from_collision_shapes: bool,
+    pub create_colliders_from_visual_shapes: bool,
+    pub apply_imported_mass_props: bool,
+    pub enable_joint_collisions: bool,
+    pub make_roots_fixed: bool,
+    pub collider_blueprint: ColliderBuilder,
+    pub rigid_body_blueprint: RigidBodyBuilder,
+}
+
+impl Default for UrdfLoaderOptions {
+    fn default() -> Self {
+        Self {
+            create_colliders_from_collision_shapes: true,
+            create_colliders_from_visual_shapes: false,
+            apply_imported_mass_props: true,
+            enable_joint_collisions: false,
+            make_roots_fixed: false,
+            collider_blueprint: ColliderBuilder::ball(0.0).density(0.0),
+            rigid_body_blueprint: RigidBodyBuilder::dynamic(),
+        }
+    }
+}
+
 /// An urdf link loaded as a rapier [`RigidBody`] and its [`Collider`]s.
 #[derive(Clone)]
 pub struct RapierLink {
@@ -64,31 +89,48 @@ pub struct RapierRobotHandles<JointHandle> {
 }
 
 impl RapierRobot {
-    pub fn from_file(path: impl AsRef<Path>) -> urdf_rs::Result<(Self, Robot)> {
+    pub fn from_file(
+        path: impl AsRef<Path>,
+        options: UrdfLoaderOptions,
+    ) -> urdf_rs::Result<(Self, Robot)> {
         let robot = urdf_rs::read_file(path)?;
-        Ok((Self::from_robot(&robot), robot))
+        Ok((Self::from_robot(&robot, options), robot))
     }
 
-    pub fn from_str(str: &str) -> urdf_rs::Result<(Self, 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), robot))
+        Ok((Self::from_robot(&robot, options), robot))
     }
 
-    pub fn from_robot(robot: &Robot) -> Self {
+    pub fn from_robot(robot: &Robot, options: UrdfLoaderOptions) -> Self {
         let mut name_to_link_id = HashMap::new();
+        println!("Num links: {}", robot.links.len());
+        println!("Robot: {:?}", robot);
 
-        let links: Vec<_> = robot
+        let mut link_is_root = vec![true; robot.links.len()];
+        let mut links: Vec<_> = robot
             .links
             .iter()
             .enumerate()
             .map(|(id, link)| {
                 name_to_link_id.insert(&link.name, id);
-                let colliders: Vec<_> = link
-                    .collision
-                    .iter()
-                    .map(|co| urdf_to_collider(co))
-                    .collect();
-                let body = urdf_to_rigid_body(&link.inertial);
+                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)),
+                    )
+                }
+                if options.create_colliders_from_visual_shapes {
+                    colliders.extend(
+                        link.visual
+                            .iter()
+                            .map(|vis| urdf_to_collider(&options, &vis.geometry, &vis.origin)),
+                    )
+                }
+                let body = urdf_to_rigid_body(&options, &link.inertial);
                 RapierLink { body, colliders }
             })
             .collect();
@@ -98,7 +140,9 @@ impl RapierRobot {
             .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(joint);
+                let joint = urdf_to_joint(&options, joint);
+                link_is_root[link2] = false;
+
                 RapierJoint {
                     joint,
                     link1,
@@ -106,6 +150,15 @@ impl RapierRobot {
                 }
             })
             .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)
+                }
+            }
+        }
 
         Self { links, joints }
     }
@@ -184,10 +237,14 @@ impl RapierRobot {
     }
 }
 
-fn urdf_to_rigid_body(inertial: &Inertial) -> RigidBody {
-    RigidBodyBuilder::dynamic()
-        .position(urdf_to_isometry(&inertial.origin))
-        .additional_mass_properties(MassProperties::with_inertia_matrix(
+fn urdf_to_rigid_body(options: &UrdfLoaderOptions, inertial: &Inertial) -> RigidBody {
+    let mut builder = options
+        .rigid_body_blueprint
+        .clone()
+        .position(urdf_to_isometry(&inertial.origin));
+
+    if options.apply_imported_mass_props {
+        builder = builder.additional_mass_properties(MassProperties::with_inertia_matrix(
             Point::origin(),
             inertial.mass.value as f32,
             na::Matrix3::new(
@@ -202,30 +259,34 @@ fn urdf_to_rigid_body(inertial: &Inertial) -> RigidBody {
                 inertial.inertia.izz as f32,
             ),
         ))
-        .build()
+    }
+
+    builder.build()
 }
 
-fn urdf_to_collider(co: &Collision) -> Collider {
-    let builder = match &co.geometry {
-        Geometry::Box { size } => ColliderBuilder::cuboid(
+fn urdf_to_collider(options: &UrdfLoaderOptions, geometry: &Geometry, origin: &Pose) -> Collider {
+    let mut builder = options.collider_blueprint.clone();
+    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,
         ),
         Geometry::Cylinder { radius, length } => {
-            ColliderBuilder::cylinder(*length as f32 / 2.0, *radius as f32)
+            SharedShape::cylinder(*length as f32 / 2.0, *radius as f32)
         }
         Geometry::Capsule { radius, length } => {
-            ColliderBuilder::capsule_y(*length as f32 / 2.0, *radius as f32)
+            SharedShape::capsule_y(*length as f32 / 2.0, *radius as f32)
         }
-        Geometry::Sphere { radius } => ColliderBuilder::ball(*radius as f32),
+        Geometry::Sphere { radius } => SharedShape::ball(*radius as f32),
         Geometry::Mesh { filename, scale } => todo!(),
     };
 
-    builder
-        .position(urdf_to_isometry(&co.origin))
-        .density(0.0)
-        .build()
+    builder.shape = shape;
+
+    println!("Collider: {:?}", builder);
+
+    builder.position(urdf_to_isometry(origin)).build()
 }
 
 fn urdf_to_isometry(pose: &Pose) -> Isometry<f32> {
@@ -239,7 +300,7 @@ fn urdf_to_isometry(pose: &Pose) -> Isometry<f32> {
     )
 }
 
-fn urdf_to_joint(joint: &Joint) -> GenericJoint {
+fn urdf_to_joint(options: &UrdfLoaderOptions, joint: &Joint) -> GenericJoint {
     let locked_axes = match joint.joint_type {
         JointType::Fixed => JointAxesMask::LOCKED_FIXED_AXES,
         JointType::Continuous | JointType::Revolute => JointAxesMask::LOCKED_REVOLUTE_AXES,
@@ -258,7 +319,8 @@ fn urdf_to_joint(joint: &Joint) -> GenericJoint {
     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());
+        .local_anchor1(joint_to_parent.translation.vector.into())
+        .contacts_enabled(options.enable_joint_collisions);
 
     match joint.joint_type {
         JointType::Revolute | JointType::Prismatic => {
diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml
index 1e44b3d..0e20bd2 100644
--- a/examples3d/Cargo.toml
+++ b/examples3d/Cargo.toml
@@ -1,23 +1,23 @@
 [package]
-name    = "rapier-examples-3d"
+name = "rapier-examples-3d"
 version = "0.1.0"
-authors = [ "Sébastien Crozet <developer@crozet.re>" ]
+authors = ["Sébastien Crozet <developer@crozet.re>"]
 edition = "2021"
 default-run = "all_examples3"
 
 [features]
-parallel = [ "rapier3d/parallel", "rapier_testbed3d/parallel" ]
-simd-stable = [ "rapier3d/simd-stable" ]
-simd-nightly = [ "rapier3d/simd-nightly" ]
-other-backends = [ "rapier_testbed3d/other-backends" ]
-enhanced-determinism = [ "rapier3d/enhanced-determinism" ]
+parallel = ["rapier3d/parallel", "rapier_testbed3d/parallel"]
+simd-stable = ["rapier3d/simd-stable"]
+simd-nightly = ["rapier3d/simd-nightly"]
+other-backends = ["rapier_testbed3d/other-backends"]
+enhanced-determinism = ["rapier3d/enhanced-determinism"]
 
 [dependencies]
 rand = "0.8"
-getrandom = { version = "0.2", features = [ "js" ] } 
-Inflector  = "0.11"
+getrandom = { version = "0.2", features = ["js"] }
+Inflector = "0.11"
 wasm-bindgen = "0.2"
-obj-rs = { version =  "0.7", default-features = false }
+obj-rs = { version = "0.7", default-features = false }
 serde = "1"
 bincode = "1"
 
@@ -27,6 +27,9 @@ path = "../crates/rapier_testbed3d"
 [dependencies.rapier3d]
 path = "../crates/rapier3d"
 
+[dependencies.rapier-urdf]
+path = "../crates/rapier-urdf"
+
 [[bin]]
 name = "all_examples3"
 path = "./all_examples3.rs"
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs
index 3e6398f..ec4f533 100644
--- a/examples3d/all_examples3.rs
+++ b/examples3d/all_examples3.rs
@@ -55,6 +55,7 @@ mod rope_joints3;
 mod sensor3;
 mod spring_joints3;
 mod trimesh3;
+mod urdf3;
 mod vehicle_controller3;
 mod vehicle_joints3;
 
@@ -119,6 +120,7 @@ pub fn main() {
         ("Sensor", sensor3::init_world),
         ("Spring Joints", spring_joints3::init_world),
         ("TriMesh", trimesh3::init_world),
+        ("Urdf", urdf3::init_world),
         ("Vehicle controller", vehicle_controller3::init_world),
         ("Vehicle joints", vehicle_joints3::init_world),
         ("Keva tower", keva3::init_world),
diff --git a/examples3d/urdf3.rs b/examples3d/urdf3.rs
new file mode 100644
index 0000000..e188c86
--- /dev/null
+++ b/examples3d/urdf3.rs
@@ -0,0 +1,38 @@
+use rapier3d::prelude::*;
+use rapier_testbed3d::Testbed;
+use rapier_urdf::{RapierRobot, UrdfLoaderOptions};
+
+pub fn init_world(testbed: &mut Testbed) {
+    /*
+     * World
+     */
+    let mut bodies = RigidBodySet::new();
+    let mut colliders = ColliderSet::new();
+    let mut impulse_joints = ImpulseJointSet::new();
+    let mut multibody_joints = MultibodyJointSet::new();
+
+    /*
+     * Ground
+     */
+    let options = UrdfLoaderOptions {
+        create_colliders_from_visual_shapes: true,
+        create_colliders_from_collision_shapes: false,
+        apply_imported_mass_props: true,
+        make_roots_fixed: true,
+        // rigid_body_blueprint: RigidBodyBuilder::dynamic().gravity_scale(0.0),
+        collider_blueprint: ColliderBuilder::ball(0.0)
+            .density(0.0)
+            .active_collision_types(ActiveCollisionTypes::empty()),
+        ..Default::default()
+    };
+    let (mut robot, _) = RapierRobot::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);
+
+    /*
+     * Set up the testbed.
+     */
+    testbed.set_world(bodies, colliders, impulse_joints, multibody_joints);
+    testbed.look_at(point![100.0, 100.0, 100.0], Point::origin());
+}