From 113ec4e45413203ae3b6891646ab4e3a7e17c2f5 Mon Sep 17 00:00:00 2001 From: Tobi Date: Sat, 27 Apr 2019 19:37:45 +0200 Subject: [PATCH] Add CozmoMovementTest script to find out rotation and movement speed in the simulation --- Assets/Scenes/CozmoTraining.unity | 89 +++++---------- Assets/Scripts/Movemnet/CozmoMovement.cs | 39 ++++--- .../Scripts/Movemnet/CozmoMovementTester.cs | 105 ++++++++++++++++++ .../Movemnet/CozmoMovementTester.cs.meta | 11 ++ 4 files changed, 169 insertions(+), 75 deletions(-) create mode 100644 Assets/Scripts/Movemnet/CozmoMovementTester.cs create mode 100644 Assets/Scripts/Movemnet/CozmoMovementTester.cs.meta diff --git a/Assets/Scenes/CozmoTraining.unity b/Assets/Scenes/CozmoTraining.unity index 3f45c43..b7b8b22 100644 --- a/Assets/Scenes/CozmoTraining.unity +++ b/Assets/Scenes/CozmoTraining.unity @@ -311,8 +311,8 @@ Camera: y: 0 width: 1 height: 1 - near clip plane: 0.03 - far clip plane: 10 + near clip plane: 0.02 + far clip plane: 1 field of view: 60 orthographic: 0 orthographic size: 5 @@ -338,13 +338,13 @@ Transform: m_PrefabInstance: {fileID: 0} m_PrefabAsset: {fileID: 0} m_GameObject: {fileID: 589453881} - m_LocalRotation: {x: 0.2588191, y: 0, z: 0, w: 0.9659258} - m_LocalPosition: {x: 0, y: 0.038900018, z: 0.03939998} + m_LocalRotation: {x: 0.2164396, y: 0, z: 0, w: 0.97629607} + m_LocalPosition: {x: 0, y: 0.0336, z: 0.03939998} m_LocalScale: {x: 1, y: 1, z: 1} m_Children: [] m_Father: {fileID: 7570006596986120124} m_RootOrder: 1 - m_LocalEulerAnglesHint: {x: 30, y: 0, z: 0} + m_LocalEulerAnglesHint: {x: 25, y: 0, z: 0} --- !u!1 &998256472 GameObject: m_ObjectHideFlags: 0 @@ -376,13 +376,13 @@ MonoBehaviour: m_EditorClassIdentifier: broadcastHub: broadcastingBrains: - - {fileID: 11400000, guid: 94e62c225e4fe8148a7543c5fd1acfd4, type: 2} + - {fileID: 11400000, guid: 0d9d5ce547064a648b466ad7f208cf3d, type: 2} _brainsToControl: - - {fileID: 11400000, guid: 94e62c225e4fe8148a7543c5fd1acfd4, type: 2} + - {fileID: 0} maxSteps: 0 trainingConfiguration: - width: 80 - height: 80 + width: 84 + height: 84 qualityLevel: 1 timeScale: 10 targetFrameRate: -1 @@ -406,55 +406,6 @@ Transform: m_LocalScale: {x: 1, y: 1, z: 1} m_Children: [] m_Father: {fileID: 0} - m_RootOrder: 4 - m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} ---- !u!1 &1387463106 -GameObject: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInstance: {fileID: 0} - m_PrefabAsset: {fileID: 0} - serializedVersion: 6 - m_Component: - - component: {fileID: 1387463108} - - component: {fileID: 1387463107} - m_Layer: 0 - m_Name: EnvironmentGenerator - m_TagString: Untagged - m_Icon: {fileID: 0} - m_NavMeshLayer: 0 - m_StaticEditorFlags: 0 - m_IsActive: 0 ---- !u!114 &1387463107 -MonoBehaviour: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInstance: {fileID: 0} - m_PrefabAsset: {fileID: 0} - m_GameObject: {fileID: 1387463106} - m_Enabled: 1 - m_EditorHideFlags: 0 - m_Script: {fileID: 11500000, guid: 6279e2499d1811045a857f890736f7ba, type: 3} - m_Name: - m_EditorClassIdentifier: - width: 2 - height: 2 - amountOfHorizontalElements: 10 - amountOfVerticalElements: 10 - groundMaterial: {fileID: 2100000, guid: 7f3a0759e06ddf74899cf4a62f849678, type: 2} - testDestroy: 0 ---- !u!4 &1387463108 -Transform: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInstance: {fileID: 0} - m_PrefabAsset: {fileID: 0} - m_GameObject: {fileID: 1387463106} - m_LocalRotation: {x: 0, y: 0, z: 0, w: 1} - m_LocalPosition: {x: 0, y: 0, z: 0} - m_LocalScale: {x: 1, y: 1, z: 1} - m_Children: [] - m_Father: {fileID: 0} m_RootOrder: 3 m_LocalEulerAnglesHint: {x: 0, y: 0, z: 0} --- !u!1001 &7570006596986120122 @@ -482,7 +433,7 @@ PrefabInstance: - target: {fileID: 7570006595835424293, guid: 0f97dac5215d69a4795763340d82925d, type: 3} propertyPath: m_LocalPosition.z - value: -0.199 + value: -0.564 objectReference: {fileID: 0} - target: {fileID: 7570006595835424293, guid: 0f97dac5215d69a4795763340d82925d, type: 3} @@ -550,10 +501,11 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: 234fd6e10ef55ad4b82b3153890ab5d9, type: 3} m_Name: m_EditorClassIdentifier: - brain: {fileID: 11400000, guid: 94e62c225e4fe8148a7543c5fd1acfd4, type: 2} + brain: {fileID: 11400000, guid: 0d9d5ce547064a648b466ad7f208cf3d, type: 2} agentParameters: agentCameras: - {fileID: 589453882} + agentRenderTextures: [] maxStep: 1000 resetOnDone: 1 onDemandDecision: 0 @@ -599,5 +551,18 @@ MonoBehaviour: m_Script: {fileID: 11500000, guid: d2056048a78a0cc4c97e7899ba1c0e31, type: 3} m_Name: m_EditorClassIdentifier: - m_Speed: 0.1 - m_TurnSpeed: 50 + m_Speed: 0.0535 + m_TurnSpeed: 55 +--- !u!114 &7570006596986120129 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 7570006596986120123} + m_Enabled: 0 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 4265d0764c65d26409f0f920e5293c82, type: 3} + m_Name: + m_EditorClassIdentifier: + testState: 1 diff --git a/Assets/Scripts/Movemnet/CozmoMovement.cs b/Assets/Scripts/Movemnet/CozmoMovement.cs index 136c6ae..6cfda1b 100644 --- a/Assets/Scripts/Movemnet/CozmoMovement.cs +++ b/Assets/Scripts/Movemnet/CozmoMovement.cs @@ -4,17 +4,17 @@ 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() { @@ -43,9 +43,9 @@ public class CozmoMovement : MonoBehaviour private void Start() { // The axes names are based on player number. - m_MovementAxisName = "Vertical"; - m_TurnAxisName = "Horizontal"; - + m_MovementAxisName = "Vertical"; + m_TurnAxisName = "Horizontal"; + } @@ -53,9 +53,9 @@ public class CozmoMovement : MonoBehaviour { // Store the value of both input axes. m_MovementInputValue = Input.GetAxis(m_MovementAxisName); - m_TurnInputValue = Input.GetAxis(m_TurnAxisName); + m_TurnInputValue = Input.GetAxis(m_TurnAxisName); } - + private void FixedUpdate() { @@ -64,21 +64,34 @@ public class CozmoMovement : MonoBehaviour Turn(); } - - private void Move() + public void Move(float directionValue) { // Create a vector in the direction the tank is facing with a magnitude based on the input, speed and the time between frames. - Vector3 movement = transform.forward * m_MovementInputValue * m_Speed * Time.deltaTime; + 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); + } - private void Turn() + //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 = m_TurnInputValue * m_TurnSpeed * Time.deltaTime; + float turn = turnValue * m_TurnSpeed * Time.deltaTime; // Make this into a rotation in the y axis. Quaternion turnRotation = Quaternion.Euler(0f, turn, 0f); diff --git a/Assets/Scripts/Movemnet/CozmoMovementTester.cs b/Assets/Scripts/Movemnet/CozmoMovementTester.cs new file mode 100644 index 0000000..3d34e3f --- /dev/null +++ b/Assets/Scripts/Movemnet/CozmoMovementTester.cs @@ -0,0 +1,105 @@ +using System; +using System.Collections; +using System.Collections.Generic; +using System.Diagnostics; +using UnityEngine; + +public enum TestStates { MovementSpeed, RotationSpeed } + +[RequireComponent(typeof(CozmoMovement))] +public class CozmoMovementTester : MonoBehaviour +{ + + 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 float curAngleX = 0; // Current angle in the x axis 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; + + 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 (testState == TestStates.MovementSpeed) + { + TestMovementSpeed(); + } + else if (testState == TestStates.RotationSpeed) + { + TestRotationSpeed(); + } + } + + private void TestRotationSpeed() + { + + 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); + } + + 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); + } + } + + 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); + } + + 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/Movemnet/CozmoMovementTester.cs.meta b/Assets/Scripts/Movemnet/CozmoMovementTester.cs.meta new file mode 100644 index 0000000..11bd8b0 --- /dev/null +++ b/Assets/Scripts/Movemnet/CozmoMovementTester.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 4265d0764c65d26409f0f920e5293c82 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: