ruckig 0.17.3

Instantaneous Motion Generation for Robots and Machines.
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

#include <algorithm>
#include <chrono>

#include "randomizer.hpp"

#include <ruckig/ruckig.hpp>


using namespace ruckig;


template<size_t DOFs, class OTGType>
double check_update(OTGType& ruckig, InputParameter<DOFs>& input) {
    OutputParameter<DOFs> output;
    const auto result = ruckig.update(input, output);
    return output.calculation_duration;
}

template<size_t DOFs, class OTGType>
double check_calculation(OTGType& ruckig, InputParameter<DOFs>& input) {
    Trajectory<DOFs> trajectory;
    const auto start = std::chrono::steady_clock::now();
    const auto result = ruckig.calculate(input, trajectory);
    const auto stop = std::chrono::steady_clock::now();
    return std::chrono::duration_cast<std::chrono::nanoseconds>(stop - start).count() / 1000.0;
}


std::tuple<double, double> analyze(const std::vector<double>& v) {
    const double sum = std::accumulate(v.begin(), v.end(), 0.0);
    const double mean = sum / v.size();

    std::vector<double> diff(v.size());
    std::transform(v.begin(), v.end(), diff.begin(), [mean](double x) { return x - mean; });
    const double sq_sum = std::inner_product(diff.begin(), diff.end(), diff.begin(), 0.0);
    const double std_deviation = std::sqrt(sq_sum / v.size());
    return std::make_tuple(mean, std_deviation);
}


template<size_t DOFs, class OTGType>
void benchmark(size_t n, double number_trajectories, bool verbose = true) {
    OTGType ruckig {0.005};

    std::normal_distribution<double> position_dist {0.0, 4.0};
    std::normal_distribution<double> dynamic_dist {0.0, 0.8};
    std::uniform_real_distribution<double> limit_dist {0.1, 12.0};

    Randomizer<DOFs, decltype(position_dist)> p { position_dist, 42 };
    Randomizer<DOFs, decltype(dynamic_dist)> d { dynamic_dist, 43 };
    Randomizer<DOFs, decltype(limit_dist)> l { limit_dist, 44 };

    InputParameter<DOFs> input;
    // input.synchronization = Synchronization::None;
    // input.control_interface = ControlInterface::Velocity;
    std::vector<double> average, worst, global;

    // Initial warm-up calculation
    // p.fill(input.current_position);
    // p.fill(input.target_position);
    // l.fill(input.max_velocity, input.target_velocity);
    // l.fill(input.max_acceleration, input.target_acceleration);
    // l.fill(input.max_jerk);
    // check_calculation(ruckig, input);

    for (size_t j = 0; j < n; ++j) {
        double average_ = 0.0;
        double worst_ = 0.0;
        size_t n {1};

        const auto start = std::chrono::steady_clock::now();

        for (size_t i = 0; i < number_trajectories; ++i) {
            p.fill(input.current_position);
            d.fill_or_zero(input.current_velocity, 0.9);
            d.fill_or_zero(input.current_acceleration, 0.8);
            p.fill(input.target_position);
            d.fill_or_zero(input.target_velocity, 0.7);
            if constexpr (std::is_same<OTGType, RuckigThrow<DOFs>>::value) {
                d.fill_or_zero(input.target_acceleration, 0.6);
            }

            l.fill(input.max_velocity, input.target_velocity);
            l.fill(input.max_acceleration, input.target_acceleration);
            l.fill(input.max_jerk);

            // input.current_velocity[0] = 0.5;
            // input.target_velocity[0] = 0.5;
            // input.target_position[0] = input.current_position[0] + 1.0;
            // input.max_jerk[0] = 0.0;

            if constexpr (std::is_same<OTGType, RuckigThrow<DOFs>>::value) {
                if (!ruckig.template validate_input<false>(input)) {
                    continue;
                }
            }

            const double time = check_calculation(ruckig, input);
            average_ = average_ + (time - average_) / n;
            worst_ = std::max(worst_, time);
            ++n;
        }

        const auto stop = std::chrono::steady_clock::now();
        const double global_ = std::chrono::duration_cast<std::chrono::nanoseconds>(stop - start).count() / 1000.0 / number_trajectories;

        average.emplace_back(average_);
        worst.emplace_back(worst_);
        global.emplace_back(global_);
    }

    const auto [average_mean, average_std] = analyze(average);
    const auto [worst_mean, worst_std] = analyze(worst);
    const auto [global_mean, global_std] = analyze(global);

    if (verbose) {
        std::cout << "---" << std::endl;
        std::cout << "Benchmark for " << ruckig.degrees_of_freedom << " DoFs on " << number_trajectories << " trajectories" << std::endl;
        std::cout << "Average Calculation Duration " << average_mean << " pm " << average_std << " [µs]" << std::endl;
        std::cout << "Worst Calculation Duration " << worst_mean << " pm " << worst_std << " [µs]" << std::endl;
        std::cout << "End-to-end Calculation Duration " << global_mean << " pm " << global_std << " [µs]" << std::endl;
    }

    // std::cout << ruckig.degrees_of_freedom << "\t" << average_mean << "\t" << average_std << "\t" << worst_mean << "\t" << worst_std << std::endl;
}


int main() {
    const size_t n {4 * 5}; // Number of iterations
    const size_t number_trajectories {32 * 1024};

    std::cout << "Ruckig size: " << sizeof(Ruckig<3>) << " Byte" << std::endl;
    std::cout << "Trajectory<3> size: " << sizeof(Trajectory<3>) << " Byte" << std::endl;

    const size_t DOFs {7};
    benchmark<DOFs, RuckigThrow<DOFs>>(n, number_trajectories);
}