/// <summary>
/// Invoke any required control logic in the Box2D world.
/// </summary>
protected override void InvokeController()
{
// Provide state info to the black box inputs.
InvertedDoublePendulumWorld simWorld = (InvertedDoublePendulumWorld)_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.CartJointAngle / __TwelveDegrees; // Rescale angle to match range of values during balancing.
box.InputSignalArray[3] = simWorld.CartJointAngularVelocity; // Pole angular velocity is typically +-1.0 radians. No scaling required.
box.InputSignalArray[4] = simWorld.ElbowJointAngle / __TwelveDegrees;
box.InputSignalArray[5] = simWorld.ElbowJointAngularVelocity;
// Activate the network.
box.Activate();
// Read the network's force signal output.
// FIXME: Force mag should be configurable somewhere.
float force = (float)(box.OutputSignalArray[0] - 0.5f) * __MaxForceNewtonsX2;
simWorld.SetCartForce(force);
}
/// <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).
InvertedDoublePendulumWorld simWorld = new InvertedDoublePendulumWorld(__TrackLength + 0.5f, __ThreeDegrees, 0f);
simWorld.InitSimulationWorld();
return simWorld;
}
/// <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).
InvertedDoublePendulumWorld simWorld = new InvertedDoublePendulumWorld(__TrackLength + 0.5f, __ThreeDegrees, 0f);
simWorld.InitSimulationWorld();
return(simWorld);
}
/// <summary>
/// Evaluate the provided IBlackBox.
/// </summary>
public FitnessInfo Evaluate(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).
InvertedDoublePendulumWorld simWorld = new InvertedDoublePendulumWorld(__TrackLength + 0.5f, _poleAngleInitial, 0f);
simWorld.InitSimulationWorld();
// Run the pole-balancing simulation.
int timestep = 1; // Start at 1 because to are using modulus timestep to determine when to apply an external destabilising force, and we don't want that to happen at t=0.
float toggle = -1f;
for (; timestep < _maxTimesteps; timestep++)
{
// 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.CartJointAngle / __TwelveDegrees; // Rescale angle to match range of values during balancing.
box.InputSignalArray[3] = simWorld.CartJointAngularVelocity; // Pole angular velocity is typically +-1.0 radians. No scaling required.
box.InputSignalArray[4] = simWorld.ElbowJointAngle / __TwelveDegrees;
box.InputSignalArray[5] = simWorld.ElbowJointAngularVelocity;
// Activate the network.
box.Activate();
// Read the network's force signal output.
float force = (float)(box.OutputSignalArray[0] - 0.5f) * __MaxForceNewtonsX2;
// Periodically we give the cart a push to test how robust the balancing strategy is to external destabilising forces.
// A prime number is a good choice here to reduce chances of applying the force at the same point in some oscillatory cycle.
if (timestep % 397 == 0)
{
force = 200f * toggle;
toggle *= -1f;
}
// Simulate one timestep.
simWorld.SetCartForce(force);
simWorld.Step();
// Check for failure state. Has the cart run off the ends of the track or has the pole
// angle gone beyond the threshold.
float cartPosX = simWorld.CartPosX;
float poleTopY = simWorld.PoleTopPos.Y;
if ((cartPosX < -__TrackLengthHalf) || (cartPosX > __TrackLengthHalf) ||
(poleTopY < _heightThreshold))
{
break;
}
}
_evalCount++;
if (timestep == _maxTimesteps)
{
_stopConditionSatisfied = true;
}
// The controller's fitness is defined as the number of timesteps that elapse before failure.
double fitness = timestep;
return(new FitnessInfo(fitness, fitness));
}
