Skip to content

Commit 88438ce

Browse files
WeihangGuoWeihangGuo
committed
sdf: add sdf examples
1 parent 46a0a67 commit 88438ce

3 files changed

Lines changed: 264 additions & 2 deletions

File tree

‎CMakeLists.txt‎

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -141,10 +141,17 @@ endif()
141141
if(VAMP_BUILD_CPP_DEMO)
142142
add_executable(vamp_rrtc_example scripts/cpp/rrtc_example.cc)
143143
target_link_libraries(vamp_rrtc_example PRIVATE vamp_cpp)
144-
145-
# Disable strict warnings for demos to maintain compatibility
144+
145+
add_executable(vamp_sdf_example scripts/cpp/sdf_example.cc)
146+
target_link_libraries(vamp_sdf_example PRIVATE vamp_cpp)
147+
148+
add_executable(vamp_sdf_example_manual scripts/cpp/sdf_example_manual.cc)
149+
target_link_libraries(vamp_sdf_example_manual PRIVATE vamp_cpp)
150+
146151
if(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
147152
target_compile_options(vamp_rrtc_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare)
153+
target_compile_options(vamp_sdf_example PRIVATE -Wno-c++11-narrowing -Wno-sign-compare)
154+
target_compile_options(vamp_sdf_example_manual PRIVATE -Wno-c++11-narrowing -Wno-sign-compare)
148155
endif()
149156
endif()
150157

‎scripts/cpp/sdf_example.cc‎

Lines changed: 101 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,101 @@
1+
#include <chrono>
2+
#include <vector>
3+
#include <array>
4+
#include <utility>
5+
#include <iostream>
6+
7+
#include <vamp/collision/factory.hh>
8+
#include <vamp/robots/panda.hh>
9+
#include <vamp/random/halton.hh>
10+
#include <vamp/optimization/project.hh>
11+
12+
using Robot = vamp::robots::Panda;
13+
static constexpr const std::size_t rake = vamp::FloatVectorWidth;
14+
using EnvironmentInput = vamp::collision::Environment<float>;
15+
using EnvironmentVector = vamp::collision::Environment<vamp::FloatVector<rake>>;
16+
17+
// Spheres for the cage problem - (x, y, z) center coordinates with fixed, common radius defined below
18+
static const std::vector<std::array<float, 3>> problem = {
19+
{0.55, 0, 0.25},
20+
{0.35, 0.35, 0.25},
21+
{0, 0.55, 0.25},
22+
{-0.55, 0, 0.25},
23+
{-0.35, -0.35, 0.25},
24+
{0, -0.55, 0.25},
25+
{0.35, -0.35, 0.25},
26+
{-0.35, 0.35, 0.25},
27+
{0.35, 0.35, 0.8},
28+
{0, 0.55, 0.8},
29+
{-0.35, 0.35, 0.8},
30+
{-0.55, 0, 0.8},
31+
{-0.35, -0.35, 0.8},
32+
{0, -0.55, 0.8},
33+
{0.35, -0.35, 0.8},
34+
{0.55, 0, 0.8},
35+
};
36+
37+
// Radius for obstacle spheres
38+
static constexpr float radius = 0.2;
39+
40+
auto main(int, char **) -> int
41+
{
42+
// Build sphere cage environment
43+
EnvironmentInput environment;
44+
for (const auto &sphere : problem)
45+
{
46+
environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius));
47+
}
48+
49+
environment.sort();
50+
auto env_v = EnvironmentVector(environment);
51+
52+
// Benchmark
53+
vamp::rng::Halton<Robot> sampler;
54+
int n_samples = 1000;
55+
int n_success = 0;
56+
double total_time_ms = 0.0;
57+
int total_iter = 0;
58+
std::cout << "Starting Benchmark with " << n_samples << " samples..." << std::endl;
59+
int i = 0;
60+
while (i < n_samples)
61+
{
62+
auto q_random = sampler.next();
63+
// std::cout << "Sample " << i << ": " << q_random << std::endl;
64+
Robot::ConfigurationBlock<rake> b;
65+
for (auto k = 0U; k < Robot::dimension; ++k)
66+
{
67+
b[k] = q_random.broadcast(k);
68+
}
69+
auto valid = Robot::fkcc<rake>(env_v, b);
70+
if (valid)
71+
{
72+
continue;
73+
}
74+
i++;
75+
76+
// Project
77+
auto start_t = std::chrono::steady_clock::now();
78+
79+
auto b_final = vamp::optimization::project_to_valid<Robot, rake>(b, env_v);
80+
81+
auto end_t = std::chrono::steady_clock::now();
82+
auto dur = std::chrono::duration_cast<std::chrono::nanoseconds>(end_t - start_t);
83+
valid = Robot::fkcc<rake>(env_v, b_final);
84+
if (valid)
85+
{
86+
n_success++;
87+
}
88+
total_time_ms += dur.count() / 1e6;
89+
}
90+
91+
std::cout << "Benchmark Results:" << std::endl;
92+
std::cout << "Total Samples: " << n_samples << std::endl;
93+
std::cout << "Successful Projections: " << n_success << " ("
94+
<< (n_samples > 0 ? (100.0 * n_success / n_samples) : 0.0) << "%)" << std::endl;
95+
if (n_success > 0)
96+
{
97+
std::cout << "Average Projection Time: " << (total_time_ms / n_samples) << " ms" << std::endl;
98+
}
99+
100+
return 0;
101+
}

‎scripts/cpp/sdf_example_manual.cc‎

