Skip to content

Batch validation API #40

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
wbthomason wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Batch validation API #40

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
92 changes: 81 additions & 11 deletions src/impl/vamp/bindings/common.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,8 +118,8 @@ namespace vamp::binding
}
#endif

inline static auto
fk(const ConfigurationArray &configuration) -> std::vector<vamp::collision::Sphere<float>>
inline static auto fk(const ConfigurationArray &configuration)
-> std::vector<vamp::collision::Sphere<float>>
{
typename Robot::template Spheres<1> out;
typename Robot::template ConfigurationBlock<1> block;
Expand All @@ -140,9 +140,9 @@ namespace vamp::binding
return result;
}

inline static auto sphere_validate(
const ConfigurationArray &configuration,
const EnvironmentInput &environment) -> std::vector<std::vector<std::string>>
inline static auto
sphere_validate(const ConfigurationArray &configuration, const EnvironmentInput &environment)
-> std::vector<std::vector<std::string>>
{
auto spheres = fk(configuration);
std::vector<std::vector<std::string>> result;
Expand All @@ -158,9 +158,9 @@ namespace vamp::binding
return result;
}

inline static auto validate_configuration(
const Configuration &configuration,
const EnvironmentInput &environment) -> bool
inline static auto
validate_configuration(const Configuration &configuration, const EnvironmentInput &environment)
-> bool
{
return vamp::planning::validate_motion<Robot, rake, 1>(
configuration, configuration, EnvironmentVector(environment));
Expand All @@ -174,6 +174,75 @@ namespace vamp::binding
configuration_v, configuration_v, EnvironmentVector(environment));
}

// TODO: nb::ndarray overload
inline static auto validate_batch(
const EnvironmentInput &environment,
const std::vector<ConfigurationArray> &configurations,
bool is_sequence = true) noexcept -> std::vector<float>
{
EnvironmentVector environment_v(environment);
std::vector<float> result(configurations.size(), 0.0);
// TODO: Figure out if cache blocking can help here, given the double-indirection of a vector of
// arrays
std::array<float, Robot::dimension * rake> block_scalars;
bool sequence_valid = true;
unsigned int num_residual_elements = configurations.size() % rake;
for (auto i = 0U; sequence_valid and i < configurations.size() - num_residual_elements; i += rake)
{
for (auto j = 0U; j < rake; ++j)
{
const auto &q = configurations[i + j];
for (auto k = 0U; k < Robot::dimension; ++k)
{
block_scalars[k * rake + j] = q[k];
}
}

typename Robot::template ConfigurationBlock<rake> block(block_scalars);
auto block_validation = vamp::planning::validate_block<Robot, rake>(environment_v, block);
for (auto j = 0U; j < rake; ++j)
{
const auto config_valid = block_validation[{0, j}];
if (is_sequence and config_valid == 0.0)
{
sequence_valid = false;
break;
}

result[i + j] = is_sequence ? static_cast<float>(sequence_valid) : config_valid;
}
}

if (sequence_valid)
{
const auto offset = configurations.size() - num_residual_elements;
for (auto i = 0U; i < num_residual_elements; ++i)
{
const auto &q = configurations[i + offset];
for (auto k = 0U; k < Robot::dimension; ++k)
{
block_scalars[k * rake + i] = q[k];
}
}

typename Robot::template ConfigurationBlock<rake> block(block_scalars);
auto block_validation = vamp::planning::validate_block<Robot, rake>(environment_v, block);
for (auto i = 0U; i < num_residual_elements; ++i)
{
const auto config_valid = block_validation[{0, i}];
if (is_sequence and config_valid == 0.0)
{
sequence_valid = false;
break;
}

result[offset + i] = is_sequence ? static_cast<float>(sequence_valid) : config_valid;
}
}

return result;
}

inline static auto rrtc_single(
const ConfigurationArray &start,
const ConfigurationArray &goal,
Expand Down Expand Up @@ -295,8 +364,8 @@ namespace vamp::binding
return filter_robot_from_pointcloud<Robot>(pc, start, environment, point_radius);
}

