k/joint/
joint.rs

Help
1/*
2  Copyright 2017 Takashi Ogura
3
4  Licensed under the Apache License, Version 2.0 (the "License");
5  you may not use this file except in compliance with the License.
6  You may obtain a copy of the License at
7
8      http://www.apache.org/licenses/LICENSE-2.0
9
10  Unless required by applicable law or agreed to in writing, software
11  distributed under the License is distributed on an "AS IS" BASIS,
12  WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  See the License for the specific language governing permissions and
14  limitations under the License.
15*/
16use super::joint_type::*;
17use super::range::*;
18use super::velocity::*;
19use crate::errors::*;
20use na::{Isometry3, RealField, Translation3, UnitQuaternion};
21use nalgebra as na;
22use simba::scalar::SubsetOf;
23use std::cell::RefCell;
24use std::fmt::{self, Display};
25
26/// Joint with type
27#[derive(Debug, Clone)]
28pub struct Joint<T: RealField> {
29    /// Name of this joint
30    pub name: String,
31    /// Type of this joint
32    pub joint_type: JointType<T>,
33    /// position (angle) of this joint
34    position: T,
35    /// velocity of this joint
36    velocity: T,
37    /// Limits of this joint
38    pub limits: Option<Range<T>>,
39    /// local origin transform of joint
40    origin: Isometry3<T>,
41    /// cache of world transform
42    world_transform_cache: RefCell<Option<Isometry3<T>>>,
43    /// cache of world velocity
44    world_velocity_cache: RefCell<Option<Velocity<T>>>,
45}
46
47impl<T> Joint<T>
48where
49    T: RealField + SubsetOf<f64>,
50{
51    /// Create new Joint with name and type
52    ///
53    /// # Examples
54    ///
55    /// ```
56    /// use nalgebra as na;
57    ///
58    /// // create fixed joint
59    /// let fixed = k::Joint::<f32>::new("f0", k::JointType::Fixed);
60    /// assert!(fixed.joint_position().is_none());
61    ///
62    /// // create rotational joint with Y-axis
63    /// let rot = k::Joint::<f64>::new("r0", k::JointType::Rotational { axis: na::Vector3::y_axis() });
64    /// assert_eq!(rot.joint_position().unwrap(), 0.0);
65    /// ```
66    ///
67    pub fn new(name: &str, joint_type: JointType<T>) -> Joint<T> {
68        Joint {
69            name: name.to_string(),
70            joint_type,
71            position: T::zero(),
72            velocity: T::zero(),
73            limits: None,
74            origin: Isometry3::identity(),
75            world_transform_cache: RefCell::new(None),
76            world_velocity_cache: RefCell::new(None),
77        }
78    }
79    /// Set the position of the joint
80    ///
81    /// It returns Err if it is out of the limits, or this is fixed joint.
82    ///
83    /// # Examples
84    ///
85    /// ```
86    /// use nalgebra as na;
87    ///
88    /// // Create fixed joint
89    /// let mut fixed = k::Joint::<f32>::new("f0", k::JointType::Fixed);
90    /// // Set position to fixed joint always fails
91    /// assert!(fixed.set_joint_position(1.0).is_err());
92    ///
93    /// // Create rotational joint with Y-axis
94    /// let mut rot = k::Joint::<f64>::new("r0", k::JointType::Rotational { axis: na::Vector3::y_axis() });
95    /// // As default, it has not limit
96    ///
97    /// // Initial position is 0.0
98    /// assert_eq!(rot.joint_position().unwrap(), 0.0);
99    /// // If it has no limits, set_joint_position always succeeds.
100    /// rot.set_joint_position(0.2).unwrap();
101    /// assert_eq!(rot.joint_position().unwrap(), 0.2);
102    /// ```
103    ///
104    pub fn set_joint_position(&mut self, position: T) -> Result<(), Error> {
105        if !self.is_movable() {
106            return Err(Error::SetToFixedError {
107                joint_name: self.name.to_string(),
108            });
109        }
110        if let Some(ref range) = self.limits {
111            if !range.is_valid(position.clone()) {
112                return Err(Error::OutOfLimitError {
113                    joint_name: self.name.to_string(),
114                    position: na::try_convert(position).unwrap_or_default(),
115                    max_limit: na::try_convert(range.max.clone()).unwrap_or_default(),
116                    min_limit: na::try_convert(range.min.clone()).unwrap_or_default(),
117                });
118            }
119        }
120        self.position = position;
121        self.clear_caches();
122        Ok(())
123    }
124    /// Set the clamped position of the joint
125    ///
126    /// It refers to the joint limit and clamps the argument. This function does nothing if this is fixed joint.
127    ///
128    /// # Examples
129    ///
130    /// ```
131    /// use nalgebra as na;
132    ///
133    /// // Create rotational joint with Y-axis
134    /// let mut rot = k::Joint::<f64>::new("r0", k::JointType::Rotational { axis: na::Vector3::y_axis() });
135    ///
136    /// let limits = k::joint::Range::new(-1.0, 1.0);
137    /// rot.limits = Some(limits);
138    ///
139    /// // Initial position is 0.0
140    /// assert_eq!(rot.joint_position().unwrap(), 0.0);
141    /// rot.set_joint_position_clamped(2.0);
142    /// assert_eq!(rot.joint_position().unwrap(), 1.0);
143    /// rot.set_joint_position_clamped(-2.0);
144    /// assert_eq!(rot.joint_position().unwrap(), -1.0);
145    /// ```
146    ///
147    pub fn set_joint_position_clamped(&mut self, position: T) {
148        if !self.is_movable() {
149            return;
150        }
151        let position_clamped = if let Some(ref range) = self.limits {
152            range.clamp(position)
153        } else {
154            position
155        };
156        self.set_joint_position_unchecked(position_clamped);
157    }
158    pub fn set_joint_position_unchecked(&mut self, position: T) {
159        self.position = position;
160        self.clear_caches();
161    }
162    /// Returns the position (angle)
163    #[inline]
164    pub fn joint_position(&self) -> Option<T> {
165        match self.joint_type {
166            JointType::Fixed => None,
167            _ => Some(self.position.clone()),
168        }
169    }
170
171    #[inline]
172    pub fn origin(&self) -> &Isometry3<T> {
173        &self.origin
174    }
175
176    #[inline]
177    pub fn set_origin(&mut self, origin: Isometry3<T>) {
178        self.origin = origin;
179        self.clear_caches();
180    }
181
182    pub fn set_joint_velocity(&mut self, velocity: T) -> Result<(), Error> {
183        if let JointType::Fixed = self.joint_type {
184            return Err(Error::SetToFixedError {
185                joint_name: self.name.to_string(),
186            });
187        }
188        self.velocity = velocity;
189        self.world_velocity_cache.replace(None);
190        Ok(())
191    }
192
193    /// Returns the velocity
194    #[inline]
195    pub fn joint_velocity(&self) -> Option<T> {
196        match self.joint_type {
197            JointType::Fixed => None,
198            _ => Some(self.velocity.clone()),
199        }
200    }
201
202    /// Calculate and returns the transform of the end of this joint
203    ///
204    /// # Examples
205    ///
206    /// ```
207    /// use nalgebra as na;
208    ///
209    /// // Create linear joint with X-axis
210    /// let mut lin = k::Joint::<f64>::new("l0", k::JointType::Linear { axis: na::Vector3::x_axis() });
211    /// assert_eq!(lin.local_transform().translation.vector.x, 0.0);
212    /// lin.set_joint_position(-1.0).unwrap();
213    /// assert_eq!(lin.local_transform().translation.vector.x, -1.0);
214    /// ```
215    ///
216    pub fn local_transform(&self) -> Isometry3<T> {
217        let joint_transform = match &self.joint_type {
218            JointType::Fixed => Isometry3::identity(),
219            JointType::Rotational { axis } => Isometry3::from_parts(
220                Translation3::new(T::zero(), T::zero(), T::zero()),
221                UnitQuaternion::from_axis_angle(axis, self.position.clone()),
222            ),
223            JointType::Linear { axis } => Isometry3::from_parts(
224                Translation3::from(axis.clone().into_inner() * self.position.clone()),
225                UnitQuaternion::identity(),
226            ),
227        };
228        self.origin.clone() * joint_transform
229    }
230
231    #[inline]
232    pub(crate) fn set_world_transform(&self, world_transform: Isometry3<T>) {
233        self.world_transform_cache.replace(Some(world_transform));
234    }
235
236    #[inline]
237    pub(crate) fn set_world_velocity(&self, world_velocity: Velocity<T>) {
238        self.world_velocity_cache.replace(Some(world_velocity));
239    }
240    /// Get the result of forward kinematics
241    ///
242    /// The value is updated by `Chain::update_transforms`
243    #[inline]
244    pub fn world_transform(&self) -> Option<Isometry3<T>> {
245        self.world_transform_cache.borrow().clone()
246    }
247
248    #[inline]
249    pub fn world_velocity(&self) -> Option<Velocity<T>> {
250        self.world_velocity_cache.borrow().clone()
251    }
252
253    #[inline]
254    pub fn is_movable(&self) -> bool {
255        !matches!(self.joint_type, JointType::Fixed)
256    }
257
258    /// Clear caches defined in the world coordinate
259    #[inline]
260    pub fn clear_caches(&self) {
261        self.world_transform_cache.replace(None);
262        self.world_velocity_cache.replace(None);
263    }
264}
265
266impl<T: RealField> Display for Joint<T> {
267    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
268        write!(f, "{} {}", self.name, self.joint_type)
269    }
270}
k/joint/joint.rs

k/joint/
joint.rs
