feat: continue urdf impl

crates/rapier-urdf/src/lib.rs

@@ -1,11 +1,16 @@
 use rapier3d::{
-    dynamics::{GenericJoint, RigidBody},
-    geometry::{Collider, ColliderBuilder, SharedShape},
+    dynamics::{
+        GenericJoint, GenericJointBuilder, ImpulseJointHandle, ImpulseJointSet, JointAxesMask,
+        JointAxis, MassProperties, MultibodyJointHandle, MultibodyJointSet, RigidBody,
+        RigidBodyBuilder, RigidBodyHandle, RigidBodySet,
+    },
+    geometry::{Collider, ColliderBuilder, ColliderHandle, ColliderSet},
     math::{Isometry, Point, Vector},
     na,
 };
+use std::collections::HashMap;
 use std::path::Path;
-use urdf_rs::{Collision, Geometry, Inertial, Pose, Robot, UrdfError};
+use urdf_rs::{Collision, Geometry, Inertial, Joint, JointType, Pose, Robot, UrdfError};
 
 pub type LinkId = usize;
 
@@ -34,21 +39,33 @@ pub struct RapierJoint {
 pub struct RapierRobot {
     /// The bodies and colliders loaded from the urdf file.
     ///
-    /// This vector matches the order of the loaded [`Robot::links`].
+    /// This vector matches the order of [`Robot::links`].
     pub links: Vec<RapierLink>,
     /// The joints loaded from the urdf file.
     ///
-    /// This vector matches the order of the loaded [`Robot::joints`].
-    pub joints: Vec<RapierLink>,
+    /// This vector matches the order of [`Robot::joints`].
+    pub joints: Vec<RapierJoint>,
+}
+
+pub struct RapierJointHandle<JointHandle> {
+    pub joint: JointHandle,
+    pub link1: RigidBodyHandle,
+    pub link2: RigidBodyHandle,
+}
+
+pub struct RapierLinkHandle {
+    pub body: RigidBodyHandle,
+    pub colliders: Vec<ColliderHandle>,
+}
+
+pub struct RapierRobotHandles<JointHandle> {
+    pub links: Vec<RapierLinkHandle>,
+    pub joints: Vec<RapierJointHandle<JointHandle>>,
 }
 
 impl RapierRobot {
     pub fn from_file(path: impl AsRef<Path>) -> urdf_rs::Result<(Self, Robot)> {
-        let robot = urdf_rs::read_from_string(
-            path.as_ref()
-                .to_str()
-                .ok_or_else(|| UrdfError::from("file path contains unsupported characters"))?,
-        )?;
+        let robot = urdf_rs::read_file(path)?;
         Ok((Self::from_robot(&robot), robot))
     }
 
@@ -58,10 +75,14 @@ impl RapierRobot {
     }
 
     pub fn from_robot(robot: &Robot) -> Self {
+        let mut name_to_link_id = HashMap::new();
+
         let links: Vec<_> = robot
             .links
             .iter()
-            .map(|link| {
+            .enumerate()
+            .map(|(id, link)| {
+                name_to_link_id.insert(&link.name, id);
                 let colliders: Vec<_> = link
                     .collision
                     .iter()
@@ -71,16 +92,121 @@ impl RapierRobot {
                 RapierLink { body, colliders }
             })
             .collect();
-        todo!()
+        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(joint);
+                RapierJoint {
+                    joint,
+                    link1,
+                    link2,
+                }
+            })
+            .collect();
+
+        Self { links, joints }
+    }
+
+    pub fn insert_using_impulse_joints(
+        self,
+        rigid_body_set: &mut RigidBodySet,
+        collider_set: &mut ColliderSet,
+        joint_set: &mut ImpulseJointSet,
+    ) -> RapierRobotHandles<ImpulseJointHandle> {
+        let links: Vec<_> = self
+            .links
+            .into_iter()
+            .map(|link| {
+                let body = rigid_body_set.insert(link.body);
+                let colliders = link
+                    .colliders
+                    .into_iter()
+                    .map(|co| collider_set.insert_with_parent(co, body, rigid_body_set))
+                    .collect();
+                RapierLinkHandle { body, colliders }
+            })
+            .collect();
+        let joints: Vec<_> = self
+            .joints
+            .into_iter()
+            .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 {
+                    joint,
+                    link1,
+                    link2,
+                }
+            })
+            .collect();
+
+        RapierRobotHandles { 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>> {
+        let links: Vec<_> = self
+            .links
+            .into_iter()
+            .map(|link| {
+                let body = rigid_body_set.insert(link.body);
+                let colliders = link
+                    .colliders
+                    .into_iter()
+                    .map(|co| collider_set.insert_with_parent(co, body, rigid_body_set))
+                    .collect();
+                RapierLinkHandle { body, colliders }
+            })
+            .collect();
+        let joints: Vec<_> = self
+            .joints
+            .into_iter()
+            .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 {
+                    joint,
+                    link1,
+                    link2,
+                }
+            })
+            .collect();
+
+        RapierRobotHandles { links, joints }
     }
 }
 
 fn urdf_to_rigid_body(inertial: &Inertial) -> RigidBody {
-    RigidBodyBuilder::dynamic().mass_props();
+    RigidBodyBuilder::dynamic()
+        .position(urdf_to_isometry(&inertial.origin))
+        .additional_mass_properties(MassProperties::with_inertia_matrix(
+            Point::origin(),
+            inertial.mass.value as f32,
+            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,
+            ),
+        ))
+        .build()
 }
 
 fn urdf_to_collider(co: &Collision) -> Collider {
-    let mut builder = match &co.geometry {
+    let builder = match &co.geometry {
         Geometry::Box { size } => ColliderBuilder::cuboid(
             size[0] as f32 / 2.0,
             size[1] as f32 / 2.0,
@@ -97,12 +223,12 @@ fn urdf_to_collider(co: &Collision) -> Collider {
     };
 
     builder
-        .position(pose_to_isometry(&co.origin))
+        .position(urdf_to_isometry(&co.origin))
         .density(0.0)
         .build()
 }
 
-fn pose_to_isometry(pose: &Pose) -> Isometry<f32> {
+fn urdf_to_isometry(pose: &Pose) -> Isometry<f32> {
     Isometry::from_parts(
         Point::new(pose.xyz[0] as f32, pose.xyz[1] as f32, pose.xyz[2] as f32).into(),
         na::UnitQuaternion::from_euler_angles(
@@ -112,3 +238,42 @@ fn pose_to_isometry(pose: &Pose) -> Isometry<f32> {
         ),
     )
 }
+
+fn urdf_to_joint(joint: &Joint) -> GenericJoint {
+    let locked_axes = match joint.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::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 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());
+
+    match joint.joint_type {
+        JointType::Revolute | JointType::Prismatic => {
+            builder = builder.limits(
+                JointAxis::X,
+                [joint.limit.lower as f32, joint.limit.upper as f32],
+            )
+        }
+        _ => {}
+    }
+
+    // TODO: the following fields are currently ignored:
+    //       - Joint::dynamics
+    //       - Joint::limit.effort / limit.velocity
+    //       - Joint::mimic
+    //       - Joint::safety_controller
+    builder.build()
+}