From 021662632e1c05a66f8384c3459285a9daa1498f Mon Sep 17 00:00:00 2001 From: Tobi Date: Mon, 29 Jul 2019 16:29:21 +0200 Subject: [PATCH] Add commetns and refactoring --- .../CannyEdgeDetection/ImageProcessor.cs | 312 +++++++------- Assets/Scripts/ML Cozmo/CozmoAcademy.cs | 50 ++- Assets/Scripts/ML Cozmo/CozmoAgent.cs | 391 +++++++++--------- Assets/Scripts/Movemnet/CozmoMovement.cs | 209 +++++----- .../Movemnet/CozmoMovementController.cs | 78 ++-- .../Scripts/Movemnet/CozmoMovementTester.cs | 197 ++++----- Assets/Scripts/SceneHelper.cs | 38 +- 7 files changed, 681 insertions(+), 594 deletions(-) diff --git a/Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs b/Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs index 8a3423e..beb9684 100644 --- a/Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs +++ b/Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs @@ -1,180 +1,192 @@ -using System.Collections; -using System.Collections.Generic; +///----------------------------------------------------------------- +/// Namespace: +/// Class: +/// Description: +/// Author: Date: <29.07.2019> +/// Notes: <> +///----------------------------------------------------------------- +/// + using UnityEngine; using OpenCvSharp; using System.Threading.Tasks; -public class ImageProcessor : MonoBehaviour +namespace Cozmo { - [Header("RenderTexture")] - [Tooltip("RenderTexture that will be passed to the LearningBrain.")] - public RenderTexture renderTextureCropped; - - [Header("Debug Helper")] - [Tooltip("Reference to the MeshRenderer that will show the processed Image from Cozmo")] - public MeshRenderer processedImageRenderer; - [Tooltip("Reference to the MeshRenderer that will show the processed and cropped Image from Cozmo")] - public MeshRenderer processedImageRendererCropped; - - /// - /// Center of Gravity in the cropped canny image - /// - public Point CenterOfGravity { get; private set; } - - // OpenCVSharp parameters - private Mat cozmoImageMat; - private Mat cannyImage; - private Texture2D finalProcessedCozmoTexture; - private Vec3b[] cozmoImageData; - private byte[] cannyImageData; - - - private int imWidth = 320; // Width of the camera image from the virtual cozmo - private int imHeight = 240; // Height of the camera image from the virtual cozmo - private int croppedImHeight = 120; // Height of the cropped camera image from the virtual cozmo - private Camera textureCamera; // Virtual Cozmo camera - - private Texture2D originalCozmoTexture; - - - private void Start() + public class ImageProcessor : MonoBehaviour { - // Get reference to the cozmo camera - textureCamera = GetComponent(); + [Header("RenderTexture")] + [Tooltip("RenderTexture that will be passed to the LearningBrain.")] + public RenderTexture renderTextureCropped; - // Set image widths and heights based on the given RenderTextures - imWidth = textureCamera.targetTexture.width; - imHeight = textureCamera.targetTexture.height; + [Header("Debug Helper")] + [Tooltip("Reference to the MeshRenderer that will show the processed Image from Cozmo")] + public MeshRenderer processedImageRenderer; + [Tooltip("Reference to the MeshRenderer that will show the processed and cropped Image from Cozmo")] + public MeshRenderer processedImageRendererCropped; - // assign the processed targetTexture to the renderer to display the image - processedImageRenderer.material.mainTexture = textureCamera.targetTexture; - processedImageRendererCropped.material.mainTexture = renderTextureCropped; + /// + /// Center of Gravity in the cropped canny image + /// + public Point CenterOfGravity { get; private set; } - // initialize video / image with given size - 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]; - - originalCozmoTexture = new Texture2D(imWidth, imHeight, TextureFormat.RGBA32, true, true); - finalProcessedCozmoTexture = new Texture2D(imWidth, croppedImHeight, TextureFormat.RGBA32, true, true); - } - - ///// - ///// Gets called when a new image arrives from the camera this script lies on - ///// - ///// - ///// - public void OnRenderImage(RenderTexture source, RenderTexture destination) - { - RenderTextureToTexture2D(source); - TextureToMat(originalCozmoTexture); - ProcessImage(cozmoImageMat); - cannyImage = CropImage(cannyImage); - FindCenterOfGravity(cannyImage); - MatToTexture(cannyImage); - Graphics.Blit(finalProcessedCozmoTexture, destination); - Graphics.Blit(finalProcessedCozmoTexture, renderTextureCropped); - } - - // Crop image to just see the middle of the original image - private Mat CropImage(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; - } + // OpenCVSharp parameters + private Mat cozmoImageMat; + private Mat cannyImage; + private Texture2D finalProcessedCozmoTexture; + private Vec3b[] cozmoImageData; + private byte[] cannyImageData; - private void RenderTextureToTexture2D(RenderTexture rTex) - { - RenderTexture.active = rTex; - originalCozmoTexture.ReadPixels(new UnityEngine.Rect(0, 0, rTex.width, rTex.height), 0, 0); - originalCozmoTexture.Apply(); - } + private int imWidth = 320; // Width of the camera image from the virtual cozmo + private int imHeight = 240; // Height of the camera image from the virtual cozmo + private int croppedImHeight = 120; // Height of the cropped camera image from the virtual cozmo + private Camera textureCamera; // Virtual Cozmo camera - // Convert Unity Texture2D object to OpenCVSharp Mat object - private void TextureToMat(Texture2D source) - { - // Color32 array : r, g, b, a - Color32[] c = source.GetPixels32(); + private Texture2D originalCozmoTexture; - // Parallel for loop - // convert Color32 object to Vec3b object - // Vec3b is the representation of pixel for Mat - Parallel.For(0, imHeight, i => + + private void Start() { - 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 - cozmoImageData[j + i * imWidth] = vec3; - } - }); + // Get reference to the cozmo camera + textureCamera = GetComponent(); - // assign the Vec3b array to Mat - cozmoImageMat.SetArray(0, 0, cozmoImageData); - } + // Set image widths and heights based on the given RenderTextures + imWidth = textureCamera.targetTexture.width; + imHeight = textureCamera.targetTexture.height; + // assign the processed targetTexture to the renderer to display the image + processedImageRenderer.material.mainTexture = textureCamera.targetTexture; + processedImageRendererCropped.material.mainTexture = renderTextureCropped; - // Simple example of canny edge detect - private void ProcessImage(Mat _image) - { - Cv2.Canny(_image, cannyImage, 100, 100); - } + // initialize video / image with given size + 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]; + originalCozmoTexture = new Texture2D(imWidth, imHeight, TextureFormat.RGBA32, true, true); + finalProcessedCozmoTexture = new Texture2D(imWidth, croppedImHeight, TextureFormat.RGBA32, true, true); + } - // Convert OpenCVSharp Mat object to Unity Texture2D object - private void 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 => + ///// + ///// Gets called when a new image arrives from the camera this script lies on + ///// + ///// + ///// + public void OnRenderImage(RenderTexture source, RenderTexture destination) { - for (var j = 0; j < imWidth; j++) + RenderTextureToTexture2D(source); + TextureToMat(originalCozmoTexture); + ProcessImage(cozmoImageMat); + cannyImage = CropImage(cannyImage); + FindCenterOfGravity(cannyImage); + MatToTexture(cannyImage); + + Graphics.Blit(finalProcessedCozmoTexture, destination); + Graphics.Blit(finalProcessedCozmoTexture, renderTextureCropped); + } + + // Crop image to just see the middle of the original image + private Mat CropImage(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; + } + + // Convert the rendertexture to a texture2d + private void RenderTextureToTexture2D(RenderTexture rTex) + { + RenderTexture.active = rTex; + originalCozmoTexture.ReadPixels(new UnityEngine.Rect(0, 0, rTex.width, rTex.height), 0, 0); + originalCozmoTexture.Apply(); + } + + // Convert Unity Texture2D object to OpenCVSharp Mat object + private void TextureToMat(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 => { - byte vec = cannyImageData[j + i * imWidth]; - var color32 = new Color32 + for (var j = 0; j < imWidth; j++) { - r = vec, - g = vec, - b = vec, - a = 0 - }; - c[j + i * imWidth] = color32; - } - }); + var col = c[j + i * imWidth]; + var vec3 = new Vec3b + { + Item0 = col.b, + Item1 = col.g, + Item2 = col.r + }; + // set pixel to an array + cozmoImageData[j + i * imWidth] = vec3; + } + }); + + // assign the Vec3b array to Mat + cozmoImageMat.SetArray(0, 0, cozmoImageData); + } - finalProcessedCozmoTexture.SetPixels32(c); - // update texture - finalProcessedCozmoTexture.Apply(); - } + // Simple example of canny edge detect + private void ProcessImage(Mat _image) + { + Cv2.Canny(_image, cannyImage, 100, 100); + } - // Find the Center of Gravity in the image - private void FindCenterOfGravity(Mat processedImage) - { - // find moments of the image - Moments m = new Moments(processedImage, true); - CenterOfGravity = new Point(m.M10 / m.M00, m.M01 / m.M00); + + // Convert OpenCVSharp Mat object to Unity Texture2D object + private void 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; + } + }); + + + finalProcessedCozmoTexture.SetPixels32(c); + // update texture + finalProcessedCozmoTexture.Apply(); + } + + // Find the Center of Gravity in the image + private void FindCenterOfGravity(Mat processedImage) + { + // find moments of the image + Moments m = new Moments(processedImage, true); + CenterOfGravity = new Point(m.M10 / m.M00, m.M01 / m.M00); #if UNITY_EDITOR - // show the image with a point mark at the centroid - Cv2.Circle(processedImage, CenterOfGravity, 5, new Scalar(128, 0, 0), -1); - Cv2.Flip(processedImage, processedImage, FlipMode.X); - Cv2.ImShow("Image with center", processedImage); + // show the image with a point mark at the centroid + Cv2.Circle(processedImage, CenterOfGravity, 5, new Scalar(128, 0, 0), -1); + Cv2.Flip(processedImage, processedImage, FlipMode.X); + Cv2.ImShow("Image with center", processedImage); #endif - } + } + } } diff --git a/Assets/Scripts/ML Cozmo/CozmoAcademy.cs b/Assets/Scripts/ML Cozmo/CozmoAcademy.cs index 1092c64..1663b27 100644 --- a/Assets/Scripts/ML Cozmo/CozmoAcademy.cs +++ b/Assets/Scripts/ML Cozmo/CozmoAcademy.cs @@ -1,26 +1,36 @@ -using MLAgents; -using System.Collections; -using System.Collections.Generic; +///----------------------------------------------------------------- +/// Namespace: +/// Class: +/// Description: +/// Author: Date: <29.07.2019> +/// Notes: <> +///----------------------------------------------------------------- +/// + +using MLAgents; using UnityEngine; -public class CozmoAcademy : Academy +namespace Cozmo { - public GameObject cozmo; - public Transform startPoint; - //public bool testAcademyReset = false; - - public override void AcademyReset() + public class CozmoAcademy : Academy { - cozmo.transform.position = startPoint.position; - cozmo.transform.rotation = startPoint.rotation; - } + [Tooltip("Reference to the cozmo gameobject in the scene.")] + public GameObject cozmo; + [Tooltip("Reference to the transform which represents the starting position for cozmo.")] + public Transform startPoint; - //private void Update() - //{ - // if (testAcademyReset) - // { - // AcademyReset(); - // testAcademyReset = !testAcademyReset; - // } - //} + + public override void AcademyReset() + { + SetCozmoBackToStartingPosition(); + } + + // Resets Cozmos position and rotation to match its starting position + private void SetCozmoBackToStartingPosition() + { + cozmo.transform.position = startPoint.position; + cozmo.transform.rotation = startPoint.rotation; + } + + } } diff --git a/Assets/Scripts/ML Cozmo/CozmoAgent.cs b/Assets/Scripts/ML Cozmo/CozmoAgent.cs index 4f443ea..fbbae1a 100644 --- a/Assets/Scripts/ML Cozmo/CozmoAgent.cs +++ b/Assets/Scripts/ML Cozmo/CozmoAgent.cs @@ -1,221 +1,232 @@ -using MLAgents; +///----------------------------------------------------------------- +/// Namespace: +/// Class: +/// Description: +/// Author: Date: <29.07.2019> +/// Notes: <> +///----------------------------------------------------------------- +/// + +using MLAgents; using OpenCvSharp; using System; -using System.Collections; -using System.Collections.Generic; using UnityEngine; -public class CozmoAgent : Agent +namespace Cozmo { - // Possible Actions - private const int STOP = 0; - private const int FORWARD = 1; - private const int RIGHT = 2; - private const int LEFT = 3; - - // Used to determine different areas in the image (near to the center, far away) - private const float NEAR_AREA_PERCENTAGE_OFFSET = 0.3f; - - [Tooltip("The virtual Cozmo camera")] - public Camera renderCamera; - [Tooltip("Reference to the CozmoMovement script")] - public CozmoMovementController movementController; - public float timeBetweenDecisionsAtInference; - - private Academy academy; // CozmoAcademy - private float timeSinceDecision; // time since last decision - 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() + public class CozmoAgent : Agent { - academy = FindObjectOfType(typeof(CozmoAcademy)) as CozmoAcademy; - imageProcessor = renderCamera.GetComponent(); - nearAreaLimit = (int)(renderCamera.targetTexture.width / 2 * NEAR_AREA_PERCENTAGE_OFFSET); - centerOfImageX = renderCamera.targetTexture.width / 2; - } + // Possible Actions + private const int STOP = 0; + private const int FORWARD = 1; + private const int RIGHT = 2; + private const int LEFT = 3; + // Used to determine different areas in the image (near to the center, far away) + private const float NEAR_AREA_PERCENTAGE_OFFSET = 0.3f; - public void FixedUpdate() - { - WaitTimeInference(); - } + [Tooltip("The virtual Cozmo camera")] + public Camera renderCamera; + [Tooltip("Reference to the CozmoMovement script")] + public CozmoMovementController movementController; + public float timeBetweenDecisionsAtInference; + private Academy academy; // CozmoAcademy + private float timeSinceDecision; // time since last decision + 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 - public override void CollectObservations() - { - SetMask(); - } + private double startTime = Time.time; - // Set ActionMask for training - private void SetMask() - { - switch (lastChosenMovement) + private void Start() { - // 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; - - AddReward(-0.01f); - - switch (action) - { - 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); - throw new ArgumentException("Invalid action value. Stop movement."); + academy = FindObjectOfType(typeof(CozmoAcademy)) as CozmoAcademy; + imageProcessor = renderCamera.GetComponent(); + nearAreaLimit = (int)(renderCamera.targetTexture.width / 2 * NEAR_AREA_PERCENTAGE_OFFSET); + centerOfImageX = renderCamera.targetTexture.width / 2; } - // Render new image after movement in order to update the centerOfGravity - if (renderCamera != null) + + public void FixedUpdate() { - renderCamera.Render(); + WaitTimeInference(); } - RewardAgent(); - } - // Set the reward for the agent based on how far away the center of gravity is from the center of the image - private void RewardAgent() - { - float centerOfGravityX = imageProcessor.CenterOfGravity.X; - float reward = 0; - - // Center of gravity is far away from the center (left) - if (centerOfGravityX <= centerOfImageX - nearAreaLimit && centerOfGravityX >= 0) + public override void CollectObservations() { - 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) / 2; - } - // Center of gravity is near left of the center - 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)) && (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) / 2; - } - // Center of gravity is near right of the center - 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"); + SetMask(); } - Debug.Log("Reward: " + reward); - SetReward(reward); - } - - // to be implemented by the developer - public override void AgentReset() - { - academy.AcademyReset(); - } - - - private void OnTriggerEnter(Collider other) - { - if (other.transform.CompareTag("Goal")) + // Set ActionMask for training + private void SetMask() { - Done(); - } - } - - private void WaitTimeInference() - { - if (renderCamera != null) - { - renderCamera.Render(); - } - - if (!academy.GetIsInference()) - { - RequestDecision(); - } - else - { - if (timeSinceDecision >= timeBetweenDecisionsAtInference) + 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; + + AddReward(-0.01f); + + switch (action) + { + 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); + throw new ArgumentException("Invalid action value. Stop movement."); + } + + // Render new image after movement in order to update the centerOfGravity + if (renderCamera != null) + { + renderCamera.Render(); + } + + RewardAgent(); + } + + // Set the reward for the agent based on how far away the center of gravity is from the center of the image + private void RewardAgent() + { + float centerOfGravityX = imageProcessor.CenterOfGravity.X; + float reward = 0; + + // Center of gravity is far away from the center (left) + 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) / 2; + } + // Center of gravity is near left of the center + 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)) && (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) / 2; + } + // Center of gravity is near right of the center + 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); + } + + // to be implemented by the developer + public override void AgentReset() + { + academy.AcademyReset(); + } + + + private void OnTriggerEnter(Collider other) + { + if (other.transform.CompareTag("Goal")) + { + Done(); + } + } + + private void WaitTimeInference() + { + if (renderCamera != null) + { + renderCamera.Render(); + } + + if (!academy.GetIsInference()) { - timeSinceDecision = 0f; RequestDecision(); } else { - timeSinceDecision += Time.fixedDeltaTime; + if (timeSinceDecision >= timeBetweenDecisionsAtInference) + { + timeSinceDecision = 0f; + RequestDecision(); + } + else + { + timeSinceDecision += Time.fixedDeltaTime; + } } } - } + } } diff --git a/Assets/Scripts/Movemnet/CozmoMovement.cs b/Assets/Scripts/Movemnet/CozmoMovement.cs index e1cb533..a288a2d 100644 --- a/Assets/Scripts/Movemnet/CozmoMovement.cs +++ b/Assets/Scripts/Movemnet/CozmoMovement.cs @@ -1,101 +1,120 @@ -using System.Collections; -using System.Collections.Generic; +///----------------------------------------------------------------- +/// Namespace: +/// Class: +/// Description: +/// Author: Date: <29.07.2019> +/// Notes: <> +///----------------------------------------------------------------- +/// + + using UnityEngine; -public class CozmoMovement : MonoBehaviour +namespace Cozmo { - - public float m_Speed = 12f; // How fast the tank moves forward and back. - public float m_TurnSpeed = 180f; // How fast the tank turns in degrees per second. - - - private string m_MovementAxisName; // The name of the input axis for moving forward and back. - private string m_TurnAxisName; // The name of the input axis for turning. - private Rigidbody m_Rigidbody; // Reference used to move the tank. - private float m_MovementInputValue; // The current value of the movement input. - private float m_TurnInputValue; // The current value of the turn input. - - - private void Awake() + public class CozmoMovement : MonoBehaviour { - m_Rigidbody = GetComponent(); + private const string MOVEMENT_AXIS_NAME = "Vertical"; // name of the movement input axis + private const string TURN_AXIS_NAME = "Horizontal"; // name of the rotation input axis + + [Tooltip("Determines how fast the robot is moving forward and backwards.")] + public float m_Speed = 12f; + [Tooltip("Determines how fast the robot is turning in degrees per second.")] + public float m_TurnSpeed = 180f; + + private Rigidbody cozmoRigidbody; // Reference to cozmos rigidbody + private float m_MovementInputValue; // current input value for movement + private float m_TurnInputValue; // current input value for rotation + + + private void Awake() + { + cozmoRigidbody = GetComponent(); + } + + + private void OnEnable() + { + // When the cozmo is turned on, make sure it's not kinematic. + cozmoRigidbody.isKinematic = false; + + // reset movement and rotation values + m_MovementInputValue = 0f; + m_TurnInputValue = 0f; + } + + + private void OnDisable() + { + // When the cozmo is turned off, set it to kinematic so it stops moving. + cozmoRigidbody.isKinematic = true; + } + + + private void Update() + { + // Store the value of both input axes. + m_MovementInputValue = Input.GetAxis(MOVEMENT_AXIS_NAME); + m_TurnInputValue = Input.GetAxis(TURN_AXIS_NAME); + } + + + private void FixedUpdate() + { + // Adjust position and rotation + Move(); + Turn(); + } + + /// + /// Move the cozmo forward (0 < directionValue <= 1) or backwards (0 > directionvalue >= -1) + /// + /// + public void Move(float directionValue) + { + // clamp directionValue to make sure its between -1 and 1 + Mathf.Clamp(directionValue, -1, 1); + + Vector3 movement = directionValue * transform.forward * m_Speed * Time.deltaTime; + + // Apply this movement to the rigidbody's position. + cozmoRigidbody.MovePosition(cozmoRigidbody.position + movement); + } + + /// + /// Move cozmo based on the input value + /// + public void Move() + { + Move(m_MovementInputValue); + } + + + /// + /// Rotate cozmo based on the input value + /// + public void Turn() + { + Turn(m_TurnInputValue); + } + + /// + /// Rotate cozmo + /// + /// + public void Turn(float turnValue) + { + // clamp turnValue to make sure its between -1 and 1 + Mathf.Clamp(turnValue, -1, 1); + + float turn = turnValue * m_TurnSpeed * Time.deltaTime; + + // Make this into a rotation in the y axis. + Quaternion turnRotation = Quaternion.Euler(0f, turn, 0f); + + // Apply this rotation to the rigidbody's rotation. + cozmoRigidbody.MoveRotation(cozmoRigidbody.rotation * turnRotation); + } + } - - - private void OnEnable() - { - // When the cozmo is turned on, make sure it's not kinematic. - m_Rigidbody.isKinematic = false; - - // Also reset the input values. - m_MovementInputValue = 0f; - m_TurnInputValue = 0f; - } - - - private void OnDisable() - { - // When the cozmo is turned off, set it to kinematic so it stops moving. - m_Rigidbody.isKinematic = true; - } - - - private void Start() - { - m_MovementAxisName = "Vertical"; - m_TurnAxisName = "Horizontal"; - } - - - private void Update() - { - // Store the value of both input axes. - m_MovementInputValue = Input.GetAxis(m_MovementAxisName); - m_TurnInputValue = Input.GetAxis(m_TurnAxisName); - } - - - private void FixedUpdate() - { - // Adjust the rigidbodies position and orientation in FixedUpdate. - Move(); - Turn(); - } - - public void Move(float directionValue) - { - // Create a vector in the direction the cozmo is facing with a magnitude based on the input, speed and the time between frames. - Vector3 movement = directionValue * transform.forward * m_Speed * Time.deltaTime; - - // Apply this movement to the rigidbody's position. - m_Rigidbody.MovePosition(m_Rigidbody.position + movement); - } - - public void Move() - { - Move(m_MovementInputValue); - } - - //public void Move(float[] act) - //{ - - //} - - public void Turn() - { - Turn(m_TurnInputValue); - } - - public void Turn(float turnValue) - { - // Determine the number of degrees to be turned based on the input, speed and time between frames. - float turn = turnValue * m_TurnSpeed * Time.deltaTime; - - // Make this into a rotation in the y axis. - Quaternion turnRotation = Quaternion.Euler(0f, turn, 0f); - - // Apply this rotation to the rigidbody's rotation. - m_Rigidbody.MoveRotation(m_Rigidbody.rotation * turnRotation); - } - } diff --git a/Assets/Scripts/Movemnet/CozmoMovementController.cs b/Assets/Scripts/Movemnet/CozmoMovementController.cs index a449e75..65e69cd 100644 --- a/Assets/Scripts/Movemnet/CozmoMovementController.cs +++ b/Assets/Scripts/Movemnet/CozmoMovementController.cs @@ -1,45 +1,55 @@ -using System; -using System.Collections; -using System.Collections.Generic; +///----------------------------------------------------------------- +/// Namespace: +/// Class: +/// Description: +/// Author: Date: <29.07.2019> +/// Notes: <> +///----------------------------------------------------------------- +/// + +using System; using UnityEngine; -public enum MovementState { Stop, Forward, Right, Left } - -[RequireComponent(typeof(CozmoMovement))] -public class CozmoMovementController : MonoBehaviour +namespace Cozmo { - [Tooltip("Current MovementState of the Robot. Change this to make the robot move.")] - public MovementState currentMovementState = MovementState.Stop; + // Different movementstates cozmo can use + public enum MovementState { Stop, Forward, Right, Left } - private CozmoMovement movement; // Movement script of the robot - - - // Start is called before the first frame update - void Start() + [RequireComponent(typeof(CozmoMovement))] + public class CozmoMovementController : MonoBehaviour { - movement = GetComponent(); - } + [Tooltip("Current MovementState of the Robot. Change this to make the robot move.")] + public MovementState currentMovementState = MovementState.Stop; - private void FixedUpdate() - { - switch (currentMovementState) + private CozmoMovement movement; // Movement script of the robot + + + void Start() { - 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."); + movement = GetComponent(); + } + 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."); + + } } } } diff --git a/Assets/Scripts/Movemnet/CozmoMovementTester.cs b/Assets/Scripts/Movemnet/CozmoMovementTester.cs index 4da517c..d55d143 100644 --- a/Assets/Scripts/Movemnet/CozmoMovementTester.cs +++ b/Assets/Scripts/Movemnet/CozmoMovementTester.cs @@ -1,113 +1,126 @@ -using System; -using System.Collections; -using System.Collections.Generic; +///----------------------------------------------------------------- +/// Namespace: +/// Class: +/// Description: +/// Author: Date: <29.07.2019> +/// Notes: <> +///----------------------------------------------------------------- +/// + +using Cozmo; using System.Diagnostics; using UnityEngine; -public enum TestStates { MovementSpeed, RotationSpeed } // enum to determine test scenarios - -[RequireComponent(typeof(CozmoMovement))] -public class CozmoMovementTester : MonoBehaviour +namespace CozmoHelpers { + public enum TestStates { MovementSpeed, RotationSpeed } // enum to determine test scenarios - private const float DISTANCE_TO_MEASURE_SPEED = 1f; // Distance that is used to calculate cozmos movementspeed (in m) - private const float ROTATION_ANGLE = 90f; // Angle that is used to calculate cozmos rotationspeed - - private CozmoMovement movement; // Reference to the movement script of cozmo - private Vector3 startPositionCozmo; // Original position of cozmo - private float lastY; // Last Y of cozmo - private Stopwatch movementWatch; // Stopwatch to measure the time for the movement usecase - private Stopwatch rotationWatch; // Stopwatch to measure the time for the rotation usecase - - [Tooltip("Choose which speed of the virtual cozmo should be tested.")] - public TestStates testState = TestStates.MovementSpeed; - - [Tooltip("Start and stop testing")] - public bool isTesting = false; - - - void Start() - { - movement = GetComponent(); - movementWatch = new Stopwatch(); - rotationWatch = new Stopwatch(); - - - startPositionCozmo = movement.transform.position; // Cache the starting position of Cozmo - lastY = movement.transform.rotation.y; // Cache the rotation Vector of Cozmo - } - - private void FixedUpdate() - { - if (isTesting) - { - if (testState == TestStates.MovementSpeed) - { - TestMovementSpeed(); - } - else if (testState == TestStates.RotationSpeed) - { - TestRotationSpeed(); - } - } - } - - private void TestRotationSpeed() + [RequireComponent(typeof(CozmoMovement))] + public class CozmoMovementTester : MonoBehaviour { - float currentY = movement.transform.rotation.eulerAngles.y; + private const float DISTANCE_TO_MEASURE_SPEED = 1f; // Distance that is used to calculate cozmos movementspeed (in m) + private const float ROTATION_ANGLE = 90f; // Angle that is used to calculate cozmos rotationspeed - // If distance between start and current position of cozmo reached the DISTANCE_TO_MEASURE_SPEED stop the stopwatch - if (currentY >= lastY + ROTATION_ANGLE || currentY <= lastY - ROTATION_ANGLE) + private CozmoMovement movement; // Reference to the movement script of cozmo + private Vector3 startPositionCozmo; // Original position of cozmo + private float lastY; // Last Y of cozmo + private Stopwatch movementWatch; // Stopwatch to measure the time for the movement usecase + private Stopwatch rotationWatch; // Stopwatch to measure the time for the rotation usecase + + [Tooltip("Choose which speed of the virtual cozmo should be tested.")] + public TestStates testState = TestStates.MovementSpeed; + + [Tooltip("Start and stop testing from the inspector.")] + public bool isTesting = false; + + + void Start() { - if (rotationWatch.IsRunning) - { - rotationWatch.Stop(); - UnityEngine.Debug.Log("Cozmo: " + gameObject.name + - "\nElapsed time in milliseconds (Rotation): " + rotationWatch.ElapsedMilliseconds); - } + movement = GetComponent(); + movementWatch = new Stopwatch(); + rotationWatch = new Stopwatch(); - isTesting = false; - return; + + startPositionCozmo = movement.transform.position; // Cache the starting position of Cozmo + lastY = movement.transform.rotation.y; // Cache the rotation Vector of Cozmo } - else - { - // Start stopwatch to measure the time cozmo needs for a specific rotation - if (!rotationWatch.IsRunning) - { - rotationWatch = Stopwatch.StartNew(); - } - // Turn right - movement.Turn(1); + private void FixedUpdate() + { + if (isTesting) + { + if (testState == TestStates.MovementSpeed) + { + TestMovementSpeed(); + } + else if (testState == TestStates.RotationSpeed) + { + TestRotationSpeed(); + } + } } - } - private void TestMovementSpeed() - { - // If distance between start and current position of cozmo reached the DISTANCE_TO_MEASURE_SPEED stop the stopwatch - if (Vector3.Distance(startPositionCozmo, movement.transform.position) >= DISTANCE_TO_MEASURE_SPEED) + // Measure time for rotation + private void TestRotationSpeed() { - if (movementWatch.IsRunning) - { - movementWatch.Stop(); - UnityEngine.Debug.Log("Cozmo: " + gameObject.name + - "/nElapsed time in milliseconds (Movement): " + movementWatch.ElapsedMilliseconds); - } - isTesting = false; - return; + float currentY = movement.transform.rotation.eulerAngles.y; + + // If distance between start and current position of cozmo reached the DISTANCE_TO_MEASURE_SPEED stop the stopwatch + if (currentY >= lastY + ROTATION_ANGLE || currentY <= lastY - ROTATION_ANGLE) + { + if (rotationWatch.IsRunning) + { + rotationWatch.Stop(); + UnityEngine.Debug.Log("Cozmo: " + gameObject.name + + "\nElapsed time in milliseconds (Rotation): " + rotationWatch.ElapsedMilliseconds); + } + + isTesting = false; + return; + } + else + { + // Start stopwatch to measure the time cozmo needs for a specific rotation + if (!rotationWatch.IsRunning) + { + rotationWatch = Stopwatch.StartNew(); + } + + // Turn right + movement.Turn(1); + } } - else - { - // Start stopwatch to measure the time cozmo needs for a specific distance - if (!movementWatch.IsRunning) - { - movementWatch = Stopwatch.StartNew(); - } - // Move cozmo in forward direction - movement.Move(1); + // Measure time for movement + private void TestMovementSpeed() + { + // If distance between start and current position of cozmo reached the DISTANCE_TO_MEASURE_SPEED stop the stopwatch + if (Vector3.Distance(startPositionCozmo, movement.transform.position) >= DISTANCE_TO_MEASURE_SPEED) + { + if (movementWatch.IsRunning) + { + movementWatch.Stop(); + UnityEngine.Debug.Log("Cozmo: " + gameObject.name + + "/nElapsed time in milliseconds (Movement): " + movementWatch.ElapsedMilliseconds); + } + + isTesting = false; + return; + } + else + { + // Start stopwatch to measure the time cozmo needs for a specific distance + if (!movementWatch.IsRunning) + { + movementWatch = Stopwatch.StartNew(); + } + + // Move cozmo in forward direction + movement.Move(1); + } } } } diff --git a/Assets/Scripts/SceneHelper.cs b/Assets/Scripts/SceneHelper.cs index 5faf13b..8701a78 100644 --- a/Assets/Scripts/SceneHelper.cs +++ b/Assets/Scripts/SceneHelper.cs @@ -1,23 +1,35 @@ -using System.Collections; +///----------------------------------------------------------------- +/// Namespace: +/// Class: +/// Description: +/// Author: Date: <29.07.2019> +/// Notes: <> +///----------------------------------------------------------------- +/// + using System.Collections.Generic; using UnityEngine; -public class SceneHelper : MonoBehaviour +namespace CozmoHelpers { - [Tooltip("All the objects in this list will be deactivated/activated when the game is running")] - public List toggleInPlayMode; - - - public void Awake() + public class SceneHelper : MonoBehaviour { - ToggleObjectList(); - } + [Tooltip("All the objects in this list will be deactivated/activated when the game is running")] + public List toggleInPlayMode; - private void ToggleObjectList() - { - foreach (GameObject go in toggleInPlayMode) + + public void Awake() { - go.SetActive(!go.activeSelf); + ToggleObjectList(); + } + + // Deactivate/Activate all gameObjects referenced in the toggleInPlayMode list + private void ToggleObjectList() + { + foreach (GameObject go in toggleInPlayMode) + { + go.SetActive(!go.activeSelf); + } } } }