Lines changed: 154 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,154 @@
1+
#include <chrono>
2+
#include <vector>
3+
#include <array>
4+
#include <utility>
5+
#include <iostream>
6+
7+
#include <vamp/collision/factory.hh>
8+
#include <vamp/robots/panda.hh>
9+
#include <vamp/random/halton.hh>
10+
11+
using Robot = vamp::robots::Panda;
12+
static constexpr const std::size_t rake = vamp::FloatVectorWidth;
13+
using EnvironmentInput = vamp::collision::Environment<float>;
14+
using EnvironmentVector = vamp::collision::Environment<vamp::FloatVector<rake>>;
15+
16+
// Spheres for the cage problem - (x, y, z) center coordinates with fixed, common radius defined below
17+
static const std::vector<std::array<float, 3>> problem = {
18+
{0.55, 0, 0.25},
19+
{0.35, 0.35, 0.25},
20+
{0, 0.55, 0.25},
21+
{-0.55, 0, 0.25},
22+
{-0.35, -0.35, 0.25},
23+
{0, -0.55, 0.25},
24+
{0.35, -0.35, 0.25},
25+
{-0.35, 0.35, 0.25},
26+
{0.35, 0.35, 0.8},
27+
{0, 0.55, 0.8},
28+
{-0.35, 0.35, 0.8},
29+
{-0.55, 0, 0.8},
30+
{-0.35, -0.35, 0.8},
31+
{0, -0.55, 0.8},
32+
{0.35, -0.35, 0.8},
33+
{0.55, 0, 0.8},
34+
};
35+
36+
// Radius for obstacle spheres
37+
static constexpr float radius = 0.2;
38+
39+
auto main(int, char **) -> int
40+
{
41+
// Build sphere cage environment
42+
EnvironmentInput environment;
43+
for (const auto &sphere : problem)
44+
{
45+
environment.spheres.emplace_back(vamp::collision::factory::sphere::array(sphere, radius));
46+
}
47+
48+
environment.sort();
49+
auto env_v = EnvironmentVector(environment);
50+
51+
// Benchmark
52+
vamp::rng::Halton<Robot> sampler;
53+
int n_samples = 1000;
54+
int n_success = 0;
55+
double total_time_ms = 0.0;
56+
int total_iter = 0;
57+
std::cout << "Starting Benchmark with " << n_samples << " samples..." << std::endl;
58+
int i = 0;
59+
while (i < n_samples)
60+
{
61+
auto q_random = sampler.next();
62+
// std::cout << "Sample " << i << ": " << q_random << std::endl;
63+
Robot::ConfigurationBlock<rake> b;
64+
for (auto k = 0U; k < Robot::dimension; ++k)
65+
{
66+
b[k] = q_random.broadcast(k);
67+
}
68+
auto valid = Robot::fkcc<rake>(env_v, b);
69+
if (valid)
70+
{
71+
continue;
72+
}
73+
i++;
74+
75+
// Project
76+
auto start_t = std::chrono::steady_clock::now();
77+
78+
float current_min_dist = -1e9f;
79+
int iter = 0;
80+
const int max_iters = 100;
81+
float alpha = 0.1f;
82+
83+
while (current_min_dist < 0 && iter < max_iters)
84+
{
85+
// Re-evaluate
86+
auto res = Robot::sdf_gradient(env_v, b);
87+
88+
auto dists_arr = res.first.to_array();
89+
current_min_dist = 1e9f;
90+
for (auto d : dists_arr)
91+
{
92+
if (d < current_min_dist)
93+
{
94+
current_min_dist = d;
95+
}
96+
}
97+
98+
if (current_min_dist >= 0)
99+
{
100+
break;
101+
}
102+
103+
Robot::ConfigurationBlock<rake> dq_block;
104+
Robot::d_collision_d_q(b, res.second, dq_block); // b is already the block for q_new
105+
std::vector<float> dq(Robot::dimension);
106+
for (auto k = 0U; k < Robot::dimension; ++k)
107+
{
108+
dq[k] = dq_block[k].element(0);
109+
}
110+
111+
float dq_norm = 0.0f;
112+
for (float v : dq)
113+
{
114+
dq_norm += v * v;
115+
}
116+
dq_norm = std::sqrt(dq_norm);
117+
118+
if (dq_norm > std::numeric_limits<float>::epsilon())
119+
{
120+
for (auto k = 0U; k < Robot::dimension; ++k)
121+
{
122+
b[k] = b[k] + alpha * (dq[k] / dq_norm);
123+
}
124+
}
125+
else
126+
{
127+
break;
128+
}
129+
iter++;
130+
}
131+
auto end_t = std::chrono::steady_clock::now();
132+
auto dur = std::chrono::duration_cast<std::chrono::nanoseconds>(end_t - start_t);
133+
auto b_final = b;
134+
valid = Robot::fkcc<rake>(env_v, b_final);
135+
if (valid)
136+
{
137+
n_success++;
138+
}
139+
total_time_ms += dur.count() / 1e6;
140+
total_iter += iter;
141+
}
142+
143+
std::cout << "Benchmark Results:" << std::endl;
144+
std::cout << "Total Samples: " << n_samples << std::endl;
145+
std::cout << "Successful Projections: " << n_success << " ("
146+
<< (n_samples > 0 ? (100.0 * n_success / n_samples) : 0.0) << "%)" << std::endl;
147+
if (n_success > 0)
148+
{
149+
std::cout << "Average Projection Time: " << (total_time_ms / n_samples) << " ms" << std::endl;
150+
std::cout << "Average Iterations: " << (total_iter / n_samples) << std::endl;
151+
}
152+
153+
return 0;
154+
}

0 commit comments

Comments
 (0)