Commit 904d781b authored by VAN TOLL Wouter's avatar VAN TOLL Wouter
Browse files

- ORCA solution is now stored in a struct, for easy re-use.

- Changed the ORCA solver and cost function accordingly.
parent 548157e9
......@@ -8,6 +8,7 @@
* - 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.
* - Added the Solution struct.
* - Changed the names of several functions.
* - Some auxiliary classes and functions have different names in the UMANS library.
*
......@@ -66,4 +67,13 @@ namespace ORCALibrary
*/
Vector2D direction;
};
struct Solution
{
Vector2D velocity;
bool isFeasible;
std::vector<Line> orcaLines;
size_t numObstLines = 0;
float currentSimulationTime = -1;
};
}
\ No newline at end of file
......@@ -8,6 +8,7 @@
* - 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.
* - Added the Solution struct.
* - Changed the names of several functions.
* - Some auxiliary classes and functions have different names in the UMANS library.
*
......@@ -57,7 +58,8 @@ namespace ORCALibrary
}
/* Search for the best new velocity. */
bool ORCASolver::solveOrcaProgram(const Agent& agent, Vector2D& result_newVelocity) const
Solution Solver::solveOrcaProgram(const Agent& agent,
const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors) const
{
const Vector2D& prefVelocity_ = agent.getPreferredVelocity();
const float maxSpeed_ = agent.getMaximumSpeed();
......@@ -71,20 +73,28 @@ namespace ORCALibrary
// radius = 2.0f;
// maxSpeed = 2.0f;
const auto& orcaLines_ = agent.GetOrcaLines();
// TODO: include obstacle lines
const size_t numObstLines = 0;
Solution result;
// solve linear program
createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors);
size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, result_newVelocity);
if (lineFail < orcaLines_.size())
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, result_newVelocity);
// TODO: include obstacle lines
// ...
return lineFail;
// solve linear program
size_t lineFail = linearProgram2(result.orcaLines, maxSpeed_, prefVelocity_, false, result.velocity);
result.isFeasible = lineFail == result.orcaLines.size();
// if the linear program is not feasible, compute the "least bad" velocity via another linear program
if (!result.isFeasible)
linearProgram3(result.orcaLines, result.numObstLines, lineFail, maxSpeed_, result.velocity);
// store the simulation time
result.currentSimulationTime = currentTime;
return result;
}
void ORCASolver::createAgentOrcaLines(const Agent& agent, std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep,
void Solver::createAgentOrcaLines(const Agent& agent, std::vector<Line>& orcaLines_, const float timeHorizon_, const float simulationTimeStep,
const AgentNeighborList& agentNeighbors_) const
{
const Vector2D& position_ = agent.getPosition();
......
......@@ -8,6 +8,7 @@
* - 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.
* - Added the Solution struct.
* - Changed the names of several functions.
* - Some auxiliary classes and functions have different names in the UMANS library.
*
......@@ -59,11 +60,13 @@ namespace ORCALibrary
*/
const float EPSILON = 0.00001f;
class ORCASolver
class Solver
{
public:
bool solveOrcaProgram(const Agent& agent, Vector2D& result_newVelocity) const;
Solution solveOrcaProgram(const Agent& agent,
const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors) 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,
......
......@@ -23,6 +23,7 @@
#define LIB_ORCA_H
#include <core/costFunction.h>
#include "../3rd-party/ORCA/ORCALine.h"
/// @ingroup costfunctions
/// <summary>The cost function of the %ORCA collision-avoidance model proposed by van den Berg et al.,
......@@ -53,6 +54,9 @@ public:
virtual float GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const override;
virtual Vector2D GetGlobalMinimum(Agent* agent, const WorldBase* world) const override;
void parseParameters(const CostFunctionParameters & params) override;
private:
const ORCALibrary::Solution& GetOrcaSolutionForAgent(Agent* agent, const WorldBase* world) const;
};
#endif //LIB_ORCA_H
......@@ -82,9 +82,7 @@ private:
// Random-number generation
std::default_random_engine RNGengine_;
std::vector<ORCALibrary::Line> orcaLines_;
bool orcaFeasible_;
Vector2D orcaResult_;
ORCALibrary::Solution orcaSolution_;
public:
......@@ -210,12 +208,8 @@ public:
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_; }
inline const ORCALibrary::Solution& GetOrcaSolution() const { return orcaSolution_; }
inline void SetOrcaSolution(const ORCALibrary::Solution& solution) { orcaSolution_ = solution; }
#pragma endregion
};
......
......@@ -23,25 +23,35 @@
#include <core/worldBase.h>
#include "../3rd-party/ORCA/ORCASolver.h"
using namespace std;
const ORCALibrary::Solution& ORCA::GetOrcaSolutionForAgent(Agent* agent, const WorldBase* world) const
{
// check if the agent needs to run ORCA again
const auto& cachedSolution = agent->GetOrcaSolution();
if (cachedSolution.currentSimulationTime < world->GetCurrentTime())
{
// run ORCA and store the solution in the agent
ORCALibrary::Solver solver;
agent->SetOrcaSolution(
solver.solveOrcaProgram(*agent, timeHorizon, world->GetCurrentTime(), world->GetDeltaTime(), agent->getNeighborSubsetInRange(range_))
);
}
// return the result
return agent->GetOrcaSolution();
}
float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
{
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();
// compute or re-use the ORCA solution for this agent
const auto& orcaSolution = GetOrcaSolutionForAgent(agent, world);
if (orcaFeasible)
if (orcaSolution.isFeasible)
{
// 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)
for (const auto& orcaLine : orcaSolution.orcaLines)
{
float distanceToOrcaLine = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction);
if (distanceToOrcaLine < 0)
......@@ -56,7 +66,7 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo
// 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)
for (const auto& orcaLine : orcaSolution.orcaLines)
{
float distanceToOrcaLine = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction);
bestDistance = std::max(bestDistance, distanceToOrcaLine);
......@@ -68,12 +78,11 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo
Vector2D ORCA::GetGlobalMinimum(Agent* agent, const WorldBase* world) const
{
// check if the agent needs to run ORCA again
if (agent->GetOrcaLines().empty())
agent->ComputeOrcaSolution(timeHorizon, world->GetDeltaTime(), agent->getNeighborSubsetInRange(range_));
// compute or re-use the ORCA solution for this agent
const auto& orcaSolution = GetOrcaSolutionForAgent(agent, world);
// return the solution that was computed by ORCA
return agent->GetOrcaResult();
// return the optimal velocity that was computed by ORCA
return orcaSolution.velocity;
}
void ORCA::parseParameters(const CostFunctionParameters & params)
......
......@@ -86,9 +86,6 @@ void Agent::UpdateVelocityAndPosition(float dt)
// update the position
position_ = position_ + (velocity_ * dt);
// clear ORCA result for the next timestep
orcaLines_.clear();
}
#pragma endregion
......@@ -146,16 +143,4 @@ 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