Commit faa35e00 by VAN TOLL Wouter

### Improved ORCA::GetCost(). It is now conceptually simpler, *and* it replaces...

```Improved ORCA::GetCost(). It is now conceptually simpler, *and* it replaces MaxFloat (for forbidden velocities) by something more meaningful.

As a result, ORCA can now safely be used in a "sampling" or "gradient" policy.```
parent a0312667
 ... ... @@ -53,7 +53,38 @@ float ORCA::GetCost(const Vector2D& velocity, Agent* agent, const WorldBase * wo // compute or re-use the ORCA solution for this agent 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. float maxDistance = -MaxFloat; 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(); } // // b) ORCA has a solution, but this velocity is not inside the solution space. // In this case, according to "the real ORCA method", we should return an infinite cost to prevent this velocity from being chosen. // However, in the context of sampling and gradients, it is better to return a finite value, to distinguish between "bad" and "even worse" velocities. // 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. // // c) ORCA does not have a solution, and we need to use ORCA's "backup function", which is just maxDistance. // In this case, it does not hurt to add the same large constant as in case (b). // // In short, both cases can actually use the same cost function: return maxDistance + 2 * agent->getMaximumSpeed(); /*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. ... ... @@ -70,15 +101,15 @@ 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 signed line distances. float bestDistance = -MaxFloat; float maxDistance = -MaxFloat; for (const auto& orcaLine : orcaSolution.orcaLines) { float distanceToOrcaLine = getSignedDistanceToOrcaLine(velocity, orcaLine); bestDistance = std::max(bestDistance, distanceToOrcaLine); maxDistance = std::max(maxDistance, distanceToOrcaLine); } return bestDistance; } return maxDistance; }*/ } Vector2D ORCA::GetGlobalMinimum(Agent* agent, const WorldBase* world) const ... ...
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