Commit 84230267 authored by VAN TOLL Wouter's avatar VAN TOLL Wouter

Merge branch '71-fix-ORCA-cost-function' into 'master'

Resolve "Implement ORCA::GetCost() correctly"

Closes #71

See merge request !106
parents 69d861fc 5cc9ddeb
...@@ -58,8 +58,9 @@ namespace ORCALibrary ...@@ -58,8 +58,9 @@ namespace ORCALibrary
} }
/* Search for the best new velocity. */ /* Search for the best new velocity. */
Solution Solver::solveOrcaProgram(const Agent& agent, void Solver::solveOrcaProgram(const Agent& agent,
const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance,
Solution& result) const
{ {
const Vector2D& prefVelocity_ = agent.getPreferredVelocity(); const Vector2D& prefVelocity_ = agent.getPreferredVelocity();
const float maxSpeed_ = agent.getMaximumSpeed(); const float maxSpeed_ = agent.getMaximumSpeed();
...@@ -73,7 +74,7 @@ namespace ORCALibrary ...@@ -73,7 +74,7 @@ namespace ORCALibrary
// radius = 2.0f; // radius = 2.0f;
// maxSpeed = 2.0f; // maxSpeed = 2.0f;
Solution result; result = Solution();
createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first, maxDistance); createAgentOrcaLines(agent, result.orcaLines, timeHorizon, simulationTimeStep, neighbors.first, maxDistance);
...@@ -90,8 +91,6 @@ namespace ORCALibrary ...@@ -90,8 +91,6 @@ namespace ORCALibrary
// store the simulation time // store the simulation time
result.currentSimulationTime = currentTime; result.currentSimulationTime = currentTime;
return result;
} }
void Solver::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,
......
...@@ -64,8 +64,8 @@ namespace ORCALibrary ...@@ -64,8 +64,8 @@ namespace ORCALibrary
class Solver class Solver
{ {
public: public:
Solution solveOrcaProgram(const Agent& agent, void solveOrcaProgram(const Agent& agent,
const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance) const; const float timeHorizon, const float currentTime, const float simulationTimeStep, const NeighborList& neighbors, const float maxDistance, Solution& result) const;
private: private:
/*void createObstacleOrcaLines(const Agent& agent, /*void createObstacleOrcaLines(const Agent& agent,
......
...@@ -31,15 +31,21 @@ const ORCALibrary::Solution& ORCA::GetOrcaSolutionForAgent(Agent* agent, const W ...@@ -31,15 +31,21 @@ const ORCALibrary::Solution& ORCA::GetOrcaSolutionForAgent(Agent* agent, const W
{ {
// run ORCA and store the solution in the agent // run ORCA and store the solution in the agent
ORCALibrary::Solver solver; ORCALibrary::Solver solver;
agent->SetOrcaSolution( solver.solveOrcaProgram(*agent, timeHorizon, (float)world->GetCurrentTime(), world->GetDeltaTime(), agent->getNeighbors(), range_, agent->GetOrcaSolution());
solver.solveOrcaProgram(*agent, timeHorizon, (float)world->GetCurrentTime(), world->GetDeltaTime(), agent->getNeighbors(), range_)
);
} }
// return the result // return the result
return agent->GetOrcaSolution(); return agent->GetOrcaSolution();
} }
inline float getSignedDistanceToOrcaLine(const Vector2D& velocity, const ORCALibrary::Line& line)
{
//return (line.point - velocity).dot(Vector2D(line.direction.y, -line.direction.x));
const Vector2D& pMinV = line.point - velocity;
return line.direction.x * pMinV.y - line.direction.y * pMinV.x;
}
float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * world) const
{ {
const Vector2D& currentVelocity = agent->getVelocity(); const Vector2D& currentVelocity = agent->getVelocity();
...@@ -47,33 +53,36 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo ...@@ -47,33 +53,36 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo
// compute or re-use the ORCA solution for this agent // compute or re-use the ORCA solution for this agent
const auto& orcaSolution = GetOrcaSolutionForAgent(agent, world); const auto& orcaSolution = GetOrcaSolutionForAgent(agent, world);
if (orcaSolution.isFeasible) // Find the maximum distance by which this velocity exceeds any ORCA plane.
{ // The "getSignedDistanceToOrcaLine" function returns a positive number if the ORCA constraint is violated.
// In this case, if the query velocity lies on the wrong side of any ORCA line, its cost is infinite. float maxDistance = -MaxFloat;
// Because the ORCA program is feasible, there should be at least one velocity for which this is not the case.
for (const auto& orcaLine : orcaSolution.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 for (const auto& orcaLine : orcaSolution.orcaLines)
maxDistance = std::max(maxDistance, getSignedDistanceToOrcaLine(velocity, orcaLine));
// There are three possible cases:
//
// a) ORCA has a solution, and this velocity is inside the solution space.
// In this case, maxDistance has to be <= 0, because the velocity is on the correct side of all ORCA lines.
// For such "allowed" velocities, ORCA uses the difference to vPref as the cost.
//
if (maxDistance <= 0)
{
// the cost is the difference to vPref
return (velocity - agent->getPreferredVelocity()).magnitude(); return (velocity - agent->getPreferredVelocity()).magnitude();
} }
else //
{ // b) ORCA has a solution, but this velocity is not inside the solution space.
// In this case, the ORCA program is infeasible, so any velocity lies on the wrong side of at least one ORCA line. // In this case, according to "the real ORCA method", we should return an infinite cost to prevent this velocity from being chosen.
// 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). // However, in the context of sampling and gradients, it is better to return a finite value, to distinguish between "bad" and "even worse" velocities.
float bestDistance = -MaxFloat; // Returning maxDistance is a good option, but we should add a sufficiently large constant, so that velocities inside the ORCA solution space will be preferred.
for (const auto& orcaLine : orcaSolution.orcaLines) //
{ // c) ORCA does not have a solution, and we need to use ORCA's "backup function", which is just maxDistance.
float distanceToOrcaLine = (velocity - currentVelocity - 0.5f*orcaLine.point).dot(orcaLine.direction); // In this case, it does not hurt to add the same large constant as in case (b).
bestDistance = std::max(bestDistance, distanceToOrcaLine); //
} // In short, both cases can actually use the same cost function:
return bestDistance; return maxDistance + 2 * agent->getMaximumSpeed();
}
} }
Vector2D ORCA::GetGlobalMinimum(Agent* agent, const WorldBase* world) const Vector2D ORCA::GetGlobalMinimum(Agent* agent, const WorldBase* world) const
......
...@@ -201,8 +201,10 @@ public: ...@@ -201,8 +201,10 @@ public:
float ComputeRandomNumber(float min, float max); float ComputeRandomNumber(float min, float max);
#pragma region [ORCA] #pragma region [ORCA]
inline const ORCALibrary::Solution& GetOrcaSolution() const { return orcaSolution_; } inline const ORCALibrary::Solution& GetOrcaSolution() const { return orcaSolution_; }
inline void SetOrcaSolution(const ORCALibrary::Solution& solution) { orcaSolution_ = solution; } inline ORCALibrary::Solution& GetOrcaSolution() { return orcaSolution_; }
#pragma endregion #pragma endregion
}; };
......
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