dynibo 0.5.2

Tree-structured robot kinematics and dynamics with runtime-size workspace APIs
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
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115

mod support;

use support::context::TestRootType as RootType;

use dynibo::{BaseState, FloatingRobot, Frame, Robot, Wrench};
use nalgebra::Vector3;
use support::{
    fixtures::{FLOATING_ARM, LoadSpec, TREE_ARM},
    matrix::MatrixCase,
    operations::{
        deterministic_operation_sequence, run_floating_workspace_sequence, run_workspace_sequence,
    },
    states::{deterministic_base_state, deterministic_joint_state},
};

#[test]
#[allow(clippy::single_element_loop)]
fn fixed_workspace_sequences_match_clean_forks_step_by_step() {
    for (fixture, base_mode, seed) in [(TREE_ARM, RootType::Fixed, 101_u64)] {
        let prototype = fixture.robot(base_mode);
        let target_names: Vec<_> = fixture
            .targets
            .iter()
            .map(|name| (*name).to_owned())
            .collect();
        let load = LoadSpec::new(
            target_names[0].clone(),
            Wrench::new(Vector3::new(0.3, -0.2, 0.4), Vector3::new(1.0, 0.5, -0.7)),
        );
        let cases: Vec<_> = (0..4)
            .map(|sample| {
                let state = deterministic_joint_state(prototype.joint_count(), sample);
                MatrixCase {
                    state,
                    base: BaseState::stationary(Frame::identity()).unwrap(),
                    loads: match sample {
                        0 => vec![load.clone()],
                        1 => vec![],
                        2 => vec![load.clone(), load.clone()],
                        _ => vec![load.clone()],
                    },
                    load_case: match sample {
                        0 => "single",
                        1 => "none",
                        2 => "duplicate",
                        _ => "single-alternate",
                    }
                    .to_owned(),
                }
            })
            .collect();
        let operations = deterministic_operation_sequence(cases.len(), &target_names, seed);

        let foreign = Robot::from_urdf(fixture.path()).unwrap();
        let foreign_link = foreign.link_id(fixture.targets[0]).unwrap();
        run_workspace_sequence(
            &prototype,
            &cases,
            &operations,
            foreign_link,
            fixture.name,
            seed,
        );
    }
}

#[test]
fn floating_workspace_sequences_match_clean_forks_step_by_step() {
    let fixture = FLOATING_ARM;
    let prototype = FloatingRobot::from_urdf(fixture.path()).unwrap();
    let target_names: Vec<_> = fixture
        .targets
        .iter()
        .map(|name| (*name).to_owned())
        .collect();
    let load = LoadSpec::new(
        target_names[0].clone(),
        Wrench::new(Vector3::new(0.3, -0.2, 0.4), Vector3::new(1.0, 0.5, -0.7)),
    );
    let cases: Vec<_> = (0..4)
        .map(|sample| {
            let mut state = deterministic_joint_state(prototype.joint_count(), sample);
            let base_tau = (0..6).map(|index| 0.3 * (sample + index + 1) as f64);
            state.tau = base_tau.chain(state.tau).collect();
            MatrixCase {
                state,
                base: deterministic_base_state(sample),
                loads: match sample {
                    0 => vec![load.clone()],
                    1 => vec![],
                    2 => vec![load.clone(), load.clone()],
                    _ => vec![load.clone()],
                },
                load_case: match sample {
                    0 => "single",
                    1 => "none",
                    2 => "duplicate",
                    _ => "single-alternate",
                }
                .to_owned(),
            }
        })
        .collect();
    let operations = deterministic_operation_sequence(cases.len(), &target_names, 202);
    let foreign = FloatingRobot::from_urdf(fixture.path()).unwrap();
    let foreign_link = foreign.link_id(fixture.targets[0]).unwrap();
    run_floating_workspace_sequence(
        &prototype,
        &cases,
        &operations,
        foreign_link,
        fixture.name,
        202,
    );
}