public override Reinforcement PerformAction(Action <double> action)
{
if (Terminal)
{
return(0);
}
int usedCompartments = System.Math.Min(action.Dimensionality / 3, Arm.Compartments.Length);
for (int i = 0; i < usedCompartments; i++)
{
Arm.Compartments[i].SetAction(action[3 * i], action[3 * i + 1], action[3 * i + 2]);
}
double[] stateCopy = this.GetCurrentState().StateVector.ToArray();
try
{
ODEState odeState = envSimulator.CurrentODEState;
odeState = solver.Solve(envSimulator, odeState, 0, 5, .2);
envSimulator.SetODEState(.1, odeState);
taskTracker.Update();
this.UpdateCurrentState();
}
catch (System.ArithmeticException)
{
this.CurrentState.StateVector = stateCopy;
this.CurrentState.IsTerminal = true;
return(-10);
}
return(taskTracker.Reward);
}
public override ODEState Solve(IODEEquation eq, ODEState initialState, double time, double timeStep)
{
ODEState deriv = eq.GetDerivative(time, initialState);
// Compute new state using Euler's method
// Yn+1 = Yn + deltaT * dYn/dt
return(initialState.AddScaled(deriv, timeStep));
}
public override ODEState Solve(IODEEquation eq, ODEState initialState, double time, double timeStep)
{
ODEState k1 = eq.GetDerivative(time, initialState);
ODEState k2 = eq.GetDerivative(time + timeStep / 2, initialState.AddScaled(k1, timeStep / 2));
ODEState k3 = eq.GetDerivative(time + timeStep / 2, initialState.AddScaled(k2, timeStep / 2));
ODEState k4 = eq.GetDerivative(time + timeStep, initialState.AddScaled(k3, timeStep));
ODEState sum = k1.AddScaled(k2, 2).AddScaled(k3, 2).AddScaled(k4, 1);
return(initialState.AddScaled(sum, timeStep / 6));
}
public virtual ODEState AddScaled(ODEState state, double scale)
{
ODEState result = new ODEState(new double[State.Length]);
for (int i = 0; i < State.Length; i++)
{
result.State[i] = State[i] + scale * state.State[i];
}
return(result);
}
public override void SetODEState(double time, ODEState state)
{
double[] s = state.State;
Debug.Assert(s.Length == RotationStateLength);
angle = s[0];
angularVelocity = s[1];
Vector2D angleVector = new Vector2D(Math.Cos(angle), Math.Sin(angle));
Vector2D posDiff = angleVector.Scale(radius);
Position = center.Add(posDiff);
other.SetPosition(center.Subtract(posDiff));
Velocity = posDiff.Rotate90().Scale(angularVelocity);
other.SetVelocity(Velocity.Scale(-1));
}
/// <summary>
/// This method solves the ODE from the initial time to the final time
/// by dividing this interval into timesteps and calling the other
/// solve method for each time step.
/// </summary>
/// <param name="eq"> The ODE to solve </param>
/// <param name="initialState"> Initial state of the ODE </param>
/// <param name="initialTime"> Time at which to start </param>
/// <param name="finalTime"> Time at which to stop </param>
/// <param name="timeStep"> Hint of the time step to use </param>
/// <returns> The state of the ODE at finalTime </returns>
public virtual ODEState Solve(IODEEquation eq, ODEState initialState, double initialTime, double finalTime, double timeStep)
{
double t;
ODEState y = initialState;
for (t = initialTime; t < finalTime; t += timeStep)
{
y = Solve(eq, y, t, timeStep);
}
if (t < finalTime)
{
y = Solve(eq, y, t, finalTime - t);
}
return(y);
}
public abstract ODEState Solve(IODEEquation eq, ODEState initialState, double time, double timeStep);
public override void SetODEState(double time, ODEState state)
{
base.SetODEState(time, state);
arm.UpdateInfluences();
}
public ODEState GetDerivative(double time, ODEState state)
{
// We first set the state of all of our constituent parts
SetODEState(time, state);
return(ODEStateDerivative);
}
public override void SetODEState(double time, ODEState state)
{
}
