/// <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). SinglePoleBalancingWorld simWorld = new SinglePoleBalancingWorld(__TrackLength + 0.5f, _poleAngleInitial); simWorld.InitSimulationWorld(); // Run the pole-balancing simulation. int timestep = 0; 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.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(); // 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 > _poleAngleThreshold) || (simWorld.PoleAngle < -_poleAngleThreshold)) { 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)); }
/// <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); }