repose_collision/
robots.rs

1//! Preconfigured collision models and the RON collision-model spec.
2
3use crate::model::CuCollisionModelParams;
4use crate::model::CuRobotCollisionModel;
5use crate::shapes::CuCollisionShape;
6use crate::shapes::CuLinkShape;
7use crate::shapes::capsule_m;
8use crate::shapes::sphere_m;
9use cu29::prelude::*;
10use cu29::units::si::f64::Length as Length64;
11use cu29::units::si::length::meter;
12use repose_kinematics::CuRobotPreset;
13use serde::Deserialize;
14use serde::Deserializer;
15use serde::Serialize;
16use serde::Serializer;
17
18/// Link shapes of the igus ReBeL, one per joint frame `0..6`.
19fn igus_rebel_link_shapes() -> [CuLinkShape; 6] {
20    let capsule = |a, b, r| CuCollisionShape::Capsule(capsule_m(a, b, r));
21    let shapes = [
22        // Link 0 (shoulder column around joint 1 frame at z = 0.1462 m)
23        capsule([0.0, 0.0, -0.10], [0.0, 0.0, 0.04], 0.052),
24        // Link 1 (upper arm from joint 2 at z = 0.2522 m toward joint 3)
25        capsule([0.0, 0.0, 0.02], [0.0, 0.0, 0.20], 0.045),
26        // Link 2 (elbow housing from joint 3 toward joint 4)
27        capsule([0.0, 0.0, 0.02], [0.0, 0.0, 0.12], 0.042),
28        // Link 3 (forearm tube from joint 4 toward joint 5)
29        capsule([0.0, 0.0, 0.02], [0.0, 0.0, 0.11], 0.038),
30        // Link 4 (wrist housing from joint 5 toward joint 6)
31        capsule([0.0, 0.0, 0.01], [0.0, 0.0, 0.05], 0.034),
32        // Link 5 (tool flange sphere at joint 6)
33        CuCollisionShape::Sphere(sphere_m([0.0, 0.0, 0.02], 0.032)),
34    ];
35    let mut frame = 0;
36    shapes.map(|shape| {
37        frame += 1;
38        CuLinkShape::new(shape, Some(frame - 1))
39    })
40}
41
42fn igus_rebel_params() -> CuCollisionModelParams {
43    CuCollisionModelParams {
44        safety_margin: Length64::new::<meter>(0.005),
45        skip_adjacent_links: true,
46    }
47}
48
49/// Returns the 6-link capsule/sphere collision model for the **igus ReBeL** 6-DOF cobot.
50///
51/// Each shape is expressed in the local coordinate frame of its corresponding joint frame `0..6`
52/// produced by `repose_kinematics::igus_rebel_chain().forward_frames(...)`. Safety margin 5 mm;
53/// adjacent links are not checked against each other.
54pub fn igus_rebel_collision_model() -> CuRobotCollisionModel<6> {
55    // Constant, known-valid inputs: skip the fallible validation path.
56    CuRobotCollisionModel::assemble(&igus_rebel_link_shapes(), igus_rebel_params())
57}
58
59/// Source of a robot collision model in RON: a preset name or an inline model.
60///
61/// ```ron
62/// "collision_model": "igus_rebel"
63/// "collision_model": {
64///     "shapes": [
65///         {"shape": {"capsule": {"a": [0.0, 0.0, 0.0], "b": [0.0, 0.0, 0.2], "radius": 0.05}}, "frame": 0},
66///         {"shape": {"sphere": {"center": [0.0, 0.0, 0.02], "radius": 0.03}}, "frame": 1},
67///     ],
68///     "safety_margin": 0.005,       // optional, default 0.0
69///     "skip_adjacent_links": true,  // optional, default true
70/// }
71/// ```
72///
73/// An inline model lists exactly `M` shapes; omit `frame` for a base-frame shape. Use
74/// [`CuRobotCollisionModel::new`] for fewer than `M` shapes.
75#[derive(Debug, Clone, Copy, PartialEq)]
76pub enum CuCollisionModelSpec<const M: usize> {
77    /// A named preset; its shape and joint counts are checked by [`Self::resolve`].
78    Preset(CuRobotPreset),
79    /// Explicit shapes and parameters.
80    Inline {
81        /// One shape per model slot.
82        shapes: [CuLinkShape; M],
83        /// Model parameters.
84        params: CuCollisionModelParams,
85    },
86}
87
88impl<const M: usize> From<CuRobotPreset> for CuCollisionModelSpec<M> {
89    fn from(preset: CuRobotPreset) -> Self {
90        Self::Preset(preset)
91    }
92}
93
94impl<const M: usize> CuCollisionModelSpec<M> {
95    /// Builds and validates the model for an `N`-joint chain.
96    pub fn resolve<const N: usize>(&self) -> CuResult<CuRobotCollisionModel<N, M>> {
97        match self {
98            Self::Preset(CuRobotPreset::IgusRebel) => {
99                if N != CuRobotPreset::IgusRebel.dof() {
100                    return Err(CuError::from(
101                        "repose_collision: preset `igus_rebel` collision model needs N = 6",
102                    ));
103                }
104                CuRobotCollisionModel::new(&igus_rebel_link_shapes(), igus_rebel_params())
105            }
106            Self::Inline { shapes, params } => CuRobotCollisionModel::new(shapes, *params),
107        }
108    }
109}
110
111#[derive(Serialize, Deserialize)]
112#[serde(deny_unknown_fields)]
113struct InlineModel<const M: usize> {
114    #[serde(with = "crate::serde_arrays::link_shape_array")]
115    shapes: [CuLinkShape; M],
116    #[serde(default = "default_safety_margin")]
117    safety_margin: Length64,
118    #[serde(default = "default_skip_adjacent_links")]
119    skip_adjacent_links: bool,
120}
121
122fn default_safety_margin() -> Length64 {
123    CuCollisionModelParams::default().safety_margin
124}
125
126fn default_skip_adjacent_links() -> bool {
127    CuCollisionModelParams::default().skip_adjacent_links
128}
129
130impl<const M: usize> Serialize for CuCollisionModelSpec<M> {
131    fn serialize<S: Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
132        match self {
133            Self::Preset(preset) => preset.serialize(serializer),
134            Self::Inline { shapes, params } => InlineModel {
135                shapes: *shapes,
136                safety_margin: params.safety_margin,
137                skip_adjacent_links: params.skip_adjacent_links,
138            }
139            .serialize(serializer),
140        }
141    }
142}
143
144impl<'de, const M: usize> Deserialize<'de> for CuCollisionModelSpec<M> {
145    fn deserialize<D: Deserializer<'de>>(deserializer: D) -> Result<Self, D::Error> {
146        use serde::de::IntoDeserializer;
147        use serde::de::MapAccess;
148        use serde::de::Visitor;
149        use serde::de::value::MapAccessDeserializer;
150
151        struct SpecVisitor<const K: usize>;
152        impl<'de, const K: usize> Visitor<'de> for SpecVisitor<K> {
153            type Value = CuCollisionModelSpec<K>;
154            fn expecting(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
155                write!(
156                    f,
157                    "a robot preset name (\"igus_rebel\") or an inline collision model {{\"shapes\": [...]}}"
158                )
159            }
160            fn visit_str<E: serde::de::Error>(self, v: &str) -> Result<Self::Value, E> {
161                CuRobotPreset::deserialize(v.into_deserializer()).map(CuCollisionModelSpec::Preset)
162            }
163            fn visit_map<A: MapAccess<'de>>(self, map: A) -> Result<Self::Value, A::Error> {
164                let inline = InlineModel::<K>::deserialize(MapAccessDeserializer::new(map))?;
165                Ok(CuCollisionModelSpec::Inline {
166                    shapes: inline.shapes,
167                    params: CuCollisionModelParams {
168                        safety_margin: inline.safety_margin,
169                        skip_adjacent_links: inline.skip_adjacent_links,
170                    },
171                })
172            }
173        }
174        deserializer.deserialize_any(SpecVisitor::<M>)
175    }
176}
repose_collision/robots.rs

repose_collision/
robots.rs
