From edaa36ac7e702f419faab4ff1b9af858fc84177f Mon Sep 17 00:00:00 2001 From: Sébastien Crozet Date: Sun, 9 Jun 2024 10:57:37 +0200 Subject: chore: add more comments --- CHANGELOG.md | 4 +- Cargo.toml | 2 +- README.md | 6 - benchmarks2d/Cargo.toml | 18 +- benchmarks3d/Cargo.toml | 18 +- crates/rapier-stl/Cargo.toml | 10 - crates/rapier-stl/src/lib.rs | 72 --- crates/rapier-urdf/Cargo.toml | 17 - crates/rapier-urdf/src/lib.rs | 472 ---------------- crates/rapier2d-f64/Cargo.toml | 2 +- crates/rapier2d/Cargo.toml | 2 +- crates/rapier3d-f64/Cargo.toml | 2 +- crates/rapier3d-stl/CHANGELOG.md | 9 + crates/rapier3d-stl/Cargo.toml | 19 + crates/rapier3d-stl/LICENSE | 201 +++++++ crates/rapier3d-stl/README.md | 18 + crates/rapier3d-stl/src/lib.rs | 110 ++++ crates/rapier3d-urdf/CHANGELOG.md | 16 + crates/rapier3d-urdf/Cargo.toml | 26 + crates/rapier3d-urdf/LICENSE | 201 +++++++ crates/rapier3d-urdf/README.md | 37 ++ crates/rapier3d-urdf/src/lib.rs | 627 +++++++++++++++++++++ crates/rapier3d/Cargo.toml | 2 +- crates/rapier_testbed2d-f64/Cargo.toml | 2 +- crates/rapier_testbed2d/Cargo.toml | 2 +- crates/rapier_testbed3d-f64/Cargo.toml | 2 +- crates/rapier_testbed3d/Cargo.toml | 2 +- examples2d/Cargo.toml | 22 +- examples2d/all_examples2.rs | 4 +- examples2d/inverse_kinematics2.rs | 1 + examples3d-f64/Cargo.toml | 18 +- examples3d/Cargo.toml | 6 +- examples3d/all_examples3.rs | 4 +- examples3d/urdf3.rs | 5 +- src/dynamics/ccd/toi_entry.rs | 4 - src/dynamics/joint/multibody_joint/multibody.rs | 4 +- .../joint/multibody_joint/multibody_joint.rs | 4 + .../joint/multibody_joint/multibody_joint_set.rs | 1 - .../contact_constraint/contact_constraints_set.rs | 1 + .../joint_constraint/joint_constraint_builder.rs | 10 +- src/geometry/mesh_converter.rs | 8 +- src/lib.rs | 2 +- 42 files changed, 1344 insertions(+), 649 deletions(-) delete mode 100644 crates/rapier-stl/Cargo.toml delete mode 100644 crates/rapier-stl/src/lib.rs delete mode 100644 crates/rapier-urdf/Cargo.toml delete mode 100644 crates/rapier-urdf/src/lib.rs create mode 100644 crates/rapier3d-stl/CHANGELOG.md create mode 100644 crates/rapier3d-stl/Cargo.toml create mode 100644 crates/rapier3d-stl/LICENSE create mode 100644 crates/rapier3d-stl/README.md create mode 100644 crates/rapier3d-stl/src/lib.rs create mode 100644 crates/rapier3d-urdf/CHANGELOG.md create mode 100644 crates/rapier3d-urdf/Cargo.toml create mode 100644 crates/rapier3d-urdf/LICENSE create mode 100644 crates/rapier3d-urdf/README.md create mode 100644 crates/rapier3d-urdf/src/lib.rs diff --git a/CHANGELOG.md b/CHANGELOG.md index 9a60209..eb6a330 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,9 +2,9 @@ This release introduces two new crates: -- `rapier-urdf` for loading URDF files into rapier3d. This will load the rigid-bodies, +- `rapier3d-urdf` for loading URDF files into rapier3d. This will load the rigid-bodies, colliders, and joints. -- `rapier-stl` for loading an STL file as a collision shape. +- `rapier3d-stl` for loading an STL file as a collision shape. ### Added diff --git a/Cargo.toml b/Cargo.toml index f176685..971dff9 100644 --- a/Cargo.toml +++ b/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/rapier-stl"] + "crates/rapier3d", "crates/rapier3d-f64", "crates/rapier_testbed3d", "crates/rapier_testbed3d-f64", "examples3d", "examples3d-f64", "benchmarks3d", "crates/rapier3d-urdf", "crates/rapier3d-stl"] resolver = "2" [patch.crates-io] diff --git a/README.md b/README.md index 1174a1c..017cf56 100644 --- a/README.md +++ b/README.md @@ -40,12 +40,6 @@ are `rapier2d`, `rapier3d`, `rapier2d-f64`, and `rapier3d-f64`. They are written programming language, by the [Dimforge](https://dimforge.com) organization. It is forever free and open-source! -## Roadmap - -We update our roadmap at the beginning of each year. Our 2021 roadmap can be seen -[there](https://www.dimforge.com/blog/2021/01/01/physics-simulation-with-rapier-2021-roadmap/#rapier-roadmap-for-2021). -We regularly give updates about our progress on [our blog](https://www.dimforge.com/blog). - ## Getting started The easiest way to get started with Rapier is to: diff --git a/benchmarks2d/Cargo.toml b/benchmarks2d/Cargo.toml index bbab4d9..0d64704 100644 --- a/benchmarks2d/Cargo.toml +++ b/benchmarks2d/Cargo.toml @@ -1,19 +1,19 @@ [package] -name = "rapier-benchmarks-2d" +name = "rapier-benchmarks-2d" version = "0.1.0" -authors = [ "Sébastien Crozet " ] +authors = ["Sébastien Crozet "] edition = "2021" [features] -parallel = [ "rapier2d/parallel", "rapier_testbed2d/parallel" ] -simd-stable = [ "rapier2d/simd-stable" ] -simd-nightly = [ "rapier2d/simd-nightly" ] -other-backends = [ "rapier_testbed2d/other-backends" ] -enhanced-determinism = [ "rapier2d/enhanced-determinism" ] +parallel = ["rapier2d/parallel", "rapier_testbed2d/parallel"] +simd-stable = ["rapier2d/simd-stable"] +simd-nightly = ["rapier2d/simd-nightly"] +other-backends = ["rapier_testbed2d/other-backends"] +enhanced-determinism = ["rapier2d/enhanced-determinism"] [dependencies] -rand = "0.8" -Inflector = "0.11" +rand = "0.8" +Inflector = "0.11" [dependencies.rapier_testbed2d] path = "../crates/rapier_testbed2d" diff --git a/benchmarks3d/Cargo.toml b/benchmarks3d/Cargo.toml index 76d1911..f264543 100644 --- a/benchmarks3d/Cargo.toml +++ b/benchmarks3d/Cargo.toml @@ -1,19 +1,19 @@ [package] -name = "rapier-benchmarks-3d" +name = "rapier-benchmarks-3d" version = "0.1.0" -authors = [ "Sébastien Crozet " ] +authors = ["Sébastien Crozet "] edition = "2021" [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" -Inflector = "0.11" +rand = "0.8" +Inflector = "0.11" [dependencies.rapier_testbed3d] path = "../crates/rapier_testbed3d" diff --git a/crates/rapier-stl/Cargo.toml b/crates/rapier-stl/Cargo.toml deleted file mode 100644 index f533170..0000000 --- a/crates/rapier-stl/Cargo.toml +++ /dev/null @@ -1,10 +0,0 @@ -[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" } diff --git a/crates/rapier-stl/src/lib.rs b/crates/rapier-stl/src/lib.rs deleted file mode 100644 index 87fafec..0000000 --- a/crates/rapier-stl/src/lib.rs +++ /dev/null @@ -1,72 +0,0 @@ -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, - /// The raw mesh read from the stl file. - pub raw_mesh: IndexedMesh, -} - -pub fn load_from_path( - file_path: impl AsRef, - converter: MeshConverter, - scale: Vector, -) -> Result { - let mut reader = BufReader::new(File::open(file_path)?); - load_from_reader(&mut reader, converter, scale) -} - -pub fn load_from_reader( - read: &mut R, - converter: MeshConverter, - scale: Vector, -) -> Result { - 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, -) -> Result { - 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, - }) -} diff --git a/crates/rapier-urdf/Cargo.toml b/crates/rapier-urdf/Cargo.toml deleted file mode 100644 index 6837950..0000000 --- a/crates/rapier-urdf/Cargo.toml +++ /dev/null @@ -1,17 +0,0 @@ -[package] -name = "rapier-urdf" -version = "0.1.0" -edition = "2021" - -[features] -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" - -rapier3d = { versions = "0.19", path = "../rapier3d" } -rapier-stl = { version = "0.1.0", path = "../rapier-stl", optional = true } diff --git a/crates/rapier-urdf/src/lib.rs b/crates/rapier-urdf/src/lib.rs deleted file mode 100644 index 6feda78..0000000 --- a/crates/rapier-urdf/src/lib.rs +++ /dev/null @@ -1,472 +0,0 @@ -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, MeshConverter, SharedShape, - TriMeshFlags, - }, - math::{Isometry, Point, Real, Vector}, - na, -}; -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)] -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 trimesh_flags: TriMeshFlags, - pub shift: Isometry, - 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, - trimesh_flags: TriMeshFlags::all(), - shift: Isometry::identity(), - 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, Debug)] -pub struct UrdfLink { - pub body: RigidBody, - pub colliders: Vec, -} - -/// An urdf joint loaded as a rapier [`GenericJoint`]. -#[derive(Clone, Debug)] -pub struct UrdfJoint { - /// The rapier version for the corresponding urdf joint. - pub joint: GenericJoint, - /// 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 [`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, Debug)] -pub struct UrdfRobot { - /// The bodies and colliders loaded from the urdf file. - /// - /// This vector matches the order of [`Robot::links`]. - pub links: Vec, - /// The joints loaded from the urdf file. - /// - /// This vector matches the order of [`Robot::joints`]. - pub joints: Vec, -} - -impl UrdfRobot { - pub fn append_transform(&mut self, transform: &Isometry) { - for link in &mut self.links { - link.body - .set_position(transform * link.body.position(), true); - } - } -} - -pub struct UrdfJointHandle { - pub joint: JointHandle, - pub link1: RigidBodyHandle, - pub link2: RigidBodyHandle, -} - -pub struct UrdfLinkHandle { - pub body: RigidBodyHandle, - pub colliders: Vec, -} - -pub struct UrdfRobotHandles { - pub links: Vec, - pub joints: Vec>, -} - -#[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 { - 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, - options: UrdfLoaderOptions, - mesh_dir: Option<&Path>, - ) -> anyhow::Result<(Self, 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, - 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, mesh_dir: Option<&Path>) -> Self { - let mut name_to_link_id = HashMap::new(); - 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 mut colliders = vec![]; - if options.create_colliders_from_collision_shapes { - colliders.extend(link.collisions.iter().filter_map(|co| { - urdf_to_collider(&options, mesh_dir, &co.geometry, &co.origin) - })) - } - if options.create_colliders_from_visual_shapes { - colliders.extend(link.visuals.iter().filter_map(|vis| { - urdf_to_collider(&options, mesh_dir, &vis.geometry, &vis.origin) - })) - } - let mut body = urdf_to_rigid_body(&options, &link.inertial); - 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]; - let link2 = name_to_link_id[&joint.child]; - 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; - - UrdfJoint { - joint, - link1, - link2, - } - }) - .collect(); - - 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, false) - } - } - } - - Self { links, joints } - } - - pub fn insert_using_impulse_joints( - self, - rigid_body_set: &mut RigidBodySet, - collider_set: &mut ColliderSet, - joint_set: &mut ImpulseJointSet, - ) -> UrdfRobotHandles { - 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(); - UrdfLinkHandle { 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, false); - UrdfJointHandle { - joint, - link1, - link2, - } - }) - .collect(); - - UrdfRobotHandles { links, joints } - } - pub fn insert_using_multibody_joints( - self, - rigid_body_set: &mut RigidBodySet, - collider_set: &mut ColliderSet, - joint_set: &mut MultibodyJointSet, - multibody_options: UrdfMultibodyOptions, - ) -> UrdfRobotHandles> { - 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(); - UrdfLinkHandle { 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 = - 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, - link2, - } - }) - .collect(); - - UrdfRobotHandles { links, joints } - } -} - -fn urdf_to_rigid_body(options: &UrdfLoaderOptions, inertial: &Inertial) -> RigidBody { - let origin = urdf_to_isometry(&inertial.origin); - let mut builder = options.rigid_body_blueprint.clone(); - builder.body_type = RigidBodyType::Dynamic; - - if options.apply_imported_mass_props { - builder = builder.additional_mass_properties(MassProperties::with_inertia_matrix( - origin.translation.vector.into(), - inertial.mass as Real, - na::Matrix3::new( - inertial.inertia.m11 as Real, - inertial.inertia.m12 as Real, - inertial.inertia.m13 as Real, - inertial.inertia.m21 as Real, - inertial.inertia.m22 as Real, - inertial.inertia.m23 as Real, - inertial.inertia.m31 as Real, - inertial.inertia.m32 as Real, - inertial.inertia.m33 as Real, - ), - )) - } - - builder.build() -} - -fn urdf_to_collider( - options: &UrdfLoaderOptions, - mesh_dir: Option<&Path>, - geometry: &Geometry, - origin: &Pose, -) -> Option { - let mut builder = options.collider_blueprint.clone(); - let mut shape_transform = Isometry::identity(); - let mut shape = match &geometry { - Geometry::Box { size } => SharedShape::cuboid( - size[0] as Real / 2.0, - size[1] as Real / 2.0, - size[2] as Real / 2.0, - ), - Geometry::Cylinder { radius, length } => { - // 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::::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::TriMeshWithFlags(options.trimesh_flags), - 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; - } - } - } - }; - - builder.shape = shape; - Some( - builder - .position(urdf_to_isometry(origin) * shape_transform) - .build(), - ) -} - -fn urdf_to_isometry(pose: &Pose) -> Isometry { - Isometry::from_parts( - 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 Real, - pose.rpy[1] as Real, - pose.rpy[2] as Real, - ), - ) -} - -fn urdf_to_joint( - options: &UrdfLoaderOptions, - joint: &Joint, - pose1: &Isometry, - 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::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::try_new( - Vector::new( - joint.axis.x as Real, - 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_anchor1(joint_to_parent.translation.vector.into()) - .contacts_enabled(options.enable_joint_collisions); - - if let Some(joint_axis) = joint_axis { - 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::LinX, - [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 - // - Joint::limit.effort / limit.velocity - // - Joint::mimic - // - Joint::safety_controller - builder.build() -} diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index 79046bb..059f52d 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "rapier2d-f64" version = "0.19.0" -authors = ["Sébastien Crozet "] +authors = ["Sébastien Crozet "] description = "2-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier2d" homepage = "https://rapier.rs" diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index f0474c8..d270c9d 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "rapier2d" version = "0.19.0" -authors = ["Sébastien Crozet "] +authors = ["Sébastien Crozet "] description = "2-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier2d" homepage = "https://rapier.rs" diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 95f0d66..98a87a9 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -1,7 +1,7 @@ [package] name = "rapier3d-f64" version = "0.19.0" -authors = ["Sébastien Crozet "] +authors = ["Sébastien Crozet "] description = "3-dimensional physics engine in Rust." documentation = "https://docs.rs/rapier3d" homepage = "https://rapier.rs" diff --git a/crates/rapier3d-stl/CHANGELOG.md b/crates/rapier3d-stl/CHANGELOG.md new file mode 100644 index 0000000..bb3ffa3 --- /dev/null +++ b/crates/rapier3d-stl/CHANGELOG.md @@ -0,0 +1,9 @@ +## Unreleased + +This is the initial release of the `rapier3d-stl` crate. + +### Added + +- Add `load_from_path` for creating a shape from a stl file. +- Add `load_from_reader` for creating a shape from an object implementing `Read`. +- Add `load_from_raw_mesh` for creating a shape from an already loaded `IndexedMesh`. diff --git a/crates/rapier3d-stl/Cargo.toml b/crates/rapier3d-stl/Cargo.toml new file mode 100644 index 0000000..dc5bd0e --- /dev/null +++ b/crates/rapier3d-stl/Cargo.toml @@ -0,0 +1,19 @@ +[package] +name = "rapier3d-stl" +version = "0.1.0" +authors = ["Sébastien Crozet "] +description = "STL file loader for the 3D rapier physics engine." +documentation = "https://docs.rs/rapier3d-stl" +homepage = "https://rapier.rs" +repository = "https://github.com/dimforge/rapier" +readme = "README.md" +categories = ["science", "game-development", "mathematics", "simulation", "wasm"] +keywords = ["physics", "joints", "multibody", "robotics", "urdf"] +license = "Apache-2.0" +edition = "2021" + +[dependencies] +thiserror = "1.0.61" +stl_io = "0.7" + +rapier3d = { version = "0.19", path = "../rapier3d" } diff --git a/crates/rapier3d-stl/LICENSE b/crates/rapier3d-stl/LICENSE new file mode 100644 index 0000000..97f4383 --- /dev/null +++ b/crates/rapier3d-stl/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2020 Sébastien Crozet + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/crates/rapier3d-stl/README.md b/crates/rapier3d-stl/README.md new file mode 100644 index 0000000..98d61a0 --- /dev/null +++ b/crates/rapier3d-stl/README.md @@ -0,0 +1,18 @@ +## STL loader for the Rapier physics engine + +Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. The `rapier3d-stl` +crate lets you create a shape compatible with `rapier3d` and `parry3d` (the underlying collision-detection +library) from an STL file. + +## Resources and discussions + +- [Dimforge](https://dimforge.com): See all the open-source projects we are working on! Follow our announcements + on our [blog](https://www.dimforge.com/blog). +- [User guide](https://www.rapier.rs/docs/): Learn to use Rapier in your project by reading the official User Guides. +- [Discord](https://discord.gg/vt9DJSW): Come chat with us, get help, suggest features, on Discord! +- [NPM packages](https://www.npmjs.com/search?q=%40dimforge): Check out our NPM packages for Rapier, if you need to + use it with JavaScript/Typescript. + +Please make sure to familiarize yourself with our [Code of Conduct](CODE_OF_CONDUCT.md) +and our [Contribution Guidelines](CONTRIBUTING.md) before contributing or participating in +discussions with the community. diff --git a/crates/rapier3d-stl/src/lib.rs b/crates/rapier3d-stl/src/lib.rs new file mode 100644 index 0000000..0273bc0 --- /dev/null +++ b/crates/rapier3d-stl/src/lib.rs @@ -0,0 +1,110 @@ +//! ## STL loader for the Rapier physics engine +//! +//! Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. The `rapier3d-stl` +//! crate lets you create a shape compatible with `rapier3d` and `parry3d` (the underlying collision-detection +//! library) from an STL file. + +#![warn(missing_docs)] + +use rapier3d::geometry::{MeshConverter, MeshConverterError, SharedShape}; +use rapier3d::math::{Isometry, Point, Real, Vector}; +use std::fs::File; +use std::io::{BufReader, Read, Seek}; +use std::path::Path; +use stl_io::IndexedMesh; + +/// Error while loading an STL file. +#[derive(thiserror::Error, Debug)] +pub enum StlLoaderError { + /// An error triggered by rapier’s [`MeshConverter`]. + #[error(transparent)] + MeshConverter(#[from] MeshConverterError), + /// A generic IO error. + #[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, + /// The raw mesh read from the stl file without any modification. + pub raw_mesh: IndexedMesh, +} + +/// Loads an STL file as a shape from a file. +/// +/// # Parameters +/// - `file_path`: the STL file’s path. +/// - `converter`: controls how the shape is computed from the STL content. In particular, it lets +/// you specify if the computed [`StlShape::shape`] is a triangle mesh, its convex hull, +/// bounding box, etc. +/// - `scale`: the scaling factor applied to the geometry input to the `converter`. This scale will +/// affect at the geometric level the [`StlShape::shape`]. Note that raw mesh value stored +/// in [`StlShape::raw_mesh`] remains unscaled. +pub fn load_from_path( + file_path: impl AsRef, + converter: MeshConverter, + scale: Vector, +) -> Result { + let mut reader = BufReader::new(File::open(file_path)?); + load_from_reader(&mut reader, converter, scale) +} + +/// Loads an STL file as a shape from an arbitrary reader. +/// +/// # Parameters +/// - `reader`: the reader. +/// - `converter`: controls how the shape is computed from the STL content. In particular, it lets +/// you specify if the computed [`StlShape::shape`] is a triangle mesh, its convex hull, +/// bounding box, etc. +/// - `scale`: the scaling factor applied to the geometry input to the `converter`. This scale will +/// affect at the geometric level the [`StlShape::shape`]. Note that raw mesh value stored +/// in [`StlShape::raw_mesh`] remains unscaled. +pub fn load_from_reader( + read: &mut R, + converter: MeshConverter, + scale: Vector, +) -> Result { + let stl_mesh = stl_io::read_stl(read)?; + Ok(load_from_raw_mesh(stl_mesh, converter, scale)?) +} + +/// Loads an STL file as a shape from a preloaded raw stl mesh. +/// +/// # Parameters +/// - `raw_mesh`: the raw stl mesh. +/// - `converter`: controls how the shape is computed from the STL content. In particular, it lets +/// you specify if the computed [`StlShape::shape`] is a triangle mesh, its convex hull, +/// bounding box, etc. +/// - `scale`: the scaling factor applied to the geometry input to the `converter`. This scale will +/// affect at the geometric level the [`StlShape::shape`]. Note that raw mesh value stored +/// in [`StlShape::raw_mesh`] remains unscaled. +pub fn load_from_raw_mesh( + raw_mesh: IndexedMesh, + converter: MeshConverter, + scale: Vector, +) -> Result { + 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, + }) +} diff --git a/crates/rapier3d-urdf/CHANGELOG.md b/crates/rapier3d-urdf/CHANGELOG.md new file mode 100644 index 0000000..852e45a --- /dev/null +++ b/crates/rapier3d-urdf/CHANGELOG.md @@ -0,0 +1,16 @@ +## Unreleased + +This is the initial release of the `rapier3d-urdf` crate. + +### Added + +- Add `UrdfRobot` which is a collection of colliders, rigid-bodies and joints representing a robot loaded from an URDF + file. +- Add `UrdfRobot::from_file` to load an `UrdfRobot` from an URDF file. +- Add `UrdfRobot::from_str` to load an `UrdfRobot` from a string in URDF format. +- Add `UrdfRobot::from_robot` to load an `UrdfRobot` from an already loaded URDF + robot (pre-parsed with the `xurdf` crate). +- Add `UrdfRobot::insert_using_impulse_joints` to insert the robot to the rapier sets. Joints are represented as + **impulse** joints. +- Add `UrdfRobot::insert_using_impulse_joints` to insert the robot to the rapier sets. Joints are represented as + **multibody** joints. diff --git a/crates/rapier3d-urdf/Cargo.toml b/crates/rapier3d-urdf/Cargo.toml new file mode 100644 index 0000000..1394f2a --- /dev/null +++ b/crates/rapier3d-urdf/Cargo.toml @@ -0,0 +1,26 @@ +[package] +name = "rapier3d-urdf" +version = "0.1.0" +authors = ["Sébastien Crozet "] +description = "URDF file loader for the 3D rapier physics engine." +documentation = "https://docs.rs/rapier3d-urdf" +homepage = "https://rapier.rs" +repository = "https://github.com/dimforge/rapier" +readme = "README.md" +categories = ["science", "game-development", "mathematics", "simulation", "wasm"] +keywords = ["physics", "joints", "multibody", "robotics", "urdf"] +license = "Apache-2.0" +edition = "2021" + +[features] +stl = ["rapier3d-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" + +rapier3d = { version = "0.19", path = "../rapier3d" } +rapier3d-stl = { version = "0.1.0", path = "../rapier3d-stl", optional = true } diff --git a/crates/rapier3d-urdf/LICENSE b/crates/rapier3d-urdf/LICENSE new file mode 100644 index 0000000..97f4383 --- /dev/null +++ b/crates/rapier3d-urdf/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2020 Sébastien Crozet + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/crates/rapier3d-urdf/README.md b/crates/rapier3d-urdf/README.md new file mode 100644 index 0000000..ae2e21d --- /dev/null +++ b/crates/rapier3d-urdf/README.md @@ -0,0 +1,37 @@ +## STL loader for the Rapier physics engine + +Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. The `rapier3d-urdf` +crate lets you convert an URDF file into a set of rigid-bodies, colliders, and joints, for usage with the +`rapier3d` physics engine. + +## Optional cargo features + +- `stl`: enables loading STL meshes referenced by the URDF file. + +## Limitations + +Are listed below some known limitations you might want to be aware of before picking this library. Contributions to +improve +these elements are very welcome! + +- Mesh file types other than `stl` are not supported yet. Contributions are welcome. You my check the `rapier3d-stl` + repository for an example of mesh loader. +- When inserting joints as multibody joints, they will be reset to their neutral position (all coordinates = 0). +- The following fields are currently ignored: + - `Joint::dynamics` + - `Joint::limit.effort` / `limit.velocity` + - `Joint::mimic` + - `Joint::safety_controller` + +## Resources and discussions + +- [Dimforge](https://dimforge.com): See all the open-source projects we are working on! Follow our announcements + on our [blog](https://www.dimforge.com/blog). +- [User guide](https://www.rapier.rs/docs/): Learn to use Rapier in your project by reading the official User Guides. +- [Discord](https://discord.gg/vt9DJSW): Come chat with us, get help, suggest features, on Discord! +- [NPM packages](https://www.npmjs.com/search?q=%40dimforge): Check out our NPM packages for Rapier, if you need to + use it with JavaScript/Typescript. + +Please make sure to familiarize yourself with our [Code of Conduct](CODE_OF_CONDUCT.md) +and our [Contribution Guidelines](CONTRIBUTING.md) before contributing or participating in +discussions with the community. diff --git a/crates/rapier3d-urdf/src/lib.rs b/crates/rapier3d-urdf/src/lib.rs new file mode 100644 index 0000000..0a9a2fc --- /dev/null +++ b/crates/rapier3d-urdf/src/lib.rs @@ -0,0 +1,627 @@ +//! ## STL loader for the Rapier physics engine +//! +//! Rapier is a set of 2D and 3D physics engines for games, animation, and robotics. The `rapier3d-urdf` +//! crate lets you convert an URDF file into a set of rigid-bodies, colliders, and joints, for usage with the +//! `rapier3d` physics engine. +//! +//! ## Optional cargo features +//! +//! - `stl`: enables loading STL meshes referenced by the URDF file. +//! +//! ## Limitations +//! +//! Are listed below some known limitations you might want to be aware of before picking this library. Contributions to +//! improve +//! these elements are very welcome! +//! +//! - Mesh file types other than `stl` are not supported yet. Contributions are welcome. You my check the `rapier3d-stl` +//! repository for an example of mesh loader. +//! - When inserting joints as multibody joints, they will be reset to their neutral position (all coordinates = 0). +//! - The following fields are currently ignored: +//! - `Joint::dynamics` +//! - `Joint::limit.effort` / `limit.velocity` +//! - `Joint::mimic` +//!