Skip to content

Replace configuration with a generic state #121

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

Closed
ShrutheeshIR wants to merge 1 commit into from
Closed

Replace configuration with a generic state #121

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
9 changes: 5 additions & 4 deletions src/impl/vamp/bindings/python/robot_helper.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -516,18 +516,19 @@ namespace vamp::binding
"numpy",
[](const Path &p) noexcept
{
constexpr auto state_dimension = Robot::State::num_scalars;
auto *path_arr = new FloatT
[Robot::dimension * p.size() +
(Robot::Configuration::num_scalars_rounded - Robot::dimension)];
[state_dimension * p.size() +
(Robot::State::num_scalars_rounded - state_dimension)];
for (auto i = 0U; i < p.size(); ++i)
{
p[i].to_array_unaligned(path_arr + i * Robot::dimension);
p[i].to_array_unaligned(path_arr + i * state_dimension);
}

nb::capsule arr_owner(
path_arr, [](void *a) noexcept { delete[] reinterpret_cast<FloatT *>(a); });
return nb::ndarray<nb::numpy, const FloatT, nb::device::cpu>(
path_arr, {p.size(), Robot::dimension}, arr_owner);
path_arr, {p.size(), state_dimension}, arr_owner);
},
"Convert this path to a numpy matrix.");

