dynamics_joint/
joint.rs

1//! Defines a generic joint model trait and a struct to wrap different joint types.
2//!
3//! This module provides both:
4//! * The `JointModel` trait, which defines the common interface for different joint types.
5//! * The `JointWrapper` struct, which encapsulates different joint model implementations and provides a
6//!   unified interface to interact with them.
7
8use std::fmt::Display;
9
10use crate::{
11    continuous::JointModelContinuous, fixed::JointModelFixed, joint_data::JointDataWrapper,
12    prismatic::JointModelPrismatic, revolute::JointModelRevolute,
13};
14use dynamics_spatial::{
15    configuration::Configuration, force::SpatialForce, motion::SpatialMotion, se3::SE3,
16};
17use rand::rngs::ThreadRng;
18
19#[derive(Clone, Debug)]
20/// Enum encapsulating different joint model implementations.
21///
22/// It serves as the inner representation for the `JointWrapper` struct.
23/// As such, users should interact with joints through the `JointWrapper` interface,
24/// and avoid using this enum directly.
25enum JointModelImpl {
26    Continuous(JointModelContinuous),
27    Prismatic(JointModelPrismatic),
28    Revolute(JointModelRevolute),
29    Fixed(JointModelFixed),
30}
31
32#[derive(Clone, Debug)]
33/// Wrapper struct for different joint model implementations.
34///
35/// This struct provides a unified interface to interact with different joint types
36/// through the `JointModel` trait. It serves as the main entry point for users of
37/// the library to work with joints.
38pub struct JointWrapper {
39    inner: JointModelImpl,
40}
41
42impl JointWrapper {
43    /// Creates a new `JointWrapper` from a `JointModelContinuous`.
44    pub fn continuous(joint: JointModelContinuous) -> Self {
45        JointWrapper {
46            inner: JointModelImpl::Continuous(joint),
47        }
48    }
49
50    /// Creates a new `JointWrapper` from a `JointModelPrismatic`.
51    pub fn prismatic(joint: JointModelPrismatic) -> Self {
52        JointWrapper {
53            inner: JointModelImpl::Prismatic(joint),
54        }
55    }
56
57    /// Creates a new `JointWrapper` from a `JointModelRevolute`.
58    pub fn revolute(joint: JointModelRevolute) -> Self {
59        JointWrapper {
60            inner: JointModelImpl::Revolute(joint),
61        }
62    }
63
64    /// Creates a new `JointWrapper` from a `JointModelFixed`.
65    pub fn fixed(joint: JointModelFixed) -> Self {
66        JointWrapper {
67            inner: JointModelImpl::Fixed(joint),
68        }
69    }
70}
71
72// The following is boilerplate to forward JointModel trait methods to the inner joint model.
73impl JointModel for JointWrapper {
74    fn get_joint_type(&self) -> JointType {
75        match &self.inner {
76            JointModelImpl::Continuous(joint) => joint.get_joint_type(),
77            JointModelImpl::Prismatic(joint) => joint.get_joint_type(),
78            JointModelImpl::Revolute(joint) => joint.get_joint_type(),
79            JointModelImpl::Fixed(joint) => joint.get_joint_type(),
80        }
81    }
82
83    fn nq(&self) -> usize {
84        match &self.inner {
85            JointModelImpl::Continuous(joint) => joint.nq(),
86            JointModelImpl::Prismatic(joint) => joint.nq(),
87            JointModelImpl::Revolute(joint) => joint.nq(),
88            JointModelImpl::Fixed(joint) => joint.nq(),
89        }
90    }
91
92    fn nv(&self) -> usize {
93        match &self.inner {
94            JointModelImpl::Continuous(joint) => joint.nv(),
95            JointModelImpl::Prismatic(joint) => joint.nv(),
96            JointModelImpl::Revolute(joint) => joint.nv(),
97            JointModelImpl::Fixed(joint) => joint.nv(),
98        }
99    }
100
101    fn neutral(&self) -> Configuration {
102        match &self.inner {
103            JointModelImpl::Continuous(joint) => joint.neutral(),
104            JointModelImpl::Prismatic(joint) => joint.neutral(),
105            JointModelImpl::Revolute(joint) => joint.neutral(),
106            JointModelImpl::Fixed(joint) => joint.neutral(),
107        }
108    }
109
110    fn create_joint_data(&self) -> JointDataWrapper {
111        match &self.inner {
112            JointModelImpl::Continuous(joint) => joint.create_joint_data(),
113            JointModelImpl::Prismatic(joint) => joint.create_joint_data(),
114            JointModelImpl::Revolute(joint) => joint.create_joint_data(),
115            JointModelImpl::Fixed(joint) => joint.create_joint_data(),
116        }
117    }
118
119    fn random_configuration(&self, rng: &mut ThreadRng) -> Configuration {
120        match &self.inner {
121            JointModelImpl::Continuous(joint) => joint.random_configuration(rng),
122            JointModelImpl::Prismatic(joint) => joint.random_configuration(rng),
123            JointModelImpl::Revolute(joint) => joint.random_configuration(rng),
124            JointModelImpl::Fixed(joint) => joint.random_configuration(rng),
125        }
126    }
127
128    fn get_axis(&self) -> &SpatialMotion {
129        match &self.inner {
130            JointModelImpl::Continuous(joint) => joint.get_axis(),
131            JointModelImpl::Prismatic(joint) => joint.get_axis(),
132            JointModelImpl::Revolute(joint) => joint.get_axis(),
133            JointModelImpl::Fixed(joint) => joint.get_axis(),
134        }
135    }
136
137    fn subspace(&self, v: &Configuration) -> SpatialMotion {
138        match &self.inner {
139            JointModelImpl::Continuous(joint) => joint.subspace(v),
140            JointModelImpl::Prismatic(joint) => joint.subspace(v),
141            JointModelImpl::Revolute(joint) => joint.subspace(v),
142            JointModelImpl::Fixed(joint) => joint.subspace(v),
143        }
144    }
145
146    fn subspace_dual(&self, f: &SpatialForce) -> Configuration {
147        match &self.inner {
148            JointModelImpl::Continuous(joint) => joint.subspace_dual(f),
149            JointModelImpl::Prismatic(joint) => joint.subspace_dual(f),
150            JointModelImpl::Revolute(joint) => joint.subspace_dual(f),
151            JointModelImpl::Fixed(joint) => joint.subspace_dual(f),
152        }
153    }
154
155    fn subspace_se3(&self, se3: &SE3) -> SpatialMotion {
156        match &self.inner {
157            JointModelImpl::Continuous(joint) => joint.subspace_se3(se3),
158            JointModelImpl::Prismatic(joint) => joint.subspace_se3(se3),
159            JointModelImpl::Revolute(joint) => joint.subspace_se3(se3),
160            JointModelImpl::Fixed(joint) => joint.subspace_se3(se3),
161        }
162    }
163
164    fn bias(&self) -> &SpatialMotion {
165        match &self.inner {
166            JointModelImpl::Continuous(joint) => joint.bias(),
167            JointModelImpl::Prismatic(joint) => joint.bias(),
168            JointModelImpl::Revolute(joint) => joint.bias(),
169            JointModelImpl::Fixed(joint) => joint.bias(),
170        }
171    }
172
173    fn integrate(&self, q: &Configuration, v: &Configuration) -> Configuration {
174        match &self.inner {
175            JointModelImpl::Continuous(joint) => joint.integrate(q, v),
176            JointModelImpl::Prismatic(joint) => joint.integrate(q, v),
177            JointModelImpl::Revolute(joint) => joint.integrate(q, v),
178            JointModelImpl::Fixed(joint) => joint.integrate(q, v),
179        }
180    }
181}
182
183impl Display for JointWrapper {
184    fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
185        match &self.inner {
186            JointModelImpl::Continuous(joint) => write!(f, "{joint}"),
187            JointModelImpl::Prismatic(joint) => write!(f, "{joint}"),
188            JointModelImpl::Revolute(joint) => write!(f, "{joint}"),
189            JointModelImpl::Fixed(joint) => write!(f, "{joint}"),
190        }
191    }
192}
193
194/// Joint trait for defining joints in a robotic system.
195///
196/// This trait provides a common interface for different joint types,
197/// allowing for polymorphic behavior when working with various joint models.
198pub trait JointModel {
199    /// Returns the joint type.
200    fn get_joint_type(&self) -> JointType;
201
202    /// Returns the number of position variables.
203    fn nq(&self) -> usize;
204
205    /// Returns the number of velocity variables.
206    fn nv(&self) -> usize;
207
208    /// Returns the neutral configuration of the joint.
209    fn neutral(&self) -> Configuration;
210
211    /// Creates the joint data.
212    fn create_joint_data(&self) -> JointDataWrapper;
213
214    /// Returns the axis of the joint, if applicable.
215    fn get_axis(&self) -> &SpatialMotion;
216
217    /// Returns a random configuration for the joint.
218    fn random_configuration(&self, rng: &mut ThreadRng) -> Configuration;
219
220    /// Applies the joint subspace constraint to obtain the motion associated with a given velocity configuration.
221    fn subspace(&self, v: &Configuration) -> SpatialMotion;
222
223    /// Applies the dual of the joint subspace constraint to obtain the force/torque associated with a given spatial force.
224    fn subspace_dual(&self, f: &SpatialForce) -> Configuration;
225
226    /// Returns the joint bias (Coriolis and centrifugal effects).
227    fn bias(&self) -> &SpatialMotion;
228
229    /// Applies the joint subspace constraint to a given SE3 transformation, returning the resulting spatial motion.
230    fn subspace_se3(&self, se3: &SE3) -> SpatialMotion;
231
232    /// Integrates the joint configuration given the current configuration and velocity.
233    fn integrate(&self, q: &Configuration, v: &Configuration) -> Configuration;
234}
235
236/// Enum representing the type of joint.
237#[cfg_attr(feature = "python", pyo3::prelude::pyclass)]
238#[derive(Clone, Copy, Debug, PartialEq, Eq)]
239pub enum JointType {
240    /// A continuous joint, which allows for rotation around a specified axis without limits.
241    Continuous,
242    /// A fixed joint, which does not allow for any relative motion between the connected bodies.
243    Fixed,
244    /// A prismatic joint, which allows for translation along a specified axis.
245    Prismatic,
246    /// A revolute joint, which allows for rotation around a specified axis, usually with limits.
247    Revolute,
248}
249
250/// Type alias for joint bias, represented as a spatial motion.
251///
252/// A joint bias typically represents the Coriolis and centrifugal effects
253/// and are computed during the joint kinematics.
254pub type JointBias = SpatialMotion;
dynamics_joint/joint.rs

dynamics_joint/
joint.rs
