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
79
80
81
82
83
84
85
86
87

// Test purpose: verify that a C++ inverse-dynamics callback is owned by Robot
// and used by torque-limit / COPP objective paths without exposing callback
// details through the public Rust bridge.

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

#include <copp/copp.hpp>

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

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

int main()
{
    const std::vector<double> s{0.0, 0.5, 1.0};
    const std::vector<double> torque_upper{10.0, 10.0};
    const std::vector<double> torque_lower{-10.0, -10.0};

    auto path = copp::Path::from_parametric(
        [](copp::Jet3 x)
        {
            return std::vector<copp::Jet3>{
                x,
                copp::powi(x, 2),
            };
        },
        0.0,
        1.0);

    std::size_t calls = 0;
    copp::Robot robot(
        2,
        [&calls](
            copp::Span<const double> q,
            copp::Span<const double> dq,
            copp::Span<const double> ddq,
            copp::Span<double> tau)
        {
            ++calls;
            for (std::size_t i = 0; i < tau.size(); ++i)
            {
                tau[i] = 2.0 * ddq[i] + 0.1 * dq[i] + std::sin(q[i]);
            }
        },
        s.size());

    if (!robot.has_inverse_dynamics())
    {
        return fail("expected inverse dynamics to be installed");
    }

    robot.append_s(s)
        .set_q_from_path_2nd(path, 0, s.size())
        .add_torque_limits(torque_upper, torque_lower, 0, s.size());

    if (calls == 0)
    {
        return fail("inverse dynamics was not called");
    }

    robot.clear_inverse_dynamics();
    if (robot.has_inverse_dynamics())
    {
        return fail("expected inverse dynamics to be cleared");
    }

    auto constraints = robot.constraints();
    const auto exported_s = constraints.s_values(0, s.size());
    if (exported_s.size() != s.size() || !near(exported_s.back(), 1.0))
    {
        return fail("unexpected station export");
    }

    return 0;
}