hasselto66176
/
BachelorarbeitCozmo
Watch 1
Star 0
Fork 0
Code Issues 0 Pull Requests 0 Releases 0 Wiki Activity
Browse Source

Add commetns and refactoring

Development
Tobi 5 years ago
parent
0cfb60bea0
commit
021662632e
7 changed files with 656 additions and 569 deletions
Unified View Show Diff Stats
  1. 164
    152
      Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs
  2. 30
    20
      Assets/Scripts/ML Cozmo/CozmoAcademy.cs
  3. 190
    179
      Assets/Scripts/ML Cozmo/CozmoAgent.cs
  4. 113
    94
      Assets/Scripts/Movemnet/CozmoMovement.cs
  5. 43
    33
      Assets/Scripts/Movemnet/CozmoMovementController.cs
  6. 92
    79
      Assets/Scripts/Movemnet/CozmoMovementTester.cs
  7. 24
    12
      Assets/Scripts/SceneHelper.cs

+ 164
- 152
Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs View File

using System.Collections;
using System.Collections.Generic;
///-----------------------------------------------------------------
/// Namespace: <Cozmo>
/// Class: <ImageProcessor>
/// Description: <Converts a rendertexture to a opencv mat in order to use the canny algorithm.
/// After processing the mat will be converted to a render texture back again.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
///

using UnityEngine; using UnityEngine;
using OpenCvSharp; using OpenCvSharp;
using System.Threading.Tasks; 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;

/// <summary>
/// Center of Gravity in the cropped canny image
/// </summary>
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<Camera>();
[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;
/// <summary>
/// Center of Gravity in the cropped canny image
/// </summary>
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];
// OpenCVSharp parameters
private Mat cozmoImageMat;
private Mat cannyImage;
private Texture2D finalProcessedCozmoTexture;
private Vec3b[] cozmoImageData;
private byte[] cannyImageData;


originalCozmoTexture = new Texture2D(imWidth, imHeight, TextureFormat.RGBA32, true, true);
finalProcessedCozmoTexture = new Texture2D(imWidth, croppedImHeight, TextureFormat.RGBA32, true, true);
}


///// <summary>
///// Gets called when a new image arrives from the camera this script lies on
///// </summary>
///// <param name="source"></param>
///// <param name="destination"></param>
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);
}
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


// 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;
}
private Texture2D originalCozmoTexture;




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();
private void Start()
{
// Get reference to the cozmo camera
textureCamera = GetComponent<Camera>();

// 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;

// 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);
}

///// <summary>
///// Gets called when a new image arrives from the camera this script lies on
///// </summary>
///// <param name="source"></param>
///// <param name="destination"></param>
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;
}

// 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();
}


// Parallel for loop
// convert Color32 object to Vec3b object
// Vec3b is the representation of pixel for Mat
Parallel.For(0, imHeight, i =>
// Convert Unity Texture2D object to OpenCVSharp Mat object
private void TextureToMat(Texture2D source)
{ {
for (var j = 0; j < imWidth; j++)
// 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 =>
{ {
var col = c[j + i * imWidth];
var vec3 = new Vec3b
for (var j = 0; j < imWidth; j++)
{ {
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);
}


// Simple example of canny edge detect
private void ProcessImage(Mat _image)
{
Cv2.Canny(_image, cannyImage, 100, 100);
}

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);
}


// Simple example of canny edge detect
private void ProcessImage(Mat _image)
{
Cv2.Canny(_image, cannyImage, 100, 100);
}


// 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 =>
// Convert OpenCVSharp Mat object to Unity Texture2D object
private void MatToTexture(Mat mat)
{ {
for (var j = 0; j < imWidth; j++)
// 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 =>
{ {
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;
}
});


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);
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 #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 #endif
}
}


}
} }

+ 30
- 20
Assets/Scripts/ML Cozmo/CozmoAcademy.cs View File

using MLAgents;
using System.Collections;
using System.Collections.Generic;
///-----------------------------------------------------------------
/// Namespace: <Cozmo>
/// Class: <CozmoAcademy>
/// Description: <The academy used for cozmo. Sets cozmo back to its starting position if the academy gets reset.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
///

using MLAgents;
using UnityEngine; 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;
}

}
} }

+ 190
- 179
Assets/Scripts/ML Cozmo/CozmoAgent.cs View File

using MLAgents;
///-----------------------------------------------------------------
/// Namespace: <Cozmo>
/// Class: <CozmoAgent>
/// Description: <The actual agent in the scene. Collects observations and executes the actions.
/// Also rewards the agent and sets an actionmask.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
///

using MLAgents;
using OpenCvSharp; using OpenCvSharp;
using System; using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine; 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<ImageProcessor>();
nearAreaLimit = (int)(renderCamera.targetTexture.width / 2 * NEAR_AREA_PERCENTAGE_OFFSET);
centerOfImageX = renderCamera.targetTexture.width / 2;
}


