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
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134

use crate::{Error, JointType};

use std::path::PathBuf;

use super::*;

fn robot() -> Robot {
    Robot::from_urdf(PathBuf::from(env!("CARGO_MANIFEST_DIR")).join("tests/data/test_arm.urdf"))
        .unwrap()
}

#[test]
fn identifiers_queries_and_forks_preserve_model_scope() {
    let robot = robot();
    let fork = robot.fork();
    assert_eq!(
        robot
            .model
            .validate_link_id(LinkId::new(robot.model.model_id, 0))
            .unwrap(),
        0
    );
    assert!(matches!(
        robot
            .model
            .validate_link_id(LinkId::new(robot.model.model_id.wrapping_add(1), 0)),
        Err(Error::InvalidLinkId)
    ));
    assert!(matches!(
        robot
            .model
            .validate_link_id(LinkId::new(robot.model.model_id, robot.link_count())),
        Err(Error::InvalidLinkId)
    ));
    assert_eq!(robot.root_link_id(), fork.root_link_id());
    assert_eq!(robot.link_id_at(0).unwrap(), robot.root_link_id());
    assert!(matches!(
        robot.link_id_at(robot.link_count()),
        Err(Error::InvalidLinkId)
    ));
    assert_eq!(robot.joint_name(0).unwrap(), "test_joint_1");
    assert_eq!(robot.joint_type(0).unwrap(), JointType::Revolute);
    assert_eq!(robot.joint_lower_limit(0).unwrap(), -0.610865238198015);
    assert_eq!(robot.joint_upper_limit(0).unwrap(), 0.610865238198015);
    assert_eq!(robot.joint_velocity_limit(0).unwrap(), 180.0);
    assert!(matches!(
        robot.joint_name(robot.joint_count()),
        Err(Error::InvalidJointIndex { .. })
    ));
}

#[test]
fn floating_identifiers_and_metadata_match_the_shared_model() {
    let path = PathBuf::from(env!("CARGO_MANIFEST_DIR")).join("tests/data/test_arm.urdf");
    let robot = FloatingRobot::from_urdf(path).unwrap();
    let fork = robot.fork();

    assert_eq!(robot.name(), "test_arm");
    assert_eq!(robot.link_count(), 5);
    assert_eq!(robot.joint_count(), 4);
    assert_eq!(robot.generalized_count(), 10);
    assert_eq!(robot.root_link_id(), fork.root_link_id());

    let link = robot.link_id("test_link_4").unwrap();
    assert_eq!(robot.link_id_at(4).unwrap(), link);
    assert_eq!(robot.link_name(link).unwrap(), "test_link_4");
    assert!(robot.link_mass(link).unwrap().is_finite());
    assert!(
        robot
            .link_center_of_mass(link)
            .unwrap()
            .iter()
            .all(|x| x.is_finite())
    );
    assert!(
        robot
            .link_inertia(link)
            .unwrap()
            .iter()
            .all(|x| x.is_finite())
    );

    assert_eq!(robot.joint_name(0).unwrap(), "test_joint_1");
    assert_eq!(robot.joint_type(0).unwrap(), JointType::Revolute);
    assert_eq!(robot.joint_lower_limit(0).unwrap(), -0.610865238198015);
    assert_eq!(robot.joint_upper_limit(0).unwrap(), 0.610865238198015);
    assert_eq!(robot.joint_velocity_limit(0).unwrap(), 180.0);

    assert!(matches!(
        robot.link_id("missing"),
        Err(Error::UnknownLink { .. })
    ));
    assert!(matches!(
        robot.link_id_at(robot.link_count()),
        Err(Error::InvalidLinkId)
    ));
    assert!(matches!(
        robot.joint_name(robot.joint_count()),
        Err(Error::InvalidJointIndex { .. })
    ));
}

#[test]
fn fixed_base_frame_is_instance_local_and_validated() {
    let mut robot = robot();
    let original = *robot.base_frame();
    let frame = Frame::translation(0.4, -0.2, 0.8);
    robot.set_base_frame(frame).unwrap();
    assert_eq!(*robot.base_frame(), frame);

    let fork = robot.fork();
    assert_eq!(*fork.base_frame(), frame);

    assert!(matches!(
        robot.set_base_frame(Frame::translation(f64::NAN, 0.0, 0.0)),
        Err(Error::InvalidBaseState { field: "frame", .. })
    ));
    assert_eq!(*robot.base_frame(), frame);
    assert_ne!(original, frame);
}

#[test]
fn joint_inputs_must_be_finite() {
    let robot = robot();
    let mut values = vec![0.0; robot.joint_count()];

    for invalid in [f64::NAN, f64::INFINITY, f64::NEG_INFINITY] {
        values[0] = invalid;
        assert!(matches!(
            robot.model.validate_slice("q", &values),
            Err(Error::NonFiniteInput { input: "q" })
        ));
    }
}