New Reward function and MovementController

Assets/Scenes/CozmoTraining.unity

@@ -1899,7 +1899,7 @@ MonoBehaviour:
     onDemandDecision: 1
     numberOfActionsBetweenDecisions: 1
   renderCamera: {fileID: 891713640}
-  movement: {fileID: 7570006596986120128}
+  movementController: {fileID: 7570006596986120130}
   timeBetweenDecisionsAtInference: 0.15
 --- !u!65 &7570006596986120126
 BoxCollider:
@@ -1958,3 +1958,16 @@ MonoBehaviour:
   m_EditorClassIdentifier: 
   testState: 1
   isTesting: 0
+--- !u!114 &7570006596986120130
+MonoBehaviour:
+  m_ObjectHideFlags: 0
+  m_CorrespondingSourceObject: {fileID: 0}
+  m_PrefabInstance: {fileID: 0}
+  m_PrefabAsset: {fileID: 0}
+  m_GameObject: {fileID: 7570006596986120123}
+  m_Enabled: 1
+  m_EditorHideFlags: 0
+  m_Script: {fileID: 11500000, guid: d3ecda30187828846b48edbee97aebd7, type: 3}
+  m_Name: 
+  m_EditorClassIdentifier: 
+  currentMovementState: 0

Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs

@@ -22,10 +22,10 @@ public class ImageProcessor : MonoBehaviour
     public Point CenterOfGravity { get; private set; }
 
     // OpenCVSharp parameters
-    private Mat videoSourceImage;
+    private Mat cozmoImageMat;
     private Mat cannyImage;
-    private Texture2D processedTexture;
-    private Vec3b[] videoSourceImageData;
+    private Texture2D finalProcessedCozmoTexture;
+    private Vec3b[] cozmoImageData;
     private byte[] cannyImageData;
 
 
@@ -34,8 +34,7 @@ public class ImageProcessor : MonoBehaviour
     private int croppedImHeight = 120;  // Height of the cropped camera image from the virtual cozmo 
     private Camera textureCamera;       // Virtual Cozmo camera
 
-
-    private Texture2D firstTexture;
+    private Texture2D originalCozmoTexture;
 
 
     private void Start()
@@ -46,21 +45,19 @@ public class ImageProcessor : MonoBehaviour
         // Set image widths and heights based on the given RenderTextures
         imWidth = textureCamera.targetTexture.width;
         imHeight = textureCamera.targetTexture.height;
-        //croppedImHeight = renderTextureCropped.height;
 
-        //assign the processed targetTexture to the renderer to display the image
+        // assign the processed targetTexture to the renderer to display the image
         processedImageRenderer.material.mainTexture = textureCamera.targetTexture;
         processedImageRendererCropped.material.mainTexture = renderTextureCropped;
 
         // initialize video / image with given size
-        videoSourceImage = new Mat(imHeight, imWidth, MatType.CV_8UC3);
-        videoSourceImageData = new Vec3b[imHeight * imWidth];
+        cozmoImageMat = new Mat(imHeight, imWidth, MatType.CV_8UC3);
+        cozmoImageData = new Vec3b[imHeight * imWidth];
         cannyImage = new Mat(imHeight, imWidth, MatType.CV_8UC1);
         cannyImageData = new byte[croppedImHeight * imWidth];
 
-
-        firstTexture = new Texture2D(imWidth, imHeight, TextureFormat.RGBA32, true, true);
-        processedTexture = new Texture2D(imWidth, croppedImHeight, TextureFormat.RGBA32, true, true);
+        originalCozmoTexture = new Texture2D(imWidth, imHeight, TextureFormat.RGBA32, true, true);
+        finalProcessedCozmoTexture = new Texture2D(imWidth, croppedImHeight, TextureFormat.RGBA32, true, true);
     }
 
     ///// <summary>
@@ -68,63 +65,16 @@ public class ImageProcessor : MonoBehaviour
     ///// </summary>
     ///// <param name="source"></param>
     ///// <param name="destination"></param>