public void FixedUpdate()
{
WaitTimeInference();
}


public override void CollectObservations()
{
SetMask();
}

// Set ActionMask for training
private void SetMask()
{
switch (lastChosenMovement)
// 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()
{ {
// 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.");
academy = FindObjectOfType(typeof(CozmoAcademy)) as CozmoAcademy;
imageProcessor = renderCamera.GetComponent<ImageProcessor>();
nearAreaLimit = (int)(renderCamera.targetTexture.width / 2 * NEAR_AREA_PERCENTAGE_OFFSET);
centerOfImageX = renderCamera.targetTexture.width / 2;
} }
}


// 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;
public void FixedUpdate()
{
WaitTimeInference();
}


AddReward(-0.01f);


switch (action)
public override void CollectObservations()
{ {
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.");
SetMask();
} }


// Render new image after movement in order to update the centerOfGravity
if (renderCamera != null)
// Set ActionMask for training
private void SetMask()
{ {
renderCamera.Render();
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.");
}
} }


RewardAgent();
}
// 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;


// 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;
int action = Mathf.FloorToInt(vectorAction[0]);
Point centerOfGravity = imageProcessor.CenterOfGravity;


// 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");
}
AddReward(-0.01f);


Debug.Log("Reward: " + reward);
SetReward(reward);
}
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.");
}


// to be implemented by the developer
public override void AgentReset()
{
academy.AcademyReset();
}
// Render new image after movement in order to update the centerOfGravity
if (renderCamera != null)
{
renderCamera.Render();
}


RewardAgent();
}


private void OnTriggerEnter(Collider other)
{
if (other.transform.CompareTag("Goal"))
// 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()
{ {
Done();
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);
} }
}


private void WaitTimeInference()
{
if (renderCamera != null)
// to be implemented by the developer
public override void AgentReset()
{ {
renderCamera.Render();
academy.AcademyReset();
} }


if (!academy.GetIsInference())

private void OnTriggerEnter(Collider other)
{ {
RequestDecision();
if (other.transform.CompareTag("Goal"))
{
Done();
}
} }
else

private void WaitTimeInference()
{ {
if (timeSinceDecision >= timeBetweenDecisionsAtInference)
if (renderCamera != null)
{
renderCamera.Render();
}

if (!academy.GetIsInference())
{ {
timeSinceDecision = 0f;
RequestDecision(); RequestDecision();
} }
else else
{ {
timeSinceDecision += Time.fixedDeltaTime;
if (timeSinceDecision >= timeBetweenDecisionsAtInference)
{
timeSinceDecision = 0f;
RequestDecision();
}
else
{
timeSinceDecision += Time.fixedDeltaTime;
}
} }
} }
}


}
} }

+ 113
- 94
Assets/Scripts/Movemnet/CozmoMovement.cs View File

using System.Collections;
using System.Collections.Generic;
using UnityEngine;

public class CozmoMovement : MonoBehaviour
{

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()
{
m_Rigidbody = GetComponent<Rigidbody>();
}


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";
}
///-----------------------------------------------------------------
/// Namespace: <Cozmo>
/// Class: <CozmoMovement>
/// Description: <Defines the possible movement of the virtual cozmo.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
///




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);
}
using UnityEngine;


public void Turn(float turnValue)
namespace Cozmo
{
public class CozmoMovement : MonoBehaviour
{ {
// Determine the number of degrees to be turned based on the input, speed and time between frames.
float turn = turnValue * m_TurnSpeed * Time.deltaTime;
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<Rigidbody>();
}


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();
}

/// <summary>
/// Move the cozmo forward (0 < directionValue <= 1) or backwards (0 > directionvalue >= -1)
/// </summary>
/// <param name="directionValue"></param>
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);
}

/// <summary>
/// Move cozmo based on the input value
/// </summary>
public void Move()
{
Move(m_MovementInputValue);
}


/// <summary>
/// Rotate cozmo based on the input value
/// </summary>
public void Turn()
{
Turn(m_TurnInputValue);
}

/// <summary>
/// Rotate cozmo
/// </summary>
/// <param name="turnValue"></param>
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);
}


// 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);
} }

} }

+ 43
- 33
Assets/Scripts/Movemnet/CozmoMovementController.cs View File

using System;
using System.Collections;
using System.Collections.Generic;
using UnityEngine;
///-----------------------------------------------------------------
/// Namespace: <Cozmo>
/// Class: <CozmoMovementController>
/// Description: <Statemachine to control the movement of the virtual cozmo.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
///


public enum MovementState { Stop, Forward, Right, Left }
using System;
using UnityEngine;


[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
[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)
void Start()
{
movement = GetComponent<CozmoMovement>();
}

private void FixedUpdate()
{ {
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.");
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.");


}
} }
} }
} }

+ 92
- 79
Assets/Scripts/Movemnet/CozmoMovementTester.cs View File

using System;
using System.Collections;
using System.Collections.Generic;
///-----------------------------------------------------------------
/// Namespace: <CozmoHelpers>
/// Class: <CozmoMovementTester>
/// Description: <Used to measure the time for rotating and moving the virtual cozmo to copy the
/// real cozmos movement behaviour.>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
///

using Cozmo;
using System.Diagnostics; using System.Diagnostics;
using UnityEngine; 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
[RequireComponent(typeof(CozmoMovement))]
public class CozmoMovementTester : MonoBehaviour
{


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
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


[Tooltip("Choose which speed of the virtual cozmo should be tested.")]
public TestStates testState = TestStates.MovementSpeed;
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("Start and stop testing")]
public bool isTesting = false;
[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()
{
movement = GetComponent<CozmoMovement>();
movementWatch = new Stopwatch();
rotationWatch = new Stopwatch();


void Start()
{
movement = GetComponent<CozmoMovement>();
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)
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 (testState == TestStates.MovementSpeed)
{
TestMovementSpeed();
}
else if (testState == TestStates.RotationSpeed)
if (isTesting)
{ {
TestRotationSpeed();
if (testState == TestStates.MovementSpeed)
{
TestMovementSpeed();
}
else if (testState == TestStates.RotationSpeed)
{
TestRotationSpeed();
}
} }
} }
}


private void TestRotationSpeed()
{
// Measure time for rotation
private void TestRotationSpeed()
{


float currentY = movement.transform.rotation.eulerAngles.y;
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)
// 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)
{ {
rotationWatch.Stop();
UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
"\nElapsed time in milliseconds (Rotation): " + rotationWatch.ElapsedMilliseconds);
if (rotationWatch.IsRunning)
{
rotationWatch.Stop();
UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
"\nElapsed time in milliseconds (Rotation): " + rotationWatch.ElapsedMilliseconds);
}

isTesting = false;
return;
} }

isTesting = false;
return;
}
else
{
// Start stopwatch to measure the time cozmo needs for a specific rotation
if (!rotationWatch.IsRunning)
else
{ {
rotationWatch = Stopwatch.StartNew();
// Start stopwatch to measure the time cozmo needs for a specific rotation
if (!rotationWatch.IsRunning)
{
rotationWatch = Stopwatch.StartNew();
}

// Turn right
movement.Turn(1);
} }

// Turn right
movement.Turn(1);
} }
}


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 movement
private void TestMovementSpeed()
{ {
if (movementWatch.IsRunning)
// 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)
{ {
movementWatch.Stop();
UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
"/nElapsed time in milliseconds (Movement): " + movementWatch.ElapsedMilliseconds);
if (movementWatch.IsRunning)
{
movementWatch.Stop();
UnityEngine.Debug.Log("Cozmo: " + gameObject.name +
"/nElapsed time in milliseconds (Movement): " + movementWatch.ElapsedMilliseconds);
}

isTesting = false;
return;
} }

isTesting = false;
return;
}
else
{
// Start stopwatch to measure the time cozmo needs for a specific distance
if (!movementWatch.IsRunning)
else
{ {
movementWatch = Stopwatch.StartNew();
// 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);
} }

// Move cozmo in forward direction
movement.Move(1);
} }
} }
} }

+ 24
- 12
Assets/Scripts/SceneHelper.cs View File

using System.Collections;
///-----------------------------------------------------------------
/// Namespace: <CozmoHelpers>
/// Class: <SceneHelper>
/// Description: <Used to deactivate specific objects that are only used in the sceneview not in the gameview>
/// Author: <Tobias Hassel> Date: <29.07.2019>
/// Notes: <>
///-----------------------------------------------------------------
///

using System.Collections.Generic; using System.Collections.Generic;
using UnityEngine; 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<GameObject> toggleInPlayMode;
public class SceneHelper : MonoBehaviour
{
[Tooltip("All the objects in this list will be deactivated/activated when the game is running")]
public List<GameObject> toggleInPlayMode;




public void Awake()
{
ToggleObjectList();
}
public void Awake()
{
ToggleObjectList();
}


private void ToggleObjectList()
{
foreach (GameObject go in toggleInPlayMode)
// Deactivate/Activate all gameObjects referenced in the toggleInPlayMode list
private void ToggleObjectList()
{ {
go.SetActive(!go.activeSelf);
foreach (GameObject go in toggleInPlayMode)
{
go.SetActive(!go.activeSelf);
}
} }
} }
} }

Write Preview
Loading…
Cancel
Save