inline static auto
eefk(const ConfigurationArray &start) -> std::pair<std::array<float, 3>, std::array<float, 4>>
inline static auto eefk(const ConfigurationArray &start)
-> std::pair<std::array<float, 3>, std::array<float, 4>>
{
const auto &result = Robot::eefk(start);

Expand Down Expand Up @@ -352,7 +421,8 @@ namespace vamp::binding
"Collision checking resolution for this robot.");
submodule.def(
"n_spheres", []() { return Robot::n_spheres; }, "Number of spheres in robot collision model.");
submodule.def("space_measure", []() { return Robot::space_measure(); }, "Measure ");
submodule.def(
"space_measure", []() { return Robot::space_measure(); }, "Measure ");

nb::class_<typename RH::Configuration>(submodule, "Configuration", "Robot configuration.")
.def(nb::init<>(), "Empty constructor. Zero initialized.")
Expand Down
118 changes: 116 additions & 2 deletions src/impl/vamp/collision/validity.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -149,6 +149,111 @@ namespace vamp
return false;
}

template <typename DataT, typename ArgT1, typename ArgT2, typename ArgT3, typename ArgT4>
inline constexpr auto sphere_block_environment_validate(
const collision::Environment<DataT> &e, //
ArgT1 sx_,
ArgT2 sy_,
ArgT3 sz_,
ArgT4 sr_) noexcept -> DataT
{
// TODO: Figure out a way to avoid needing to upcast floats to vectors
auto sx = static_cast<DataT>(sx_);
auto sy = static_cast<DataT>(sy_);
auto sz = static_cast<DataT>(sz_);
auto sr = static_cast<DataT>(sr_);
const auto max_extent = collision::sqrt(collision::dot_3(sx, sy, sz, sx, sy, sz)) + sr;

DataT result(0.5);
const DataT offset(0.5);
constexpr float SIGN_MASK = -0.0;
for (const auto &es : e.spheres)
{
const auto diff = es.min_distance - max_extent;
if (diff.test_zero())
{
break;
}

result = result | collision::sphere_sphere_sql2(es, sx, sy, sz, sr) & SIGN_MASK;
if (result.all())
{
return result + offset;
}
}

for (const auto &ec : e.capsules)
{
const auto diff = ec.min_distance - max_extent;
if (diff.test_zero())
{
break;
}

result = result | collision::sphere_capsule(ec, sx, sy, sz, sr) & SIGN_MASK;
if (result.all())
{
return result + offset;
}
}

for (const auto &ec : e.z_aligned_capsules)
{
const auto diff = ec.min_distance - max_extent;
if (diff.test_zero())
{
break;
}
result = result | collision::sphere_z_aligned_capsule(ec, sx, sy, sz, sr) & SIGN_MASK;
if (result.all())
{
return result + offset;
}
}

const auto rsq = sr * sr;
for (const auto &ec : e.cuboids)
{
const auto diff = ec.min_distance - max_extent;
if (diff.test_zero())
{
break;
}

result = result | collision::sphere_cuboid(ec, sx, sy, sz, rsq) & SIGN_MASK;
if (result.all())
{
return result + offset;
}
}

for (const auto &ec : e.z_aligned_cuboids)
{
const auto diff = ec.min_distance - max_extent;
if (diff.test_zero())
{
break;
}

result = result | collision::sphere_z_aligned_cuboid(ec, sx, sy, sz, rsq) & SIGN_MASK;
if (result.all())
{
return result + offset;
}
}

for (const auto &eh : e.heightfields)
{
result = result | collision::sphere_heightfield(eh, sx, sy, sz, sr) & SIGN_MASK;
if (result.all())
{
return result + offset;
}
}

return result + offset;
}

template <typename DataT, typename ArgT1, typename ArgT2, typename ArgT3, typename ArgT4>
inline auto sphere_environment_get_collisions(
const collision::Environment<DataT> &e, //
Expand Down Expand Up @@ -249,8 +354,8 @@ namespace vamp
}

template <typename DataT>
inline constexpr auto
attachment_environment_collision(const collision::Environment<DataT> &e) noexcept -> bool
inline constexpr auto attachment_environment_collision(const collision::Environment<DataT> &e) noexcept
-> bool
{
for (const auto &s : e.attachments->posed_spheres)
{
Expand Down Expand Up @@ -291,4 +396,13 @@ namespace vamp

return false;
}

template <typename Robot, std::size_t rake>
inline constexpr auto validate_block(
const collision::Environment<FloatVector<rake>> &environment,
const typename Robot::template ConfigurationBlock<rake> &block) -> FloatVector<rake>
{
return Robot::template validate_block<rake>(environment, block);
}

} // namespace vamp