-    //private void OnRenderImage(RenderTexture source, RenderTexture destination)
-    //{
-    //    Texture2D tex = RenderTextureToTexture2D(source);
-    //    videoSourceImage = TextureToMat(tex);
-    //    cannyImage = ProcessImage(videoSourceImage);
-    //    cannyImage = CropImage(cannyImage);
-    //    CenterOfGravityTest(cannyImage);
-    //    processedTexture = MatToTexture(cannyImage);
-    //    //processedTexture.Resize(80, 30);
-    //    //processedTexture.Apply();
-    //    //Graphics.Blit(processedTexture, destination);
-    //    Graphics.Blit(processedTexture, renderTextureCropped);
-    //}
-
-    public void ProcessRenderTexture(RenderTexture source)
-    {
-        ProcessRenderTexture(source, source);
-    }
-
-    public void ProcessRenderTexture(RenderTexture source, RenderTexture target)
-    {
-        Texture2D tex = RenderTextureToTexture2D(source);
-        videoSourceImage = TextureToMat(tex);
-        cannyImage = ProcessImage(videoSourceImage);
-        cannyImage = CropImage(cannyImage);
-        CenterOfGravityTest(cannyImage);
-        processedTexture = MatToTexture(cannyImage);
-        //processedTexture.Resize(80, 30);
-        //processedTexture.Apply();
-        //Graphics.Blit(processedTexture, destination);
-        Graphics.Blit(processedTexture, target);
-    }
-
-    //public void ProcessRenderTexture_NEW(RenderTexture source, RenderTexture target)
-    //{
-    //    RenderTextureToTexture2D_NEW(source);
-    //    TextureToMat_NEW(firstTexture);
-    //    ProcessImage_NEW(videoSourceImage);
-    //    cannyImage = CropImage(cannyImage);
-    //    CenterOfGravityTest(cannyImage);
-    //    MatToTexture_NEW(cannyImage);
-    //    //processedTexture.Resize(80, 30);
-    //    //processedTexture.Apply();
-    //    //Graphics.Blit(processedTexture, destination);
-    //    Graphics.Blit(processedTexture, target);
-    //}
-
     public void OnRenderImage(RenderTexture source, RenderTexture destination)
     {
-        RenderTextureToTexture2D_NEW(source);
-        TextureToMat_NEW(firstTexture);
-        ProcessImage_NEW(videoSourceImage);
+        RenderTextureToTexture2D(source);
+        TextureToMat(originalCozmoTexture);
+        ProcessImage(cozmoImageMat);
         cannyImage = CropImage(cannyImage);
-        CenterOfGravityTest(cannyImage);
-        MatToTexture_NEW(cannyImage);
-        Graphics.Blit(processedTexture, destination);
-        Graphics.Blit(processedTexture, renderTextureCropped);
+        FindCenterOfGravity(cannyImage);
+        MatToTexture(cannyImage);
+        Graphics.Blit(finalProcessedCozmoTexture, destination);
+        Graphics.Blit(finalProcessedCozmoTexture, renderTextureCropped);
     }
 
     // Crop image to just see the middle of the original image 
@@ -136,33 +86,16 @@ public class ImageProcessor : MonoBehaviour
         return croppedImage;
     }
 
-    //// Crop image to just see the middle of the original image 
-    //private Mat CropImage_NEW(Mat image)
-    //{
-    //    //cut a fourth out of the top and bottom of the image 
-    //    OpenCvSharp.Rect rectCroped = new OpenCvSharp.Rect(0, image.Height / 4, image.Width, image.Height / 2);
-    //    Mat croppedImage = new Mat(image, rectCroped);
-    //    return croppedImage;
-    //}
 
-    private Texture2D RenderTextureToTexture2D(RenderTexture rTex)
+    private void RenderTextureToTexture2D(RenderTexture rTex)
     {
-        Texture2D tex = new Texture2D(imWidth, imHeight, TextureFormat.RGBA32, true, true);
         RenderTexture.active = rTex;
-        tex.ReadPixels(new UnityEngine.Rect(0, 0, rTex.width, rTex.height), 0, 0);
-        tex.Apply();
-        return tex;
-    }
-
-    private void RenderTextureToTexture2D_NEW(RenderTexture rTex)
-    {
-        RenderTexture.active = rTex;
-        firstTexture.ReadPixels(new UnityEngine.Rect(0, 0, rTex.width, rTex.height), 0, 0);
-        firstTexture.Apply();
+        originalCozmoTexture.ReadPixels(new UnityEngine.Rect(0, 0, rTex.width, rTex.height), 0, 0);
+        originalCozmoTexture.Apply();
     }
 
     // Convert Unity Texture2D object to OpenCVSharp Mat object
