copp 0.2.2

Convex-objective path parameterization for robotic trajectory planning.
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

// Test purpose: validate TOPP2 interpolation wrappers.
//
// It covers `a_to_b_topp2`, `s_to_t_topp2`, uniform inverse sampling, and
// explicit inverse sampling on a profile with analytically simple timing.

#include <cmath>
#include <iostream>
#include <vector>

#include <copp/copp.hpp>

namespace
{

    bool near(double lhs, double rhs)
    {
        return std::abs(lhs - rhs) < 1.0e-12;
    }

    int fail(const char *message)
    {
        std::cerr << message << "\n";
        return 1;
    }

} // namespace

int main()
{
    const std::vector<double> s{0.0, 0.5, 1.0};
    const std::vector<double> a{1.0, 1.0, 1.0};

    const auto b = copp::interpolation::a_to_b_topp2(s, a);
    const auto profile = copp::interpolation::s_to_t_topp2(s, a, 0.0);
    const auto s_t_uniform = copp::interpolation::t_to_s_topp2_uniform(s, a, profile.t_s, 0.25);
    const std::vector<double> t_sample{0.0, 0.25, 0.5, 0.75, 1.0};
    const auto s_t_samples = copp::interpolation::t_to_s_topp2_samples(s, a, profile.t_s, t_sample);

    if (b.size() != s.size() - 1)
    {
        return fail("unexpected b length");
    }
    for (double value : b)
    {
        if (!near(value, 0.0))
        {
            return fail("unexpected b value");
        }
    }
    if (!near(profile.t_final, 1.0))
    {
        return fail("unexpected t_final");
    }
    if (profile.t_s.size() != s.size())
    {
        return fail("unexpected t_s length");
    }
    for (std::size_t i = 0; i < s.size(); ++i)
    {
        if (!near(profile.t_s[i], s[i]))
        {
            return fail("unexpected t_s value");
        }
    }
    if (s_t_uniform.size() != 5 || s_t_samples.size() != 5)
    {
        return fail("unexpected s(t) length");
    }
    for (std::size_t i = 0; i < t_sample.size(); ++i)
    {
        if (!near(s_t_uniform[i], t_sample[i]) || !near(s_t_samples[i], t_sample[i]))
        {
            return fail("unexpected s(t) value");
        }
    }

    return 0;
}