|
|
@@ -11,6 +11,7 @@ |
|
|
|
using MLAgents; |
|
|
|
using OpenCvSharp; |
|
|
|
using System; |
|
|
|
using System.Collections.Generic; |
|
|
|
using UnityEngine; |
|
|
|
|
|
|
|
namespace Cozmo |
|
|
@@ -31,6 +32,7 @@ namespace Cozmo |
|
|
|
[Tooltip("Reference to the CozmoMovement script")] |
|
|
|
public CozmoMovementController movementController; |
|
|
|
public float timeBetweenDecisionsAtInference; |
|
|
|
public int maxStoredMovementStates = 1; |
|
|
|
|
|
|
|
private Academy academy; // CozmoAcademy |
|
|
|
private float timeSinceDecision; // time since last decision |
|
|
@@ -39,6 +41,8 @@ namespace Cozmo |
|
|
|
private int centerOfImageX = 0; // Middle of the image in x direction |
|
|
|
private MovementState lastChosenMovement = MovementState.Stop; // The last action/movement that was executed |
|
|
|
|
|
|
|
private Queue<MovementState> lastActions = new Queue<MovementState>(); |
|
|
|
|
|
|
|
private double startTime = Time.time; |
|
|
|
|
|
|
|
private void Start() |
|
|
@@ -64,29 +68,48 @@ namespace Cozmo |
|
|
|
// Set ActionMask for training |
|
|
|
private void SetMask() |
|
|
|
{ |
|
|
|
switch (lastChosenMovement) |
|
|
|
// Do not allow stop decision after a stop |
|
|
|
if (lastChosenMovement == MovementState.Stop) |
|
|
|
{ |
|
|
|
// Do not allow stop decision after a stop |
|
|
|
case (MovementState.Stop): |
|
|
|
SetActionMask(STOP); |
|
|
|
break; |
|
|
|
// Do not allow stop after forward |
|
|
|
case (MovementState.Forward): |
|
|
|
SetActionMask(STOP); |
|
|
|
break; |
|
|
|
// Do not allow stop & left after right |
|
|
|
case (MovementState.Right): |
|
|
|
SetActionMask(STOP); |
|
|
|
SetActionMask(LEFT); |
|
|
|
break; |
|
|
|
// Do not allow stop & right after left |
|
|
|
case (MovementState.Left): |
|
|
|
SetActionMask(STOP); |
|
|
|
SetActionMask(RIGHT); |
|
|
|
break; |
|
|
|
default: |
|
|
|
throw new ArgumentException("Invalid MovementState."); |
|
|
|
SetActionMask(STOP); |
|
|
|
} |
|
|
|
|
|
|
|
// Do not allow left decision if right was in the last actions |
|
|
|
if (lastActions.Contains(MovementState.Right)) |
|
|
|
{ |
|
|
|
SetActionMask(LEFT); |
|
|
|
} |
|
|
|
|
|
|
|
// Do not allow right decision if left was in the last actions |
|
|
|
if (lastActions.Contains(MovementState.Left)) |
|
|
|
{ |
|
|
|
SetActionMask(RIGHT); |
|
|
|
} |
|
|
|
|
|
|
|
//switch (lastChosenMovement) |
|
|
|
//{ |
|
|
|
// // Do not allow stop decision after a stop |
|
|
|
// case (MovementState.Stop): |
|
|
|
// SetActionMask(STOP); |
|
|
|
// break; |
|
|
|
// // Do not allow stop after forward |
|
|
|
// case (MovementState.Forward): |
|
|
|
// //SetActionMask(STOP); |
|
|
|
// break; |
|
|
|
// // Do not allow stop & left after right |
|
|
|
// case (MovementState.Right): |
|
|
|
// //SetActionMask(STOP); |
|
|
|
// if (lastActions.Contains(MovementState.)) |
|
|
|
// SetActionMask(LEFT); |
|
|
|
// break; |
|
|
|
// // Do not allow stop & right after left |
|
|
|
// case (MovementState.Left): |
|
|
|
// //SetActionMask(STOP); |
|
|
|
// SetActionMask(RIGHT); |
|
|
|
// break; |
|
|
|
// default: |
|
|
|
// throw new ArgumentException("Invalid MovementState."); |
|
|
|
//} |
|
|
|
} |
|
|
|
|
|
|
|
// to be implemented by the developer |
|
|
@@ -107,31 +130,33 @@ namespace Cozmo |
|
|
|
movementController.currentMovementState = MovementState.Stop; |
|
|
|
lastChosenMovement = MovementState.Stop; |
|
|
|
//Test |
|
|
|
SetReward(-0.1f); |
|
|
|
SetReward(-0.02f); |
|
|
|
break; |
|
|
|
case FORWARD: |
|
|
|
movementController.currentMovementState = MovementState.Forward; |
|
|
|
lastChosenMovement = MovementState.Forward; |
|
|
|
//Test |
|
|
|
SetReward(0.01f); |
|
|
|
SetReward(0.02f); |
|
|
|
break; |
|
|
|
case RIGHT: |
|
|
|
movementController.currentMovementState = MovementState.Right; |
|
|
|
lastChosenMovement = MovementState.Right; |
|
|
|
//Test |
|
|
|
SetReward(-0.02f); |
|
|
|
SetReward(0.01f); |
|
|
|
break; |
|
|
|
case LEFT: |
|
|
|
movementController.currentMovementState = MovementState.Left; |
|
|
|
lastChosenMovement = MovementState.Left; |
|
|
|
//Test |
|
|
|
SetReward(-0.02f); |
|
|
|
SetReward(0.01f); |
|
|
|
break; |
|
|
|
default: |
|
|
|
//movement.Move(0); |
|
|
|
throw new ArgumentException("Invalid action value. Stop movement."); |
|
|
|
} |
|
|
|
|
|
|
|
CollectLastMovementStates(lastChosenMovement); |
|
|
|
|
|
|
|
// Render new image after movement in order to update the centerOfGravity |
|
|
|
if (renderCamera != null) |
|
|
|
{ |
|
|
@@ -188,6 +213,26 @@ namespace Cozmo |
|
|
|
SetReward(reward); |
|
|
|
} |
|
|
|
|
|
|
|
// Store the last movementStates in a Queue |
|
|
|
private void CollectLastMovementStates(MovementState movementState) |
|
|
|
{ |
|
|
|
// Check if Queue exists and values should be stored |
|
|
|
if ((lastActions != null) && (maxStoredMovementStates > 0)) |
|
|
|
{ |
|
|
|
// maxStoredMovementStates is reached |
|
|
|
if (lastActions.Count >= maxStoredMovementStates) |
|
|
|
{ |
|
|
|
// deque first value(s) when maxStoredMovementStates is reached |
|
|
|
for (int i = 0; i <= (lastActions.Count - maxStoredMovementStates); i++) |
|
|
|
{ |
|
|
|
lastActions.Dequeue(); |
|
|
|
} |
|
|
|
} |
|
|
|
// add last action to queue |
|
|
|
lastActions.Enqueue(movementState); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
// to be implemented by the developer |
|
|
|
public override void AgentReset() |
|
|
|
{ |