openrr_planner/collision/
urdf.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
use std::path::Path;

use k::{nalgebra as na, RealField, Vector3};
use ncollide3d::{
    procedural::IndexBuffer::{Split, Unified},
    shape::{Ball, Cuboid, Cylinder, ShapeHandle, TriMesh},
    transformation::ToTriMesh,
};
use tracing::*;

use super::mesh::load_mesh;

pub(crate) fn urdf_geometry_to_shape_handle<T>(
    collision_geometry: &urdf_rs::Geometry,
    base_dir: Option<&Path>,
) -> Option<ShapeHandle<T>>
where
    T: RealField + Copy,
{
    match *collision_geometry {
        urdf_rs::Geometry::Box { ref size } => {
            let cube = Cuboid::new(Vector3::new(
                na::convert(size[0] * 0.5),
                na::convert(size[1] * 0.5),
                na::convert(size[2] * 0.5),
            ));
            Some(ShapeHandle::new(cube))
        }
        urdf_rs::Geometry::Cylinder { radius, length } => {
            let y_cylinder = Cylinder::new(na::convert(length * 0.5), na::convert(radius));
            let tri_mesh = ncollide3d::transformation::convex_hull(
                &y_cylinder
                    .to_trimesh(30)
                    .coords
                    .iter()
                    .map(|point| point.xzy())
                    .collect::<Vec<_>>(),
            );
            let ind = match tri_mesh.indices {
                Unified(ind) => ind
                    .into_iter()
                    .map(|p| na::Point3::new(p[0] as usize, p[1] as usize, p[2] as usize))
                    .collect(),
                Split(_) => {
                    panic!("convex_hull implementation has been changed by ncollide3d update?");
                }
            };
            Some(ShapeHandle::new(TriMesh::new(
                tri_mesh.coords,
                ind,
                tri_mesh.uvs,
            )))
        }
        urdf_rs::Geometry::Capsule { .. } => {
            todo!()
        }
        urdf_rs::Geometry::Sphere { radius } => {
            Some(ShapeHandle::new(Ball::new(na::convert(radius))))
        }
        urdf_rs::Geometry::Mesh {
            ref filename,
            scale,
        } => {
            let scale = scale.unwrap_or(DEFAULT_MESH_SCALE);
            let replaced_filename = match urdf_rs::utils::expand_package_path(filename, base_dir) {
                Ok(replaced_filename) => replaced_filename,
                Err(e) => {
                    error!("{e}");
                    return None;
                }
            };
            let path = Path::new(&*replaced_filename);
            if !path.exists() {
                error!("{replaced_filename} not found");
                return None;
            }
            match load_mesh(path, &scale) {
                Ok(mesh) => Some(ShapeHandle::new(mesh)),
                Err(err) => {
                    error!("load_mesh {path:?} failed: {err}");
                    None
                }
            }
        }
    }
}

pub(crate) fn k_link_geometry_to_shape_handle<T>(
    collision_geometry: &k::link::Geometry<T>,
) -> Option<ShapeHandle<T>>
where
    T: RealField + Copy + k::SubsetOf<f64>,
{
    let converted_geometry = match collision_geometry {
        k::link::Geometry::Box {
            depth,
            width,
            height,
        } => urdf_rs::Geometry::Box {
            size: urdf_rs::Vec3([
                na::convert(*depth),
                na::convert(*width),
                na::convert(*height),
            ]),
        },
        k::link::Geometry::Cylinder { radius, length } => urdf_rs::Geometry::Cylinder {
            radius: na::convert(*radius),
            length: na::convert(*length),
        },
        k::link::Geometry::Capsule { .. } => {
            todo!()
        }
        k::link::Geometry::Sphere { radius } => urdf_rs::Geometry::Sphere {
            radius: na::convert(*radius),
        },
        k::link::Geometry::Mesh { filename, scale } => urdf_rs::Geometry::Mesh {
            filename: filename.to_string(),
            scale: Some(urdf_rs::Vec3([
                na::convert(scale[0]),
                na::convert(scale[1]),
                na::convert(scale[2]),
            ])),
        },
    };
    urdf_geometry_to_shape_handle(&converted_geometry, None)
}

// https://github.com/openrr/urdf-rs/pull/3/files#diff-0fb2eeea3273a4c9b3de69ee949567f546dc8c06b1e190336870d00b54ea0979L36-L38
const DEFAULT_MESH_SCALE: urdf_rs::Vec3 = urdf_rs::Vec3([1.0f64; 3]);