posture 0.1.0

positional posture of spatial robot
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93

mod position;
pub mod rotation;

use core::ops::Mul;
use nalgebra::{ArrayStorage, Matrix3, Matrix4};
pub use position::Position;
pub use rotation::{Rotation, RotationMatrix};
use serde::{Deserialize, Serialize};

pub type CartesianMatrix = Matrix4<f64>;

#[derive(Debug, Clone, Serialize, Deserialize)]
pub struct CartesianPose {
    pub position: Position,
    pub rotation: Rotation,
}
impl CartesianPose {
    pub fn new(position: Position, rotation: Rotation) -> Self {
        Self { position, rotation }
    }
    pub fn into_matrix(self) -> CartesianMatrix {
        CartesianMatrix::from(self)
    }
    pub fn from_matrix(matrix: CartesianMatrix) -> Self {
        Self::from(matrix)
    }
}

impl From<CartesianPose> for CartesianMatrix {
    fn from(item: CartesianPose) -> Self {
        let rot = item.rotation.into_matrix();
        let Position { x, y, z } = item.position;
        Self::from_data(ArrayStorage([
            [
                *rot.matrix().index((0, 0)),
                *rot.matrix().index((1, 0)),
                *rot.matrix().index((2, 0)),
                0.0,
            ],
            [
                *rot.matrix().index((0, 1)),
                *rot.matrix().index((1, 1)),
                *rot.matrix().index((2, 1)),
                0.0,
            ],
            [
                *rot.matrix().index((0, 2)),
                *rot.matrix().index((1, 2)),
                *rot.matrix().index((2, 2)),
                0.0,
            ],
            [x, y, z, 1.0],
        ]))
    }
}

impl From<CartesianMatrix> for CartesianPose {
    fn from(item: CartesianMatrix) -> Self {
        let position = Position::new(
            *item.index((0, 3)),
            *item.index((1, 3)),
            *item.index((2, 3)),
        );
        let rotation = Matrix3::from_data(ArrayStorage([
            [
                *item.index((0, 0)),
                *item.index((1, 0)),
                *item.index((2, 0)),
            ],
            [
                *item.index((0, 1)),
                *item.index((1, 1)),
                *item.index((2, 1)),
            ],
            [
                *item.index((0, 2)),
                *item.index((1, 2)),
                *item.index((2, 2)),
            ],
        ]));
        let rotation = Rotation::Matrix(RotationMatrix::from_matrix(&rotation));
        Self { position, rotation }
    }
}

impl Mul<CartesianPose> for CartesianPose {
    type Output = CartesianPose;
    fn mul(self, other: CartesianPose) -> Self::Output {
        let rh = CartesianMatrix::from(self);
        let lh = CartesianMatrix::from(other);
        Self::from(rh * lh)
    }
}