Skip to content
This repository has been archived by the owner on Jan 12, 2022. It is now read-only.

Commit

Permalink
discrete search added with 2d test
Browse files Browse the repository at this point in the history
  • Loading branch information
@baskinburak
baskinburak committed May 10, 2020
1 parent 58d4b08 commit 8731571
Show file tree
Hide file tree
Showing 6 changed files with 119 additions and 82 deletions.
14 changes: 8 additions & 6 deletions include/rlss/CollisionShapes/AlignedBoxCollisionShape.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,28 +2,30 @@
#define RLSS_COLLISIONSHAPES_ALIGNED_BOX_HPP

#include <rlss/CollisionShapes/CollisionShape.hpp>
#include <rlss/internal/Util.hpp>

namespace rlss {

template<typename T, unsigned int DIM>
class AlignedBoxCollisionShape : CollisionShape<T, DIM> {
class AlignedBoxCollisionShape : public CollisionShape<T, DIM> {
public:
using Base = CollisionShape<T, DIM>;
using VectorDIM = Base::VectorDIM;
using StdVectorVectorDIM = Base::StdVectorVectorDIM:
using AlignedBox = rlss::AlignedBox<T, DIM>;
using VectorDIM = typename Base::VectorDIM;
using StdVectorVectorDIM = typename Base::StdVectorVectorDIM;
using AlignedBox = rlss::internal::AlignedBox<T, DIM>;

AlignedBoxCollisionShape(const AlignedBox& abox) : m_collision_box(abox) {

}


StdVectorVectorDIM convexHullPoints(const VectorDIM& com) const override {
AlignedBox box(m_collision_box.min() + com, m_collision_box.max() + com);
return rlss::internal::cornerPoints(box);
return rlss::internal::cornerPoints<T, DIM>(box);
}

AlignedBox boundingBox(const VectorDIM& com) const override {
return AlignedBox box(m_collision_box.min() + com, m_collision_box.max() + com);
return AlignedBox(m_collision_box.min() + com, m_collision_box.max() + com);
}

private:
Expand Down
2 changes: 1 addition & 1 deletion include/rlss/CollisionShapes/CollisionShape.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ class CollisionShape {
using StdVectorVectorDIM = rlss::internal::StdVectorVectorDIM<T, DIM>;
using AlignedBox = rlss::internal::AlignedBox<T, DIM>;

virtual ~CollisionShape() = 0;
virtual ~CollisionShape() = default;

// get the points on the convex hull of the collision shape
// at given center of mass
Expand Down
64 changes: 14 additions & 50 deletions include/rlss/OccupancyGrid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ class OccupancyGrid {
std::queue<Index> occupied_indexes;
occupied_indexes.push(min);
while(!occupied_indexes.empty()) {
const Index& occ = occupied_indexes.front();
Index& occ = occupied_indexes.front();
if(!this->isOccupied(occ)) {
filled_indexes.push_back(occ);
this->setOccupancy(occ);
Expand All @@ -127,7 +127,15 @@ class OccupancyGrid {
return this->fillOccupancy(this->getIndex(min), this->getIndex(max));
}

bool isOccupied(const Index& idx) const {
bool isOccupied(const Index& idx) const {
AlignedBox idxBox = this->toBox(idx);

for(const auto& bbox: m_temporary_obstacles) {
if(bbox.intersects(idxBox)) {
return true;
}
}

return m_grid.find(idx) != m_grid.end();
}

Expand All @@ -142,7 +150,7 @@ class OccupancyGrid {
std::queue<Index> indexes;
indexes.push(min);
while(!indexes.empty()) {
const Index& occ = indexes.front();
Index& occ = indexes.front();
if(this->isOccupied(occ))
return true;

Expand All @@ -160,60 +168,16 @@ class OccupancyGrid {

}

bool isOccupied(const StdVectorVectorDIM& ch) {
VectorDIM min = ch[0];
VectorDIM max = ch[0];
for(const auto& v : ch) {
for(unsigned int d = 0; d < DIM; d++) {
min(d) = std::min(min(d), v(d));
max(d) = std::max(max(d), v(d));
}
}
return this->isOccupied(AlignedBox(min, max));
}

void addObstacle(const AlignedBox& box) {
this->fillOccupancy(box.min(), box.max());
}

void addObstacle(const StdVectorVectorDIM& ch) {
VectorDIM min = ch[0];
VectorDIM max = ch[0];
for(const auto& v : ch) {
for(unsigned int d = 0; d < DIM; d++) {
min(d) = std::min(min(d), v(d));
max(d) = std::max(max(d), v(d));
}
}

this->addObstacle(AlignedBox(min, max));
}

void addTemporaryObstacle(const AlignedBox& box) {
std::vector<Index> newly_filled
= this->fillOccupancy(box.min(), box.max());

m_temporary_occupancy.insert(newly_filled.begin(), newly_filled.end());
}

void addTemporaryObstacle(const StdVectorVectorDIM& ch) {
VectorDIM min = ch[0];
VectorDIM max = ch[0];
for(const auto& v : ch) {
for(unsigned int d = 0; d < DIM; d++) {
min(d) = std::min(min(d), v(d));
max(d) = std::max(max(d), v(d));
}
}

this->addTemporaryObstacle(AlignedBox(min, max));
m_temporary_obstacles.push_back(box);
}

void clearTemporaryObstacles() {
for(const auto& idx : m_temporary_occupancy) {
m_grid.erase(idx);
}
m_temporary_occupancy.clear();
m_temporary_obstacles.clear();
}

AlignedBox toBox(const Index& idx) const {
Expand All @@ -231,7 +195,7 @@ class OccupancyGrid {
Coordinate m_step_size;
std::unordered_set<Index, IndexHasher> m_grid;

std::vector<Index> m_temporary_occupancy;
std::vector<AlignedBox> m_temporary_obstacles;
}; // class OccupancyGrid

} // namespace rlss
Expand Down
30 changes: 17 additions & 13 deletions include/rlss/RLSS.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,16 +55,16 @@ class RLSS {
std::optional<PiecewiseCurve> plan(
T current_time,
const StdVectorVectorDIM& current_robot_state,
const std::vector<StdVectorVectorDIM>&
other_robot_collision_shape_convex_hulls) {
const std::vector<AlignedBox>&
other_robot_collision_shape_bounding_boxes) {

std::vector<Hyperplane> robot_safety_hyperplanes
= robotSafetyHyperplanes(
current_robot_state[0],
other_robot_collision_shape_convex_hulls);
other_robot_collision_shape_bounding_boxes);

for(const auto& ch: other_robot_collision_shape_convex_hulls) {
m_occupancy_grid.addTemporaryObstacle(ch);
for(const auto& bbox: other_robot_collision_shape_bounding_boxes) {
m_occupancy_grid.addTemporaryObstacle(bbox);
}


Expand Down Expand Up @@ -148,10 +148,12 @@ class RLSS {
}

StdVectorVectorDIM path = discreteSearch(
current_robot_state[0],
m_original_trajectory.eval(
target_time_in_original_trajectory,
0
m_occupancy_grid.getIndex(current_robot_state[0]),
m_occupancy_grid.getIndex(
m_original_trajectory.eval(
target_time_in_original_trajectory,
0
)
),
m_occupancy_grid,
m_workspace,
Expand Down Expand Up @@ -232,16 +234,18 @@ class RLSS {

std::vector<Hyperplane> robotSafetyHyperplanes(
const VectorDIM& robot_position,
const std::vector<StdVectorVectorDIM>&
other_robot_collision_shape_convex_hulls) const {
const std::vector<AlignedBox>&
other_robot_collision_shape_bounding_boxes) const {

std::vector<Hyperplane> hyperplanes;

StdVectorVectorDIM robot_points
= m_collision_shape->convexHullPoints(robot_position);
= m_collision_shape->boundingBox(robot_position);

for(const auto& oth_collision_shape: other_robot_collision_shape_convex_hulls) {
for(const auto& oth_collision_shape_bbox: other_robot_collision_shape_bounding_boxes) {
StdVectorVectorDIM oth_points = rlss::internal::cornerPoints(oth_collision_shape_bbox);
Hyperplane svm_hp = rlss::internal::svm(robot_points, oth_collision_shape);

Hyperplane svm_shifted = this->shiftHyperplane(
robot_position,
robot_points,
Expand Down
49 changes: 37 additions & 12 deletions include/rlss/internal/DiscreteSearch.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,15 @@ std::optional<StdVectorVectorDIM<T, DIM>> discreteSearch(
struct State {
Index idx;
Index dir;
State(Index i) : idx(i), dir(Index::Zeros()) {}
State(Index i) : idx(i), dir(Index::Zero()) {}
State(Index i, Index d) : idx(i), dir(d) {}
bool operator==(const State& rhs) const {
return idx == rhs.idx && dir == rhs.dir;
}
};

struct StateHasher {
std::size_t operator()(const State& s) {
std::size_t operator()(const State& s) const {
std::size_t seed = 0;
for(unsigned int d = 0; d < DIM; d++) {
boost::hash_combine(seed, s.idx(d));
Expand All @@ -59,7 +62,7 @@ std::optional<StdVectorVectorDIM<T, DIM>> discreteSearch(
public:
Environment(
const OccupancyGrid& occ,
const AlignedBox* works,
const AlignedBox& works,
std::shared_ptr<CollisionShape> cols,
const Index& goal
)
Expand All @@ -85,30 +88,30 @@ std::optional<StdVectorVectorDIM<T, DIM>> discreteSearch(
neighbors) {
neighbors.clear();

if(s.dir == VectorDIM::Zero()) {
if(s.dir == Index::Zero()) {
for(unsigned int d = 0; d < DIM; d++) {
Index dir = Index::Zeros();
Index dir = Index::Zero();
dir(d) = 1;
Index idx = s.idx + dir;
if(this->indexValid(idx)) {
if(this->actionValid(s.idx, idx)) {
neighbors.emplace_back(State(idx, dir), Action::FORWARD, 1);
}

dir(d) = -1;
idx = s.idx + dir;
if(this->stateValid(idx)) {
if(this->actionValid(s.idx, idx)) {
neighbors.emplace_back(State(idx, dir), Action::FORWARD, 1);
}
}
} else {
Index idx = s.idx + s.dir;
if(this->indexValid(idx)) {
if(this->actionValid(s.idx, idx)) {
neighbors.emplace_back(State(idx, s.dir), Action::FORWARD, 1);
}

for(unsigned int d = 0; d < DIM; d++) {
if(s.dir(d) == 0) {
Index dir = Index::Zeros();
Index dir = Index::Zero();
dir(d) = 1;
neighbors.emplace_back(State(s.idx, dir), Action::ROTATE, 1);
dir(d) = -1;
Expand All @@ -123,15 +126,37 @@ std::optional<StdVectorVectorDIM<T, DIM>> discreteSearch(
void onDiscover(const State& /*s*/, int /*fScore*/, int /*gScore*/) {}

public:
bool actionValid(const Index& from_idx, const Index& to_idx) {
Coordinate from_center = m_occupancy_grid.getCenter(from_idx);
Coordinate to_center = m_occupancy_grid.getCenter(to_idx);

AlignedBox from_box = m_collision_shape->boundingBox(from_center);
AlignedBox to_box = m_collision_shape->boundingBox(to_center);

from_box.extend(to_box);

StdVectorVectorDIM from_pts = rlss::internal::cornerPoints<T, DIM>(from_box);

for(const auto& pt : from_pts) {
if(!m_workspace.contains(pt))
return false;
}

return !m_occupancy_grid.isOccupied(from_box);
}

bool indexValid(const Index& idx) {
Coordinate center = m_occupancy_grid.getCenter(idx);
StdVectorVectorDIM robot_pts = m_collision_shape->convexHullPoints(center);
AlignedBox robot_box = m_collision_shape->boundingBox(center);

StdVectorVectorDIM robot_pts = rlss::internal::cornerPoints<T, DIM>(robot_box);

for(const auto& pt : robot_pts) {
if(!m_workspace.contains(pt))
return false;
}

return !m_occupancy_grid.isOccupied(robot_pts);
return !m_occupancy_grid.isOccupied(robot_box);
}

private:
Expand Down Expand Up @@ -159,7 +184,7 @@ std::optional<StdVectorVectorDIM<T, DIM>> discreteSearch(
segments.push_back(occupancy_grid.getCenter(solution.states[i+1].first.idx));
}
}
segments.push_back(occupancy_grid.getCenter(solution.back().first.idx));
segments.push_back(occupancy_grid.getCenter(solution.states.back().first.idx));
return segments;
}

Expand Down
42 changes: 42 additions & 0 deletions tests/internal_DiscreteSearch_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,9 +2,51 @@
#include "catch.hpp"

#include <rlss/internal/DiscreteSearch.hpp>
#include <rlss/OccupancyGrid.hpp>
#include <rlss/CollisionShapes/AlignedBoxCollisionShape.hpp>
#include <optional>
#include <memory>

TEST_CASE("discrete search in 2d", "internal::DiscreteSearch") {
using OccupancyGrid = rlss::OccupancyGrid<double, 2U>;
using Coordinate = OccupancyGrid::Coordinate;
using Index = OccupancyGrid::Index;
using AlignedBox = OccupancyGrid::AlignedBox;
using AlignedBoxCollisionShape = rlss::AlignedBoxCollisionShape<double, 2U>;
using CollisionShape = rlss::CollisionShape<double, 2U>;
using StdVectorVectorDIM = rlss::internal::StdVectorVectorDIM<double, 2U>;
using VectorDIM = rlss::internal::VectorDIM<double, 2U>;

OccupancyGrid grid(Coordinate(0.5, 0.5));
grid.setOccupancy(Index(1,3));
grid.setOccupancy(Index(2,4));
grid.setOccupancy(Index(3,4));
grid.setOccupancy(Index(4,5));
grid.setOccupancy(Index(5,6));

auto al_collision_shape = std::make_shared<AlignedBoxCollisionShape>(AlignedBox(VectorDIM(-0.35, -0.35), VectorDIM(0.35, 0.35)));
auto collision_shape = std::static_pointer_cast<CollisionShape>(al_collision_shape);
Index start_idx(1,1);
Index goal_idx(7,7);

AlignedBox workspace(VectorDIM(0, 0), VectorDIM(4.5, 4.5));

auto result = rlss::internal::discreteSearch<double, 2U>(
start_idx,
goal_idx,
grid,
workspace,
collision_shape
);

REQUIRE(result != std::nullopt);

StdVectorVectorDIM result_vector = *result;

REQUIRE(result_vector.size() == 3);
REQUIRE((result_vector[0] - VectorDIM(0.75, 0.75)).squaredNorm() < 1e-9);
REQUIRE((result_vector[1] - VectorDIM(3.75, 0.75)).squaredNorm() < 1e-9);
REQUIRE((result_vector[2] - VectorDIM(3.75, 3.75)).squaredNorm() < 1e-9);
}

TEST_CASE("discrete search in 3d", "internal::DiscreteSearch") {
Expand Down

0 comments on commit 8731571

Please sign in to comment.