Action Mask and working cozmo

Assets/Scripts/ML Cozmo/CozmoAgent.cs

@@ -27,6 +27,9 @@ public class CozmoAgent : Agent
     private ImageProcessor imageProcessor;          // reference to the ImageProcessor
     private int nearAreaLimit = 0;                  // X coordinate limit for the near to the imagecenter area 
     private int centerOfImageX = 0;                 // Middle of the image in x direction
+    private MovementState lastChosenMovement = MovementState.Stop;  // The last action/movement that was executed 
+
+    private double startTime = Time.time;
 
     private void Start()
     {
@@ -43,9 +46,46 @@ public class CozmoAgent : Agent
     }
 
 
+    public override void CollectObservations()
+    {
+        SetMask();
+    }
+
+    // Set ActionMask for training
+    private void SetMask()
+    {
+        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);
+                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
     public override void AgentAction(float[] vectorAction, string textAction)
     {
+        double elapsedTime = Time.time - startTime;
+        print("Elapsed time: " + elapsedTime);
+        startTime = Time.time;
+
         int action = Mathf.FloorToInt(vectorAction[0]);
         Point centerOfGravity = imageProcessor.CenterOfGravity;
 
@@ -55,15 +95,27 @@ public class CozmoAgent : Agent
         {
             case STOP:
                 movementController.currentMovementState = MovementState.Stop;
+                lastChosenMovement = MovementState.Stop;
+                //Test
+                SetReward(-0.1f);
                 break;
             case FORWARD:
                 movementController.currentMovementState = MovementState.Forward;
+                lastChosenMovement = MovementState.Forward;
+                //Test
+                SetReward(0.01f);
                 break;
             case RIGHT:
                 movementController.currentMovementState = MovementState.Right;
+                lastChosenMovement = MovementState.Right;
+                //Test
+                SetReward(-0.02f);
                 break;
             case LEFT:
                 movementController.currentMovementState = MovementState.Left;
+                lastChosenMovement = MovementState.Left;
+                //Test
+                SetReward(-0.02f);
                 break;
             default:
                 //movement.Move(0);
@@ -86,36 +138,43 @@ public class CozmoAgent : Agent
         float reward = 0;
 
         // Center of gravity is far away from the center (left)
-        if (centerOfGravityX <= centerOfImageX - nearAreaLimit)
+        if (centerOfGravityX <= centerOfImageX - nearAreaLimit && centerOfGravityX >= 0)
         {
             float range = centerOfImageX - nearAreaLimit;
             reward = -(1 - (centerOfGravityX / range));
             // Clamp the reward to max -1 in order to handle rewards if the center of gravity is outside of the image
-            reward = Mathf.Clamp(reward, -1, 0);
+            reward = Mathf.Clamp(reward, -1, 0) / 2;
         }
         // Center of gravity is near left of the center
-        else if (centerOfGravityX <= centerOfImageX)
+        else if ((centerOfGravityX <= centerOfImageX) && (centerOfGravityX >= (centerOfImageX - nearAreaLimit)))
         {
             float range = centerOfImageX - (centerOfImageX - nearAreaLimit);
             float distanceToLeftFarBorder = centerOfGravityX - (centerOfImageX - nearAreaLimit);
             reward = (distanceToLeftFarBorder / range);
         }
         // Center of gravity is far away from the center (right)
-        else if (centerOfGravityX >= centerOfImageX + nearAreaLimit)
+        else if ((centerOfGravityX >= (centerOfImageX + nearAreaLimit)) && (centerOfGravityX <= renderCamera.targetTexture.width))
         {
             float range = renderCamera.targetTexture.width - (centerOfImageX + nearAreaLimit);
             reward = -(((centerOfGravityX - (centerOfImageX + nearAreaLimit)) / range));
             // Clamp the reward to max -1 in order to handle rewards if the center of gravity is outside of the image
-            reward = Mathf.Clamp(reward, -1, 0);
+            reward = Mathf.Clamp(reward, -1, 0) / 2;
         }
         // Center of gravity is near right of the center
-        else if (centerOfGravityX >= centerOfImageX)
+        else if ((centerOfGravityX >= centerOfImageX) && (centerOfGravityX <= (centerOfImageX + nearAreaLimit)))
         {
             float range = (centerOfImageX + nearAreaLimit) - centerOfImageX;
             float distanceToCenterOfImage = centerOfGravityX - centerOfImageX;
             reward = (1 - distanceToCenterOfImage / range);
         }
+        else
+        {
+            SetReward(-1);
+            AgentReset();
+            Debug.Log("Out of image range");
+        }
 
+        Debug.Log("Reward: " + reward);
         SetReward(reward);
     }