-    private Mat TextureToMat(Texture2D source)
+    private void TextureToMat(Texture2D source)
     {
         // Color32 array : r, g, b, a
         Color32[] c = source.GetPixels32();
@@ -182,94 +115,24 @@ public class ImageProcessor : MonoBehaviour
                     Item2 = col.r
                 };
                 // set pixel to an array
-                videoSourceImageData[j + i * imWidth] = vec3;
+                cozmoImageData[j + i * imWidth] = vec3;
             }
         });
 
         // assign the Vec3b array to Mat
-        Mat tmpMat = new Mat(imHeight, imWidth, MatType.CV_8UC3);
-        tmpMat.SetArray(0, 0, videoSourceImageData);
-        return tmpMat;
+        cozmoImageMat.SetArray(0, 0, cozmoImageData);
     }
 
-    // Convert Unity Texture2D object to OpenCVSharp Mat object
-    private void TextureToMat_NEW(Texture2D source)
-    {
-        // Color32 array : r, g, b, a
-        Color32[] c = source.GetPixels32();
-
-        // Parallel for loop
-        // convert Color32 object to Vec3b object
-        // Vec3b is the representation of pixel for Mat
-        Parallel.For(0, imHeight, i =>
-        {
-            for (var j = 0; j < imWidth; j++)
-            {
-                var col = c[j + i * imWidth];
-                var vec3 = new Vec3b
-                {
-                    Item0 = col.b,
-                    Item1 = col.g,
-                    Item2 = col.r
-                };
-                // set pixel to an array
-                videoSourceImageData[j + i * imWidth] = vec3;
-            }
-        });
-
-        // assign the Vec3b array to Mat
-
-        videoSourceImage.SetArray(0, 0, videoSourceImageData);
-    }
-
-    // Simple example of canny edge detect
-    private Mat ProcessImage(Mat _image)
-    {
-        Mat cannyImg = new Mat();
-        Cv2.Canny(_image, cannyImg, 100, 100);
-        return cannyImg;
-    }
 
     // Simple example of canny edge detect
-    private void ProcessImage_NEW(Mat _image)
+    private void ProcessImage(Mat _image)
     {
         Cv2.Canny(_image, cannyImage, 100, 100);
     }
 
-    // Convert OpenCVSharp Mat object to Unity Texture2D object
-    private Texture2D MatToTexture(Mat mat)
-    {
-        // cannyImageData is byte array, because canny image is grayscale
-        mat.GetArray(0, 0, cannyImageData);
-        // create Color32 array that can be assigned to Texture2D directly
-        Color32[] c = new Color32[croppedImHeight * imWidth];
-
-        // parallel for loop
-        Parallel.For(0, croppedImHeight, i =>
-        {
-            for (var j = 0; j < imWidth; j++)
-            {
-                byte vec = cannyImageData[j + i * imWidth];
-                var color32 = new Color32
-                {
-                    r = vec,
-                    g = vec,
-                    b = vec,
-                    a = 0
-                };
-                c[j + i * imWidth] = color32;
-            }
-        });
-
-        Texture2D texture = new Texture2D(imWidth, croppedImHeight, TextureFormat.RGBA32, true, true);
-        texture.SetPixels32(c);
-        // to update the texture, OpenGL manner
-        texture.Apply();
-        return texture;
-    }
 
     // Convert OpenCVSharp Mat object to Unity Texture2D object
-    private void MatToTexture_NEW(Mat mat)
+    private void MatToTexture(Mat mat)
     {
         // cannyImageData is byte array, because canny image is grayscale
         mat.GetArray(0, 0, cannyImageData);
@@ -294,13 +157,13 @@ public class ImageProcessor : MonoBehaviour
         });
 
 
-        processedTexture.SetPixels32(c);
-        // to update the texture, OpenGL manner
-        processedTexture.Apply();
+        finalProcessedCozmoTexture.SetPixels32(c);
+        // update texture
+        finalProcessedCozmoTexture.Apply();
     }
 
