private double Evaluate_Phase2_Balancing(IBlackBox box)
{
// Init sim world. We add extra length to the track to allow cart to overshoot, we then detect overshooting by monitoring the cart's X position
// (this is just simpler and more robust than detecting if the cart has hit the ends of the track exactly).
SinglePoleBalancingWorld simWorld = new SinglePoleBalancingWorld(__TrackLength + 0.5f, __SixDegrees);
simWorld.InitSimulationWorld();
// Run the pole-balancing simulation.
int timestep = 0;
for (; timestep < _maxTimestepsPhase2; timestep++)
{
SimulateOneStep(simWorld, box);
// Check for failure state. Has the cart run off the ends of the track or has the pole
// angle gone beyond the threshold.
if ((simWorld.CartPosX < -__TrackLengthHalf) || (simWorld.CartPosX > __TrackLengthHalf) ||
(simWorld.PoleAngle > __TwelveDegrees) || (simWorld.PoleAngle < -__TwelveDegrees))
{
break;
}
}
return(timestep);
}
/// <summary>
/// Create a Box2D simulation world.
/// </summary>
protected override SimulationWorld CreateSimulationWorld()
{
// Init sim world. We add extra length to the track to allow cart to overshoot, we then detect overshooting by monitoring the cart's X position
// (this is just simpler and more robust than detecting if the cart has hit the ends of the track exactly).
SinglePoleBalancingWorld simWorld = new SinglePoleBalancingWorld(__TrackLength + 0.5f, __180Degrees);
simWorld.InitSimulationWorld();
return(simWorld);
}
private double Evaluate_Phase1_SwingUp(IBlackBox box)
{
// Init sim world. We add extra length to the track to allow cart to overshoot, we then detect overshooting by monitoring the cart's X position
// (this is just simpler and more robust than detecting if the cart has hit the ends of the track exactly).
SinglePoleBalancingWorld simWorld = new SinglePoleBalancingWorld(__TrackLength + 0.5f, __180Degrees);
simWorld.InitSimulationWorld();
// Record closest approach to target state and the timestep that it occured on.
double lowestError = double.MaxValue;
int timestepLowest = 0;
// Run the pole-balancing simulation.
int timestep = 0;
for (; timestep < _maxTimestepsPhase1; timestep++)
{
SimulateOneStep(simWorld, box);
// Calc state distance from target state.
double cartPosError = Math.Abs(simWorld.CartPosX);
double cartVelocityError = Math.Abs(simWorld.CartVelocityX);
double poleAngleError = Math.Abs(simWorld.PoleAngle);
double poleAnglularVelocityError = Math.Abs(simWorld.PoleAngularVelocity);
double error = (poleAngleError) + (poleAnglularVelocityError) + (cartPosError);
// Check for failure state. Has the cart run off the ends of the track.
if ((simWorld.CartPosX < -__TrackLengthHalf) || (simWorld.CartPosX > __TrackLengthHalf))
{
return(0.0);
}
// Track best state achieved.
if (error < lowestError)
{
lowestError = error;
timestepLowest = timestep;
}
}
if (0.0 == lowestError)
{
return(10e9);
}
// Alternative form of 1/x that avoids rapid rise to infinity as lowestError tends towards zero.
return(Math.Log10(1.0 + (1 / (lowestError + 0.1))));
}
private void SimulateOneStep(SinglePoleBalancingWorld simWorld, IBlackBox box)
{
// Provide state info to the black box inputs.
box.InputSignalArray[0] = simWorld.CartPosX / __TrackLengthHalf; // CartPosX range is +-trackLengthHalf. Here we normalize it to [-1,1].
box.InputSignalArray[1] = simWorld.CartVelocityX; // Cart track velocity x is typically +-0.75.
box.InputSignalArray[2] = simWorld.PoleAngle / __TwelveDegrees; // Rescale angle to match range of values during balancing.
box.InputSignalArray[3] = simWorld.PoleAngularVelocity; // Pole angular velocity is typically +-1.0 radians. No scaling required.
// Activate the network.
box.Activate();
// Read the network's force signal output.
float force = (float)(box.OutputSignalArray[0] - 0.5f) * __MaxForceNewtonsX2;
// Simulate one timestep.
simWorld.SetCartForce(force);
simWorld.Step();
}
/// <summary>
/// Invoke any required control logic in the Box2D world.
/// </summary>
protected override void InvokeController()
{
// Provide state info to the black box inputs.
SinglePoleBalancingWorld simWorld = (SinglePoleBalancingWorld)_simWorld;
// _box is updated by other threads so copy the reference so that we know we are workign with the same IBlackBox within this method.
IBlackBox box = _box;
box.InputSignalArray[0] = simWorld.CartPosX / __TrackLengthHalf; // CartPosX range is +-trackLengthHalf. Here we normalize it to [-1,1].
box.InputSignalArray[1] = simWorld.CartVelocityX; // Cart track velocity x is typically +-0.75.
box.InputSignalArray[2] = simWorld.PoleAngle / __TwelveDegrees; // Rescale angle to match range of values during balancing.
box.InputSignalArray[3] = simWorld.PoleAngularVelocity; // Pole angular velocity is typically +-1.0 radians. No scaling required.
// Activate the network.
box.Activate();
// Read the network's force signal output.
float force = (float)(box.OutputSignalArray[0] - 0.5f) * __MaxForceNewtonsX2;
simWorld.SetCartForce(force);
}
