dynibo 0.1.0

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
135
136
137
138

use std::{
    alloc::{GlobalAlloc, Layout, System},
    hint::black_box,
    path::PathBuf,
    sync::atomic::{AtomicBool, AtomicUsize, Ordering},
};

use dynibo::{Frame, InverseKinematicsOptions, Robot, Twist};

struct CountingAllocator;

static COUNTING: AtomicBool = AtomicBool::new(false);
static ALLOCATIONS: AtomicUsize = AtomicUsize::new(0);

unsafe impl GlobalAlloc for CountingAllocator {
    unsafe fn alloc(&self, layout: Layout) -> *mut u8 {
        if COUNTING.load(Ordering::Relaxed) {
            ALLOCATIONS.fetch_add(1, Ordering::Relaxed);
        }
        // SAFETY: Delegates the unchanged layout to the system allocator.
        unsafe { System.alloc(layout) }
    }

    unsafe fn dealloc(&self, pointer: *mut u8, layout: Layout) {
        // SAFETY: Delegates the pointer and layout supplied by the caller.
        unsafe { System.dealloc(pointer, layout) }
    }

    unsafe fn realloc(&self, pointer: *mut u8, layout: Layout, new_size: usize) -> *mut u8 {
        if COUNTING.load(Ordering::Relaxed) {
            ALLOCATIONS.fetch_add(1, Ordering::Relaxed);
        }
        // SAFETY: Delegates the caller-supplied allocation to the system allocator.
        unsafe { System.realloc(pointer, layout, new_size) }
    }

    unsafe fn alloc_zeroed(&self, layout: Layout) -> *mut u8 {
        if COUNTING.load(Ordering::Relaxed) {
            ALLOCATIONS.fetch_add(1, Ordering::Relaxed);
        }
        // SAFETY: Delegates the unchanged layout to the system allocator.
        unsafe { System.alloc_zeroed(layout) }
    }
}

#[global_allocator]
static ALLOCATOR: CountingAllocator = CountingAllocator;

#[test]
fn dynamic_calculations_do_not_allocate_after_workspace_creation() {
    let path = PathBuf::from(env!("CARGO_MANIFEST_DIR")).join("tests/data/test_arm.urdf");
    let robot = Robot::from_urdf(path).unwrap();
    let target_id = robot.link_id("test_link_4").unwrap();
    let q = [0.1, 0.8, -0.6, 0.3];
    let qd = [-0.2, 0.4, -0.1, 0.5];
    let qdd = [0.3, -0.2, 0.4, -0.1];
    let initial = [0.0; 4];
    let mut workspace = robot.workspace();
    let mut jacobian = [0.0; 24];
    let mut output = [0.0; 4];
    let desired = robot
        .forward_kinematics(&q, target_id, &mut workspace)
        .unwrap();

    robot
        .jacobian(&q, target_id, &mut workspace, &mut jacobian)
        .unwrap();
    robot
        .inverse_kinematics(
            &initial,
            target_id,
            &desired,
            InverseKinematicsOptions::default(),
            &mut workspace,
            &mut output,
        )
        .unwrap();

    ALLOCATIONS.store(0, Ordering::Relaxed);
    COUNTING.store(true, Ordering::SeqCst);
    for _ in 0..10 {
        black_box(
            robot
                .forward_kinematics(&q, target_id, &mut workspace)
                .unwrap(),
        );
        robot
            .jacobian(&q, target_id, &mut workspace, &mut jacobian)
            .unwrap();
        black_box(
            robot
                .forward_velocity_kinematics(
                    &q,
                    &qd,
                    target_id,
                    &Frame::identity(),
                    &Frame::identity(),
                    &mut workspace,
                )
                .unwrap(),
        );
        black_box(
            robot
                .forward_acceleration_kinematics(&q, &qd, &qdd, target_id, &mut workspace)
                .unwrap(),
        );
        robot
            .gravity(&q, &Frame::identity(), &[], &mut workspace, &mut output)
            .unwrap();
        robot
            .inverse_dynamics(
                &q,
                &qd,
                &qdd,
                &Frame::identity(),
                Twist::zeros(),
                Twist::zeros(),
                &[],
                &mut workspace,
                &mut output,
            )
            .unwrap();
        robot
            .inverse_kinematics(
                &initial,
                target_id,
                &desired,
                InverseKinematicsOptions::default(),
                &mut workspace,
                &mut output,
            )
            .unwrap();
        black_box((&jacobian, &output));
    }
    COUNTING.store(false, Ordering::SeqCst);

    assert_eq!(ALLOCATIONS.load(Ordering::Relaxed), 0);
}