ur_script/vars/
pose.rs

1/*!
2# [`Pose`](Pose)
3
4An important data structure required for proper manipulation of both frames of reference and more specifically the position of the robot's tool.
5The `URScript` implementation uses scaled axis angled format, but eventually this will allow other formats to be manipulated as well.
6*/
7
8use core::fmt;
9use core::ops::{Div, DivAssign, Mul, MulAssign};
10
11use nalgebra::RealField;
12use nalgebra::{Isometry3, Vector3};
13
14use num::Float;
15
16/// Pose, a kind of [Isometry](https://en.wikipedia.org/wiki/Isometry) in 3-Dimensions (currently a wrapper of [`Isometry3`] which is itself wrapping a float).
17#[derive(Debug, Default, PartialEq, Clone, Copy)]
18pub struct Pose<T>(Isometry3<T>)
19where
20	T: Float + RealField;
21
22impl<T> Pose<T>
23where
24	T: Float + RealField,
25{
26	/// Creates and initialises a Pose using scaled axis-angle format.
27	pub fn new_pose(x: T, y: T, z: T, ax: T, ay: T, az: T) -> Self {
28		Self(Isometry3::new(
29			Vector3::new(x, y, z),
30			Vector3::new(ax, ay, az),
31		))
32	}
33
34	/// Creates and initialises a Pose with only a position, and no orientation.
35	/// This is especially useful for generating new targets relative to a home position.
36	pub fn new_pos(x: T, y: T, z: T) -> Self {
37		Self(Isometry3::translation(x, y, z))
38	}
39
40	/// Creates and initialises a Pose with only an orientation, and position.
41	/// This is useful for rotating targets without moving them.
42	pub fn new_rot(ax: T, ay: T, az: T) -> Self {
43		Self(Isometry3::rotation(Vector3::new(ax, ay, az)))
44	}
45}
46
47impl<T> fmt::Display for Pose<T>
48where
49	T: Float + RealField,
50{
51	fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
52		let r: _ = self.0.rotation.scaled_axis();
53		write!(
54			f,
55			"p[{},{},{},{},{},{}]",
56			self.0.translation.x.to_f32().unwrap(),
57			self.0.translation.y.to_f32().unwrap(),
58			self.0.translation.z.to_f32().unwrap(),
59			r.x.to_f32().unwrap(),
60			r.y.to_f32().unwrap(),
61			r.z.to_f32().unwrap()
62		)
63	}
64}
65
66impl<T> Div for Pose<T>
67where
68	T: Float + RealField,
69{
70	type Output = Self;
71
72	fn div(self, rhs: Self) -> Self::Output {
73		Self(self.0 / rhs.0)
74	}
75}
76
77impl<T> DivAssign for Pose<T>
78where
79	T: Float + RealField,
80{
81	fn div_assign(&mut self, rhs: Self) {
82		self.0 /= rhs.0;
83	}
84}
85
86impl<T> Mul for Pose<T>
87where
88	T: Float + RealField,
89{
90	type Output = Self;
91
92	fn mul(self, rhs: Self) -> Self::Output {
93		Self(self.0 * rhs.0)
94	}
95}
96
97impl<T> MulAssign for Pose<T>
98where
99	T: Float + RealField,
100{
101	fn mul_assign(&mut self, rhs: Self) {
102		self.0 *= rhs.0;
103	}
104}
105
106#[cfg(test)]
107mod tests {
108	use super::*;
109
110	#[test]
111	fn mul() {
112		use core::f32::consts::PI;
113
114		let h1 = Pose::new_pose(0., 1., 2., 0., PI, 0.);
115		let h2 = Pose::new_pos(0., 2., 4.);
116		let h3 = Pose::new_pose(0., 3., 6., 0., PI, 0.);
117
118		assert_eq!(h3, h2 * h1);
119	}
120}
ur_script/vars/pose.rs

ur_script/vars/
pose.rs
