vamp/scripts/cpp/sdf_example_manual.cc at 141908383b11c955d11a47bb3c54f01e75903ace · KavrakiLab/vamp · GitHub

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
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
#include <chrono>
#include <vector>
#include <array>
#include <utility>
#include <iostream>

#include <vamp/collision/factory.hh>
#include <vamp/robots/panda.hh>
#include <vamp/random/halton.hh>

using Robot = vamp::robots::Panda;
static constexpr const std::size_t rake = vamp::FloatVectorWidth;
using EnvironmentInput = vamp::collision::Environment<float>;
using EnvironmentVector = vamp::collision::Environment<vamp::FloatVector<rake>>;

// Spheres for the cage problem - (x, y, z) center coordinates with fixed, common radius defined below
static const std::vector<std::array<float, 3>> problem = {
    {0.55, 0, 0.25},
    {0.35, 0.35, 0.25},
    {0, 0.55, 0.25},
    {-0.55, 0, 0.25},
    {-0.35, -0.35, 0.25},
    {0, -0.55, 0.25},
    {0.35, -0.35, 0.25},
    {-0.35, 0.35, 0.25},
    {0.35, 0.35, 0.8},
    {0, 0.55, 0.8},
    {-0.35, 0.35, 0.8},
    {-0.55, 0, 0.8},
    {-0.35, -0.35, 0.8},
    {0, -0.55, 0.8},
    {0.35, -0.35, 0.8},
    {0.55, 0, 0.8},
};

// Radius for obstacle spheres
static constexpr float radius = 0.2;

auto main(int, char **) -> int
{
    // Build sphere cage environment
    EnvironmentInput environment;
    for (const auto &sphere : problem)
    {
        environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius));
    }

    environment.sort();
    auto env_v = EnvironmentVector(environment);

    // Benchmark
    vamp::rng::Halton<Robot> sampler;
    int n_samples = 1000;
    int n_success = 0;
    double total_time_ms = 0.0;
    int total_iter = 0;
    std::cout << "Starting Benchmark with " << n_samples << " samples..." << std::endl;
    int i = 0;
    while (i < n_samples)
    {
        auto q_random = sampler.next();
        // std::cout << "Sample " << i << ": " << q_random << std::endl;
        Robot::ConfigurationBlock<rake> b;
        for (auto k = 0U; k < Robot::dimension; ++k)
        {
            b[k] = q_random.broadcast(k);
        }
        auto valid = Robot::fkcc<rake>(env_v, b);
        if (valid)
        {
            continue;
        }
        i++;

        // Project
        auto start_t = std::chrono::steady_clock::now();

        float current_min_dist = -1e9f;
        int iter = 0;
        const int max_iters = 100;
        float alpha = 0.1f;

        while (current_min_dist < 0 && iter < max_iters)
        {
            // Re-evaluate
            auto res = Robot::sdf_gradient(env_v, b);

            auto dists_arr = res.first.to_array();
            current_min_dist = 1e9f;
            for (auto d : dists_arr)
            {
                if (d < current_min_dist)
                {
                    current_min_dist = d;
                }
            }

            if (current_min_dist >= 0)
            {
                break;
            }

            Robot::ConfigurationBlock<rake> dq_block;
            Robot::d_collision_d_q(b, res.second, dq_block);  // b is already the block for q_new
            std::vector<float> dq(Robot::dimension);
            for (auto k = 0U; k < Robot::dimension; ++k)
            {
                dq[k] = dq_block[k].element(0);
            }

            float dq_norm = 0.0f;
            for (float v : dq)
            {
                dq_norm += v * v;
            }
            dq_norm = std::sqrt(dq_norm);

            if (dq_norm > std::numeric_limits<float>::epsilon())
            {
                for (auto k = 0U; k < Robot::dimension; ++k)
                {
                    b[k] = b[k] + alpha * (dq[k] / dq_norm);
                }
            }
            else
            {
                break;
            }
            iter++;
        }
        auto end_t = std::chrono::steady_clock::now();
        auto dur = std::chrono::duration_cast<std::chrono::nanoseconds>(end_t - start_t);
        auto b_final = b;
        valid = Robot::fkcc<rake>(env_v, b_final);
        if (valid)
        {
            n_success++;
        }
        total_time_ms += dur.count() / 1e6;
        total_iter += iter;
    }

    std::cout << "Benchmark Results:" << std::endl;
    std::cout << "Total Samples: " << n_samples << std::endl;
    std::cout << "Successful Projections: " << n_success << " ("
              << (n_samples > 0 ? (100.0 * n_success / n_samples) : 0.0) << "%)" << std::endl;
    if (n_success > 0)
    {
        std::cout << "Average Projection Time: " << (total_time_ms / n_samples) << " ms" << std::endl;
        std::cout << "Average Iterations: " << (total_iter / n_samples) << std::endl;
    }

    return 0;
}