repose_kinematics/
joint.rs

1//! Typed coordinates and bounds for serial revolute, prismatic, and fixed joints.
2
3use bincode::Decode;
4use bincode::Encode;
5use cu29::prelude::*;
6use cu29::units::si::acceleration::meter_per_second_squared;
7use cu29::units::si::angle::radian;
8use cu29::units::si::angular_acceleration::radian_per_second_squared;
9use cu29::units::si::angular_jerk::radian_per_second_cubed;
10use cu29::units::si::angular_velocity::radian_per_second;
11use cu29::units::si::f64::Acceleration;
12use cu29::units::si::f64::Angle;
13use cu29::units::si::f64::AngularAcceleration;
14use cu29::units::si::f64::AngularJerk;
15use cu29::units::si::f64::AngularVelocity;
16use cu29::units::si::f64::Jerk;
17use cu29::units::si::f64::Length;
18use cu29::units::si::f64::Velocity;
19use cu29::units::si::jerk::meter_per_second_cubed;
20use cu29::units::si::length::meter;
21use cu29::units::si::velocity::meter_per_second;
22use serde::Deserialize;
23use serde::Serialize;
24
25/// A joint coordinate carries the physical unit of its joint.
26#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
27#[serde(rename_all = "snake_case")]
28pub enum CuJointPosition {
29    Revolute(Angle),
30    Prismatic(Length),
31    Fixed,
32}
33
34impl Default for CuJointPosition {
35    fn default() -> Self {
36        Self::Revolute(Angle::default())
37    }
38}
39
40impl From<Angle> for CuJointPosition {
41    fn from(value: Angle) -> Self {
42        Self::Revolute(value)
43    }
44}
45
46impl From<Length> for CuJointPosition {
47    fn from(value: Length) -> Self {
48        Self::Prismatic(value)
49    }
50}
51
52impl CuJointPosition {
53    /// Extracts an angle, rejecting linear and fixed coordinates.
54    pub fn angle(self) -> CuResult<Angle> {
55        match self {
56            Self::Revolute(v) => Ok(v),
57            _ => Err(CuError::from("Expected revolute coordinate")),
58        }
59    }
60    /// Extracts a displacement, rejecting angular and fixed coordinates.
61    pub fn displacement(self) -> CuResult<Length> {
62        match self {
63            Self::Prismatic(v) => Ok(v),
64            _ => Err(CuError::from("Expected prismatic coordinate")),
65        }
66    }
67    /// Returns the coordinate in SI base units (radians or meters; `0.0` for fixed joints).
68    ///
69    /// Intended for numerical kernels that operate on dimensionless arrays; convert back with
70    /// [`Self::with_si_value`] so the unit tag is never lost.
71    pub fn si_value(self) -> f64 {
72        match self {
73            Self::Revolute(v) => v.get::<radian>(),
74            Self::Prismatic(v) => v.get::<meter>(),
75            Self::Fixed => 0.0,
76        }
77    }
78    /// Returns a coordinate of the same joint kind holding `value` in SI base units
79    /// (radians or meters). Fixed coordinates stay fixed.
80    pub fn with_si_value(self, value: f64) -> Self {
81        match self {
82            Self::Revolute(_) => Self::Revolute(Angle::new::<radian>(value)),
83            Self::Prismatic(_) => Self::Prismatic(Length::new::<meter>(value)),
84            Self::Fixed => Self::Fixed,
85        }
86    }
87    /// Interpolates between coordinates of the same joint type.
88    pub fn interpolate(self, other: Self, fraction: f64) -> CuResult<Self> {
89        if core::mem::discriminant(&self) != core::mem::discriminant(&other)
90            || !fraction.is_finite()
91        {
92            return Err(CuError::from(
93                "Incompatible joint coordinates or interpolation fraction",
94            ));
95        }
96        Ok(self.with_si_value(self.si_value() + fraction * (other.si_value() - self.si_value())))
97    }
98}
99
100/// A joint velocity carries angular or linear velocity units.
101#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
102#[serde(rename_all = "snake_case")]
103pub enum CuJointVelocity {
104    Revolute(AngularVelocity),
105    Prismatic(Velocity),
106    Fixed,
107}
108impl Default for CuJointVelocity {
109    fn default() -> Self {
110        Self::Revolute(AngularVelocity::default())
111    }
112}
113impl CuJointVelocity {
114    /// Creates a velocity of the same joint kind as `position` from an SI value
115    /// (rad/s or m/s). Fixed joints yield [`CuJointVelocity::Fixed`].
116    pub fn from_coordinate(position: CuJointPosition, value: f64) -> Self {
117        match position {
118            CuJointPosition::Revolute(_) => {
119                Self::Revolute(AngularVelocity::new::<radian_per_second>(value))
120            }
121            CuJointPosition::Prismatic(_) => {
122                Self::Prismatic(Velocity::new::<meter_per_second>(value))
123            }
124            CuJointPosition::Fixed => Self::Fixed,
125        }
126    }
127    /// Returns the velocity in SI base units (rad/s or m/s; `0.0` for fixed joints).
128    pub fn si_value(self) -> f64 {
129        match self {
130            Self::Revolute(v) => v.get::<radian_per_second>(),
131            Self::Prismatic(v) => v.get::<meter_per_second>(),
132            Self::Fixed => 0.0,
133        }
134    }
135}
136
137/// Position and velocity bounds, with units determined by joint type.
138#[derive(
139    Default, Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect,
140)]
141#[serde(rename_all = "snake_case")]
142pub enum CuJointLimits {
143    Revolute {
144        lower: Angle,
145        upper: Angle,
146        max_velocity: AngularVelocity,
147    },
148    Prismatic {
149        lower: Length,
150        upper: Length,
151        max_velocity: Velocity,
152    },
153    /// Unbounded rotation; finite bounds returned by `lower`/`upper` are a sampling interval.
154    Continuous { max_velocity: AngularVelocity },
155    #[default]
156    Fixed,
157}
158impl CuJointLimits {
159    /// Creates bounded revolute limits.
160    pub const fn revolute(lower: Angle, upper: Angle, max_velocity: AngularVelocity) -> Self {
161        Self::Revolute {
162            lower,
163            upper,
164            max_velocity,
165        }
166    }
167    /// Creates bounded prismatic limits.
168    pub const fn prismatic(lower: Length, upper: Length, max_velocity: Velocity) -> Self {
169        Self::Prismatic {
170            lower,
171            upper,
172            max_velocity,
173        }
174    }
175    /// Lower position bound (`-pi` sampling bound for continuous joints).
176    pub fn lower(self) -> CuJointPosition {
177        match self {
178            Self::Revolute { lower, .. } => lower.into(),
179            Self::Prismatic { lower, .. } => lower.into(),
180            Self::Continuous { .. } => Angle::new::<radian>(-core::f64::consts::PI).into(),
181            Self::Fixed => CuJointPosition::Fixed,
182        }
183    }
184    /// Upper position bound (`+pi` sampling bound for continuous joints).
185    pub fn upper(self) -> CuJointPosition {
186        match self {
187            Self::Revolute { upper, .. } => upper.into(),
188            Self::Prismatic { upper, .. } => upper.into(),
189            Self::Continuous { .. } => Angle::new::<radian>(core::f64::consts::PI).into(),
190            Self::Fixed => CuJointPosition::Fixed,
191        }
192    }
193    /// Maximum absolute joint velocity.
194    pub fn max_velocity(self) -> CuJointVelocity {
195        match self {
196            Self::Revolute { max_velocity, .. } => CuJointVelocity::Revolute(max_velocity),
197            Self::Prismatic { max_velocity, .. } => CuJointVelocity::Prismatic(max_velocity),
198            Self::Continuous { max_velocity } => CuJointVelocity::Revolute(max_velocity),
199            Self::Fixed => CuJointVelocity::Fixed,
200        }
201    }
202    /// Checks that `q` has this joint's unit kind and that the limits themselves are well formed.
203    pub fn validate(self, q: CuJointPosition) -> CuResult<()> {
204        if core::mem::discriminant(&q) != core::mem::discriminant(&self.lower())
205            || !q.si_value().is_finite()
206        {
207            return Err(CuError::from(
208                "Joint coordinate has incorrect units or is non-finite",
209            ));
210        }
211        let lo = self.lower().si_value();
212        let hi = self.upper().si_value();
213        let velocity = self.max_velocity().si_value();
214        if !lo.is_finite() || !hi.is_finite() || lo > hi || !velocity.is_finite() || velocity < 0.0
215        {
216            return Err(CuError::from("Invalid joint limits"));
217        }
218        Ok(())
219    }
220    /// Returns `true` if `q` is valid and inside the bounds (always inside for continuous joints).
221    pub fn contains(self, q: CuJointPosition) -> bool {
222        self.validate(q).is_ok()
223            && (matches!(self, Self::Continuous { .. })
224                || (q.si_value() >= self.lower().si_value()
225                    && q.si_value() <= self.upper().si_value()))
226    }
227    /// Clamps `q` to the bounds (continuous joints are returned unchanged).
228    pub fn clamp(self, q: CuJointPosition) -> CuResult<CuJointPosition> {
229        self.validate(q)?;
230        if matches!(self, Self::Continuous { .. }) {
231            return Ok(q);
232        }
233        Ok(q.with_si_value(
234            q.si_value()
235                .clamp(self.lower().si_value(), self.upper().si_value()),
236        ))
237    }
238}
239
240/// A joint acceleration carries angular (rad/s²) or linear (m/s²) acceleration units.
241#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
242#[serde(rename_all = "snake_case")]
243pub enum CuJointAcceleration {
244    Revolute(AngularAcceleration),
245    Prismatic(Acceleration),
246    Fixed,
247}
248impl Default for CuJointAcceleration {
249    fn default() -> Self {
250        Self::Revolute(AngularAcceleration::default())
251    }
252}
253impl From<AngularAcceleration> for CuJointAcceleration {
254    fn from(value: AngularAcceleration) -> Self {
255        Self::Revolute(value)
256    }
257}
258impl From<Acceleration> for CuJointAcceleration {
259    fn from(value: Acceleration) -> Self {
260        Self::Prismatic(value)
261    }
262}
263impl CuJointAcceleration {
264    /// Creates a value of the same joint kind as `position` from an SI value
265    /// (rad/s² or m/s²). Fixed joints yield `Fixed`.
266    pub fn from_coordinate(position: CuJointPosition, value: f64) -> Self {
267        match position {
268            CuJointPosition::Revolute(_) => {
269                Self::Revolute(AngularAcceleration::new::<radian_per_second_squared>(value))
270            }
271            CuJointPosition::Prismatic(_) => {
272                Self::Prismatic(Acceleration::new::<meter_per_second_squared>(value))
273            }
274            CuJointPosition::Fixed => Self::Fixed,
275        }
276    }
277    /// Returns the value in SI base units (rad/s² or m/s²; `0.0` for fixed joints).
278    pub fn si_value(self) -> f64 {
279        match self {
280            Self::Revolute(v) => v.get::<radian_per_second_squared>(),
281            Self::Prismatic(v) => v.get::<meter_per_second_squared>(),
282            Self::Fixed => 0.0,
283        }
284    }
285}
286
287/// A joint jerk carries angular (rad/s³) or linear (m/s³) jerk units.
288///
289/// An unbounded jerk (for example a jerk limit that should not constrain) is expressed as
290/// `f64::INFINITY` inside the unit value, e.g.
291/// `CuJointJerk::Revolute(AngularJerk::new::<radian_per_second_cubed>(f64::INFINITY))`;
292/// consumers may alternatively use `Option<CuJointJerk>` at the use site.
293#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
294#[serde(rename_all = "snake_case")]
295pub enum CuJointJerk {
296    Revolute(AngularJerk),
297    Prismatic(Jerk),
298    Fixed,
299}
300impl Default for CuJointJerk {
301    fn default() -> Self {
302        Self::Revolute(AngularJerk::default())
303    }
304}
305impl From<AngularJerk> for CuJointJerk {
306    fn from(value: AngularJerk) -> Self {
307        Self::Revolute(value)
308    }
309}
310impl From<Jerk> for CuJointJerk {
311    fn from(value: Jerk) -> Self {
312        Self::Prismatic(value)
313    }
314}
315impl CuJointJerk {
316    /// Creates a value of the same joint kind as `position` from an SI value
317    /// (rad/s³ or m/s³). Fixed joints yield `Fixed`.
318    pub fn from_coordinate(position: CuJointPosition, value: f64) -> Self {
319        match position {
320            CuJointPosition::Revolute(_) => {
321                Self::Revolute(AngularJerk::new::<radian_per_second_cubed>(value))
322            }
323            CuJointPosition::Prismatic(_) => {
324                Self::Prismatic(Jerk::new::<meter_per_second_cubed>(value))
325            }
326            CuJointPosition::Fixed => Self::Fixed,
327        }
328    }
329    /// Returns the value in SI base units (rad/s³ or m/s³; `0.0` for fixed joints).
330    pub fn si_value(self) -> f64 {
331        match self {
332            Self::Revolute(v) => v.get::<radian_per_second_cubed>(),
333            Self::Prismatic(v) => v.get::<meter_per_second_cubed>(),
334            Self::Fixed => 0.0,
335        }
336    }
337}
338
339/// Physical scaling used to compare angular and linear coordinates without mixing units.
340#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
341pub struct CuJointMetric {
342    pub angular_scale: Angle,
343    pub linear_scale: Length,
344}
345impl Default for CuJointMetric {
346    fn default() -> Self {
347        Self {
348            angular_scale: Angle::new::<radian>(1.0),
349            linear_scale: Length::new::<meter>(1.0),
350        }
351    }
352}
353impl CuJointMetric {
354    /// Checks that both scales are positive and finite.
355    pub fn validate(self) -> CuResult<()> {
356        let a = self.angular_scale.get::<radian>();
357        let l = self.linear_scale.get::<meter>();
358        if !a.is_finite() || a <= 0.0 || !l.is_finite() || l <= 0.0 {
359            return Err(CuError::from(
360                "Joint metric scales must be positive and finite",
361            ));
362        }
363        Ok(())
364    }
365    /// Returns a dimensionless joint coordinate for numerical algorithms.
366    pub fn normalized(self, q: CuJointPosition) -> f64 {
367        q.si_value() / self.scale(q)
368    }
369    /// Returns the SI scale (radians or meters) that normalizes coordinates of `q`'s joint kind;
370    /// `1.0` for fixed joints.
371    pub fn scale(self, q: CuJointPosition) -> f64 {
372        match q {
373            CuJointPosition::Revolute(_) => self.angular_scale.get::<radian>(),
374            CuJointPosition::Prismatic(_) => self.linear_scale.get::<meter>(),
375            CuJointPosition::Fixed => 1.0,
376        }
377    }
378}
379
380/// Per-joint sampling resolution for discrete collision checks.
381#[derive(Debug, Clone, Copy, PartialEq, Encode, Decode, Serialize, Deserialize, Reflect)]
382pub struct CuJointResolution {
383    pub angular: Angle,
384    pub linear: Length,
385}
386impl Default for CuJointResolution {
387    fn default() -> Self {
388        Self {
389            angular: Angle::new::<radian>(0.08),
390            linear: Length::new::<meter>(0.01),
391        }
392    }
393}
394impl From<Angle> for CuJointResolution {
395    fn from(angular: Angle) -> Self {
396        Self {
397            angular,
398            ..Self::default()
399        }
400    }
401}
402impl CuJointResolution {
403    /// Uses the resolution as a joint-space metric.
404    pub fn metric(self) -> CuJointMetric {
405        CuJointMetric {
406            angular_scale: self.angular,
407            linear_scale: self.linear,
408        }
409    }
410}
repose_kinematics/joint.rs

repose_kinematics/
joint.rs