Expand Down
8 changes: 4 additions & 4 deletions src/impl/vamp/planning/aorrtc.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,8 +20,8 @@ namespace vamp::planning
template <typename Robot, std::size_t rake, std::size_t resolution>
struct AOX_RRTC
{
using Configuration = typename Robot::Configuration;
static constexpr auto dimension = Robot::dimension;
using Configuration = typename Robot::State;
static constexpr auto dimension = Robot::State::num_scalars;
using RNG = typename vamp::rng::RNG<Robot>;

using NNNode = GNATNode<dimension>;
Expand Down Expand Up @@ -356,8 +356,8 @@ namespace vamp::planning
template <typename Robot, std::size_t rake, std::size_t resolution>
struct AORRTC
{
using Configuration = typename Robot::Configuration;
static constexpr auto dimension = Robot::dimension;
using Configuration = typename Robot::State;
static constexpr auto dimension = Robot::State::num_scalars;
using RNG = typename vamp::rng::RNG<Robot>;
using AOX_RRTC = typename vamp::planning::AOX_RRTC<Robot, rake, resolution>;
using RRTC = typename vamp::planning::RRTC<Robot, rake, resolution>;
Expand Down
6 changes: 3 additions & 3 deletions src/impl/vamp/planning/fcit.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -65,8 +65,8 @@ namespace vamp::planning
typename NeighborParamsT = FCITStarNeighborParams>
struct FCIT
{
using Configuration = typename Robot::Configuration;
static constexpr auto dimension = Robot::dimension;
using Configuration = typename Robot::State;
static constexpr auto dimension = Robot::State::num_scalars;
using RNG = typename vamp::rng::RNG<Robot>;

inline static auto solve(
Expand All @@ -92,7 +92,7 @@ namespace vamp::planning
NN<dimension> roadmap;

std::size_t iter = 0;
typename Robot::template ConfigurationBlock<rake> temp_block;
typename Robot::template StateBlock<rake> temp_block;
auto states = std::unique_ptr<float, decltype(&free)>(
vamp::utils::vector_alloc<float, FloatVectorAlignment, FloatVectorWidth>(
settings.max_samples * Configuration::num_scalars_rounded),
Expand Down
6 changes: 3 additions & 3 deletions src/impl/vamp/planning/grrtstar.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,8 +48,8 @@ namespace vamp::planning
template <typename Robot, std::size_t rake, std::size_t resolution>
struct GRRTStar
{
using Configuration = typename Robot::Configuration;
static constexpr auto dimension = Robot::dimension;
using Configuration = typename Robot::State;
static constexpr auto dimension = Robot::State::num_scalars;
using RNG = typename vamp::rng::RNG<Robot>;
using NNNodeType = NNNode<dimension>;
using NNTree = NN<dimension>;
Expand Down Expand Up @@ -343,7 +343,7 @@ namespace vamp::planning
continue;
}

typename Robot::ConfigurationBuffer temp_array;
typename Robot::StateBuffer temp_array;
temp.to_array(temp_array.data());

auto nearest_result = tree_a->nearest(NNFloatArray<dimension>{temp_array.data()});
Expand Down
12 changes: 6 additions & 6 deletions src/impl/vamp/planning/phs.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,7 +35,7 @@ namespace vamp::planning
template <typename Robot>
class ProlateHyperspheroid
{
static constexpr auto dimension = Robot::dimension;
static constexpr auto dimension = Robot::State::num_scalars;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

public:
Expand Down Expand Up @@ -160,7 +160,7 @@ namespace vamp::planning
rng->dist.reset();
}

inline auto next() noexcept -> FloatVector<Robot::dimension> override
inline auto next() noexcept -> FloatVector<Robot::State::num_scalars> override
{
auto x = phs.transform(uniform_in_ball());

Expand All @@ -172,22 +172,22 @@ namespace vamp::planning
return x;
}

inline auto logit() noexcept -> vamp::FloatVector<Robot::dimension>
inline auto logit() noexcept -> vamp::FloatVector<Robot::State::num_scalars>
{
auto U1 = rng->next();
Robot::descale_configuration(U1);
return (U1 * (1 - U1).rcp()).log() * std::sqrt(vamp::utils::constants::pi / 8.F);
}

inline auto uniform_on_ball() noexcept -> vamp::FloatVector<Robot::dimension>
inline auto uniform_on_ball() noexcept -> vamp::FloatVector<Robot::State::num_scalars>
{
const auto unv = logit().trim();
return unv / unv.l2_norm();
}

inline auto uniform_in_ball() noexcept -> vamp::FloatVector<Robot::dimension>
inline auto uniform_in_ball() noexcept -> vamp::FloatVector<Robot::State::num_scalars>
{
return std::pow(rng->dist.uniform_01(), 1.0 / static_cast<float>(Robot::dimension)) *
return std::pow(rng->dist.uniform_01(), 1.0 / static_cast<float>(Robot::State::num_scalars)) *
uniform_on_ball();
}

Expand Down
4 changes: 2 additions & 2 deletions src/impl/vamp/planning/plan.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
namespace vamp::planning
{
template <typename Robot>
struct Path : public std::vector<FloatVector<Robot::dimension>>
struct Path : public std::vector<FloatVector<Robot::State::num_scalars>>
{
[[nodiscard]] inline auto cost() const noexcept -> float
{
Expand Down Expand Up @@ -181,7 +181,7 @@ namespace vamp::planning
template <typename Robot>
struct Roadmap
{
std::vector<FloatVector<Robot::dimension>> vertices;
std::vector<FloatVector<Robot::State::num_scalars>> vertices;
std::vector<std::vector<std::size_t>> edges;
std::size_t nanoseconds{0};
std::size_t iterations{0};
Expand Down
8 changes: 4 additions & 4 deletions src/impl/vamp/planning/prm.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,8 +26,8 @@ namespace vamp::planning
typename NeighborParamsT = PRMStarNeighborParams>
struct PRM
{
using Configuration = typename Robot::Configuration;
static constexpr auto dimension = Robot::dimension;
using Configuration = typename Robot::State;
static constexpr auto dimension = Robot::State::num_scalars;
using RNG = typename vamp::rng::RNG<Robot>;

inline static auto solve(
Expand Down Expand Up @@ -71,7 +71,7 @@ namespace vamp::planning

std::size_t iter = 0;
std::vector<std::pair<NNNode<dimension>, float>> neighbors;
typename Robot::template ConfigurationBlock<rake> temp_block;
typename Robot::template StateBlock<rake> temp_block;
auto states = std::unique_ptr<float>(
vamp::utils::vector_alloc<float, FloatVectorAlignment, FloatVectorWidth>(
settings.max_samples * Configuration::num_scalars_rounded));
Expand Down Expand Up @@ -211,7 +211,7 @@ namespace vamp::planning

std::size_t iter = 0;
std::vector<std::pair<NNNode<dimension>, float>> neighbors;
typename Robot::template ConfigurationBlock<rake> temp_block;
typename Robot::template StateBlock<rake> temp_block;
auto states = std::unique_ptr<float, decltype(&free)>(
vamp::utils::vector_alloc<float, FloatVectorAlignment, FloatVectorWidth>(
settings.max_samples * Configuration::num_scalars_rounded),
Expand Down
6 changes: 3 additions & 3 deletions src/impl/vamp/planning/rrtc.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,8 @@ namespace vamp::planning
template <typename Robot, std::size_t rake, std::size_t resolution>
struct RRTC
{
using Configuration = typename Robot::Configuration;
static constexpr auto dimension = Robot::dimension;
using Configuration = typename Robot::State;
static constexpr auto dimension = Robot::State::num_scalars;
using RNG = typename vamp::rng::RNG<Robot>;

inline static auto solve(
Expand Down Expand Up @@ -108,7 +108,7 @@ namespace vamp::planning
}

auto temp = rng->next();
typename Robot::ConfigurationBuffer temp_array;
typename Robot::StateBuffer temp_array;
temp.to_array(temp_array.data());

const auto nearest = tree_a->nearest(NNFloatArray<dimension>{temp_array.data()});
Expand Down
8 changes: 4 additions & 4 deletions src/impl/vamp/random/halton.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ namespace vamp::rng
// Numerical precision degrades around 1.4M iterations, this value can be increased up to that point.
static constexpr const std::size_t max_iterations = 1000000U;

using Configuration = typename Robot::Configuration;
using Configuration = typename Robot::State;

static constexpr const std::array<float, 16> primes{
3.F,
Expand Down Expand Up @@ -47,16 +47,16 @@ namespace vamp::rng

inline constexpr auto bases() noexcept -> Configuration
{
alignas(FloatVectorAlignment) std::array<float, Robot::dimension> a;
std::copy_n(primes.cbegin(), Robot::dimension, a.begin());
alignas(FloatVectorAlignment) std::array<float, Robot::State::num_scalars> a;
std::copy_n(primes.cbegin(), Robot::State::num_scalars, a.begin());
return Configuration(a);
}

auto rotate_bases() noexcept
{
alignas(FloatVectorAlignment) std::array<float, Configuration::num_scalars_rounded> a;
b.to_array(a.data());
std::rotate(a.begin(), a.begin() + 1, a.begin() + Robot::dimension);
std::rotate(a.begin(), a.begin() + 1, a.begin() + Robot::State::num_scalars);
b = Configuration(a.data());
}

Expand Down
2 changes: 1 addition & 1 deletion src/impl/vamp/random/rng.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ namespace vamp::rng
{
using Ptr = std::shared_ptr<RNG<Robot>>;
virtual inline void reset() noexcept = 0;
virtual inline auto next() noexcept -> FloatVector<Robot::dimension> = 0;
virtual inline auto next() noexcept -> FloatVector<Robot::State::num_scalars> = 0;

Distribution dist;
};
Expand Down
6 changes: 3 additions & 3 deletions src/impl/vamp/random/xorshift.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,22 +26,22 @@ namespace vamp::rng
uint64_t key2_init;

avx_xorshift128plus_key_t key{};
using IntVector = Vector<SIMDVector<__m256i>, 1, Robot::dimension>;
using IntVector = Vector<SIMDVector<__m256i>, 1, Robot::State::num_scalars>;
IntVector buffer;

inline void reset() noexcept override final
{
avx_xorshift128plus_init(key1_init, key2_init, &key);
}

inline auto next() noexcept -> FloatVector<Robot::dimension> override final
inline auto next() noexcept -> FloatVector<Robot::State::num_scalars> override final
{
for (auto i = 0U; i < IntVector::num_vectors; ++i)
{
buffer.data[i] = avx_xorshift128plus(&key);
}

auto result = FloatVector<Robot::dimension>::map_to_range(buffer, 0.F, 1.F);
auto result = FloatVector<Robot::State::num_scalars>::map_to_range(buffer, 0.F, 1.F);
Robot::scale_configuration(result);
return result;
}
Expand Down
9 changes: 9 additions & 0 deletions src/impl/vamp/robots/baxter.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,15 @@ namespace vamp::robots
template <std::size_t rake>
using ConfigurationBlock = FloatVector<rake, dimension>;

// Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over.
// Defaults to Configuration; robots that plan in a different space (e.g. task space) can
// redefine this family independently of the FK/FKCC-facing Configuration types above.
using State = Configuration;
using StateArray = ConfigurationArray;
using StateBuffer = ConfigurationBuffer;
template <std::size_t rake>
using StateBlock = ConfigurationBlock<rake>;

template <std::size_t rake>
struct Spheres
{
Expand Down
9 changes: 9 additions & 0 deletions src/impl/vamp/robots/fetch.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,15 @@ namespace vamp::robots
template <std::size_t rake>
using ConfigurationBlock = FloatVector<rake, dimension>;

// Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over.
// Defaults to Configuration; robots that plan in a different space (e.g. task space) can
// redefine this family independently of the FK/FKCC-facing Configuration types above.
using State = Configuration;
using StateArray = ConfigurationArray;
using StateBuffer = ConfigurationBuffer;
template <std::size_t rake>
using StateBlock = ConfigurationBlock<rake>;

template <std::size_t rake>
struct Spheres
{
Expand Down
9 changes: 9 additions & 0 deletions src/impl/vamp/robots/panda.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,15 @@ namespace vamp::robots
template <std::size_t rake>
using ConfigurationBlock = FloatVector<rake, dimension>;

// Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over.
// Defaults to Configuration; robots that plan in a different space (e.g. task space) can
// redefine this family independently of the FK/FKCC-facing Configuration types above.
using State = Configuration;
using StateArray = ConfigurationArray;
using StateBuffer = ConfigurationBuffer;
template <std::size_t rake>
using StateBlock = ConfigurationBlock<rake>;

template <std::size_t rake>
struct Spheres
{
Expand Down
9 changes: 9 additions & 0 deletions src/impl/vamp/robots/sphere.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,15 @@ namespace vamp::robots
{
};

// Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over.
// Defaults to Configuration; robots that plan in a different space (e.g. task space) can
// redefine this family independently of the FK/FKCC-facing Configuration types above.
using State = Configuration;
using StateArray = ConfigurationArray;
using StateBuffer = ConfigurationBuffer;
template <std::size_t rake>
using StateBlock = ConfigurationBlock<rake>;

static constexpr std::array<std::string_view, dimension> joint_names = {"x", "y", "z"};
static constexpr const char *end_effector = "";

Expand Down
9 changes: 9 additions & 0 deletions src/impl/vamp/robots/ur5.hh
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,15 @@ namespace vamp::robots
template <std::size_t rake>
using ConfigurationBlock = FloatVector<rake, dimension>;

// Planning-space types: what planners (RRTC, PRM, FCIT, ...) sample and interpolate over.
// Defaults to Configuration; robots that plan in a different space (e.g. task space) can
// redefine this family independently of the FK/FKCC-facing Configuration types above.
using State = Configuration;
using StateArray = ConfigurationArray;
using StateBuffer = ConfigurationBuffer;
template <std::size_t rake>
using StateBlock = ConfigurationBlock<rake>;

template <std::size_t rake>
struct Spheres
{
Expand Down
Loading