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

#include "example_common.h"

int main(void)
{
    /*
     * 1) Deterministic 3-axis Lissajous path q(s), s in [0, 1].
     */
    double s[EXAMPLE_NUM_POINTS];
    struct CoppPath *path = NULL;
    struct CoppRobot *robot = NULL;
    struct CoppReachSet2Result reach_back = {{NULL, 0, 0}, {NULL, 0, 0}};
    struct CoppReachSet2Result reach_bidir = {{NULL, 0, 0}, {NULL, 0, 0}};
    int rc = 1;

    example_fill_stations(s, EXAMPLE_NUM_POINTS);
    if (example_create_analytic_path_2nd(&path))
    {
        goto cleanup;
    }

    /*
     * 2) Build robot constraints (3-axis), then apply symmetric limits
     * velocity/acceleration = [-1, 1].
     */
    if (example_create_robot_2nd(path, s, EXAMPLE_NUM_POINTS, &robot))
    {
        goto cleanup;
    }

    /*
     * 3) Build TOPP2 problem and compute reachable sets. The backward-only
     * pass constrains the terminal boundary; the bidirectional pass constrains
     * both the start and terminal boundaries.
     */
    struct Topp2RaOptions options;
    enum CoppStatus status = topp2_ra_default_options(&options);
    if (example_expect_ok(status, "topp2_ra_default_options"))
    {
        goto cleanup;
    }

    struct Topp2Problem problem = {robot, 0, EXAMPLE_NUM_POINTS - 1, 0.0, 0.0};
    status = copp_reach_set2_backward(problem, options, &reach_back);
    if (example_expect_ok(status, "copp_reach_set2_backward"))
    {
        goto cleanup;
    }

    status = copp_reach_set2_bidirectional(problem, options, &reach_bidir);
    if (example_expect_ok(status, "copp_reach_set2_bidirectional"))
    {
        goto cleanup;
    }

    /*
     * 4) Print the tutorial summary.
     */
    const size_t k0 = 0;
    const size_t km = EXAMPLE_NUM_POINTS / 2;
    const size_t k1 = EXAMPLE_NUM_POINTS - 1;

    printf("reach_set2 done.\n");
    printf("dim = %d, N = %d\n", EXAMPLE_DIM, EXAMPLE_NUM_POINTS);
    printf("backward-only: a_max.len() = %zu, a_min.len() = %zu\n",
           reach_back.a_max.len,
           reach_back.a_min.len);
    printf("bidirectional: a_max.len() = %zu, a_min.len() = %zu\n",
           reach_bidir.a_max.len,
           reach_bidir.a_min.len);
    printf("bidirectional bounds @k=0/mid/end: [%.6f, %.6f], [%.6f, %.6f], [%.6f, %.6f]\n",
           reach_bidir.a_min.data[k0],
           reach_bidir.a_max.data[k0],
           reach_bidir.a_min.data[km],
           reach_bidir.a_max.data[km],
           reach_bidir.a_min.data[k1],
           reach_bidir.a_max.data[k1]);
    rc = 0;

cleanup:
    copp_reach_set2_result_free(reach_bidir);
    copp_reach_set2_result_free(reach_back);
    copp_robot_free(robot);
    copp_path_free(path);
    return rc;
}