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

use core::f64::consts::PI;
use posture::cartesian::{rotation::Euler, CartesianPose, Position, Rotation};

#[test]
fn mul() {
    let base = CartesianPose {
        position: Position::new(0.0, 0.0, 0.0),
        rotation: Rotation::Euler(Euler::new(0.0, 0.0, PI / 4.0)),
    };

    let target1 = CartesianPose {
        position: Position::new(2.0f64.powf(0.5), 0.0, 0.0),
        rotation: Rotation::Euler(Euler::new(0.0, 0.0, 0.0)),
    };
    let _expect1 = CartesianPose {
        position: Position::new(1.0, 1.0, 0.0),
        rotation: Rotation::Euler(Euler::new(0.0, 0.0, PI / 4.0)),
    };
    let target = base.clone() * target1;
    println!("position1: {:?}", target.position);
    println!(
        "rotation1: {:?}",
        Euler::from_matrix(target.rotation.into())
    );

    let target2 = CartesianPose {
        position: Position::new(0.0, 2.0f64.powf(0.5), 0.0),
        rotation: Rotation::Euler(Euler::new(0.0, 0.0, 0.0)),
    };
    let _expect2 = CartesianPose {
        position: Position::new(-1.0, 1.0, 0.0),
        rotation: Rotation::Euler(Euler::new(0.0, 0.0, PI / 4.0)),
    };
    let target = base.clone() * target2;
    println!("position2: {:?}", target.position);
    println!(
        "rotation2: {:?}",
        Euler::from_matrix(target.rotation.into())
    );

    let target3 = CartesianPose {
        position: Position::new(0.0, 0.0, 2.0f64.powf(0.5)),
        rotation: Rotation::Euler(Euler::new(0.0, 0.0, 0.0)),
    };
    let _expect3 = CartesianPose {
        position: Position::new(0.0, 0.0, 2.0f64.powf(0.5)),
        rotation: Rotation::Euler(Euler::new(0.0, 0.0, PI / 4.0)),
    };
    let target = base.clone() * target3;
    println!("position3: {:?}", target.position);
    println!(
        "rotation3: {:?}",
        Euler::from_matrix(target.rotation.into())
    );
}