-
-    private void CenterOfGravityTest(Mat processedImage)
+    // Find the Center of Gravity in the image
+    private void FindCenterOfGravity(Mat processedImage)
     {
         // find moments of the image
         Moments m = new Moments(processedImage, true);

Assets/Scripts/ML Cozmo/CozmoAgent.cs

@@ -19,32 +19,32 @@ public class CozmoAgent : Agent
 
     [Tooltip("The virtual Cozmo camera")]
     public Camera renderCamera;
+    //[Tooltip("Final cropped and scaled rendertexture")]
+    //public RenderTexture renderTextureScaled;
     [Tooltip("Reference to the CozmoMovement script")]
-    public CozmoMovement movement;
+    public CozmoMovementController movementController;
     public float timeBetweenDecisionsAtInference;
 
     private Academy academy;                        // CozmoAcademy
     private float timeSinceDecision;                // time since last decision
-    private ImageProcessor onRenderImageTest;       // reference to the ImageProcessor
+    private ImageProcessor imageProcessor;          // reference to the ImageProcessor
     private int nearAreaLimit = 0;                  // X coordinate limit for the near to the imagecenter area 
     private int farAreaLimit = 0;                   // X coordinate limit for the far away to the imagecenter area
 
-    // for testing
-    //private float[] floats = { 1.0f, 2.0f, 3.0f };
+
 
     private void Start()
     {
         academy = FindObjectOfType(typeof(CozmoAcademy)) as CozmoAcademy;
-        onRenderImageTest = renderCamera.GetComponent<ImageProcessor>();
+        imageProcessor = renderCamera.GetComponent<ImageProcessor>();
         nearAreaLimit = (int)(renderCamera.targetTexture.width / 2 * NEAR_AREA_PERCENTAGE_OFFSET);
         farAreaLimit = (int)(renderCamera.targetTexture.width / 2 * FAR_AREA_PERCENTAGE_OFFSET);
     }
 
+
     public void FixedUpdate()
     {
         WaitTimeInference();
-        // for testing
-        //AgentAction(floats, "ActionText");
     }
 
 
@@ -53,7 +53,7 @@ public class CozmoAgent : Agent
     {
         //print("Action before FloorToInt: " + vectorAction[0]);
         int action = Mathf.FloorToInt(vectorAction[0]);
-        Point centerOfGravity = onRenderImageTest.CenterOfGravity;
+        Point centerOfGravity = imageProcessor.CenterOfGravity;
         //Vector3 targetPos = transform.position;
 
         //print("Action after FloorToInt: " + action);
@@ -63,16 +63,16 @@ public class CozmoAgent : Agent
         switch (action)
         {
             case STOP:
-                movement.Move(0);
+                movementController.currentMovementState = MovementState.Stop;
                 break;
             case FORWARD:
-                movement.Move(1);
+                movementController.currentMovementState = MovementState.Forward;
                 break;
             case RIGHT:
-                movement.Turn(1);
+                movementController.currentMovementState = MovementState.Right;
                 break;
             case LEFT:
-                movement.Turn(-1);
+                movementController.currentMovementState = MovementState.Left;
                 break;
             default:
                 //movement.Move(0);
@@ -83,28 +83,61 @@ public class CozmoAgent : Agent
         if (renderCamera != null)
         {
             renderCamera.Render();
-            //onRenderImageTest.ProcessRenderTexture_NEW(renderCamera.targetTexture, onRenderImageTest.renderTextureCropped);
         }
 
+        RewardAgent();
+        imageProcessor.enabled = false;
+    }
+
+    /// <summary>
+    /// TODO: Cleanup code
+    /// </summary>
+    private void RewardAgent()
+    {
+        float centerOfImageX = renderCamera.targetTexture.width / 2;
+        float centerOfGravityX = imageProcessor.CenterOfGravity.X;
+        float reward = 0;
 
-        // If centerOfGravity lies near to the center of the image horizontally
-        if (centerOfGravity.X > renderCamera.targetTexture.width / 2 - nearAreaLimit && centerOfGravity.X < renderCamera.targetTexture.width / 2 + nearAreaLimit)
+        // Center of gravity is far left of the center
+        if (centerOfGravityX <= centerOfImageX - farAreaLimit)
         {
-            SetReward(1);
-            print("Reward: +1");
+            reward = -1;
         }
-        else if (centerOfGravity.X > renderCamera.targetTexture.width / 2 - farAreaLimit && centerOfGravity.X < renderCamera.targetTexture.width / 2 + farAreaLimit)
+        // Center of gravity is between far and near left of the center
+        else if (centerOfGravityX <= centerOfImageX - nearAreaLimit)
         {
-            SetReward(-0.3f);
-            print("Reward: -0.3");
+            float range = (centerOfImageX - nearAreaLimit) - (centerOfImageX - farAreaLimit);
+            float distanceToLeftFarBorder = centerOfGravityX - (centerOfImageX - farAreaLimit);
+            reward = -(1 - (distanceToLeftFarBorder / range));
         }
-        else
+        // Center of gravity is near left of the center
+        else if (centerOfGravityX <= centerOfImageX)
+        {
+            float range = centerOfImageX - (centerOfImageX - nearAreaLimit);
+            float distanceToLeftFarBorder = centerOfGravityX - (centerOfImageX - nearAreaLimit);
+            reward = (distanceToLeftFarBorder / range);
+        }
+        // Center of gravity is far right of the center
+        else if (centerOfGravityX >= centerOfImageX + farAreaLimit)
+        {
+            reward = -1;
+        }
+        // Center of gravity is between far and near right of the center
+        else if (centerOfGravityX >= centerOfImageX + nearAreaLimit)
+        {
+            float range = (centerOfImageX + farAreaLimit) - (centerOfImageX + nearAreaLimit);
+            float distanceToLeftFarBorder = centerOfGravityX - (centerOfImageX + nearAreaLimit);
+            reward = -(distanceToLeftFarBorder / range);
+        }
+        // Center of gravity is near right of the center
+        else if (centerOfGravityX >= centerOfImageX)
         {
-            SetReward(-1);
-            print("Reward: -1");
+            float range = (centerOfImageX + nearAreaLimit) - centerOfImageX;
+            float distanceToLeftFarBorder = centerOfGravityX - centerOfImageX;
+            reward = (1 - distanceToLeftFarBorder / range);
         }
 
-        onRenderImageTest.enabled = false;
+        SetReward(reward);
     }
 
     // to be implemented by the developer
@@ -132,7 +165,6 @@ public class CozmoAgent : Agent
 
         if (!academy.GetIsInference())
         {
-            //onRenderImageTest.ProcessRenderTexture_NEW(renderCamera.targetTexture, onRenderImageTest.renderTextureCropped);
             RequestDecision();
         }
         else
@@ -140,7 +172,6 @@ public class CozmoAgent : Agent
             if (timeSinceDecision >= timeBetweenDecisionsAtInference)
             {
                 timeSinceDecision = 0f;
-                //onRenderImageTest.ProcessRenderTexture_NEW(renderCamera.targetTexture, onRenderImageTest.renderTextureCropped);
                 RequestDecision();
             }
             else

Assets/Scripts/Movemnet/CozmoMovementController.cs

@@ -0,0 +1,45 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public enum MovementState { Stop, Forward, Right, Left }
+
+[RequireComponent(typeof(CozmoMovement))]
+public class CozmoMovementController : MonoBehaviour
+{
+    [Tooltip("Current MovementState of the Robot. Change this to make the robot move.")]
+    public MovementState currentMovementState = MovementState.Stop;
+
+    private CozmoMovement movement; // Movement script of the robot
+
+
+    // Start is called before the first frame update
+    void Start()
+    {
+        movement = GetComponent<CozmoMovement>();
+    }
+
+    private void FixedUpdate()
+    {
+        switch (currentMovementState)
+        {
+            case MovementState.Stop:
+                movement.Move(0);
+                break;
+            case MovementState.Forward:
+                movement.Move(1);
+                break;
+            case MovementState.Right:
+                movement.Turn(1);
+                break;
+            case MovementState.Left:
+                movement.Turn(-1);
+                break;
+            default:
+                movement.Move(0);
+                throw new ArgumentException("No real Movementstate was given. Default 'Stop' was chosen.");
+
+        }
+    }
+}

Assets/Scripts/Movemnet/CozmoMovementController.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: d3ecda30187828846b48edbee97aebd7
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: