organisation & splat changes

Assets/3_MarathonEnvs/Scripts/Shared/CameraHelper.cs

@@ -0,0 +1,124 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using UnityEngine.SceneManagement;
+
+public class CameraHelper : MonoBehaviour {
+
+	Transform _camera;
+	Vector3 _defaultCameraRotation;
+	SmoothFollow _smoothFollow;
+	float _cameraRotY;
+	float _defaultTimeScale;
+	float _defaultHeight;
+	float _defaultDistance;
+	float _height = 0f;
+	float _zoom = 0;
+	// Use this for initialization
+	void Start () {
+		_defaultTimeScale = 1f;
+		_camera = transform;
+		_defaultCameraRotation = _camera.eulerAngles;
+		_cameraRotY = _defaultCameraRotation.y;
+		_smoothFollow = GetComponent<SmoothFollow>();
+		_defaultHeight = _smoothFollow.height;
+		_defaultDistance = _smoothFollow.distance;
+	}
+	
+	// Update is called once per frame
+	void Update () {
+		// if (Input.GetKeyDown(KeyCode.Escape) || Input.GetKeyDown("`") || Input.GetKeyDown(KeyCode.Delete))
+		// {
+		// 	ToggleTimeScale(_defaultTimeScale);
+		// 	SceneManager.LoadSceneAsync(SceneManager.GetActiveScene().buildIndex);
+		// }
+
+		if (Input.GetKeyDown("0"))
+		{
+			ToggleTimeScale(0f);
+		}
+		if (Input.GetKeyDown("1"))
+		{
+			ToggleTimeScale(.05f);
+		}
+		if (Input.GetKeyDown("2"))
+		{
+			ToggleTimeScale(0.1f);
+		}
+		if (Input.GetKeyDown("3"))
+		{
+			ToggleTimeScale(0.2f);
+		}
+		if (Input.GetKey(KeyCode.LeftShift) || Input.GetKey(KeyCode.RightShift))
+		{
+			if (Input.GetKeyDown("a") || Input.GetKeyDown(KeyCode.LeftArrow))
+			{
+				AddAngle(30f);
+			}
+			if (Input.GetKeyDown("d") || Input.GetKeyDown(KeyCode.RightArrow))
+			{
+				AddAngle(-30f);
+			}
+			if (Input.GetKeyDown("w") || Input.GetKeyDown(KeyCode.UpArrow))
+			{
+				Height(+1f);
+			}
+			if (Input.GetKeyDown("s") || Input.GetKeyDown(KeyCode.DownArrow))
+			{
+				Height(-1f);
+			}
+			if (Input.GetKeyDown("e"))
+			{
+				Zoom(-1f);
+			}
+			if (Input.GetKeyDown("q"))
+			{
+				Zoom(1f);
+			}
+			if (Input.GetKeyDown(KeyCode.Return))
+			{
+				ResetCamera();
+			}
+		}
+
+	}
+	void Zoom(float diff)
+	{
+		_zoom += diff;
+		_zoom = Mathf.Clamp(_zoom, -10f, 10f);
+		_smoothFollow.distance = _defaultDistance + (_zoom /5);
+	}
+	void Height(float diff)
+	{
+		_height += diff;
+		_height = Mathf.Clamp(_height, -10f, 10f);
+		_smoothFollow.height = _defaultHeight + (_height /3);
+	}
+	void AddAngle(float diff)
+	{
+		_cameraRotY += diff;
+		if (_cameraRotY <= -360)
+			_cameraRotY += 720;
+		if (_cameraRotY >= 360)
+			_cameraRotY -= 720;
+		SetAngle(_cameraRotY);
+	}
+	void ResetCamera()
+	{
+		_cameraRotY = _defaultCameraRotation.y;
+		_zoom = 0f;
+		_height = 0f;
+		SetAngle(_cameraRotY);
+	}
+	void SetAngle(float newAngle)
+	{
+		_camera.eulerAngles = new Vector3(0f, newAngle, 0f);
+	}
+	void ToggleTimeScale(float newTimeScale)
+	{
+		float setTimeScale = newTimeScale;
+		if (Time.timeScale == newTimeScale)
+			setTimeScale = _defaultTimeScale;
+		Time.timeScale = setTimeScale;
+	}
+}

Assets/3_MarathonEnvs/Scripts/Shared/CameraHelper.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/DebugMotor.cs

@@ -0,0 +1,9 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class DebugMotor : MonoBehaviour
+{
+    public Vector3 Actions;
+}

Assets/3_MarathonEnvs/Scripts/Shared/DebugMotor.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/HandleOverlap.cs

@@ -0,0 +1,22 @@
+using UnityEngine;
+
+namespace Unity.MLAgents
+{
+    public class HandleOverlap : MonoBehaviour
+    {
+        public GameObject Parent;
+
+        void OnEnable()
+        {
+            enabled = false;
+            if (Parent == null)
+                return;
+            var collider = GetComponent<Collider>();
+            var parentCollider = Parent.GetComponent<Collider>();
+            if (collider == null || parentCollider == null)
+                return;
+            // Debug.Log($"Physics.IgnoreCollision: {collider.name} and {parentCollider.name}");
+            Physics.IgnoreCollision(collider, parentCollider);
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/HandleOverlap.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/IOnSensorCollision.cs

@@ -0,0 +1,11 @@
+using UnityEngine;
+
+namespace Unity.MLAgents
+{
+    public interface IOnSensorCollision
+    {
+         void OnSensorCollisionEnter(Collider sensorCollider, GameObject other);
+         void OnSensorCollisionExit(Collider sensorCollider, GameObject other);
+
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/IOnSensorCollision.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/IOnTerrainCollision.cs

@@ -0,0 +1,9 @@
+using UnityEngine;
+
+namespace Unity.MLAgents 
+{
+    public interface IOnTerrainCollision
+    {
+        void OnTerrainCollision(GameObject other, GameObject terrain);
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/IOnTerrainCollision.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/InputController.cs

@@ -0,0 +1,210 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using Unity.MLAgents;
+
+public class InputController : MonoBehaviour
+{
+    [Header("Options")]
+    public float MaxVelocity = 8f;
+    [Range(0f,1f)]
+    public float ClipInput;
+    public bool NoJumpsInMockMode = true;
+
+    [Header("User or Mock input states")]
+    public Vector2 TargetMovementVector; // User-input desired horizontal center of mass velocity.
+    public float TargetMovementMagnatude;
+    public Vector2 MovementVector; // smoothed version of TargetMovementVector.
+    public float MovementMagnatude;
+    public Vector2 CameraRotation; // User-input desired rotation for camera.
+    public bool Jump; // User wants to jump
+    public bool Backflip; // User wants to backflip
+    public bool Stand;
+    public bool WalkOrRun;
+
+    [Header("Read only (or debug)")]
+    public Vector2 DesiredHorizontalVelocity; // MovementVector * Max Velovity
+    public Vector3 HorizontalDirection; // Normalized vector in direction of travel (assume right angle to floor)
+    public bool UseHumanInput = true;
+    public bool DemoMockIfNoInput = true; // Demo mock mode if no human input
+
+    float _delayUntilNextAction;
+    float _timeUnillDemo;
+
+    const float kGroundAcceleration = .6f/2;
+    const float kGroundDeceleration = .75f/4;
+    const float kDeadzone = .1f;            // below this value is stationary
+
+
+    // Start is called before the first frame update
+    void Awake()
+    {
+        UseHumanInput = !Academy.Instance.IsCommunicatorOn;
+        _timeUnillDemo = 1f;
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        
+    }
+    void FixedUpdate()
+    {
+        DoUpdate(Time.fixedDeltaTime);
+    }
+    void DoUpdate(float deltaTime)
+    {
+        if (UseHumanInput)
+            GetHumanInput();
+        else
+            GetMockInput();
+        SmoothAcceleration(deltaTime);
+    }
+    public void OnReset()
+    {
+        MovementVector = Vector3.zero;
+        MovementMagnatude = MovementVector.magnitude;
+        SetRandomHorizontalDirection();
+        _delayUntilNextAction = -1f;
+        // DoUpdate(Time.fixedDeltaTime);
+        if (UseHumanInput)
+            GetHumanInput();
+        else
+            GetMockInput();
+        // handle direction
+        if (!Mathf.Approximately(TargetMovementVector.sqrMagnitude, 0f))
+            HorizontalDirection = new Vector3(TargetMovementVector.normalized.x, 0f, TargetMovementVector.normalized.y);
+        DesiredHorizontalVelocity = TargetMovementVector.normalized * MaxVelocity * TargetMovementVector.magnitude;
+    }
+    void SmoothAcceleration(float deltaTime)
+    {
+        // Determine change to speed based on whether there is currently any move input.
+        float acceleration = TargetMovementVector.magnitude > 0 ? kGroundAcceleration : kGroundDeceleration;
+
+        var difference = (MovementVector - TargetMovementVector);
+        if (difference.magnitude > MovementVector.magnitude)
+        {
+            acceleration *= 5f;
+        }
+
+        // Adjust the forward speed towards the desired speed.
+        MovementVector = Vector2.MoveTowards(MovementVector, TargetMovementVector, acceleration * deltaTime);
+
+        // Handle deadzone
+        if (TargetMovementVector.magnitude < kDeadzone && MovementVector.magnitude < kDeadzone)
+        {
+            TargetMovementVector = Vector2.zero;
+            TargetMovementMagnatude = TargetMovementVector.magnitude;
+            MovementVector = Vector2.zero;
+            WalkOrRun = false;
+            Stand = true;
+        }
+        else
+        {
+            WalkOrRun = true;
+            Stand = false;
+        }
+        MovementMagnatude = MovementVector.magnitude;
+
+        // handle direction
+        if (!Mathf.Approximately(MovementVector.sqrMagnitude, 0f))
+            HorizontalDirection = new Vector3(MovementVector.normalized.x, 0f, MovementVector.normalized.y);
+        DesiredHorizontalVelocity = MovementVector.normalized * MaxVelocity * MovementVector.magnitude;
+    }
+    void GetHumanInput()
+    {
+        if (Input.GetKey(KeyCode.LeftShift) || Input.GetKey(KeyCode.RightShift))
+            return;
+        bool resetTimeUntilDemo = false;
+        _timeUnillDemo -= Time.deltaTime;
+        var newMovementVector = new Vector2(
+            Input.GetAxis("Horizontal"),
+            Input.GetAxis("Vertical")
+        );
+        if (ClipInput > 0f)
+        {
+            newMovementVector = new Vector2(
+                Mathf.Clamp(newMovementVector.x, -ClipInput, ClipInput),
+                Mathf.Clamp(newMovementVector.y, -ClipInput, ClipInput));
+        }
+        if (!Mathf.Approximately(newMovementVector.sqrMagnitude, 0f))
+        {
+            TargetMovementVector = newMovementVector;
+            TargetMovementMagnatude = TargetMovementVector.magnitude;
+            resetTimeUntilDemo = true;
+        }
+        else if (DemoMockIfNoInput && _timeUnillDemo <= 0)
+        {
+            GetMockInput();
+            _timeUnillDemo = 0f;
+            return;
+        }
+        CameraRotation = Vector2.zero;
+        Jump = Input.GetKey(KeyCode.Space); //Input.GetButtonDown("Fire1");
+        Backflip = Input.GetKey(KeyCode.B);
+        if (Jump || Backflip)
+        {
+            resetTimeUntilDemo = true;
+        }
+        if (resetTimeUntilDemo)
+        {
+            _timeUnillDemo = 3f;
+        }
+    }
+    void GetMockInput()
+    {
+        _delayUntilNextAction -= Time.deltaTime;
+        if (_delayUntilNextAction > 0)
+            return;
+        if (ChooseBackflip())
+        {
+            Backflip = true;
+            _delayUntilNextAction = 2f;
+            return;
+        }
+        Backflip = false;
+        Jump = false;
+        float direction = UnityEngine.Random.Range(0f, 360f);
+        float power = UnityEngine.Random.value;
+        // keep power above deadzone as we handle that below
+        power = kDeadzone + (power * (1f-kDeadzone)); 
+        // 2 in 5 times we are going to stand still 
+        if (UnityEngine.Random.value < .4f)
+            power = 0.001f;
+        // float direction = UnityEngine.Random.Range(-Mathf.PI/8, Mathf.PI/8);
+        // float power = UnityEngine.Random.Range(1f, 1f);
+        if (ClipInput > 0f)
+        {
+            power *= ClipInput;
+        }
+        TargetMovementVector = new Vector2(Mathf.Cos(direction), Mathf.Sin(direction));
+        TargetMovementVector *= power;
+        TargetMovementMagnatude = TargetMovementVector.magnitude;
+        Jump = ChooseJump();
+        _delayUntilNextAction = 1f + (UnityEngine.Random.value * 5f);
+    }
+    bool ChooseBackflip()
+    {
+        if (NoJumpsInMockMode)
+            return false;
+        var rnd = UnityEngine.Random.Range(0, 10);
+        return rnd == 0;
+    }
+    bool ChooseJump()
+    {
+        if (NoJumpsInMockMode)
+            return false;
+        var rnd = UnityEngine.Random.Range(0, 5);
+        return rnd == 0;
+    }
+    void SetRandomHorizontalDirection()
+    {
+        float direction = UnityEngine.Random.Range(0f, 360f);
+        var movementVector = new Vector2(Mathf.Cos(direction), Mathf.Sin(direction));
+        HorizontalDirection = new Vector3(movementVector.normalized.x, 0f, movementVector.normalized.y);
+        movementVector /= float.MinValue;
+        TargetMovementVector = new Vector2(movementVector.x, movementVector.y);
+        TargetMovementMagnatude = TargetMovementVector.magnitude;
+    }
+
+}

Assets/3_MarathonEnvs/Scripts/Shared/InputController.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/MapRangeOfMotion2Constraints.cs

@@ -0,0 +1,199 @@
+using System.Collections;
+using System.Collections.Generic;
+
+using System;
+using System.Linq;
+using UnityEngine;
+
+using Unity.MLAgents.Policies;
+using Unity.MLAgents;
+
+public class MapRangeOfMotion2Constraints : MonoBehaviour
+{
+
+    /*
+    [SerializeField]
+    bool applyROMInGamePlay;
+
+    public bool ApplyROMInGamePlay {  set => applyROMInGamePlay = value; }
+    */
+
+
+    public RangeOfMotionValues info2store;
+
+    [Range(0, 359)]
+    int MinROMNeededForJoint = 5;
+
+
+
+
+    [SerializeField]
+    bool debugWithLargestROM = false;
+
+    [Tooltip("extra range for upper and lower angles of moving articulations ")]
+    [SerializeField]
+    float extraoffset = 10;
+
+
+    public void ConfigureTrainingForRagdoll(int minROM)
+    {
+
+        MinROMNeededForJoint = minROM;
+
+        int dof = ApplyRangeOfMotionToRagDoll();
+        if (dof == -1)
+        {
+            Debug.LogError("Problems applying the range of motion to the ragdoll");
+        }
+        else
+        {
+
+            ApplyDoFOnBehaviorParameters(dof);
+        }
+
+    }
+
+
+    int ApplyRangeOfMotionToRagDoll()
+    {
+        if (info2store == null || info2store.Values.Length == 0)
+            return -1;
+
+        ArticulationBody[] articulationBodies = 
+            GetComponentsInChildren<ArticulationBody>(true)
+            .Where(x=>x.name.StartsWith("articulation:"))
+            .ToArray();
+
+        int DegreesOfFreedom = 0;
+
+
+  
+        foreach (ArticulationBody body in articulationBodies)
+        {
+            if (body.isRoot)
+                continue;
+
+            string keyword1 = "articulation:";
+            string keyword2 = "collider:";
+            string valuename = body.name.TrimStart(keyword1.ToArray<char>());
+            valuename = valuename.TrimStart(keyword2.ToArray<char>());
+
+            RangeOfMotionValue rom = info2store.Values.FirstOrDefault(x => x.name == valuename);
+
+            if (rom == null)
+            {
+                Debug.LogError("Could not find a rangoe of motionvalue for articulation: " + body.name);
+                return -1;
+            }
+
+            bool isLocked = true;
+            body.twistLock = ArticulationDofLock.LockedMotion;
+            body.swingYLock = ArticulationDofLock.LockedMotion;
+            body.swingZLock = ArticulationDofLock.LockedMotion;
+            body.jointType = ArticulationJointType.FixedJoint;
+
+            body.anchorRotation = Quaternion.identity; //we make sure the anchor has no Rotation, otherwise the constraints do not make any sense
+
+            if (rom.rangeOfMotion.x > (float)MinROMNeededForJoint)
+            {
+                DegreesOfFreedom++;
+                isLocked = false;
+                body.twistLock = ArticulationDofLock.LimitedMotion;
+                var drive = body.xDrive;
+                drive.lowerLimit = rom.lower.x - extraoffset;
+                drive.upperLimit = rom.upper.x + extraoffset;
+                body.xDrive = drive;
+                if (debugWithLargestROM)
+                {
+                    drive.lowerLimit = -170;
+                    drive.upperLimit = +170;
+                }
+
+            }
+            if (rom.rangeOfMotion.y >= (float)MinROMNeededForJoint)
+            {
+                DegreesOfFreedom++;
+                isLocked = false;
+                body.swingYLock = ArticulationDofLock.LimitedMotion;
+                var drive = body.yDrive;
+                drive.lowerLimit = rom.lower.y - extraoffset;
+                drive.upperLimit = rom.upper.y + extraoffset;
+                body.yDrive = drive;
+
+                if (debugWithLargestROM)
+                {
+                    drive.lowerLimit = -170 - extraoffset;
+                    drive.upperLimit = +170 + extraoffset;
+                }
+
+
+            }
+            if (rom.rangeOfMotion.z >= (float)MinROMNeededForJoint)
+            {
+                DegreesOfFreedom++;
+                isLocked = false;
+                body.swingZLock = ArticulationDofLock.LimitedMotion;
+                var drive = body.zDrive;
+                drive.lowerLimit = rom.lower.z - extraoffset;
+                drive.upperLimit = rom.upper.z + extraoffset;
+                body.zDrive = drive;
+
+                if (debugWithLargestROM)
+                {
+                    drive.lowerLimit = -170;
+                    drive.upperLimit = +170;
+                }
+
+            }
+
+            if (!isLocked)
+            {
+                body.jointType = ArticulationJointType.SphericalJoint;
+            }
+
+        }
+
+        return DegreesOfFreedom;
+
+    }
+
+
+
+
+
+    void ApplyDoFOnBehaviorParameters(int DegreesOfFreedom)
+    {
+       
+        BehaviorParameters bp = GetComponent<BehaviorParameters>();
+
+        Unity.MLAgents.Actuators.ActionSpec myActionSpec = bp.BrainParameters.ActionSpec;
+
+
+
+        myActionSpec.NumContinuousActions = DegreesOfFreedom;
+        myActionSpec.BranchSizes = new List<int>().ToArray();
+        bp.BrainParameters.ActionSpec = myActionSpec;
+        Debug.Log("Space of actions calculated at:" + myActionSpec.NumContinuousActions + " continuous dimensions");
+
+
+        /*
+         * To calculate the space of observations:
+        
+        */
+
+     
+        int numsensors = GetComponentsInChildren<SensorBehavior>().Length;
+        int num_miscelaneous = GetComponent<ProcRagdollAgent>().calculateDreConObservationsize();
+
+        //apparently the number of sensors is already acocunted for in the degrees of freedom, so:
+        //   int ObservationDimensions = DegreesOfFreedom + numsensors + num_miscelaneous;
+        int ObservationDimensions = DegreesOfFreedom +  num_miscelaneous;
+        bp.BrainParameters.VectorObservationSize = ObservationDimensions;
+        Debug.Log("Space of perceptions calculated at:" + bp.BrainParameters.VectorObservationSize + " continuous dimensions, with: " + " sensors: " + numsensors + "and DreCon miscelaneous: " + num_miscelaneous);
+
+
+    }
+
+
+
+}

Assets/3_MarathonEnvs/Scripts/Shared/MapRangeOfMotion2Constraints.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/MarathonAgent.cs

@@ -0,0 +1,802 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+using ManyWorlds;
+
+namespace Unity.MLAgents
+{
+    public class MarathonAgent : Agent
+    {
+        //
+        // Params for prefabs
+
+        //
+        // Params for instances
+        [Tooltip("Set to camera to follow this instance")]
+        /**< \brief Set to camera to follow this instance*/
+        public GameObject CameraTarget;
+
+        [Tooltip("Set to true for this instance to show monitor")]
+        /**< \brief Set to true for this instance to show monitor*/
+        public bool ShowMonitor;
+
+        //
+        // Parms to set in subclass.OnEpisodeBegin() 
+        [Tooltip("Reward value to set on termination")]
+        /**< \brief Reward value to set on termination*/
+        protected float OnTerminateRewardValue = -1;
+
+        [Tooltip("Function which returns true to request termination of episode")]
+        /**< \brief Function which returns true to request termination of episode*/
+        protected Func<bool> TerminateFunction;
+
+        [Tooltip("Function which sets reward based on actions")]
+        /**< \brief Function which sets reward based on actions*/
+        protected Func<float> StepRewardFunction;
+
+        [Tooltip("Function which collections observations")]
+        /**< \brief Function which collections observations*/
+        protected Action<VectorSensor> ObservationsFunction;
+
+        [Tooltip("Optional Function for additional reward at end of Episode")]
+        /**< \brief Optional Function for additional reward at end of Episode*/
+        protected Func<float> OnEpisodeCompleteGetRewardFunction;
+
+        [Tooltip("Helper for tracking body parts")]
+        /**< \brief Helper for tracking body parts*/
+        protected Dictionary<string, Rigidbody> BodyParts = new Dictionary<string, Rigidbody>();
+
+        [Tooltip("Helper for body parts rotation to focal point")]
+        /**< \brief Helper for body parts rotation to focal point*/
+        protected Dictionary<string, Quaternion> BodyPartsToFocalRoation = new Dictionary<string, Quaternion>();
+
+        //
+        // read only status
+        [Tooltip("True if foot hit terrain since last logic frame")]
+        /**< \brief True if foot hit terrain since last logic frame*/
+        public bool FootHitTerrain;
+
+        [Tooltip(
+            "True if body part other than foot hit terrain since last logic frame. Note: bodyparts which connect to foot maybe flagged as foot")]
+        /**< \brief True if body part other than foot hit terrain since last logic frame. Note: bodyparts which connect to foot maybe flagged as foot*/
+        public bool NonFootHitTerrain;
+
+        [Tooltip("Last set of Actions")]
+        /**< \brief Last set of Actions*/
+        public List<float> Actions;
+
+        [Tooltip("Current state of each sensor")]
+        /**< \brief Current state of each sensor*/
+        public List<float> SensorIsInTouch;
+
+        [Tooltip("Gameobject for FocalPoint")]
+        /**< \brief Gameobject for FocalPoint*/
+        public GameObject FocalPoint;
+
+        [Tooltip("Rigidbody for FocalPoint")]
+        /**< \brief Rigidbody for FocalPoint*/
+        public Rigidbody FocalRidgedBody;
+        [Tooltip("Distance travelled this episode")]
+        /**< \brief Distance travelled this episode*/
+        public float DistanceTraveled = float.MinValue;
+
+        [Tooltip("Max distance travelled across all episodes")]
+        /**< \brief Max distance travelled across all episodes*/
+        public float FocalPointMaxDistanceTraveled;
+
+        [Tooltip("Current angle of each Joint")]
+        /**< \brief Current angle of each Joint*/
+        List<float> JointAngles;
+
+        [Tooltip("Current velocity of each Joint")]
+        /**< \brief Current velocity of each Joint*/
+        public List<float> JointVelocity;
+
+        [Tooltip("Current rotation of each Joint")]
+        /**< \brief Current rotation of each Joint*/
+        public List<Quaternion> JointRotations;
+
+        [Tooltip("Current angular velocity of each Joint")]
+        /**< \brief Current angular velocity of each Joint*/
+        List<Vector3> JointAngularVelocities;
+
+        [Tooltip("Joints created by MarathonSpawner")]
+        /**< \brief Joints created by MarathonSpawner*/
+        public List<MarathonJoint> MarathonJoints;
+
+        [Tooltip("Sensors created by MarathonSpawner")]
+        /**< \brief Sensors created by MarathonSpawner*/
+        public List<MarathonSensor> MarathonSensors;
+
+        public List<float> Observations;
+        public int ObservationNormalizedErrors;
+        public int MaxObservationNormalizedErrors;
+
+        //
+        // local variables
+        internal int NumSensors;
+        Dictionary<GameObject, Vector3> transformsPosition;
+        Dictionary<GameObject, Quaternion> transformsRotation;
+        MarathonSpawner marathonSpawner;
+        bool _hasValidModel;
+        List<float> qpos;
+        List<float> qglobpos;
+        List<float> qvel;
+        List<float> recentVelocity;
+
+        List <Vector3> mphBuffer;
+
+        float[] lastVectorAction;
+        float[] vectorDifference;
+    	SpawnableEnv _spawnableEnv;
+    	Vector3 startPosition;
+    	bool _isDone;
+        bool _hasLazyInitialized;
+
+        public override void OnEpisodeBegin()
+        {
+            if (!_hasLazyInitialized)
+            {
+                _hasLazyInitialized = true;
+            }
+            _isDone = true;
+            if (DistanceTraveled != float.MinValue)
+            {
+                var scorer = FindObjectOfType<Scorer>();
+                scorer?.ReportScore(DistanceTraveled, "Distance Traveled");
+            }            
+            if (_spawnableEnv == null)
+        		_spawnableEnv = GetComponentInParent<SpawnableEnv>();
+            if (marathonSpawner == null)
+                marathonSpawner = GetComponent<MarathonSpawner>();
+
+            mphBuffer = new List<Vector3>();
+
+            Transform[] allChildren = GetComponentsInChildren<Transform>();
+            if (_hasValidModel)
+            {
+                // restore
+                foreach (Transform child in allChildren)
+                {
+                    if (child.gameObject.name.Contains("OpenAIHumanoid"))
+                    {
+                        continue;
+                    }
+
+                    child.position = transformsPosition[child.gameObject];
+                    child.rotation = transformsRotation[child.gameObject];
+                    var childRb = child.GetComponent<Rigidbody>();
+                    if (childRb != null)
+                    {
+                        childRb.angularVelocity = Vector3.zero;
+                        childRb.velocity = Vector3.zero;
+                    }
+                }
+
+                marathonSpawner?.ApplyRandom();
+                SetupMarathon();
+                UpdateQ();
+                return;
+            }
+			startPosition = transform.position;
+            // HACK first spawned marathon agent should grab the camera
+            var agentWithCamera = FindObjectsOfType<MarathonAgent>().FirstOrDefault(x=>x.CameraTarget != null);
+            if (agentWithCamera == null) {
+                CameraTarget = FindObjectOfType<SmoothFollow>()?.gameObject;
+                ShowMonitor = true;                
+            }
+
+            MarathonJoints = null;
+            MarathonSensors = null;
+            var rbs = this.GetComponentsInChildren<Rigidbody>().ToList();
+            foreach (var item in rbs)
+            {
+                if (item != null)
+                    DestroyImmediate(item.gameObject);
+            }
+
+            Resources.UnloadUnusedAssets();
+
+            marathonSpawner?.SpawnFromXml();
+            allChildren = GetComponentsInChildren<Transform>();
+            transformsPosition = new Dictionary<GameObject, Vector3>();
+            transformsRotation = new Dictionary<GameObject, Quaternion>();
+            foreach (Transform child in allChildren)
+            {
+                transformsPosition[child.gameObject] = child.position;
+                transformsRotation[child.gameObject] = child.rotation;
+            }
+
+            marathonSpawner?.ApplyRandom();
+            SetupMarathon();
+            UpdateQ();
+            _hasValidModel = true;
+            recentVelocity = new List<float>();
+        }
+
+        void SetupMarathon()
+        {
+            NumSensors = MarathonSensors.Count;
+        }
+
+        internal void SetupBodyParts()
+        {
+            // set body part directions
+            foreach (var bodyPart in BodyParts)
+            {
+                var name = bodyPart.Key;
+                var rigidbody = bodyPart.Value;
+
+                // find up
+                var focalPoint = rigidbody.position;
+                focalPoint.x += 10;
+                var focalPointRotation = rigidbody.rotation;
+                focalPointRotation.SetLookRotation(focalPoint - rigidbody.position);
+                BodyPartsToFocalRoation[name] = focalPointRotation;
+            }
+        }
+
+        public override void CollectObservations(VectorSensor sensor)
+        {
+            if (!_hasLazyInitialized)
+            {
+                OnEpisodeBegin();
+            }
+            UpdateQ();
+            ObservationsFunction(sensor);
+
+            // var info = GetInfo();
+            // if (Observations?.Count != info.vectorObservation.Count)
+            //     Observations = Enumerable.Range(0, info.vectorObservation.Count).Select(x => 0f).ToList();
+            // ObservationNormalizedErrors = 0;
+            // for (int i = 0; i < Observations.Count; i++)
+            // {
+            //     Observations[i] = info.vectorObservation[i];
+            //     var x = Mathf.Abs(Observations[i]);
+            //     var e = Mathf.Epsilon;
+            //     bool is1 = Mathf.Approximately(x, 1f);
+            //     if ((x > 1f + e) && !is1)
+            //         ObservationNormalizedErrors++;
+            // }
+            // if (ObservationNormalizedErrors > MaxObservationNormalizedErrors)
+            //     MaxObservationNormalizedErrors = ObservationNormalizedErrors;
+        }
+
+        public override void OnActionReceived(ActionBuffers actions)
+        {
+            float[] vectorAction = actions.ContinuousActions.Select(x=>x).ToArray();
+
+            if (!_hasLazyInitialized)
+            {
+                return;
+            }
+    		_isDone = false;
+            if (lastVectorAction == null){
+                lastVectorAction = vectorAction.Select(x=>0f).ToArray();
+                vectorDifference = vectorAction.Select(x=>0f).ToArray();
+            }
+            
+            Actions = vectorAction
+                .Select(x => x)
+                .ToList();
+            for (int i = 0; i < MarathonJoints.Count; i++)
+            {
+                var inp = (float) Actions[i];
+                ApplyAction(MarathonJoints[i], inp);
+				vectorDifference[i] = vectorAction[i]-lastVectorAction[i];
+            }
+
+            UpdateQ();
+
+            if (!_isDone)
+            {
+                bool done = TerminateFunction();
+
+                if (done)
+                {
+                    EndEpisode();
+                    SetReward(OnTerminateRewardValue);
+                }
+                else if (StepRewardFunction != null)
+                {
+                    SetReward(StepRewardFunction());
+                }
+
+                done |= (this.StepCount >= MaxStep && MaxStep > 0);
+                if (done && OnEpisodeCompleteGetRewardFunction != null)
+                    AddReward(OnEpisodeCompleteGetRewardFunction());
+            }
+
+            FootHitTerrain = false;
+            NonFootHitTerrain = false;
+        }
+
+        internal void KillJointPower(string[] hints)
+        {
+            var mJoints = hints
+                .SelectMany(hint =>
+                    MarathonJoints
+                        .Where(x => x.JointName.ToLowerInvariant().Contains(hint.ToLowerInvariant()))
+                ).ToList();
+            foreach (var joint in mJoints)
+                Actions[MarathonJoints.IndexOf(joint)] = 0f;
+        }
+
+        internal float GetHeight()
+        {
+            var feetYpos = MarathonJoints
+                .Where(x => x.JointName.ToLowerInvariant().Contains("foot"))
+                .Select(x => x.Joint.transform.position.y)
+                .OrderBy(x => x)
+                .ToList();
+            float lowestFoot = 0f;
+            if (feetYpos != null && feetYpos.Count != 0)
+                lowestFoot = feetYpos[0];
+            var height = FocalPoint.transform.position.y - lowestFoot;
+            return height;
+        }
+
+        internal float GetAverageVelocity(string bodyPart = null)
+        {
+            var v = GetVelocity(bodyPart);
+            recentVelocity.Add(v);
+            if (recentVelocity.Count >= 10)
+                recentVelocity.RemoveAt(0);
+            return recentVelocity.Average();
+        }
+
+        Vector3 GetRawVelocity(string bodyPart = null)
+        {
+            Vector3 rawVelocity;
+            if (!string.IsNullOrWhiteSpace(bodyPart))
+                rawVelocity = BodyParts[bodyPart].velocity;
+            else
+                rawVelocity = FocalRidgedBody.velocity;
+            return rawVelocity;
+        }
+
+        internal float GetVelocity(string bodyPart = null)
+        {
+            float rawVelocity = GetRawVelocity().x;
+
+            var maxSpeed = 4f; // meters per second
+            var velocity = rawVelocity / maxSpeed;
+            return velocity;
+        }
+
+        internal Vector3 GetNormalizedVelocity(Vector3 metersPerSecond)
+        {
+            var maxMetersPerSecond = _spawnableEnv.bounds.size
+                / MaxStep
+                / Time.fixedDeltaTime;
+            var maxXZ = Mathf.Max(maxMetersPerSecond.x, maxMetersPerSecond.z);
+            maxMetersPerSecond.x = maxXZ;
+            maxMetersPerSecond.z = maxXZ;
+            maxMetersPerSecond.y = 53; // override with
+            float x = metersPerSecond.x / maxMetersPerSecond.x;
+            float y = metersPerSecond.y / maxMetersPerSecond.y;
+            float z = metersPerSecond.z / maxMetersPerSecond.z;
+            // clamp result
+            x = Mathf.Clamp(x, -1f, 1f);
+            y = Mathf.Clamp(y, -1f, 1f);
+            z = Mathf.Clamp(z, -1f, 1f);
+            Vector3 normalizedVelocity = new Vector3(x,y,z);
+            return normalizedVelocity;
+        }
+        internal Vector3 GetNormalizedPosition(Vector3 inputPos)
+        {
+            Vector3 pos = inputPos - startPosition;
+            var maxPos = _spawnableEnv.bounds.size;
+            float x = pos.x / maxPos.x;
+            float y = pos.y / maxPos.y;
+            float z = pos.z / maxPos.z;
+            // clamp result
+            x = Mathf.Clamp(x, -1f, 1f);
+            y = Mathf.Clamp(y, -1f, 1f);
+            z = Mathf.Clamp(z, -1f, 1f);
+            Vector3 normalizedPos = new Vector3(x,y,z);
+            return normalizedPos;
+        }
+
+        internal Vector3 GetNormalizedVelocity(string bodyPart = null)
+        {
+            var metersPerSecond = GetRawVelocity(bodyPart);
+            var normalizedVelocity = this.GetNormalizedVelocity(metersPerSecond);
+            Vector3 mph = metersPerSecond * 2.236936f;
+            mphBuffer.Add(mph);
+            if (mphBuffer.Count > 100)
+                mphBuffer.RemoveAt(0);
+            var aveMph = new Vector3(
+                mphBuffer.Select(x=>x.x).Average(),
+                mphBuffer.Select(x=>x.y).Average(),
+                mphBuffer.Select(x=>x.z).Average()
+            );
+            //if (ShowMonitor)
+            //{
+            //    Monitor.Log("MaxDistance", FocalPointMaxDistanceTraveled.ToString());
+            //    Monitor.Log("MPH: ", (aveMph).ToString());
+            //}            
+            return normalizedVelocity;
+        }
+
+        internal Vector3 GetNormalizedPosition(string bodyPart = null)
+        {
+            Vector3 pos = BodyParts[bodyPart].position;
+            Vector3 normalizedPos = this.GetNormalizedPosition(BodyParts[bodyPart].position);
+            return normalizedPos;
+        }
+
+        internal float GetUprightBonus()
+        {
+            var qpos2 = (GetAngleFromUp() % 180) / 180;
+            var uprightBonus = 0.5f * (2 - (Mathf.Abs(qpos2) * 2) - 1);
+            return uprightBonus;
+        }
+
+        internal float GetUprightBonus(string bodyPart, float maxBonus = 0.5f)
+        {
+            var toFocalAngle = BodyPartsToFocalRoation[bodyPart] * -BodyParts[bodyPart].transform.forward;
+            var angleFromUp = Vector3.Angle(toFocalAngle, Vector3.up);
+            var qpos2 = (angleFromUp % 180) / 180;
+            var uprightBonus = maxBonus * (2 - (Mathf.Abs(qpos2) * 2) - 1);
+            return uprightBonus;
+        }
+
+        internal float GetDirectionBonus(string bodyPart, Vector3 direction, float maxBonus = 0.5f)
+        {
+            var toFocalAngle = BodyPartsToFocalRoation[bodyPart] * BodyParts[bodyPart].transform.right;
+            var angle = Vector3.Angle(toFocalAngle, direction);
+            var qpos2 = (angle % 180) / 180;
+            var bonus = maxBonus * (2 - (Mathf.Abs(qpos2) * 2) - 1);
+            return bonus;
+        }
+
+        internal void GetDirectionDebug(string bodyPart)
+        {
+            var toFocalAngle = BodyPartsToFocalRoation[bodyPart] * BodyParts[bodyPart].transform.right;
+            var angleFromLeft = Vector3.Angle(toFocalAngle, Vector3.left);
+            var angleFromUp = Vector3.Angle(toFocalAngle, Vector3.up);
+            var angleFromDown = Vector3.Angle(toFocalAngle, Vector3.down);
+            var angleFromRight = Vector3.Angle(toFocalAngle, Vector3.right);
+            var angleFromForward = Vector3.Angle(toFocalAngle, Vector3.forward);
+            var angleFromBack = Vector3.Angle(toFocalAngle, Vector3.back);
+            print(
+                $"{bodyPart}: l: {angleFromLeft}, r: {angleFromRight}, f: {angleFromForward}, b: {angleFromBack}, u: {angleFromUp}, d: {angleFromDown}");
+        }
+
+        internal float GetLeftBonus(string bodyPart, float maxBonus = 0.5f)
+        {
+            var bonus = GetDirectionBonus(bodyPart, Vector3.left, maxBonus);
+            return bonus;
+        }
+
+        internal float GetRightBonus(string bodyPart, float maxBonus = 0.5f)
+        {
+            var bonus = GetDirectionBonus(bodyPart, Vector3.right, maxBonus);
+            return bonus;
+        }
+
+        internal float GetForwardBonus(string bodyPart, float maxBonus = 0.5f)
+        {
+            var bonus = GetDirectionBonus(bodyPart, Vector3.forward, maxBonus);
+            return bonus;
+        }
+
+        internal float GetHeightPenality(float maxHeight)
+        {
+            var height = GetHeight();
+            var heightPenality = maxHeight - height;
+            heightPenality = Mathf.Clamp(heightPenality, 0f, maxHeight);
+            return heightPenality;
+        }
+
+        internal float GetEffort(string[] ignorJoints = null)
+        {
+            double effort = 0;
+            for (int i = 0; i < Actions.Count; i++)
+            {
+                if (i >= MarathonJoints.Count)
+                    continue; // handle case when to many actions
+                var name = MarathonJoints[i].JointName;
+                if (ignorJoints != null && ignorJoints.Contains(name))
+                    continue;
+                var jointEffort = Mathf.Pow(Mathf.Abs(Actions[i]), 2);
+                effort += jointEffort;
+            }
+
+            return (float) effort;
+        }
+
+        internal float GetEffortNormalized(string[] ignorJoints = null)
+        {
+            double effort = 0;
+            double joints = 0;
+            for (int i = 0; i < Actions.Count; i++)
+            {
+                if (i >= MarathonJoints.Count)
+                    continue; // handle case when to many actions
+                var name = MarathonJoints[i].JointName;
+                if (ignorJoints != null && ignorJoints.Contains(name))
+                    continue;
+                var jointEffort = Mathf.Pow(Mathf.Abs(Actions[i]), 2);
+                effort += jointEffort;
+                joints++;
+            }
+
+            return (float) (effort / joints);
+        }
+        internal float GetActionDifferenceNormalized()
+        {
+            float actionDifference = vectorDifference.Average();
+		    actionDifference = Mathf.Clamp(actionDifference, 0, 1);
+		    actionDifference = Mathf.Pow(actionDifference,2);
+            return actionDifference;
+        }
+
+
+        internal float GetJointsAtLimitPenality(string[] ignorJoints = null)
+        {
+            int atLimitCount = 0;
+            for (int i = 0; i < Actions.Count; i++)
+            {
+                if (i >= MarathonJoints.Count)
+                    continue; // handle case when to many actions
+                var name = MarathonJoints[i].JointName;
+                if (ignorJoints != null && ignorJoints.Contains(name))
+                    continue;
+                bool atLimit = Mathf.Abs(Actions[i]) >= 1f;
+                if (atLimit)
+                    atLimitCount++;
+            }
+
+            float penality = atLimitCount * 0.2f;
+            return (float) penality;
+        }
+
+        internal float GetEffortSum()
+        {
+            var effort = Actions
+                .Select(x => Mathf.Abs(x))
+                .Sum();
+            return effort;
+        }
+
+        internal float GetEffortMean()
+        {
+            var effort = Actions
+                .Average();
+            return effort;
+        }
+
+        internal float GetAngleFromUp()
+        {
+            var angleFromUp = Vector3.Angle(FocalPoint.transform.forward, Vector3.up);
+            if (ShowMonitor)
+            {
+            }
+
+            return angleFromUp;
+        }
+
+        public virtual void OnTerrainCollision(GameObject other, GameObject terrain)
+        {
+            // if (string.Compare(terrain.name, "Terrain", true) != 0)
+    		if (terrain.GetComponent<Terrain>() == null)
+                return;
+
+            switch (other.name.ToLowerInvariant().Trim())
+            {
+                case "right_leg": // dm_walker
+                case "left_leg": // dm_walker
+                case "foot": // dm_hopper
+                case "calf": // dm_hopper
+                case "left_left_foot": // dm_humanoid
+                case "left_right_foot": // dm_humanoid
+                case "right_left_foot": // dm_humanoid
+                case "right_right_foot": // dm_humanoid
+                case "left_shin": // dm_humanoid
+                case "right_shin": // dm_humanoid
+                case "left_ankle_geom": // oai_ant
+                case "right_ankle_geom": // oai_ant
+                case "third_ankle_geom": // oai_ant
+                case "fourth_ankle_geom": // oai_ant
+                case "right_foot": // dm_walker
+                case "left_foot": // dm_walker
+                    FootHitTerrain = true;
+                    break;
+                default:
+                    NonFootHitTerrain = true;
+                    break;
+            }
+        }
+
+        internal bool TerminateNever()
+        {
+            return false;
+        }
+
+        internal bool TerminateOnNonFootHitTerrain()
+        {
+            return NonFootHitTerrain;
+        }
+
+        internal void ApplyAction(MarathonJoint mJoint, float? target = null)
+        {
+            float powerMultiplier = 2.5f;
+            ConfigurableJoint configurableJoint = mJoint.Joint as ConfigurableJoint;
+            if (!target.HasValue) // handle random
+                target = UnityEngine.Random.value * 2 - 1;
+            var t = configurableJoint.targetAngularVelocity;
+            t.x = target.Value * mJoint.MaximumForce;
+            configurableJoint.targetAngularVelocity = t;
+            var angX = configurableJoint.angularXDrive;
+            angX.positionSpring = 1f;
+            var scale = mJoint.MaximumForce * Mathf.Pow(Mathf.Abs(target.Value), 3);
+            angX.positionDamper = Mathf.Max(1f, scale);
+            angX.maximumForce = Mathf.Max(1f, mJoint.MaximumForce * powerMultiplier);
+            configurableJoint.angularXDrive = angX;
+        }
+
+
+        List<System.Tuple<ConfigurableJoint, Transform>> _baseTargetPairs;
+
+        public void SetMarathonSensors(List<MarathonSensor> marathonSensors)
+        {
+            MarathonSensors = marathonSensors;
+            SensorIsInTouch = Enumerable.Range(0, marathonSensors.Count).Select(x => 0f).ToList();
+            foreach (var sensor in marathonSensors)
+            {
+                sensor.SiteObject.gameObject.AddComponent<SensorBehavior>();
+            }
+        }
+
+        public void SetMarathonJoints(List<MarathonJoint> marathonJoints)
+        {
+            MarathonJoints = marathonJoints;
+            var target = FindTopMesh(MarathonJoints.FirstOrDefault()?.Joint.gameObject, null);
+            if (CameraTarget != null && MarathonJoints != null)
+            {
+                var smoothFollow = CameraTarget.GetComponent<SmoothFollow>();
+                if (smoothFollow != null)
+                    smoothFollow.target = target.transform;
+            }
+
+            FocalPoint = target;
+            FocalRidgedBody = FocalPoint.GetComponent<Rigidbody>();
+            var qlen = MarathonJoints.Count + 3;
+            qpos = Enumerable.Range(0, qlen).Select(x => 0f).ToList();
+            qglobpos = Enumerable.Range(0, qlen).Select(x => 0f).ToList();
+            qvel = Enumerable.Range(0, qlen).Select(x => 0f).ToList();
+            JointAngles = Enumerable.Range(0, MarathonJoints.Count).Select(x => 0f).ToList();
+            JointVelocity = Enumerable.Range(0, MarathonJoints.Count).Select(x => 0f).ToList();
+            _baseTargetPairs = MarathonJoints
+                .Select(x => new System.Tuple<ConfigurableJoint, Transform>(x.TrueBase, x.TrueTarget))
+                .Distinct()
+                .ToList();
+            JointRotations = Enumerable.Range(0, _baseTargetPairs.Count).Select(x => Quaternion.identity).ToList();
+            JointAngularVelocities = Enumerable.Range(0, _baseTargetPairs.Count).Select(x => Vector3.zero).ToList();
+        }
+
+        GameObject FindTopMesh(GameObject curNode, GameObject topmostNode = null)
+        {
+            var meshRenderer = curNode.GetComponent<MeshRenderer>();
+            if (meshRenderer != null)
+                topmostNode = meshRenderer.gameObject;
+            var root = this;
+            var meshRenderers = root.GetComponentsInChildren<MeshRenderer>();
+            if (meshRenderers != null && meshRenderers.Length > 0)
+                topmostNode = meshRenderers[0].gameObject;
+
+            return (topmostNode);
+        }
+
+        void UpdateQ()
+        {
+            if (MarathonJoints == null || MarathonJoints.Count == 0)
+                return;
+
+            float dt = Time.fixedDeltaTime;
+            DistanceTraveled = FocalPoint.transform.position.x;
+            FocalPointMaxDistanceTraveled = Mathf.Max(FocalPointMaxDistanceTraveled, DistanceTraveled);
+
+            var topJoint = MarathonJoints[0];
+            var topTransform = topJoint.Joint.transform;
+            var topRidgedBody = topJoint.Joint.transform.GetComponent<Rigidbody>();
+            qpos[0] = topTransform.position.x;
+            qglobpos[0] = topTransform.position.x;
+            qvel[0] = topRidgedBody.velocity.x;
+            qpos[1] = topTransform.position.y;
+            qglobpos[1] = topTransform.position.y;
+            qvel[1] = topRidgedBody.velocity.y;
+            qpos[2] = ((topTransform.rotation.eulerAngles.z - 180f) % 180) / 180;
+            qglobpos[2] = ((topTransform.rotation.eulerAngles.z - 180f) % 180) / 180;
+            qvel[2] = topRidgedBody.velocity.z;
+            for (int i = 0; i < MarathonJoints.Count; i++)
+            {
+                var joint = MarathonJoints[i].Joint;
+                var targ = joint.transform;
+                float pos = 0f;
+                float globPos = 0f;
+                if (joint.axis.x != 0f)
+                {
+                    pos = targ.localEulerAngles.x;
+                    globPos = targ.eulerAngles.x;
+                }
+                else if (joint.axis.y != 0f)
+                {
+                    pos = targ.localEulerAngles.y;
+                    globPos = targ.eulerAngles.y;
+                }
+                else if (joint.axis.z != 0f)
+                {
+                    pos = targ.localEulerAngles.z;
+                    globPos = targ.eulerAngles.z;
+                }
+
+                pos = ((pos - 180f) % 180) / 180;
+                globPos = ((globPos - 180f) % 180) / 180;
+                var lastPos = qpos[3 + i];
+                qpos[3 + i] = pos;
+                JointAngles[i] = pos;
+                var lastgPos = qglobpos[3 + i];
+                qglobpos[3 + i] = globPos;
+                var vel = (qpos[3 + i] - lastPos) / (dt);
+                qvel[3 + i] = vel;
+                // JointVelocity[i] = vel;
+                var metersPerSecond = new Vector3(vel,0f,0f);
+                Vector3 normalizedVelocity = this.GetNormalizedVelocity(metersPerSecond);
+                JointVelocity[i] = normalizedVelocity.x;
+            }
+
+            for (int i = 0; i < _baseTargetPairs.Count; i++)
+            {
+                var x = _baseTargetPairs[i];
+                var baseRot = x.Item1.transform.rotation;
+                var targetRot = x.Item2.rotation;
+                var rotation = Quaternion.Inverse(baseRot) * targetRot;
+                JointRotations[i] = rotation;
+
+                var baseAngVel = x.Item1.GetComponent<Rigidbody>().angularVelocity;
+                var targetAngVel = x.Item2.GetComponent<Rigidbody>().angularVelocity;
+                var angVel = baseAngVel - targetAngVel;
+                angVel /= dt;
+                angVel /= 10000f;
+                JointAngularVelocities[i] = angVel;
+            }
+        }
+
+        public void SensorCollisionEnter(Collider sensorCollider, Collision other)
+        {
+            // if (string.Compare(other.gameObject.name, "Terrain", true) != 0)
+    		if (other.gameObject.GetComponent<Terrain>() == null)
+                return;
+            var otherGameobject = other.gameObject;
+            var sensor = MarathonSensors
+                .FirstOrDefault(x => x.SiteObject == sensorCollider);
+            if (sensor != null)
+            {
+                var idx = MarathonSensors.IndexOf(sensor);
+                SensorIsInTouch[idx] = 1f;
+            }
+        }
+
+        public void SensorCollisionExit(Collider sensorCollider, Collision other)
+        {
+            // if (string.Compare(other.gameObject.name, "Terrain", true) != 0)
+    		if (other.gameObject.GetComponent<Terrain>() == null)
+                return;
+            var otherGameobject = other.gameObject;
+            var sensor = MarathonSensors
+                .FirstOrDefault(x => x.SiteObject == sensorCollider);
+            if (sensor != null)
+            {
+                var idx = MarathonSensors.IndexOf(sensor);
+                SensorIsInTouch[idx] = 0f;
+            }
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/MarathonAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/MarathonTestBedController.cs

@@ -0,0 +1,141 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+
+public class MarathonTestBedController : MonoBehaviour
+{
+    [Tooltip("Action applied to each motor")]
+    /**< \brief Edit to manually test each motor (+1/-1)*/
+    public float[] Actions;
+
+    [Tooltip("Apply a random number to each action each framestep")]
+    /**< \brief Apply a random number to each action each framestep*/
+    public bool ApplyRandomActions = true;
+    [Range(0f,1f)]
+    public float RandomRange = 1f;
+
+    //public bool FreezeHead = false;
+    public bool FreezeHips = false;
+    public bool DontUpdateMotor = false;
+
+   // public bool setTpose;
+
+
+    bool _hasFrozen;
+
+
+
+
+  //  bool tposeanimisloaded = false;
+
+    // Start is called before the first frame update
+    void Start()
+    {
+
+    }
+
+    void loadTposeanim() {
+
+        GameObject sourceAgent = GameObject.Find("AgentMove-source");
+
+        Animator anim = sourceAgent.GetComponent<Animator>();
+        anim.runtimeAnimatorController = null; // Resources.Load("MarathonEnvs/Animations/Tpose") as RuntimeAnimatorController;
+
+        MocapAnimatorController animControl =  sourceAgent.GetComponent<MocapAnimatorController>();
+        animControl.doFixedUpdate = false;
+        animControl.MaxForwardVelocity = 0;
+
+        MocapControllerArtanim animControlartanim = sourceAgent.GetComponent<MocapControllerArtanim>();
+        animControlartanim.doFixedUpdate = false;
+
+        InputController input = FindObjectOfType<InputController>();
+        input.DemoMockIfNoInput = false;
+
+
+
+    }
+
+
+    void FreezeBodyParts()
+    {
+
+        var marathonAgents = FindObjectsOfType<Agent>(true);
+        
+        foreach (var agent in marathonAgents)
+        {
+            ArticulationBody head = null;
+            ArticulationBody butt = null;
+            ArticulationBody[] children = null;
+            switch (agent.name)
+            {
+                case "MarathonMan":
+                    
+                    _hasFrozen = true;
+                    children = agent.GetComponentsInChildren<ArticulationBody>();
+                    head = children.FirstOrDefault(x=>x.name=="torso");
+                    butt = children.FirstOrDefault(x=>x.name=="butt");
+                    // var rb = children.FirstOrDefault(x=>x.name == "MarathonMan");
+                    // if (FreezeHead || FreezeHips)
+                    //     rb.constraints = RigidbodyConstraints.FreezeAll;
+                    // if (FreezeHead && !FreezeHips)
+                    //     rb.GetComponentInChildren<FixedJoint>().connectedBody = head;
+                    break;
+                case "RagDoll":
+                    //if (!_hasFrozen && setTpose)
+                    //    loadTposeanim();
+                    _hasFrozen = true;
+                    children = agent.GetComponentsInChildren<ArticulationBody>();
+                    head = children.FirstOrDefault(x=>x.name=="torso");
+                    butt = children.FirstOrDefault(x=>x.name=="butt");
+                    break;
+                case "Ragdoll-MarathonMan004":
+                case "MarathonMan004":
+                case "MarathonMan004Constrained":
+                   // if (!_hasFrozen && setTpose)
+                   //     loadTposeanim();
+                    _hasFrozen = true;
+                    children = agent.GetComponentsInChildren<ArticulationBody>();
+                    head = children.FirstOrDefault(x=>x.name=="head");
+                    butt = children.FirstOrDefault(x=>x.name=="articulation:Hips");
+                    break;
+                case "humanoid":
+                    _hasFrozen = true;
+                    children = agent.GetComponentsInChildren<ArticulationBody>();
+                    head = children.FirstOrDefault(x=>x.name=="head");
+                    butt = children.FirstOrDefault(x=>x.name=="butt");
+                    break;
+                default:
+                    children = agent.GetComponentsInChildren<ArticulationBody>();
+                    head = children.FirstOrDefault(x=>x.name.ToLower().Contains("head"));
+                    butt = children.FirstOrDefault(x=>x.isRoot);
+                    if (head == null || butt == null)
+                        throw new System.ArgumentException($"agent.name: {agent.name}");
+                    _hasFrozen = true;
+                    break;
+            }
+        //    if (FreezeHead && head != null)
+        //        head.immovable = true;
+            if (FreezeHips && butt != null)
+                butt.immovable = true;
+        }
+    }
+
+    public void OnAgentEpisodeBegin()
+    {
+        if (!_hasFrozen)
+            FreezeBodyParts();
+       // if (setTpose)
+       //     loadTposeanim();
+    }
+
+    // Update is called once per frame
+    void FixedUpdate()
+    {
+        if (ApplyRandomActions)
+        {
+            Actions = Actions.Select(x=>Random.Range(-1f,1f)).ToArray();
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/MarathonTestBedController.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/RenderingOptions.cs

@@ -0,0 +1,68 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using Unity.MLAgents;
+
+public class RenderingOptions : MonoBehaviour
+{
+
+    [SerializeField]
+    bool renderOnlyTarget;
+
+    public
+    GameObject movementsource;
+    
+    public
+    GameObject ragdollcontroller;
+
+    SkinnedMeshRenderer[] SkinnedRenderers;
+    MeshRenderer[] MeshRenderers;
+    MeshRenderer[] MeshRenderersRagdoll;
+
+    bool currentRenderingState = false;
+
+    // Start is called before the first frame update
+    void Start()
+    {
+        SkinnedRenderers = movementsource.GetComponentsInChildren<SkinnedMeshRenderer>(true);
+        MeshRenderers = movementsource.GetComponentsInChildren<MeshRenderer>(true);
+        MeshRenderersRagdoll = ragdollcontroller.GetComponentsInChildren<MeshRenderer>(true);
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        bool isTrainingMode = Academy.Instance.IsCommunicatorOn;
+        bool onlyTarget = renderOnlyTarget || isTrainingMode;
+        if (onlyTarget != currentRenderingState)
+        {
+            currentRenderingState = onlyTarget;
+            if (onlyTarget)
+            {
+                foreach (SkinnedMeshRenderer r in SkinnedRenderers)
+                    r.enabled = false;
+
+                foreach (MeshRenderer r in MeshRenderers)
+                    r.enabled = false;
+
+                foreach (MeshRenderer r in MeshRenderersRagdoll)
+                    r.enabled = false;
+
+            }
+            else {
+
+                foreach (SkinnedMeshRenderer r in SkinnedRenderers)
+                    r.enabled = true;
+
+                foreach (MeshRenderer r in MeshRenderers)
+                    r.enabled = true;
+
+                foreach (MeshRenderer r in MeshRenderersRagdoll)
+                    r.enabled = true;
+
+
+            }
+
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/RenderingOptions.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared/SensorBehavior.cs

@@ -0,0 +1,51 @@
+using UnityEngine;
+
+namespace Unity.MLAgents
+{
+    public class SensorBehavior : MonoBehaviour
+    {
+        MarathonAgent _marathonAgent;
+        IOnSensorCollision _onSensorCollision;
+
+        Collider _collider;
+
+        void Start()
+        {
+            _marathonAgent = GetComponentInParent<MarathonAgent>();
+            _onSensorCollision = GetComponentInParent<IOnSensorCollision>();
+            _collider = GetComponent<Collider>();
+        }
+
+        void OnCollisionEnter(Collision other)
+        {
+            if (_marathonAgent != null)
+                _marathonAgent.SensorCollisionEnter(_collider, other);
+            if (_onSensorCollision != null)
+                _onSensorCollision.OnSensorCollisionEnter(_collider, other.gameObject);
+        }
+
+        void OnCollisionExit(Collision other)
+        {
+            if (_marathonAgent != null)
+                _marathonAgent.SensorCollisionExit(_collider, other);
+            if (_onSensorCollision != null)
+                _onSensorCollision.OnSensorCollisionExit(_collider, other.gameObject);
+        }
+
+        void OnTriggerEnter(Collider other)
+        {
+            if (_onSensorCollision != null)
+                _onSensorCollision.OnSensorCollisionEnter(_collider, other.gameObject);
+        }
+        void OnTriggerExit(Collider other)
+        {
+            if (_onSensorCollision != null)
+                _onSensorCollision.OnSensorCollisionExit(_collider, other.gameObject);
+        }
+        void OnTriggerStay(Collider other)
+        {
+            if (_onSensorCollision != null)
+                _onSensorCollision.OnSensorCollisionEnter(_collider, other.gameObject);
+        }        
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/SensorBehavior.cs.meta

@@ -0,0 +1,13 @@
+fileFormatVersion: 2
+guid: d041794481cd24f4d92e5f3da728fe64
+timeCreated: 1520407687
+licenseType: Free
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/3_MarathonEnvs/Scripts/Shared/SmoothFollow.cs

@@ -0,0 +1,62 @@
+// Smooth Follow from Standard Assets
+// If you have C# code and you want to edit SmoothFollow's vars ingame, use this instead.
+
+using UnityEngine;
+
+public class SmoothFollow : MonoBehaviour
+{
+    // The target we are following
+    public Transform target;
+
+    // The distance in the x-z plane to the target
+    public float distance = 10.0f;
+
+    // the height we want the camera to be above the target
+    public float height = 5.0f;
+
+    public bool clampToFloor;
+
+    // How much we 
+    public float heightDamping = 2.0f;
+    public float rotationDamping = 3.0f;
+
+    // Place the script in the Camera-Control group in the component menu
+    [AddComponentMenu("Camera-Control/Smooth Follow")]
+    void Start()
+    {
+    }
+
+    void LateUpdate()
+    {
+        // Early out if we don't have a target
+        if (!target) return;
+
+        // Calculate the current rotation angles
+        float wantedRotationAngle = target.eulerAngles.y;
+        float wantedHeight = clampToFloor ? height : target.position.y + height;
+
+        float currentRotationAngle = transform.eulerAngles.y;
+        float currentHeight = transform.position.y;
+
+        // Damp the rotation around the y-axis
+        currentRotationAngle =
+            Mathf.LerpAngle(currentRotationAngle, wantedRotationAngle, rotationDamping * Time.deltaTime);
+
+        // Damp the height
+        currentHeight = Mathf.Lerp(currentHeight, wantedHeight, heightDamping * Time.deltaTime);
+
+        // Convert the angle into a rotation
+        var currentRotation = Quaternion.Euler(0, currentRotationAngle, 0);
+
+        // Set the position of the camera on the x-z plane to:
+        // distance meters behind the target
+        transform.position = target.position;
+        transform.position -= currentRotation * Vector3.forward * distance;
+
+        // Set the height of the camera
+        transform.position = new Vector3(transform.position.x, currentHeight, transform.position.z);
+
+        // Always look at the target
+        transform.LookAt(target);
+    }
+}

Assets/3_MarathonEnvs/Scripts/Shared/SmoothFollow.cs.meta

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