private double GetCost(RobotTwoWheelCommand command)
{
double lookAheadRemaining = lookAhead;
double cost = 0;
PointOnPath closestPoint = path.GetClosest(state.Pose.ToVector2());
PointOnPath lookAheadPoint = path.AdvancePoint(closestPoint, ref lookAheadRemaining);
RobotTwoWheelState newState = RobotTwoWheelModel.Simulate(command, state, (lookAhead - lookAheadRemaining) / command.velocity);
return(newState.Pose.ToVector2().DistanceTo(lookAheadPoint.pt) * DISTANCE_WEIGHT); // - command.velocity * VELOCITY_WEIGHT - command.turn * TURN_WEIGHT;
}
/// <summary>
/// Calculate a point starting from a RRTNode, while avoiding obstacles
/// </summary>
/// <param name="startNode">The node that path predicted from</param>
/// <param name="vInput">Velocity control input</param>
/// <param name="wInput">Turn Rate control input</param>
/// <param name="obstacles">Obstalces (List of Polygon)</param>
/// <returns>a RRTNode that has startNode as its parent. Returns null if fails</returns>
private RRTNode CalculateNextNode(RRTNode startNode, double vInput, double wInput, List <Polygon> obstacles)
{
//4) Divide the total RRT timestep into smaller sub-steps
//4a) calculate the trajectory at one substep given the control inputs and closest node initial conditions
//4b) check at each the linear path between the initial and simulation end does not intersect a polygon
//4c) if intersects, terminate and go to 1.
//4d) if not intersects
//4da) if last substep, add the results of this simulation to the closest node as a child
//4db) else simulate the next subtime step by going to 4a
RobotTwoWheelState lastSliceState = startNode.State;
RobotTwoWheelState curSliceState = startNode.State;
List <Vector2> simulationPoints = new List <Vector2>();
for (int i = 0; i < numSlice; i++)
{
simulationTimer.Start();
curSliceState = RobotTwoWheelModel.Simulate(new RobotTwoWheelCommand(vInput, wInput), lastSliceState, (timeStep / numSlice));
simulationTime.Add(simulationTimer.ElapsedMilliseconds);
simulationTimer.Reset();
obstacleTimer.Start();
if (IsHittingObstacle(lastSliceState.Pose.ToVector2(), curSliceState.Pose.ToVector2(), obstacles))
{
return(null);
}
obstacleTime.Add(obstacleTimer.ElapsedMilliseconds);
obstacleTimer.Reset();
simulationPoints.Add(lastSliceState.Pose.ToVector2());
lastSliceState = curSliceState;
}
simulationPoints.Add(lastSliceState.Pose.ToVector2());
RRTNode r = new RRTNode(curSliceState, startNode);
r.simulationPoints = simulationPoints;
return(r);
//return new RRTNode(rOut, vOut, wOut, 0, startNode);
}