/// <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> /// 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)); }