openrr_planner/collision/
urdf.rs

1use std::path::Path;
2
3use k::{nalgebra as na, RealField, Vector3};
4use ncollide3d::{
5    procedural::IndexBuffer::{Split, Unified},
6    shape::{Ball, Cuboid, Cylinder, ShapeHandle, TriMesh},
7    transformation::ToTriMesh,
8};
9use tracing::*;
10
11use super::mesh::load_mesh;
12
13pub(crate) fn urdf_geometry_to_shape_handle<T>(
14    collision_geometry: &urdf_rs::Geometry,
15    base_dir: Option<&Path>,
16) -> Option<ShapeHandle<T>>
17where
18    T: RealField + Copy,
19{
20    match *collision_geometry {
21        urdf_rs::Geometry::Box { ref size } => {
22            let cube = Cuboid::new(Vector3::new(
23                na::convert(size[0] * 0.5),
24                na::convert(size[1] * 0.5),
25                na::convert(size[2] * 0.5),
26            ));
27            Some(ShapeHandle::new(cube))
28        }
29        urdf_rs::Geometry::Cylinder { radius, length } => {
30            let y_cylinder = Cylinder::new(na::convert(length * 0.5), na::convert(radius));
31            let tri_mesh = ncollide3d::transformation::convex_hull(
32                &y_cylinder
33                    .to_trimesh(30)
34                    .coords
35                    .iter()
36                    .map(|point| point.xzy())
37                    .collect::<Vec<_>>(),
38            );
39            let ind = match tri_mesh.indices {
40                Unified(ind) => ind
41                    .into_iter()
42                    .map(|p| na::Point3::new(p[0] as usize, p[1] as usize, p[2] as usize))
43                    .collect(),
44                Split(_) => {
45                    panic!("convex_hull implementation has been changed by ncollide3d update?");
46                }
47            };
48            Some(ShapeHandle::new(TriMesh::new(
49                tri_mesh.coords,
50                ind,
51                tri_mesh.uvs,
52            )))
53        }
54        urdf_rs::Geometry::Capsule { .. } => {
55            todo!()
56        }
57        urdf_rs::Geometry::Sphere { radius } => {
58            Some(ShapeHandle::new(Ball::new(na::convert(radius))))
59        }
60        urdf_rs::Geometry::Mesh {
61            ref filename,
62            scale,
63        } => {
64            let scale = scale.unwrap_or(DEFAULT_MESH_SCALE);
65            let replaced_filename = match urdf_rs::utils::expand_package_path(filename, base_dir) {
66                Ok(replaced_filename) => replaced_filename,
67                Err(e) => {
68                    error!("{e}");
69                    return None;
70                }
71            };
72            let path = Path::new(&*replaced_filename);
73            if !path.exists() {
74                error!("{replaced_filename} not found");
75                return None;
76            }
77            match load_mesh(path, &scale) {
78                Ok(mesh) => Some(ShapeHandle::new(mesh)),
79                Err(err) => {
80                    error!("load_mesh {path:?} failed: {err}");
81                    None
82                }
83            }
84        }
85    }
86}
87
88pub(crate) fn k_link_geometry_to_shape_handle<T>(
89    collision_geometry: &k::link::Geometry<T>,
90) -> Option<ShapeHandle<T>>
91where
92    T: RealField + Copy + k::SubsetOf<f64>,
93{
94    let converted_geometry = match collision_geometry {
95        k::link::Geometry::Box {
96            depth,
97            width,
98            height,
99        } => urdf_rs::Geometry::Box {
100            size: urdf_rs::Vec3([
101                na::convert(*depth),
102                na::convert(*width),
103                na::convert(*height),
104            ]),
105        },
106        k::link::Geometry::Cylinder { radius, length } => urdf_rs::Geometry::Cylinder {
107            radius: na::convert(*radius),
108            length: na::convert(*length),
109        },
110        k::link::Geometry::Capsule { .. } => {
111            todo!()
112        }
113        k::link::Geometry::Sphere { radius } => urdf_rs::Geometry::Sphere {
114            radius: na::convert(*radius),
115        },
116        k::link::Geometry::Mesh { filename, scale } => urdf_rs::Geometry::Mesh {
117            filename: filename.to_string(),
118            scale: Some(urdf_rs::Vec3([
119                na::convert(scale[0]),
120                na::convert(scale[1]),
121                na::convert(scale[2]),
122            ])),
123        },
124    };
125    urdf_geometry_to_shape_handle(&converted_geometry, None)
126}
127
128// https://github.com/openrr/urdf-rs/pull/3/files#diff-0fb2eeea3273a4c9b3de69ee949567f546dc8c06b1e190336870d00b54ea0979L36-L38
129const DEFAULT_MESH_SCALE: urdf_rs::Vec3 = urdf_rs::Vec3([1.0f64; 3]);
openrr_planner/collision/urdf.rs

openrr_planner/collision/
urdf.rs
