repose_kinematics/
robots.rs

1//! Kinematic chain presets for standard manipulators and the RON chain source.
2
3use crate::chain::CuJointAxis;
4use crate::chain::CuJointSpec;
5use crate::chain::CuKinematicChain;
6use crate::joint::CuJointLimits;
7use cu29::prelude::*;
8use cu29::units::si::angle::radian;
9use cu29::units::si::angular_velocity::radian_per_second;
10use cu29::units::si::f64::Angle;
11use cu29::units::si::f64::AngularVelocity;
12use cu29::units::si::f64::Length;
13use cu29::units::si::length::meter;
14use serde::Deserialize;
15use serde::Deserializer;
16use serde::Serialize;
17use serde::Serializer;
18
19fn m(v: f64) -> Length {
20    Length::new::<meter>(v)
21}
22
23fn rad(v: f64) -> Angle {
24    Angle::new::<radian>(v)
25}
26
27fn rebel_joint(xyz: [f64; 3], rpy_z: f64, axis: CuJointAxis, lo: f64, hi: f64) -> CuJointSpec {
28    CuJointSpec::revolute(
29        [m(xyz[0]), m(xyz[1]), m(xyz[2])],
30        [rad(0.0), rad(0.0), rad(rpy_z)],
31        axis,
32        CuJointLimits::revolute(
33            rad(lo),
34            rad(hi),
35            AngularVelocity::new::<radian_per_second>(2.16),
36        ),
37    )
38}
39
40/// Returns the 6-DOF kinematic chain for the **igus ReBeL** cobot.
41///
42/// Link origins, axes, and joint limits are derived directly from `igus_rebel.urdf`
43/// (combining fixed motor-housing offsets with their corresponding revolute joints).
44/// In RON the same chain is selected with `"chain": "igus_rebel"`.
45pub fn igus_rebel_chain() -> CuKinematicChain<6> {
46    CuKinematicChain::new([
47        // J_motor1 (xyz: [0, 0, 0.1462], rpy: [0, 0, pi]) + joint1 (axis: +Z)
48        rebel_joint(
49            [0.0, 0.0, 0.1462],
50            core::f64::consts::PI,
51            CuJointAxis::PosZ,
52            -3.1241,
53            3.1241,
54        ),
55        // J_motor2 (xyz: [0, -0.0225, 0.106]) + joint2 (xyz: [0, 0.049, 0], axis: -Y)
56        rebel_joint(
57            [0.0, 0.0265, 0.106],
58            0.0,
59            CuJointAxis::NegY,
60            -1.3500,
61            2.4435,
62        ),
63        // J_motor3 (xyz: [0, -0.049, 0.24152]) + joint3 (xyz: [0, 0.0225, 0], axis: -Y)
64        rebel_joint(
65            [0.0, -0.0265, 0.24152],
66            0.0,
67            CuJointAxis::NegY,
68            -1.39626,
69            2.4000,
70        ),
71        // J_motor4 (xyz: [0.0013449814287804, 0, 0.105479407401356]) + joint4 (xyz: [0, 0, 0.052], axis: +Z)
72        rebel_joint(
73            [0.0013449814287804, 0.0, 0.157479407401356],
74            0.0,
75            CuJointAxis::PosZ,
76            -3.12414,
77            3.12414,
78        ),
79        // J_motor5 (xyz: [0, -0.0182, 0.142]) + joint5 (xyz: [0, 0.0182, 0], axis: -Y)
80        rebel_joint([0.0, 0.0, 0.142], 0.0, CuJointAxis::NegY, -1.65806, 1.65806),
81        // joint6 (xyz: [0, 0, 0.0768], axis: +Z)
82        rebel_joint(
83            [0.0, 0.0, 0.0768],
84            0.0,
85            CuJointAxis::PosZ,
86            -3.12414,
87            3.12414,
88        ),
89    ])
90}
91
92/// Named robot chain presets, selected in RON by their snake_case name.
93#[derive(Debug, Clone, Copy, PartialEq, Eq, Serialize, Deserialize, Reflect)]
94#[serde(rename_all = "snake_case")]
95pub enum CuRobotPreset {
96    /// 6-DOF igus ReBeL ([`igus_rebel_chain`]); RON name `"igus_rebel"`.
97    IgusRebel,
98}
99
100impl CuRobotPreset {
101    /// RON name of the preset.
102    pub const fn name(self) -> &'static str {
103        match self {
104            Self::IgusRebel => "igus_rebel",
105        }
106    }
107
108    /// Number of joints of the preset chain.
109    pub const fn dof(self) -> usize {
110        match self {
111            Self::IgusRebel => 6,
112        }
113    }
114
115    /// Builds the preset chain, failing when its joint count differs from `N`.
116    pub fn chain<const N: usize>(self) -> CuResult<CuKinematicChain<N>> {
117        let joints = match self {
118            Self::IgusRebel => *igus_rebel_chain().joints(),
119        };
120        let joints: [CuJointSpec; N] = joints.as_slice().try_into().map_err(|_| {
121            CuError::from(alloc::format!(
122                "Robot preset `{}` has {} joints but the task expects N = {N}",
123                self.name(),
124                self.dof()
125            ))
126        })?;
127        Ok(CuKinematicChain::new(joints))
128    }
129}
130
131/// Source of a robot chain in RON: a preset name or an inline chain.
132///
133/// ```ron
134/// "chain": "igus_rebel"
135/// "chain": {"joints": [ /* N CuJointSpec */ ], "tool_offset": /* optional */}
136/// ```
137#[derive(Debug, Clone, Copy, PartialEq)]
138pub enum CuChainSpec<const N: usize> {
139    /// A named preset; its joint count is checked against `N` by [`Self::resolve`].
140    Preset(CuRobotPreset),
141    /// An explicit chain.
142    Inline(CuKinematicChain<N>),
143}
144
145impl<const N: usize> From<CuKinematicChain<N>> for CuChainSpec<N> {
146    fn from(chain: CuKinematicChain<N>) -> Self {
147        Self::Inline(chain)
148    }
149}
150
151impl<const N: usize> From<CuRobotPreset> for CuChainSpec<N> {
152    fn from(preset: CuRobotPreset) -> Self {
153        Self::Preset(preset)
154    }
155}
156
157impl<const N: usize> CuChainSpec<N> {
158    /// Builds and validates the chain.
159    pub fn resolve(&self) -> CuResult<CuKinematicChain<N>> {
160        let chain = match *self {
161            Self::Preset(preset) => preset.chain()?,
162            Self::Inline(chain) => chain,
163        };
164        chain.validate()?;
165        Ok(chain)
166    }
167}
168
169impl<const N: usize> Serialize for CuChainSpec<N> {
170    fn serialize<S: Serializer>(&self, serializer: S) -> Result<S::Ok, S::Error> {
171        match self {
172            Self::Preset(preset) => preset.serialize(serializer),
173            Self::Inline(chain) => chain.serialize(serializer),
174        }
175    }
176}
177
178impl<'de, const N: usize> Deserialize<'de> for CuChainSpec<N> {
179    fn deserialize<D: Deserializer<'de>>(deserializer: D) -> Result<Self, D::Error> {
180        use serde::de::IntoDeserializer;
181        use serde::de::MapAccess;
182        use serde::de::Visitor;
183        use serde::de::value::MapAccessDeserializer;
184
185        struct SpecVisitor<const M: usize>;
186        impl<'de, const M: usize> Visitor<'de> for SpecVisitor<M> {
187            type Value = CuChainSpec<M>;
188            fn expecting(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
189                write!(
190                    f,
191                    "a robot preset name (\"igus_rebel\") or an inline chain {{\"joints\": [...]}}"
192                )
193            }
194            fn visit_str<E: serde::de::Error>(self, v: &str) -> Result<Self::Value, E> {
195                CuRobotPreset::deserialize(v.into_deserializer()).map(CuChainSpec::Preset)
196            }
197            fn visit_map<A: MapAccess<'de>>(self, map: A) -> Result<Self::Value, A::Error> {
198                CuKinematicChain::deserialize(MapAccessDeserializer::new(map))
199                    .map(CuChainSpec::Inline)
200            }
201        }
202        deserializer.deserialize_any(SpecVisitor::<N>)
203    }
204}
repose_kinematics/robots.rs

repose_kinematics/
robots.rs
