Une MAJ de sécurité est nécessaire sur notre version actuelle. Elle sera effectuée lundi 02/08 entre 12h30 et 13h. L'interruption de service devrait durer quelques minutes (probablement moins de 5 minutes).

Commit 548157e9 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

- Agent now caches ORCA results.

- Implemented ORCA::GetCost() using those cached results.
- Added ORCA folder to CMakeLists.txt.
parent 9a66a65b
/*
* =====================================
* NOTICE: This is an adapted version of the file Agent.h from the RVO2 library.
* It has been changed by Wouter van Toll for integration into the UMANS library.
*
* Overview of changes:
* - Renamed RVO(2) to ORCA.
* - Removed all Agent details; these are now managed by our own Agent class.
* Only the ORCA-specific calculations remain.
* - Moved the Line class to a separate file.
* - Changed the names of several functions.
* - Some auxiliary classes and functions have different names in the UMANS library.
*
* Original RVO2 license header below:
* =====================================
*
* Agent.h
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#pragma once
#include <tools/vector2D.h>
namespace ORCALibrary
{
/**
* \brief Defines a directed line.
*/
class Line {
public:
/**
* \brief A point on the directed line.
*/
Vector2D point;
/**
* \brief The direction of the directed line.
*/
Vector2D direction;
};
}
\ No newline at end of file
......@@ -4,8 +4,10 @@
* It has been changed by Wouter van Toll for integration into the UMANS library.
*
* Overview of changes:
* - Renamed RVO(2) to ORCA.
* - Removed all Agent details; these are now managed by our own Agent class.
* Only the ORCA-specific calculations remain.
* - Moved the Line class to a separate file.
* - Changed the names of several functions.
* - Some auxiliary classes and functions have different names in the UMANS library.
*
......@@ -55,8 +57,7 @@ namespace ORCALibrary
}
/* Search for the best new velocity. */
Vector2D ORCASolver::computeNewVelocity(const Agent& agent,
const float timeHorizonObst_, const float timeHorizon_, const float simulationTimeStep, const NeighborList& neighbors_) const
bool ORCASolver::solveOrcaProgram(const Agent& agent, Vector2D& result_newVelocity) const
{
const Vector2D& prefVelocity_ = agent.getPreferredVelocity();
const float maxSpeed_ = agent.getMaximumSpeed();
......@@ -70,23 +71,17 @@ namespace ORCALibrary
// radius = 2.0f;
// maxSpeed = 2.0f;
std::vector<Line> orcaLines_;
// TODO: handle obstacle line segments
//createObstacleOrcaLines(agent, orcaLines_, timeHorizonObst_, simulationTimeStep, neighbors_.second);
const size_t numObstLines = orcaLines_.size();
createAgentOrcaLines(agent, orcaLines_, timeHorizon_, simulationTimeStep, neighbors_);
const auto& orcaLines_ = agent.GetOrcaLines();
// TODO: include obstacle lines
const size_t numObstLines = 0;
// solve linear program
Vector2D newVelocity_; // will store the result
size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);
size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, result_newVelocity);
if (lineFail < orcaLines_.size())
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, result_newVelocity);
return newVelocity_;
return lineFail;
}
void ORCASolver::createAgentOrcaLines(const Agent& agent, std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep,
......@@ -181,7 +176,7 @@ namespace ORCALibrary
const float denominator = det(lines[lineNo].direction, lines[i].direction);
const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (std::fabs(denominator) <= RVO_EPSILON) {
if (std::fabs(denominator) <= EPSILON) {
/* Lines lineNo and i are (almost) parallel. */
if (numerator < 0.0f) {
return false;
......@@ -283,7 +278,7 @@ namespace ORCALibrary
float determinant = det(lines[i].direction, lines[j].direction);
if (std::fabs(determinant) <= RVO_EPSILON) {
if (std::fabs(determinant) <= EPSILON) {
/* Line i and line j are parallel. */
if (lines[i].direction.dot(lines[j].direction) > 0.0f) {
/* Line i and line j point in the same direction. */
......
......@@ -4,8 +4,10 @@
* It has been changed by Wouter van Toll for integration into the UMANS library.
*
* Overview of changes:
* - Renamed RVO(2) to ORCA.
* - Removed all Agent details; these are now managed by our own Agent class.
* Only the ORCA-specific calculations remain.
* - Moved the Line class to a separate file.
* - Changed the names of several functions.
* - Some auxiliary classes and functions have different names in the UMANS library.
*
......@@ -46,7 +48,7 @@
#pragma once
#include <core/agent.h>
#include <tools/vector2D.h>
#include "ORCALine.h"
namespace ORCALibrary
{
......@@ -55,31 +57,13 @@ namespace ORCALibrary
/**
* \brief A sufficiently small positive number.
*/
const float RVO_EPSILON = 0.00001f;
/**
* \brief Defines a directed line.
*/
class Line {
public:
/**
* \brief A point on the directed line.
*/
Vector2D point;
/**
* \brief The direction of the directed line.
*/
Vector2D direction;
};
const float EPSILON = 0.00001f;
class ORCASolver
{
public:
Vector2D computeNewVelocity(const Agent& agent,
const float timeHorizonObst_, const float timeHorizon_, const float simulationTimeStep, const NeighborList& agentNeighbors_) const;
bool solveOrcaProgram(const Agent& agent, Vector2D& result_newVelocity) const;
private:
/*void createObstacleOrcaLines(const Agent& agent,
std::vector<Line>& orcaLines_, const float timeHorizonObst_, const float simulationTimeStep, const ObstacleNeighborList& obstacleNeighbors_) const;*/
void createAgentOrcaLines(const Agent& agent,
......
......@@ -27,12 +27,12 @@ set (CMAKE_CXX_STANDARD 14)
# === Include 3rd-party code
include_directories( ./include )
include_directories(./3rd-party/tinyxml/ ./3rd-party/nanoflann/)
include_directories(./3rd-party/tinyxml/ ./3rd-party/nanoflann/ ./3rd-party/ORCA/)
#link_directories( ./lib/${CMAKE_BUILD_TYPE} )
file( GLOB_RECURSE source_files src/* include/*)
file( GLOB_RECURSE 3rd_party 3rd-party/tinyxml/* 3rd-party/nanoflann/*)
file( GLOB_RECURSE 3rd_party 3rd-party/*)
# === Use multithreading?
......
......@@ -27,6 +27,7 @@
#include <core/policy.h>
#include <memory>
#include <random>
#include "../3rd-party/ORCA/ORCALine.h"
class WorldBase;
......@@ -81,6 +82,10 @@ private:
// Random-number generation
std::default_random_engine RNGengine_;
std::vector<ORCALibrary::Line> orcaLines_;
bool orcaFeasible_;
Vector2D orcaResult_;
public:
#pragma region [Simulation-loop methods]
......@@ -204,6 +209,15 @@ public:
/// <returns>A random number between min and max, obtained via uniform random sampling.</returns>
float ComputeRandomNumber(float min, float max);
#pragma region [ORCA]
void ComputeOrcaSolution(float timeHorizon, float simulationTimeStep, const NeighborList& agentNeighbors);
inline const std::vector<ORCALibrary::Line>& GetOrcaLines() const { return orcaLines_; }
inline bool IsOrcaFeasible() const { return orcaFeasible_; }
inline const Vector2D& GetOrcaResult() const { return orcaResult_; }
#pragma endregion
};
#endif //LIB_AGENT_H
......@@ -27,14 +27,53 @@ using namespace std;
float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
{
// TODO: implement cost function for ORCA
return 0;
const Vector2D& currentVelocity = agent->getVelocity();
// check if the agent needs to run ORCA again
if (agent->GetOrcaLines().empty())
agent->ComputeOrcaSolution(timeHorizon, world->GetDeltaTime(), agent->getNeighborSubsetInRange(range_));
// get the result of the ORCA program
const auto& orcaLines = agent->GetOrcaLines();
const bool orcaFeasible = agent->IsOrcaFeasible();
if (orcaFeasible)
{
// In this case, if the query velocity lies on the wrong side of any ORCA line, its cost is infinite.
// Because the ORCA program is feasible, there should be at least one velocity for which this is not the case.
for (const auto& orcaLine : orcaLines)
{
float distanceToOrcaLine = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction);
if (distanceToOrcaLine < 0)
return MaxFloat;
}
// otherwise, this velocity is apparently allowed, and the cost is the difference to vPref
return (velocity - agent->getPreferredVelocity()).magnitude();
}
else
{
// In this case, the ORCA program is infeasible, so any velocity lies on the wrong side of at least one ORCA line.
// We should then look for the "least bad" velocity. The cost of a velocity is the largest of all line distances (= the max of all negative numbers).
float bestDistance = -MaxFloat;
for (const auto& orcaLine : orcaLines)
{
float distanceToOrcaLine = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction);
bestDistance = std::max(bestDistance, distanceToOrcaLine);
}
return bestDistance;
}
}
Vector2D ORCA::GetGlobalMinimum(Agent* agent, const WorldBase* world) const
{
ORCALibrary::ORCASolver solver;
return solver.computeNewVelocity(*agent, timeHorizon, timeHorizon, world->GetDeltaTime(), agent->getNeighborSubsetInRange(range_));
// check if the agent needs to run ORCA again
if (agent->GetOrcaLines().empty())
agent->ComputeOrcaSolution(timeHorizon, world->GetDeltaTime(), agent->getNeighborSubsetInRange(range_));
// return the solution that was computed by ORCA
return agent->GetOrcaResult();
}
void ORCA::parseParameters(const CostFunctionParameters & params)
......
......@@ -21,6 +21,7 @@
#include <core/agent.h>
#include <core/worldBase.h>
#include "../3rd-party/ORCA/ORCASolver.h"
Agent::Agent(size_t id, const Agent::Settings& settings) :
id_(id), settings_(settings),
......@@ -85,6 +86,9 @@ void Agent::UpdateVelocityAndPosition(float dt)
// update the position
position_ = position_ + (velocity_ * dt);
// clear ORCA result for the next timestep
orcaLines_.clear();
}
#pragma endregion
......@@ -142,4 +146,16 @@ float Agent::ComputeRandomNumber(float min, float max)
{
auto distribution = std::uniform_real_distribution<float>(min, max);
return distribution(RNGengine_);
}
void Agent::ComputeOrcaSolution(float timeHorizon, float simulationTimeStep, const NeighborList& agentNeighbors)
{
ORCALibrary::ORCASolver solver;
// compute ORCA lines
solver.createAgentOrcaLines(*this, orcaLines_, timeHorizon, simulationTimeStep, agentNeighbors);
// TODO: include obstacle lines
// try to solve the linear program
orcaFeasible_ = solver.solveOrcaProgram(*this, orcaResult_);
}
\ No newline at end of file
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment