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

New Reward function and MovementController

Development
Tobi 5 years ago
parent
411338a35d
commit
221ffb54dc
5 changed files with 156 additions and 193 deletions
Unified View Show Diff Stats
  1. 14
    1
      Assets/Scenes/CozmoTraining.unity
  2. 29
    166
      Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs
  3. 57
    26
      Assets/Scripts/ML Cozmo/CozmoAgent.cs
  4. 45
    0
      Assets/Scripts/Movemnet/CozmoMovementController.cs
  5. 11
    0
      Assets/Scripts/Movemnet/CozmoMovementController.cs.meta

+ 14
- 1
Assets/Scenes/CozmoTraining.unity View File

onDemandDecision: 1 onDemandDecision: 1
numberOfActionsBetweenDecisions: 1 numberOfActionsBetweenDecisions: 1
renderCamera: {fileID: 891713640} renderCamera: {fileID: 891713640}
movement: {fileID: 7570006596986120128}
movementController: {fileID: 7570006596986120130}
timeBetweenDecisionsAtInference: 0.15 timeBetweenDecisionsAtInference: 0.15
--- !u!65 &7570006596986120126 --- !u!65 &7570006596986120126
BoxCollider: BoxCollider:
m_EditorClassIdentifier: m_EditorClassIdentifier:
testState: 1 testState: 1
isTesting: 0 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

+ 29
- 166
Assets/Scripts/CannyEdgeDetection/ImageProcessor.cs View File

public Point CenterOfGravity { get; private set; } public Point CenterOfGravity { get; private set; }


// OpenCVSharp parameters // OpenCVSharp parameters
private Mat videoSourceImage;
private Mat cozmoImageMat;
private Mat cannyImage; private Mat cannyImage;
private Texture2D processedTexture;
private Vec3b[] videoSourceImageData;
private Texture2D finalProcessedCozmoTexture;
private Vec3b[] cozmoImageData;
private byte[] cannyImageData; private byte[] cannyImageData;




private int croppedImHeight = 120; // Height of the cropped 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 Camera textureCamera; // Virtual Cozmo camera



private Texture2D firstTexture;
private Texture2D originalCozmoTexture;




private void Start() private void Start()
// Set image widths and heights based on the given RenderTextures // Set image widths and heights based on the given RenderTextures
imWidth = textureCamera.targetTexture.width; imWidth = textureCamera.targetTexture.width;
imHeight = textureCamera.targetTexture.height; 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; processedImageRenderer.material.mainTexture = textureCamera.targetTexture;
processedImageRendererCropped.material.mainTexture = renderTextureCropped; processedImageRendererCropped.material.mainTexture = renderTextureCropped;


// initialize video / image with given size // 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); cannyImage = new Mat(imHeight, imWidth, MatType.CV_8UC1);
cannyImageData = new byte[croppedImHeight * imWidth]; 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> ///// <summary>
///// </summary> ///// </summary>
///// <param name="source"></param> ///// <param name="source"></param>
///// <param name="destination"></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) 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); 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 // Crop image to just see the middle of the original image
return croppedImage; 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; 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 // 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 array : r, g, b, a
Color32[] c = source.GetPixels32(); Color32[] c = source.GetPixels32();
Item2 = col.r Item2 = col.r
}; };
// set pixel to an array // set pixel to an array
videoSourceImageData[j + i * imWidth] = vec3;
cozmoImageData[j + i * imWidth] = vec3;
} }
}); });


// assign the Vec3b array to Mat // 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 // Simple example of canny edge detect
private void ProcessImage_NEW(Mat _image)
private void ProcessImage(Mat _image)
{ {
Cv2.Canny(_image, cannyImage, 100, 100); 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 // 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 // cannyImageData is byte array, because canny image is grayscale
mat.GetArray(0, 0, cannyImageData); mat.GetArray(0, 0, cannyImageData);
}); });




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 // find moments of the image
Moments m = new Moments(processedImage, true); Moments m = new Moments(processedImage, true);

+ 57
- 26
Assets/Scripts/ML Cozmo/CozmoAgent.cs View File



[Tooltip("The virtual Cozmo camera")] [Tooltip("The virtual Cozmo camera")]
public Camera renderCamera; public Camera renderCamera;
//[Tooltip("Final cropped and scaled rendertexture")]
//public RenderTexture renderTextureScaled;
[Tooltip("Reference to the CozmoMovement script")] [Tooltip("Reference to the CozmoMovement script")]
public CozmoMovement movement;
public CozmoMovementController movementController;
public float timeBetweenDecisionsAtInference; public float timeBetweenDecisionsAtInference;


private Academy academy; // CozmoAcademy private Academy academy; // CozmoAcademy
private float timeSinceDecision; // time since last decision 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 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 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() private void Start()
{ {
academy = FindObjectOfType(typeof(CozmoAcademy)) as CozmoAcademy; academy = FindObjectOfType(typeof(CozmoAcademy)) as CozmoAcademy;
onRenderImageTest = renderCamera.GetComponent<ImageProcessor>();
imageProcessor = renderCamera.GetComponent<ImageProcessor>();
nearAreaLimit = (int)(renderCamera.targetTexture.width / 2 * NEAR_AREA_PERCENTAGE_OFFSET); nearAreaLimit = (int)(renderCamera.targetTexture.width / 2 * NEAR_AREA_PERCENTAGE_OFFSET);
farAreaLimit = (int)(renderCamera.targetTexture.width / 2 * FAR_AREA_PERCENTAGE_OFFSET); farAreaLimit = (int)(renderCamera.targetTexture.width / 2 * FAR_AREA_PERCENTAGE_OFFSET);
} }



public void FixedUpdate() public void FixedUpdate()
{ {
WaitTimeInference(); WaitTimeInference();
// for testing
//AgentAction(floats, "ActionText");
} }




{ {
//print("Action before FloorToInt: " + vectorAction[0]); //print("Action before FloorToInt: " + vectorAction[0]);
int action = Mathf.FloorToInt(vectorAction[0]); int action = Mathf.FloorToInt(vectorAction[0]);
Point centerOfGravity = onRenderImageTest.CenterOfGravity;
Point centerOfGravity = imageProcessor.CenterOfGravity;
//Vector3 targetPos = transform.position; //Vector3 targetPos = transform.position;


//print("Action after FloorToInt: " + action); //print("Action after FloorToInt: " + action);
switch (action) switch (action)
{ {
case STOP: case STOP:
movement.Move(0);
movementController.currentMovementState = MovementState.Stop;
break; break;
case FORWARD: case FORWARD:
movement.Move(1);
movementController.currentMovementState = MovementState.Forward;
break; break;
case RIGHT: case RIGHT:
movement.Turn(1);
movementController.currentMovementState = MovementState.Right;
break; break;
case LEFT: case LEFT:
movement.Turn(-1);
movementController.currentMovementState = MovementState.Left;
break; break;
default: default:
//movement.Move(0); //movement.Move(0);
if (renderCamera != null) if (renderCamera != null)
{ {
renderCamera.Render(); 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 // to be implemented by the developer


if (!academy.GetIsInference()) if (!academy.GetIsInference())
{ {
//onRenderImageTest.ProcessRenderTexture_NEW(renderCamera.targetTexture, onRenderImageTest.renderTextureCropped);
RequestDecision(); RequestDecision();
} }
else else
if (timeSinceDecision >= timeBetweenDecisionsAtInference) if (timeSinceDecision >= timeBetweenDecisionsAtInference)
{ {
timeSinceDecision = 0f; timeSinceDecision = 0f;
//onRenderImageTest.ProcessRenderTexture_NEW(renderCamera.targetTexture, onRenderImageTest.renderTextureCropped);
RequestDecision(); RequestDecision();
} }
else else

+ 45
- 0
Assets/Scripts/Movemnet/CozmoMovementController.cs View File

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.");

}
}
}

+ 11
- 0
Assets/Scripts/Movemnet/CozmoMovementController.cs.meta View File

fileFormatVersion: 2
guid: d3ecda30187828846b48edbee97aebd7
MonoImporter:
externalObjects: {}
serializedVersion: 2
defaultReferences: []
executionOrder: 0
icon: {instanceID: 0}
userData:
assetBundleName:
assetBundleVariant:

Write Preview
Loading…
Cancel
Save