diff options
| -rw-r--r-- | crates/rapier-urdf/Cargo.toml | 1 | ||||
| -rw-r--r-- | crates/rapier-urdf/src/lib.rs | 45 | ||||
| -rw-r--r-- | examples3d/inverse_kinematics3.rs | 1 | ||||
| -rw-r--r-- | examples3d/urdf3.rs | 31 |
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. |