organisation & splat changes

Assets/3_MarathonEnvs/Scripts/OriginalAgents.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 4add1e84a05e3234d886e655a3f598ce
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/3_MarathonEnvs/Scripts/OriginalAgents/DeepMindHopperAgent.cs

@@ -0,0 +1,99 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+public class DeepMindHopperAgent : MarathonAgent
+{
+
+    public List<float> RewardHackingVector;
+
+    public override void OnEpisodeBegin()
+    {
+        base.OnEpisodeBegin();
+
+        // set to true this to show monitor while training
+        //Monitor.SetActive(true);
+
+        StepRewardFunction = StepRewardHopper101;
+        TerminateFunction = TerminateOnNonFootHitTerrain;
+        ObservationsFunction = ObservationsDefault;
+
+        BodyParts["pelvis"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "torso");
+        BodyParts["foot"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "foot");
+        SetupBodyParts();
+    }
+
+    void ObservationsDefault(VectorSensor sensor)
+    {
+        if (ShowMonitor)
+        {
+        }
+
+        var pelvis = BodyParts["pelvis"];
+        Vector3 normalizedVelocity = this.GetNormalizedVelocity(pelvis.velocity);
+        sensor.AddObservation(normalizedVelocity);
+        sensor.AddObservation(pelvis.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.transform.up);
+
+        sensor.AddObservation(SensorIsInTouch);
+        JointRotations.ForEach(x => sensor.AddObservation(x));
+        sensor.AddObservation(JointVelocity);
+        var foot = BodyParts["foot"];
+        Vector3 normalizedFootPosition = this.GetNormalizedPosition(foot.transform.position);
+        sensor.AddObservation(normalizedFootPosition.y);
+    }
+
+    float GetRewardOnEpisodeComplete()
+    {
+        return FocalPoint.transform.position.x;
+    }
+
+    void UpdateRewardHackingVector()
+    {
+        // float uprightBonus = GetForwardBonus("pelvis");
+        float uprightBonus = GetDirectionBonus("pelvis", Vector3.forward, 1f);
+        uprightBonus = Mathf.Clamp(uprightBonus, 0f, 1f);
+        float velocity = Mathf.Clamp(GetNormalizedVelocity("pelvis").x, 0f, 1f);
+        float position = Mathf.Clamp(GetNormalizedPosition("pelvis").x, 0f, 1f);
+        float effort = 1f - GetEffortNormalized();
+
+        if (RewardHackingVector?.Count == 0)
+            RewardHackingVector = Enumerable.Range(0, 6).Select(x => 0f).ToList();
+        RewardHackingVector[0] = velocity;
+        RewardHackingVector[1] = position;
+        RewardHackingVector[2] = effort;
+        RewardHackingVector[3] = uprightBonus;
+    }
+
+    float StepRewardHopper101()
+    {
+        UpdateRewardHackingVector();
+        float uprightBonus = GetDirectionBonus("pelvis", Vector3.forward, 1f);
+        uprightBonus = Mathf.Clamp(uprightBonus, 0f, 1f);
+        float velocity = Mathf.Clamp(GetNormalizedVelocity("pelvis").x, 0f, 1f);
+        // float position = Mathf.Clamp(GetNormalizedPosition("pelvis").x, 0f, 1f);
+        float effort = 1f - GetEffortNormalized();
+
+        uprightBonus *= 0.05f;
+        velocity *= 0.7f;
+        if (velocity >= .25f)
+            effort *= 0.25f;
+        else
+            effort *= velocity;
+
+        var reward = velocity
+                     + uprightBonus
+                     + effort;
+        if (ShowMonitor)
+        {
+            //var hist = new[] {reward, velocity, uprightBonus, effort};
+            //Monitor.Log("rewardHist", hist, displayType: Monitor.DisplayType.Independent);
+        }
+
+        return reward;
+    }
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/DeepMindHopperAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/DeepMindWalkerAgent.cs

@@ -0,0 +1,82 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+
+public class DeepMindWalkerAgent : MarathonAgent
+{
+    public override void OnEpisodeBegin()
+    {
+        base.OnEpisodeBegin();
+
+        // set to true this to show monitor while training
+        //Monitor.SetActive(true);
+
+        StepRewardFunction = StepRewardWalker106;
+        TerminateFunction = TerminateOnNonFootHitTerrain;
+        ObservationsFunction = ObservationsDefault;
+
+        BodyParts["pelvis"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "torso");
+        BodyParts["left_thigh"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "left_thigh");
+        BodyParts["right_thigh"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "right_thigh");
+        BodyParts["right_foot"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "right_foot");
+        BodyParts["left_foot"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "left_foot");
+        SetupBodyParts();
+    }
+
+    void ObservationsDefault(VectorSensor sensor)
+    {
+        if (ShowMonitor)
+        {
+        }
+
+        var pelvis = BodyParts["pelvis"];
+        Vector3 normalizedVelocity = this.GetNormalizedVelocity(pelvis.velocity);
+        sensor.AddObservation(normalizedVelocity);
+        sensor.AddObservation(pelvis.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.transform.up);
+
+        sensor.AddObservation(SensorIsInTouch);
+        JointRotations.ForEach(x => sensor.AddObservation(x));
+        sensor.AddObservation(JointVelocity);
+        sensor.AddObservation(new []{
+            this.GetNormalizedPosition(BodyParts["left_foot"].transform.position).y,
+            this.GetNormalizedPosition(BodyParts["right_foot"].transform.position).y
+        });
+    }
+
+    float StepRewardWalker106()
+    {
+        float heightPenality = 1f-GetHeightPenality(1.1f);
+        heightPenality = Mathf.Clamp(heightPenality, 0f, 1f);
+        float uprightBonus = GetDirectionBonus("pelvis", Vector3.forward, 1f);
+        uprightBonus = Mathf.Clamp(uprightBonus, 0f, 1f);
+        float velocity = Mathf.Clamp(GetNormalizedVelocity("pelvis").x, 0f, 1f);
+        float effort = 1f - GetEffortNormalized();
+
+        //if (ShowMonitor)
+        //{
+        //    var hist = new[] {velocity, uprightBonus, heightPenality, effort}.ToList();
+        //    Monitor.Log("rewardHist", hist.ToArray(), displayType: Monitor.DisplayType.Independent);
+        //}
+
+        heightPenality *= 0.05f;
+        uprightBonus *= 0.05f;
+        velocity *= 0.4f;
+        if (velocity >= .4f)
+            effort *= 0.4f;
+        else
+            effort *= velocity;
+
+        var reward = velocity
+                     + uprightBonus
+                     + heightPenality
+                     + effort;
+
+        return reward;
+    }
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/DeepMindWalkerAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/MarathonManAgent.cs

@@ -0,0 +1,197 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+using System.Linq;
+using static BodyHelper002;
+
+public class MarathonManAgent : Agent, IOnTerrainCollision
+{
+	BodyManager002 _bodyManager;
+	bool _isDone;
+	bool _hasLazyInitialized;
+
+	public static BodyConfig BodyConfig = new BodyConfig
+	{
+		GetBodyPartGroup = (name) =>
+		{
+			name = name.ToLower();
+			if (name.Contains("mixamorig"))
+				return BodyPartGroup.None;
+
+			if (name.Contains("butt"))
+				return BodyPartGroup.Hips;
+			if (name.Contains("torso"))
+				return BodyPartGroup.Torso;
+			if (name.Contains("head"))
+				return BodyPartGroup.Head;
+			if (name.Contains("waist"))
+				return BodyPartGroup.Spine;
+
+			if (name.Contains("thigh"))
+				return BodyPartGroup.LegUpper;
+			if (name.Contains("shin"))
+				return BodyPartGroup.LegLower;
+			if (name.Contains("right_right_foot") || name.Contains("left_left_foot"))
+				return BodyPartGroup.Foot;
+			if (name.Contains("upper_arm"))
+				return BodyPartGroup.ArmUpper;
+			if (name.Contains("larm"))
+				return BodyPartGroup.ArmLower;
+			if (name.Contains("hand"))
+				return BodyPartGroup.Hand;
+
+			return BodyPartGroup.None;
+		},
+		GetMuscleGroup = (name) =>
+		{
+			name = name.ToLower();
+			if (name.Contains("mixamorig"))
+				return MuscleGroup.None;
+			if (name.Contains("butt"))
+				return MuscleGroup.Hips;
+			if (name.Contains("lower_waist")
+				|| name.Contains("abdomen_y"))
+				return MuscleGroup.Spine;
+			if (name.Contains("thigh")
+				|| name.Contains("hip"))
+				return MuscleGroup.LegUpper;
+			if (name.Contains("shin"))
+				return MuscleGroup.LegLower;
+			if (name.Contains("right_right_foot")
+				|| name.Contains("left_left_foot")
+				|| name.Contains("ankle_x"))
+				return MuscleGroup.Foot;
+			if (name.Contains("upper_arm"))
+				return MuscleGroup.ArmUpper;
+			if (name.Contains("larm"))
+				return MuscleGroup.ArmLower;
+			if (name.Contains("hand"))
+				return MuscleGroup.Hand;
+
+			return MuscleGroup.None;
+		},
+        GetRootBodyPart = () => BodyPartGroup.Hips,
+        GetRootMuscle = () => MuscleGroup.Hips
+    };
+
+
+
+	override public void CollectObservations(VectorSensor sensor)
+	{
+		if (!_hasLazyInitialized)
+		{
+			OnEpisodeBegin();
+		}
+
+		Vector3 normalizedVelocity = _bodyManager.GetNormalizedVelocity();
+        var pelvis = _bodyManager.GetFirstBodyPart(BodyPartGroup.Hips);
+        var shoulders = _bodyManager.GetFirstBodyPart(BodyPartGroup.Torso);
+
+        sensor.AddObservation(normalizedVelocity); 
+        sensor.AddObservation(pelvis.Rigidbody.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.Rigidbody.transform.up);
+
+        sensor.AddObservation(shoulders.Rigidbody.transform.forward); // gyroscope 
+        sensor.AddObservation(shoulders.Rigidbody.transform.up);
+
+		sensor.AddObservation(_bodyManager.GetSensorIsInTouch());
+		foreach (var bodyPart in _bodyManager.BodyParts)
+		{
+			bodyPart.UpdateObservations();
+			sensor.AddObservation(bodyPart.ObsLocalPosition);
+			sensor.AddObservation(bodyPart.ObsRotation);
+			sensor.AddObservation(bodyPart.ObsRotationVelocity);
+			sensor.AddObservation(bodyPart.ObsVelocity);
+		}
+		sensor.AddObservation(_bodyManager.GetSensorObservations());
+
+		// _bodyManager.OnCollectObservationsHandleDebug(GetInfo());
+	}
+
+	public override void OnActionReceived(ActionBuffers actions)
+    {
+        float[] vectorAction = actions.ContinuousActions.Select(x=>x).ToArray();
+		
+		if (!_hasLazyInitialized)
+		{
+			return;
+		}
+		_isDone = false;
+		// apply actions to body
+		_bodyManager.OnAgentAction(vectorAction);
+
+		// manage reward
+        float velocity = Mathf.Clamp(_bodyManager.GetNormalizedVelocity().x, 0f, 1f);
+		var actionDifference = _bodyManager.GetActionDifference();
+		var actionsAbsolute = vectorAction.Select(x=>Mathf.Abs(x)).ToList();
+		var actionsAtLimit = actionsAbsolute.Select(x=> x>=1f ? 1f : 0f).ToList();
+		float actionaAtLimitCount = actionsAtLimit.Sum();
+        float notAtLimitBonus = 1f - (actionaAtLimitCount / (float) actionsAbsolute.Count);
+        float reducedPowerBonus = 1f - actionsAbsolute.Average();
+
+		// velocity *= 0.85f;
+		// reducedPowerBonus *=0f;
+		// notAtLimitBonus *=.1f;
+		// actionDifference *=.05f;
+        // var reward = velocity
+		// 				+ notAtLimitBonus
+		// 				+ reducedPowerBonus
+		// 				+ actionDifference;		
+        var pelvis = _bodyManager.GetFirstBodyPart(BodyPartGroup.Hips);
+		if (pelvis.Transform.position.y<0){
+			EndEpisode();
+		}
+
+        var reward = velocity;
+
+		AddReward(reward);
+		_bodyManager.SetDebugFrameReward(reward);
+	}
+
+	public override void OnEpisodeBegin()
+	{
+		if (!_hasLazyInitialized)
+		{
+			_bodyManager = GetComponent<BodyManager002>();
+			_bodyManager.BodyConfig = MarathonManAgent.BodyConfig;
+			_bodyManager.OnInitializeAgent();
+			_hasLazyInitialized = true;
+		}
+		_isDone = true;
+		_bodyManager.OnAgentReset();
+	}
+	public virtual void OnTerrainCollision(GameObject other, GameObject terrain)
+	{
+		// if (string.Compare(terrain.name, "Terrain", true) != 0)
+		if (terrain.GetComponent<Terrain>() == null)
+			return;
+		// if (!_styleAnimator.AnimationStepsReady)
+		// 	return;
+        // HACK - for when agent has not been initialized
+		if (_bodyManager == null)
+			return;
+		var bodyPart = _bodyManager.BodyParts.FirstOrDefault(x=>x.Transform.gameObject == other);
+		if (bodyPart == null)
+			return;
+		switch (bodyPart.Group)
+		{
+			case BodyHelper002.BodyPartGroup.None:
+			case BodyHelper002.BodyPartGroup.Foot:
+			// case BodyHelper002.BodyPartGroup.LegUpper:
+			case BodyHelper002.BodyPartGroup.LegLower:
+			case BodyHelper002.BodyPartGroup.Hand:
+			// case BodyHelper002.BodyPartGroup.ArmLower:
+			// case BodyHelper002.BodyPartGroup.ArmUpper:
+				break;
+			default:
+				// AddReward(-100f);
+				if (!_isDone){
+					EndEpisode();
+				}
+				break;
+		}
+	}
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/MarathonManAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/MarathonTestBedDecision.cs

@@ -0,0 +1,51 @@
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+
+public class MarathonTestBedDecision //: Decision
+{
+    // Brain _brain;
+
+    // MarathonTestBedController _controller;
+
+
+    // // [Tooltip("Lock the top most element")]
+    // // /**< \brief Lock the top most element*/
+    // // public bool FreezeTop = false;
+    // // bool _lastFreezeTop;
+
+    // public override float[] Decide(
+    //     List<float> vectorObs,
+    //     List<Texture2D> visualObs,
+    //     float reward,
+    //     bool done,
+    //     List<float> memory)
+    // {
+    //     // lazy init
+    //     if (_controller == null)
+    //     {
+    //         _controller = FindObjectOfType<MarathonTestBedController>();
+    //         // _brain = GetComponent<Brain>();
+    //         // Actions = Enumerable.Repeat(0f, _brain.brainParameters.vectorActionSize[0]).ToArray();
+    //         // Actions = Enumerable.Repeat(0f, 100).ToArray();
+    //     }
+    //     if (_controller.ApplyRandomActions)
+    //     {
+    //         for (int i = 0; i < _controller.Actions.Length; i++)
+    //             _controller.Actions[i] = Random.value * 2 - 1;
+    //     }
+
+    //     return _controller.Actions;
+    // }
+
+    // public override List<float> MakeMemory(
+    //         List<float> vectorObs,
+    //         List<Texture2D> visualObs,
+    //         float reward,
+    //         bool done,
+    //         List<float> memory)
+    // {
+    //     return new List<float>();
+    // }
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/MarathonTestBedDecision.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/OpenAIAntAgent.cs

@@ -0,0 +1,81 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+
+public class OpenAIAntAgent : MarathonAgent
+{
+    public override void OnEpisodeBegin()
+    {
+        base.OnEpisodeBegin();
+
+        // set to true this to show monitor while training
+        //Monitor.SetActive(true);
+
+        StepRewardFunction = StepRewardAnt101;
+        TerminateFunction = TerminateAnt;
+        ObservationsFunction = ObservationsDefault;
+
+        BodyParts["pelvis"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "torso_geom");
+        SetupBodyParts();
+    }
+
+    void ObservationsDefault(VectorSensor sensor)
+    {
+        if (ShowMonitor)
+        {
+        }
+
+        var pelvis = BodyParts["pelvis"];
+        Vector3 normalizedVelocity = GetNormalizedVelocity(pelvis.velocity);
+        sensor.AddObservation(normalizedVelocity);
+        sensor.AddObservation(pelvis.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.transform.up);
+
+        sensor.AddObservation(SensorIsInTouch);
+        JointRotations.ForEach(x => sensor.AddObservation(x));
+        sensor.AddObservation(JointVelocity);
+        Vector3 normalizedFootPosition = this.GetNormalizedPosition(pelvis.transform.position);
+        sensor.AddObservation(normalizedFootPosition.y);
+
+    }
+
+    bool TerminateAnt()
+    {
+        var pelvis = BodyParts["pelvis"];
+		if (pelvis.transform.position.y<0){
+			return true;
+		}
+
+        var angle = GetForwardBonus("pelvis");
+        bool endOnAngle = (angle < .2f);
+        return endOnAngle;
+    }
+
+    float StepRewardAnt101()
+    {
+        float velocity = Mathf.Clamp(GetNormalizedVelocity("pelvis").x, 0f, 1f);
+        float effort = 1f - GetEffortNormalized();
+
+        velocity *= 0.7f;
+        if (velocity >= .3f)
+            effort *= 0.3f;
+        else
+            effort *= velocity;
+
+
+        var reward = velocity
+                     + effort;
+        //if (ShowMonitor)
+        //{
+        //    var hist = new[] {reward, velocity}.ToList();
+        //    Monitor.Log("rewardHist", hist.ToArray(), displayType: Monitor.DisplayType.Independent);
+        //}
+
+        return reward;
+    }
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/OpenAIAntAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/SparceMarathonManAgent.cs

@@ -0,0 +1,216 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+using System.Linq;
+using static BodyHelper002;
+using System;
+
+public class RollingAverage
+{
+	List<double> _window;
+	int _size;
+	int _count;
+	double _sum;
+	double _sumOfSquares;
+	public double Mean;
+	public double StandardDeviation;
+
+	public RollingAverage(int size)
+	{
+		_window = new List<double>(size);
+		_size = size;
+		_count = 0;
+		_sum = 0;
+		_sumOfSquares = 0;
+	}
+	public double Normalize(double val)
+	{
+		Add(val);
+		double normalized = val;
+		if (StandardDeviation != 0) 
+			normalized = (val - Mean) / StandardDeviation;
+		return normalized;
+	}
+	void Add (double val)
+	{
+		if (_count >= _size)
+		{
+			var removedVal = _window[0];
+			_window.RemoveAt(0);
+			_count--;
+			_sum -= removedVal;
+			_sumOfSquares -= removedVal * removedVal;
+		}
+		_window.Add(val);
+		_count++;
+		_sum += val;
+		_sumOfSquares += val * val;
+		// set Mean to Sum / Count, 
+		Mean = _sum / _count;
+		// set StandardDeviation to Math.Sqrt(SumOfSquares / Count - Mean * Mean).
+		StandardDeviation = Math.Sqrt(_sumOfSquares / _count - Mean * Mean);
+	}
+}
+
+public class SparceMarathonManAgent : Agent, IOnTerrainCollision
+{
+	BodyManager002 _bodyManager;
+	public float _heightReward;
+	public float _torsoUprightReward;
+	public float _torsoForwardReward;
+	public float _hipsUprightReward;
+	public float _hipsForwardReward;
+	public float _notAtLimitBonus;
+	public float _reducedPowerBonus;
+	public float _episodeMaxDistance;
+
+	static RollingAverage rollingAverage;
+
+	bool _isDone;
+	bool _hasLazyInitialized;
+
+	override public void CollectObservations(VectorSensor sensor)
+	{
+		if (!_hasLazyInitialized)
+		{
+			OnEpisodeBegin();
+		}
+
+		Vector3 normalizedVelocity = _bodyManager.GetNormalizedVelocity();
+        var pelvis = _bodyManager.GetFirstBodyPart(BodyPartGroup.Hips);
+        var shoulders = _bodyManager.GetFirstBodyPart(BodyPartGroup.Torso);
+
+        sensor.AddObservation(normalizedVelocity); 
+        sensor.AddObservation(pelvis.Rigidbody.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.Rigidbody.transform.up);
+
+        sensor.AddObservation(shoulders.Rigidbody.transform.forward); // gyroscope 
+        sensor.AddObservation(shoulders.Rigidbody.transform.up);
+
+		sensor.AddObservation(_bodyManager.GetSensorIsInTouch());
+		foreach (var bodyPart in _bodyManager.BodyParts)
+		{
+			bodyPart.UpdateObservations();
+			sensor.AddObservation(bodyPart.ObsLocalPosition);
+			sensor.AddObservation(bodyPart.ObsRotation);
+			sensor.AddObservation(bodyPart.ObsRotationVelocity);
+			sensor.AddObservation(bodyPart.ObsVelocity);
+		}
+		sensor.AddObservation(_bodyManager.GetSensorObservations());
+
+		sensor.AddObservation(_notAtLimitBonus);
+		sensor.AddObservation(_reducedPowerBonus);
+		// _bodyManager.OnCollectObservationsHandleDebug(GetInfo());
+	}
+
+	public override void OnActionReceived(ActionBuffers actions)
+    {
+        float[] vectorAction = actions.ContinuousActions.Select(x=>x).ToArray();
+
+		if (!_hasLazyInitialized)
+		{
+			return;
+		}
+		_isDone = false;
+		// apply actions to body
+		_bodyManager.OnAgentAction(vectorAction);
+
+		// manage reward
+		var actionDifference = _bodyManager.GetActionDifference();
+		var actionsAbsolute = vectorAction.Select(x=>Mathf.Abs(x)).ToList();
+		var actionsAtLimit = actionsAbsolute.Select(x=> x>=1f ? 1f : 0f).ToList();
+		float actionaAtLimitCount = actionsAtLimit.Sum();
+        _notAtLimitBonus = 1f - (actionaAtLimitCount / (float) actionsAbsolute.Count);
+        _reducedPowerBonus = 1f - actionsAbsolute.Average();
+        _heightReward = _bodyManager.GetHeightNormalizedReward(1.2f);
+		_torsoUprightReward = _bodyManager.GetUprightNormalizedReward(BodyPartGroup.Torso);
+		_torsoForwardReward = _bodyManager.GetDirectionNormalizedReward(BodyPartGroup.Torso, Vector3.forward);
+		_hipsUprightReward = _bodyManager.GetUprightNormalizedReward(BodyPartGroup.Hips);
+		_hipsForwardReward = _bodyManager.GetDirectionNormalizedReward(BodyPartGroup.Hips, Vector3.forward);
+		_torsoUprightReward = Mathf.Clamp(_torsoUprightReward, 0f, 1f);
+		_torsoForwardReward = Mathf.Clamp(_torsoForwardReward, 0f, 1f);
+		_hipsUprightReward = Mathf.Clamp(_hipsUprightReward, 0f, 1f);
+		_hipsForwardReward = Mathf.Clamp(_hipsForwardReward, 0f, 1f);
+
+		var stepCount = StepCount > 0 ? StepCount : 1;
+		if ((stepCount >= MaxStep)
+                && (MaxStep > 0))
+        {
+            AddEpisodeEndReward();
+        }
+		else{
+			var pelvis = _bodyManager.GetFirstBodyPart(BodyPartGroup.Hips);
+			if (pelvis.Transform.position.y<0){
+	            AddEpisodeEndReward();
+				EndEpisode();
+			}
+		}
+	}
+
+	public override void OnEpisodeBegin()
+	{
+		if (!_hasLazyInitialized)
+		{
+			_bodyManager = GetComponent<BodyManager002>();
+			_bodyManager.BodyConfig = MarathonManAgent.BodyConfig;
+			_bodyManager.OnInitializeAgent();
+			_hasLazyInitialized = true;
+		}
+		_isDone = true;
+		_bodyManager.OnAgentReset();
+		_episodeMaxDistance = 0f;
+		if (rollingAverage == null)
+			rollingAverage = new RollingAverage(100);
+	}
+	public virtual void OnTerrainCollision(GameObject other, GameObject terrain)
+	{
+		// if (string.Compare(terrain.name, "Terrain", true) != 0)
+		if (terrain.GetComponent<Terrain>() == null)
+			return;
+		// if (!_styleAnimator.AnimationStepsReady)
+		// 	return;
+        // HACK - for when agent has not been initialized
+		if (_bodyManager == null)
+			return;
+		var bodyPart = _bodyManager.BodyParts.FirstOrDefault(x=>x.Transform.gameObject == other);
+		if (bodyPart == null)
+			return;
+		switch (bodyPart.Group)
+		{
+			case BodyHelper002.BodyPartGroup.Foot:
+				_episodeMaxDistance = _bodyManager.GetNormalizedPosition().x;
+				break;
+			case BodyHelper002.BodyPartGroup.None:
+			// case BodyHelper002.BodyPartGroup.LegUpper:
+			case BodyHelper002.BodyPartGroup.LegLower:
+			case BodyHelper002.BodyPartGroup.Hand:
+			// case BodyHelper002.BodyPartGroup.ArmLower:
+			// case BodyHelper002.BodyPartGroup.ArmUpper:
+				break;
+			default:
+				// AddReward(-100f);
+				if (!_isDone){
+					AddEpisodeEndReward();
+					EndEpisode();
+				}
+				break;
+		}
+	}
+
+	void AddEpisodeEndReward()
+	{
+		var reward = _episodeMaxDistance;
+
+		AddReward(reward);
+		_bodyManager.SetDebugFrameReward(reward);
+
+		// # normalized reward
+		// float normalizedReward = (float)rollingAverage.Normalize(reward);
+		// normalizedReward += (float)rollingAverage.Mean;
+		// AddReward(normalizedReward);
+		// _bodyManager.SetDebugFrameReward(normalizedReward);
+	}
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/SparceMarathonManAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainAntAgent.cs

@@ -0,0 +1,152 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+
+public class TerrainAntAgent : MarathonAgent {
+
+    TerrainGenerator _terrainGenerator;
+    int _lastXPosInMeters;
+    int _stepCountAtLastMeter;
+    float _pain;
+    Vector3 _centerOfMass;
+
+    public override void OnEpisodeBegin()
+    {
+        base.OnEpisodeBegin();
+
+        BodyParts["pelvis"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "torso_geom");
+
+        SetCenterOfMass();
+
+        if (_terrainGenerator == null)
+            _terrainGenerator = GetComponent<TerrainGenerator>();
+        _lastXPosInMeters = (int) BodyParts["pelvis"].transform.position.x;
+        _terrainGenerator.Reset();
+
+        // set to true this to show monitor while training
+        //Monitor.SetActive(true);
+
+        StepRewardFunction = StepRewardAnt101;
+        TerminateFunction = LocalTerminate;
+        ObservationsFunction = ObservationsDefault;
+        OnTerminateRewardValue = 0f;
+        // OnTerminateRewardValue = -100f;
+        _pain = 0f;
+
+        base.SetupBodyParts();
+        SetCenterOfMass();
+    }
+
+    bool LocalTerminate()
+    {
+        int newXPosInMeters = (int) BodyParts["pelvis"].transform.position.x;
+        if (newXPosInMeters > _lastXPosInMeters) {
+            _lastXPosInMeters = newXPosInMeters;
+            _stepCountAtLastMeter = this.StepCount;
+        }
+
+        SetCenterOfMass();
+        var xpos = _centerOfMass.x;
+        var terminate = false;
+		if (_terrainGenerator.IsPointOffEdge(BodyParts["pelvis"].transform.position)){
+            terminate = true;
+            AddReward(-1f);
+        }
+        if (this.StepCount-_stepCountAtLastMeter >= (200*5))
+            terminate = true;
+        else if (xpos < 4f && _pain > 1f)
+            terminate = true;
+        else if (xpos < 2f && _pain > 0f)
+            terminate = true;
+
+        return terminate;
+    }
+    public override void OnTerrainCollision(GameObject other, GameObject terrain) {
+        if (terrain.GetComponent<Terrain>() == null)
+            return;
+
+        switch (other.name.ToLowerInvariant().Trim())
+        {
+            case "pelvis": // dm_hopper
+                _pain += 5f;
+                NonFootHitTerrain = true;
+                break;
+            case "left_ankle_geom": // oai_ant
+            case "right_ankle_geom": // oai_ant
+            case "third_ankle_geom": // oai_ant
+            case "fourth_ankle_geom": // oai_ant
+                FootHitTerrain = true;
+                break;
+            default:
+                _pain += 5f;
+                NonFootHitTerrain = true;
+                break;
+        }
+    }
+  
+    void ObservationsDefault(VectorSensor sensor)
+    {
+        var pelvis = BodyParts["pelvis"];
+        Vector3 normalizedVelocity = GetNormalizedVelocity(pelvis.velocity);
+        sensor.AddObservation(normalizedVelocity);
+        sensor.AddObservation(pelvis.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.transform.up);
+
+        sensor.AddObservation(SensorIsInTouch);
+        JointRotations.ForEach(x => sensor.AddObservation(x));
+        sensor.AddObservation(JointVelocity);
+        // Vector3 normalizedFootPosition = this.GetNormalizedPosition(pelvis.transform.position);
+        // sensor.AddObservation(normalizedFootPosition.y);
+
+        (List<float> distances, float fraction) = 
+            _terrainGenerator.GetDistances2d(
+                pelvis.transform.position, ShowMonitor);
+   
+        sensor.AddObservation(distances);
+        sensor.AddObservation(fraction);
+    }
+
+
+    void SetCenterOfMass()
+    {
+        _centerOfMass = Vector3.zero;
+        float c = 0f;
+        var bodyParts = this.gameObject.GetComponentsInChildren<Rigidbody>();
+ 
+        foreach (var part in bodyParts)
+        {
+            _centerOfMass += part.worldCenterOfMass * part.mass;
+            c += part.mass;
+        }
+        _centerOfMass /= c;
+    }
+
+    float StepRewardAnt101()
+    {
+        float velocity = Mathf.Clamp(GetNormalizedVelocity("pelvis").x, 0f, 1f);
+        float effort = 1f - GetEffortNormalized();
+
+        // velocity *= 0.7f;
+        // if (velocity >= .25f)
+        //     effort *= 0.25f;
+        // else
+        //     effort *= velocity;
+
+        // var reward = velocity
+        //              + effort;
+        // if (ShowMonitor)
+        // {
+        //     var hist = new[] {reward, velocity, effort};
+        //     Monitor.Log("rewardHist", hist, displayType: Monitor.DisplayType.Independent);
+        // }
+        _pain = 0f;
+        var reward = velocity;
+        return reward;
+        // return 0f;
+    }
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainAntAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainGenerator.cs

@@ -0,0 +1,229 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using Unity.MLAgents;
+using System.Linq;
+using ManyWorlds;
+
+public class TerrainGenerator : MonoBehaviour
+{
+
+    Terrain terrain;
+	Agent _agent;
+	DecisionRequester _decisionRequester;
+	public int posXInTerrain;
+	public int posYInTerrain;
+	float[,] _heights;
+	float[,] _rowHeight;
+
+	public int heightIndex;
+	public float curHeight;
+	public float actionReward;
+
+	internal const float _minHeight = 0f;
+	internal const float _maxHeight = 10f;
+	internal const float _minSpawnHeight = 0f;//2f;
+	internal const float _maxSpawnHeight = 10f;//8f;
+	const float _midHeight = 5f;
+	float _mapScaleY;
+	float[,] _heightMap;
+	public List<float> debugLastHeights;
+	public List<float> debugLastNormHeights;
+	public float debugLastFraction;
+
+	PhysicsScene physicsScene;
+
+    // Start is called before the first frame update
+    void Start()
+    {
+        physicsScene = (GetComponentInParent<SpawnableEnv>().GetPhysicsScene());
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        
+    }
+
+    public void Reset()
+    {
+		if (this.terrain == null)
+		{
+			var parent = gameObject.transform.parent;
+			terrain = parent.GetComponentInChildren<Terrain>();
+			var sharedTerrainData = terrain.terrainData;
+			terrain.terrainData = new TerrainData();
+			terrain.terrainData.heightmapResolution = sharedTerrainData.heightmapResolution;
+			terrain.terrainData.baseMapResolution = sharedTerrainData.baseMapResolution;
+			terrain.terrainData.SetDetailResolution(sharedTerrainData.detailResolution, sharedTerrainData.detailResolutionPerPatch);
+			terrain.terrainData.size = sharedTerrainData.size;
+			//terrain.terrainData.thickness = sharedTerrainData.thickness;
+			// terrain.terrainData.splatPrototypes = sharedTerrainData.splatPrototypes;
+			terrain.terrainData.terrainLayers = sharedTerrainData.terrainLayers;
+			var collider = terrain.GetComponent<TerrainCollider>();
+			collider.terrainData = terrain.terrainData;
+			_rowHeight = new float[terrain.terrainData.heightmapResolution,1];
+		}
+		if (this._agent == null)
+		{
+			_agent = GetComponent<Agent>();
+			_decisionRequester = GetComponent<DecisionRequester>();
+		}
+		//print($"HeightMap {this.terrain.terrainData.heightmapWidth}, {this.terrain.terrainData.heightmapHeight}. 
+		// Scale {this.terrain.terrainData.heightmapScale}. Resolution {this.terrain.terrainData.heightmapResolution}");
+		_mapScaleY = this.terrain.terrainData.heightmapScale.y;
+		// get the normalized position of this game object relative to the terrain
+        Vector3 tempCoord = (transform.position - terrain.gameObject.transform.position);
+        Vector3 coord;
+		tempCoord.x = Mathf.Clamp(tempCoord.x,0f, terrain.terrainData.size.x-0.000001f);
+		tempCoord.z = Mathf.Clamp(tempCoord.z,0f, terrain.terrainData.size.z-0.000001f);
+        coord.x = (tempCoord.x-1) / terrain.terrainData.size.x;
+        coord.y = tempCoord.y / terrain.terrainData.size.y;
+        coord.z = tempCoord.z / terrain.terrainData.size.z;
+        // get the position of the terrain heightmap where this game object is
+        posXInTerrain = (int) (coord.x * terrain.terrainData.heightmapResolution); 
+        posYInTerrain = (int) (coord.z * terrain.terrainData.heightmapResolution);
+        // we set an offset so that all the raising terrain is under this game object
+        int offset = 0 / 2;
+        // get the heights of the terrain under this game object
+        _heights = terrain.terrainData.GetHeights(posXInTerrain-offset,posYInTerrain-offset, 100,1);
+		curHeight = _midHeight;
+		heightIndex = posXInTerrain;
+		actionReward = 0f;
+        
+		ResetHeights();        
+    }
+	void ResetHeights()
+	{
+		if (_heightMap == null){
+			_heightMap = new float [terrain.terrainData.heightmapResolution, terrain.terrainData.heightmapResolution];
+        }
+		heightIndex = 0;
+		while(heightIndex <posXInTerrain)
+			SetNextHeight(0);
+
+		SetNextHeight(0);
+		SetNextHeight(0);
+		SetNextHeight(0);
+		SetNextHeight(0);
+		SetNextHeight(0);
+		SetNextHeight(0);
+		while(heightIndex < terrain.terrainData.heightmapResolution)
+		{
+			int action = Random.Range(0,21);
+			try
+			{
+				SetNextHeight(action);				
+			}
+			catch (System.Exception)
+			{
+				SetNextHeight(action);
+				throw;
+			}
+		}
+		this.terrain.terrainData.SetHeights(0, 0, _heightMap);
+
+	}
+	void SetNextHeight(int action)
+	{
+		float actionSize = 0f;
+		bool actionPos = (action-1) % 2 == 0;
+		if (action != 0)
+		{
+			actionSize = ((float)((action+1)/2)) * 0.1f;
+			curHeight += actionPos ? actionSize : -actionSize;
+			if (curHeight < _minSpawnHeight) {
+				curHeight = _minSpawnHeight;
+				actionSize = 0;
+			}
+			if (curHeight > _maxSpawnHeight)
+			{
+				curHeight = _maxSpawnHeight;
+				actionSize = 0;
+			}
+		}
+
+		float height = curHeight / _mapScaleY;
+		// var unit = terrain.terrainData.heightmapWidth / (int)_mapScaleY;
+		int unit = 1;
+		int startH = heightIndex * unit;
+        for (int h = startH; h < startH+unit; h++)
+        {
+            for (int w = 0; w < terrain.terrainData.heightmapResolution; w++){
+                _heightMap[w,h] = height;
+			}
+			height += 1/300f/_mapScaleY;
+        }
+		heightIndex++;
+	}
+
+	public List<float> GetDistances2d(IEnumerable<Vector3> points)
+	{
+		List<float> distances = points
+			.Select(x=> GetDistance2d(x))
+			.ToList();
+		return distances;
+	}
+	float GetDistance2d(Vector3 point)
+	{
+		int layerMask = ~(1 << 14);
+		RaycastHit hit;
+		if (!physicsScene.Raycast(point, Vector3.down, out hit,_maxHeight,layerMask))
+			return 1f;
+		float distance = hit.distance;
+		distance = Mathf.Clamp(distance, -1f, 1f);
+		return distance;
+	}
+
+	public bool IsPointOffEdge(Vector3 point)
+	{
+        Vector3 localPos = (point - terrain.gameObject.transform.position);
+		bool isOffEdge = false;
+		isOffEdge |= (localPos.z < 0f);
+		isOffEdge |= (localPos.z >= terrain.terrainData.size.z);
+		return isOffEdge;
+	}
+
+	public (List<float>, float) GetDistances2d(Vector3 pos, bool showDebug)
+	{
+		int layerMask = ~(1 << 14);
+        var xpos = pos.x;
+        xpos -= 2f;
+        float fraction = (xpos - (Mathf.Floor(xpos*5)/5)) * 5;
+        float ypos = pos.y;
+        List<Ray> rays = Enumerable.Range(0, 5*7).Select(x => new Ray(new Vector3(xpos+(x*.2f), TerrainGenerator._maxHeight, pos.z), Vector3.down)).ToList();
+		RaycastHit hit;
+		List<float> distances = rays.Select
+			( ray=> {
+				if (!physicsScene.Raycast(ray.origin, ray.direction, out hit,_maxHeight,layerMask))
+					return _maxHeight;
+				return ypos - (_maxHeight - hit.distance);
+			}).ToList();
+        if (Application.isEditor && showDebug)
+        {
+            var view = distances.Skip(10).Take(20).Select(x=>x).ToList();
+            //Monitor.Log("distances", view.ToArray());
+            var time = Time.deltaTime;
+			if (_decisionRequester?.DecisionPeriod > 1)
+				time *= _decisionRequester.DecisionPeriod;
+			for (int i = 0; i < rays.Count; i++)
+            {
+                var distance = distances[i];
+                var origin = new Vector3(rays[i].origin.x, ypos,0f);
+                var direction = distance > 0 ? Vector3.down : Vector3.up;
+                var color = distance > 0 ? Color.yellow : Color.red;
+                Debug.DrawRay(origin, direction*Mathf.Abs(distance), color, time, false);
+            }
+        }
+		List<float> normalizedDistances = distances
+			.Select(x => Mathf.Clamp(x, -10f, 10f))
+			.Select(x => x/10f)
+			.ToList();
+		;
+		debugLastNormHeights = normalizedDistances;
+		debugLastHeights = distances;
+		debugLastFraction = fraction;
+
+		return (normalizedDistances, fraction); 
+	}    
+}    

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainGenerator.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainHopperAgent.cs

@@ -0,0 +1,167 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+
+public class TerrainHopperAgent : MarathonAgent {
+
+    TerrainGenerator _terrainGenerator;
+    int _lastXPosInMeters;
+    int _stepCountAtLastMeter;
+    float _pain;
+    Vector3 _centerOfMass;
+
+    public override void OnEpisodeBegin()
+    {
+        base.OnEpisodeBegin();
+
+        BodyParts["pelvis"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x=>x.name=="torso");
+        BodyParts["foot"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x=>x.name=="foot");
+
+        SetCenterOfMass();
+
+        if (_terrainGenerator == null)
+            _terrainGenerator = GetComponent<TerrainGenerator>();
+        _lastXPosInMeters = (int) BodyParts["foot"].transform.position.x;
+        _terrainGenerator.Reset();
+        _stepCountAtLastMeter = 0;
+
+        // set to true this to show monitor while training
+        //Monitor.SetActive(true);
+
+        StepRewardFunction = StepRewardHopper101;
+        TerminateFunction = LocalTerminate;
+        ObservationsFunction = ObservationsDefault;
+        OnTerminateRewardValue = 0f;
+        _pain = 0f;
+
+        base.SetupBodyParts();
+        SetCenterOfMass();
+    }
+
+    bool LocalTerminate()
+    {
+        int newXPosInMeters = (int) BodyParts["foot"].transform.position.x;
+        if (newXPosInMeters > _lastXPosInMeters) {
+            _lastXPosInMeters = newXPosInMeters;
+            _stepCountAtLastMeter = this.StepCount;
+        }
+
+        SetCenterOfMass();
+        var xpos = _centerOfMass.x;
+        var terminate = false;
+        if (this.StepCount-_stepCountAtLastMeter >= (100*5))
+            terminate = true;
+        else if (xpos < 2f && _pain > 0f)
+            terminate = true;
+        else if (_pain > 1f)
+            terminate = true;
+
+        return terminate;
+    }
+    public override void OnTerrainCollision(GameObject other, GameObject terrain) {
+        if (terrain.GetComponent<Terrain>() == null)
+            return;
+
+        switch (other.name.ToLowerInvariant().Trim())
+        {
+            // case "torso": // dm_hopper
+            //     _pain += 5f;
+            //     NonFootHitTerrain = true;
+            //     break;
+            case "foot": // dm_hopper
+            case "calf": // dm_hopper
+                FootHitTerrain = true;
+                break;
+            default:
+            case "thigh": // dm_hopper
+            case "pelvis": // dm_hopper
+            case "torso": // dm_hopper
+                _pain += .5f;
+                NonFootHitTerrain = true;
+                break;
+        }
+    }
+  
+    void ObservationsDefault(VectorSensor sensor)
+    {
+        // var pelvis = BodyParts["pelvis"];
+        // sensor.AddObservation(pelvis.velocity);
+        // sensor.AddObservation(pelvis.transform.forward); // gyroscope 
+        // sensor.AddObservation(pelvis.transform.up);
+
+        // sensor.AddObservation(SensorIsInTouch);
+        // JointRotations.ForEach(x=>sensor.AddObservation(x));
+        // sensor.AddObservation(JointVelocity);
+        // var foot = BodyParts["foot"];
+        // sensor.AddObservation(foot.transform.position.y);
+
+        var pelvis = BodyParts["pelvis"];
+        Vector3 normalizedVelocity = this.GetNormalizedVelocity(pelvis.velocity);
+        sensor.AddObservation(normalizedVelocity);
+        sensor.AddObservation(pelvis.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.transform.up);
+
+        sensor.AddObservation(SensorIsInTouch);
+        JointRotations.ForEach(x => sensor.AddObservation(x));
+        sensor.AddObservation(JointVelocity);
+        var foot = BodyParts["foot"];
+        Vector3 normalizedFootPosition = this.GetNormalizedPosition(foot.transform.position);
+        sensor.AddObservation(normalizedFootPosition.y);        
+
+        (List<float> distances, float fraction) = 
+            _terrainGenerator.GetDistances2d(
+                pelvis.transform.position, ShowMonitor);
+   
+        sensor.AddObservation(distances);
+        sensor.AddObservation(fraction);
+    }
+
+
+    void SetCenterOfMass()
+    {
+        _centerOfMass = Vector3.zero;
+        float c = 0f;
+        var bodyParts = this.gameObject.GetComponentsInChildren<Rigidbody>();
+ 
+        foreach (var part in bodyParts)
+        {
+            _centerOfMass += part.worldCenterOfMass * part.mass;
+            c += part.mass;
+        }
+        _centerOfMass /= c;
+    }
+
+    float StepRewardHopper101()
+    {
+        float uprightBonus = GetDirectionBonus("pelvis", Vector3.forward, 1f);
+        uprightBonus = Mathf.Clamp(uprightBonus, 0f, 1f);
+        float velocity = Mathf.Clamp(GetNormalizedVelocity("pelvis").x, 0f, 1f);
+        // float position = Mathf.Clamp(GetNormalizedPosition("pelvis").x, 0f, 1f);
+        float effort = 1f - GetEffortNormalized();
+
+        // uprightBonus *= 0.05f;
+        // velocity *= 0.7f;
+        // if (velocity >= .25f)
+        //     effort *= 0.25f;
+        // else
+        //     effort *= velocity;
+
+        // var reward = velocity
+        //              + uprightBonus
+        //              + effort;
+        // if (ShowMonitor)
+        // {
+        //     var hist = new[] {reward, velocity, uprightBonus, effort};
+        //     Monitor.Log("rewardHist", hist, displayType: Monitor.DisplayType.Independent);
+        // }
+        var reward = velocity;
+
+        _pain = 0f;
+        return reward;
+    }
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainHopperAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainMarathonManAgent.cs

@@ -0,0 +1,183 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+using System.Linq;
+using static BodyHelper002;
+using ManyWorlds;
+
+public class TerrainMarathonManAgent : Agent, IOnTerrainCollision
+{
+	BodyManager002 _bodyManager;
+
+    TerrainGenerator _terrainGenerator;
+	SpawnableEnv _spawnableEnv;
+    int _stepCountAtLastMeter;
+    public int lastXPosInMeters;
+    public int maxXPosInMeters;
+	float _pain;
+
+	List<float> distances;
+	float fraction;
+	bool _hasLazyInitialized;
+
+	override public void CollectObservations(VectorSensor sensor)
+	{
+		if (!_hasLazyInitialized)
+		{
+			OnEpisodeBegin();
+		}
+
+		Vector3 normalizedVelocity = _bodyManager.GetNormalizedVelocity();
+        var pelvis = _bodyManager.GetFirstBodyPart(BodyPartGroup.Hips);
+        var shoulders = _bodyManager.GetFirstBodyPart(BodyPartGroup.Torso);
+
+        sensor.AddObservation(normalizedVelocity); 
+        sensor.AddObservation(pelvis.Rigidbody.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.Rigidbody.transform.up);
+
+        sensor.AddObservation(shoulders.Rigidbody.transform.forward); // gyroscope 
+        sensor.AddObservation(shoulders.Rigidbody.transform.up);
+
+		sensor.AddObservation(_bodyManager.GetSensorIsInTouch());
+		foreach (var bodyPart in _bodyManager.BodyParts)
+		{
+			bodyPart.UpdateObservations();
+			sensor.AddObservation(bodyPart.ObsLocalPosition);
+			sensor.AddObservation(bodyPart.ObsRotation);
+			sensor.AddObservation(bodyPart.ObsRotationVelocity);
+			sensor.AddObservation(bodyPart.ObsVelocity);
+		}
+		sensor.AddObservation(_bodyManager.GetSensorObservations());
+        
+        (distances, fraction) = 
+            _terrainGenerator.GetDistances2d(
+                pelvis.Rigidbody.transform.position, _bodyManager.ShowMonitor);
+    
+        sensor.AddObservation(distances);
+        sensor.AddObservation(fraction);
+		// _bodyManager.OnCollectObservationsHandleDebug(GetInfo());
+	}
+
+	public override void OnActionReceived(ActionBuffers actions)
+    {
+        float[] vectorAction = actions.ContinuousActions.Select(x=>x).ToArray();
+
+		if (!_hasLazyInitialized)
+		{
+			return;
+		}
+		// apply actions to body
+		_bodyManager.OnAgentAction(vectorAction);
+
+		// manage reward
+        float velocity = Mathf.Clamp(_bodyManager.GetNormalizedVelocity().x, 0f, 1f);
+		var actionDifference = _bodyManager.GetActionDifference();
+		var actionsAbsolute = vectorAction.Select(x=>Mathf.Abs(x)).ToList();
+		var actionsAtLimit = actionsAbsolute.Select(x=> x>=1f ? 1f : 0f).ToList();
+		float actionaAtLimitCount = actionsAtLimit.Sum();
+        float notAtLimitBonus = 1f - (actionaAtLimitCount / (float) actionsAbsolute.Count);
+        float reducedPowerBonus = 1f - actionsAbsolute.Average();
+
+		// velocity *= 0.85f;
+		// reducedPowerBonus *=0f;
+		// notAtLimitBonus *=.1f;
+		// actionDifference *=.05f;
+        // var reward = velocity
+		// 				+ notAtLimitBonus
+		// 				+ reducedPowerBonus
+		// 				+ actionDifference;		
+        var reward = velocity;
+		AddReward(reward);
+		_bodyManager.SetDebugFrameReward(reward);
+
+        var pelvis = _bodyManager.GetFirstBodyPart(BodyPartGroup.Hips);
+		float xpos = 
+            _bodyManager.GetBodyParts(BodyPartGroup.Foot)
+            .Average(x=>x.Transform.position.x);
+		int newXPosInMeters = (int) xpos;
+        if (newXPosInMeters > lastXPosInMeters) {
+            lastXPosInMeters = newXPosInMeters;
+            _stepCountAtLastMeter = this.StepCount;
+        }
+		if (newXPosInMeters > maxXPosInMeters)
+			maxXPosInMeters = newXPosInMeters;
+		var terminate = false;
+		// bool isInBounds = _spawnableEnv.IsPointWithinBoundsInWorldSpace(pelvis.Transform.position);
+		// if (!isInBounds)
+        // if (pelvis.Rigidbody.transform.position.y < 0f)
+		if (_terrainGenerator.IsPointOffEdge(pelvis.Transform.position)){
+            terminate = true;
+            AddReward(-1f);
+		}
+        if (this.StepCount-_stepCountAtLastMeter >= (200*5))
+            terminate = true;
+		else if (xpos < 4f && _pain > 1f)
+            terminate = true;
+        else if (xpos < 2f && _pain > 0f)
+            terminate = true;
+		else if (_pain > 2f)
+            terminate = true;
+        if (terminate){
+			EndEpisode();
+		}
+        _pain = 0f;
+	}
+
+	public override void OnEpisodeBegin()
+	{
+		if (!_hasLazyInitialized)
+		{
+			_bodyManager = GetComponent<BodyManager002>();
+			_bodyManager.BodyConfig = MarathonManAgent.BodyConfig;
+			_bodyManager.OnInitializeAgent();
+			_hasLazyInitialized = true;
+		}
+
+		if (_bodyManager == null)
+			_bodyManager = GetComponent<BodyManager002>();
+		_bodyManager.OnAgentReset();
+        if (_terrainGenerator == null)
+            _terrainGenerator = GetComponent<TerrainGenerator>();
+		if (_spawnableEnv == null)
+			_spawnableEnv = GetComponentInParent<SpawnableEnv>();
+        _terrainGenerator.Reset();
+		lastXPosInMeters = (int)
+            _bodyManager.GetBodyParts(BodyPartGroup.Foot)
+            .Average(x=>x.Transform.position.x);
+        _pain = 0f;
+	}
+	public virtual void OnTerrainCollision(GameObject other, GameObject terrain)
+	{
+		// if (string.Compare(terrain.name, "Terrain", true) != 0)
+		if (terrain.GetComponent<Terrain>() == null)
+			return;
+		// if (!_styleAnimator.AnimationStepsReady)
+		// 	return;
+        // HACK - for when agent has not been initialized
+		if (_bodyManager == null)
+			return;
+		var bodyPart = _bodyManager.BodyParts.FirstOrDefault(x=>x.Transform.gameObject == other);
+		if (bodyPart == null)
+			return;
+		switch (bodyPart.Group)
+		{
+			case BodyHelper002.BodyPartGroup.None:
+			case BodyHelper002.BodyPartGroup.Foot:
+			case BodyHelper002.BodyPartGroup.LegLower:
+				break;
+			case BodyHelper002.BodyPartGroup.LegUpper:
+			case BodyHelper002.BodyPartGroup.Hand:
+			case BodyHelper002.BodyPartGroup.ArmLower:
+			case BodyHelper002.BodyPartGroup.ArmUpper:
+				_pain += .1f;
+				break;
+			default:
+				// AddReward(-100f);
+				_pain += 5f;
+				break;
+		}
+	}
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainMarathonManAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainWalkerAgent.cs

@@ -0,0 +1,159 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+
+public class TerrainWalkerAgent : MarathonAgent {
+
+    TerrainGenerator _terrainGenerator;
+
+    int _lastXPosInMeters;
+    int _stepCountAtLastMeter;
+    float _pain;
+    Vector3 _centerOfMass;
+
+    public override void OnEpisodeBegin()
+    {
+        base.OnEpisodeBegin();
+
+        BodyParts["pelvis"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "torso");
+        BodyParts["left_thigh"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "left_thigh");
+        BodyParts["right_thigh"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "right_thigh");
+        BodyParts["right_foot"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "right_foot");
+        BodyParts["left_foot"] = GetComponentsInChildren<Rigidbody>().FirstOrDefault(x => x.name == "left_foot");
+
+        SetCenterOfMass();
+
+        if (_terrainGenerator == null)
+            _terrainGenerator = GetComponent<TerrainGenerator>();
+        _lastXPosInMeters = (int) BodyParts["pelvis"].transform.position.x;
+        _terrainGenerator.Reset();
+        _stepCountAtLastMeter = 0;
+
+        // set to true this to show monitor while training
+        //Monitor.SetActive(true);
+
+        StepRewardFunction = StepRewardWalker106;
+        TerminateFunction = LocalTerminate;
+        ObservationsFunction = ObservationsDefault;
+        OnTerminateRewardValue = 0f;
+        _pain = 0f;
+
+        base.SetupBodyParts();
+        SetCenterOfMass();
+    }
+
+    bool LocalTerminate()
+    {
+        int newXPosInMeters = (int) BodyParts["pelvis"].transform.position.x;
+        if (newXPosInMeters > _lastXPosInMeters) {
+            _lastXPosInMeters = newXPosInMeters;
+            _stepCountAtLastMeter = this.StepCount;
+        }
+
+        SetCenterOfMass();
+        var xpos = _centerOfMass.x;
+        var terminate = false;
+        if (this.StepCount-_stepCountAtLastMeter >= (100*5))
+            terminate = true;
+        else if (xpos < 4f && _pain > 1f)
+            terminate = true;
+        else if (xpos < 2f && _pain > 0f)
+            terminate = true;
+
+        return terminate;
+    }
+    public override void OnTerrainCollision(GameObject other, GameObject terrain) {
+        if (terrain.GetComponent<Terrain>() == null)
+            return;
+
+        switch (other.name.ToLowerInvariant().Trim())
+        {
+            case "pelvis": // dm_hopper
+                _pain += 5f;
+                NonFootHitTerrain = true;
+                break;
+            case "right_leg": // dm_walker
+            case "left_leg": // dm_walker
+            case "right_foot": // dm_walker
+            case "left_foot": // dm_walker
+                FootHitTerrain = true;
+                break;
+            default:
+                _pain += 5f;
+                NonFootHitTerrain = true;
+                break;
+        }
+    }
+  
+    void ObservationsDefault(VectorSensor sensor)
+    {
+        var pelvis = BodyParts["pelvis"];
+        Vector3 normalizedVelocity = this.GetNormalizedVelocity(pelvis.velocity);
+        sensor.AddObservation(normalizedVelocity);
+        sensor.AddObservation(pelvis.transform.forward); // gyroscope 
+        sensor.AddObservation(pelvis.transform.up);
+
+        sensor.AddObservation(SensorIsInTouch);
+        JointRotations.ForEach(x => sensor.AddObservation(x));
+        sensor.AddObservation(JointVelocity);
+        // sensor.AddObservation(new []{
+        //     this.GetNormalizedPosition(BodyParts["left_foot"].transform.position).y,
+        //     this.GetNormalizedPosition(BodyParts["right_foot"].transform.position).y
+        // });
+
+        (List<float> distances, float fraction) = 
+            _terrainGenerator.GetDistances2d(
+                pelvis.transform.position, ShowMonitor);
+   
+        sensor.AddObservation(distances);
+        sensor.AddObservation(fraction);
+    }
+
+
+    void SetCenterOfMass()
+    {
+        _centerOfMass = Vector3.zero;
+        float c = 0f;
+        var bodyParts = this.gameObject.GetComponentsInChildren<Rigidbody>();
+ 
+        foreach (var part in bodyParts)
+        {
+            _centerOfMass += part.worldCenterOfMass * part.mass;
+            c += part.mass;
+        }
+        _centerOfMass /= c;
+    }
+
+    float StepRewardWalker106()
+    {
+        float uprightBonus = GetDirectionBonus("pelvis", Vector3.forward, 1f);
+        uprightBonus = Mathf.Clamp(uprightBonus, 0f, 1f);
+        float velocity = Mathf.Clamp(GetNormalizedVelocity("pelvis").x, 0f, 1f);
+        float effort = 1f - GetEffortNormalized();
+
+        //if (ShowMonitor)
+        //{
+        //    var hist = new[] {velocity, uprightBonus, effort}.ToList();
+        //    Monitor.Log("rewardHist", hist.ToArray(), displayType: Monitor.DisplayType.Independent);
+        //}
+
+        // uprightBonus *= 0.1f;
+        // velocity *= 0.45f;
+        // if (velocity >= .45f)
+        //     effort *= 0.45f;
+        // else
+        //     effort *= velocity;
+
+        // var reward = velocity
+        //              + uprightBonus
+        //              + effort;
+        var reward = velocity;
+        _pain = 0f;
+        return reward;
+    }
+}

Assets/3_MarathonEnvs/Scripts/OriginalAgents/TerrainWalkerAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: bdc7ea2698158f74b9e797210e567d04
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyConfig.cs

@@ -0,0 +1,11 @@
+using System;
+using static BodyHelper002;
+
+public class BodyConfig
+{
+	public Func<string, BodyPartGroup> GetBodyPartGroup;
+	public Func<string, MuscleGroup> GetMuscleGroup;
+	public Func<BodyPartGroup> GetRootBodyPart;
+	public Func<MuscleGroup> GetRootMuscle;
+
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyConfig.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyHelper002.cs

@@ -0,0 +1,33 @@
+
+public static class BodyHelper002
+{
+    [System.Serializable]
+    public enum BodyPartGroup {
+        None,
+        Hips,
+        Torso,
+        Spine,
+        Head,
+        ArmUpper,
+        ArmLower,
+        Hand,
+        LegUpper,
+        LegLower,
+        Foot,
+    }
+
+    [System.Serializable]
+    public enum MuscleGroup {
+        None,
+        Hips,
+        Torso,
+        Spine,
+        Head,
+        ArmUpper,
+        ArmLower,
+        Hand,
+        LegUpper,
+        LegLower,
+        Foot,
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyHelper002.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyManager002.cs

@@ -0,0 +1,594 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+using System.Linq;
+using static BodyHelper002;
+using System;
+using ManyWorlds;
+
+public class BodyManager002 : MonoBehaviour, IOnSensorCollision
+{
+    // Options / Configurables global properties
+	public Transform CameraTarget;
+	public float FixedDeltaTime = 0.005f;
+	public bool ShowMonitor = false;
+	public bool DebugDisableMotor;
+	public bool DebugShowWithOffset;
+
+
+    // Observations / Read only global properties
+	public List<Muscle002> Muscles;
+	public List<BodyPart002> BodyParts;
+	public List<float> SensorIsInTouch;
+	public List<float> Observations;
+	public int ObservationNormalizedErrors;
+	public int MaxObservationNormalizedErrors;
+	public List<GameObject> Sensors;
+	public float FrameReward;
+	public float AverageReward;
+
+
+    // private properties
+	Vector3 startPosition;
+
+	Dictionary<GameObject, Vector3> transformsPosition;
+	Dictionary<GameObject, Quaternion> transformsRotation;
+
+	Agent _agent;
+	SpawnableEnv _spawnableEnv;
+	TerrainGenerator _terrainGenerator;
+	DecisionRequester _decisionRequester;
+
+	static int _startCount;
+
+    float[] lastVectorAction;
+	float[] vectorDifference;
+	List <Vector3> mphBuffer;
+
+	[Tooltip("Max distance travelled across all episodes")]
+	/**< \brief Max distance travelled across all episodes*/
+	public float MaxDistanceTraveled;
+
+	[Tooltip("Distance travelled this episode")]
+	/**< \brief Distance travelled this episode*/
+	public float DistanceTraveled;
+
+	List<SphereCollider> sensorColliders;
+	static int _spawnCount;
+
+	// static ScoreHistogramData _scoreHistogramData;
+
+
+	// void FixedUpdate()
+	// {
+	// 	foreach (var muscle in Muscles)
+	// 	{
+	// 	// 	var i = Muscles.IndexOf(muscle);
+	// 	// 	muscle.UpdateObservations();
+	// 	// 	if (!DebugShowWithOffset && !DebugDisableMotor)
+	// 	// 		muscle.UpdateMotor();
+	// 	// 	if (!muscle.Rigidbody.useGravity)
+	// 	// 		continue; // skip sub joints
+	// 	// }
+	// }
+
+	public BodyConfig BodyConfig;
+
+	// Start is called before the first frame update
+	void Start()
+    {
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+    }
+
+    public void OnInitializeAgent()
+    {
+		_spawnableEnv = GetComponentInParent<SpawnableEnv>();
+		_terrainGenerator = GetComponentInParent<TerrainGenerator>();
+		SetupBody();
+		DistanceTraveled = float.MinValue;
+	}
+
+	public void OnAgentReset()
+	{
+		if (DistanceTraveled != float.MinValue)
+		{
+			var scorer = FindObjectOfType<Scorer>();
+			scorer?.ReportScore(DistanceTraveled, "Distance Traveled");
+		}
+		HandleModelReset();
+		Sensors = _agent.GetComponentsInChildren<SensorBehavior>()
+			.Select(x=>x.gameObject)
+			.ToList();
+		sensorColliders = Sensors
+			.Select(x=>x.GetComponent<SphereCollider>())
+			.ToList();
+		SensorIsInTouch = Enumerable.Range(0,Sensors.Count).Select(x=>0f).ToList();
+		// HACK first spawned agent should grab the camera
+		var smoothFollow = GameObject.FindObjectOfType<SmoothFollow>();
+		if (smoothFollow != null && smoothFollow.target == null) {
+			if (_spawnCount == 0) // HACK follow nth agent
+			{
+				smoothFollow.target = CameraTarget;
+				ShowMonitor = true;   
+			}
+			else
+				_spawnCount++;             
+		}
+		lastVectorAction = null;
+		vectorDifference = null;		
+		mphBuffer = new List<Vector3>();
+	}
+
+	public void OnAgentAction(float[] vectorAction)
+	{
+		if (lastVectorAction == null){
+			lastVectorAction = vectorAction.Select(x=>0f).ToArray();
+			vectorDifference = vectorAction.Select(x=>0f).ToArray();
+		}
+		int i = 0;
+		foreach (var muscle in Muscles)
+		{
+			// if(muscle.Parent == null)
+			// 	continue;
+			if (muscle.ConfigurableJoint.angularXMotion != ConfigurableJointMotion.Locked){
+				vectorDifference[i] = Mathf.Abs(vectorAction[i]-lastVectorAction[i]);
+				muscle.TargetNormalizedRotationX = vectorAction[i++];
+			}
+			if (muscle.ConfigurableJoint.angularYMotion != ConfigurableJointMotion.Locked){
+				vectorDifference[i] = Mathf.Abs(vectorAction[i]-lastVectorAction[i]);
+				muscle.TargetNormalizedRotationY = vectorAction[i++];
+			}
+			if (muscle.ConfigurableJoint.angularZMotion != ConfigurableJointMotion.Locked){
+				vectorDifference[i] = Mathf.Abs(vectorAction[i]-lastVectorAction[i]);
+				muscle.TargetNormalizedRotationZ = vectorAction[i++];
+			}
+			if (!DebugDisableMotor)
+				muscle.UpdateMotor();
+		}
+
+        if (ShowMonitor)
+        {
+            // var hist = new[] {velocity, uprightBonus, heightPenality, effort}.ToList();
+            // Monitor.Log("rewardHist", hist.ToArray(), displayType: Monitor.DisplayType.Independent);
+        }
+	}
+
+    public BodyPart002 GetFirstBodyPart(BodyPartGroup bodyPartGroup)
+    {
+        var bodyPart = BodyParts.FirstOrDefault(x=>x.Group == bodyPartGroup);
+        return bodyPart;
+    }
+    public List<BodyPart002> GetBodyParts()
+    {
+        return BodyParts;
+    }
+    public List<BodyPart002> GetBodyParts(BodyPartGroup bodyPartGroup)
+    {
+        return BodyParts.Where(x=>x.Group == bodyPartGroup).ToList();
+    }
+
+    public float GetActionDifference()
+    {
+		float actionDifference = 1f - vectorDifference.Average();
+		actionDifference = Mathf.Clamp(actionDifference, 0, 1);
+		actionDifference = Mathf.Pow(actionDifference,2);
+        return actionDifference;
+    }
+
+    void SetupBody()
+    {
+        _agent = GetComponent<Agent>();
+		_decisionRequester = GetComponent<DecisionRequester>();
+		Time.fixedDeltaTime = FixedDeltaTime;
+
+		BodyParts = new List<BodyPart002> ();
+		BodyPart002 root = null;
+		foreach (var t in GetComponentsInChildren<Transform>())
+		{
+			if (BodyConfig.GetBodyPartGroup(t.name) == BodyHelper002.BodyPartGroup.None)
+				continue;
+			
+			var bodyPart = new BodyPart002{
+				Rigidbody = t.GetComponent<Rigidbody>(),
+				Transform = t,
+				Name = t.name,
+				Group = BodyConfig.GetBodyPartGroup(t.name), 
+			};
+			if (bodyPart.Group == BodyConfig.GetRootBodyPart())
+				root = bodyPart;
+			bodyPart.Root = root;
+			bodyPart.Init();
+			BodyParts.Add(bodyPart);
+		}
+		var partCount = BodyParts.Count;
+
+		Muscles = new List<Muscle002> ();
+		var muscles = GetComponentsInChildren<ConfigurableJoint>();
+		ConfigurableJoint rootConfigurableJoint = null;
+		var ragDoll = GetComponent<RagDoll002>();
+		foreach (var m in muscles)
+		{
+			var maximumForce = ragDoll.MusclePowers.First(x=>x.Muscle == m.name).PowerVector;
+			maximumForce *= ragDoll.MotorScale;
+			var muscle = new Muscle002{
+				Rigidbody = m.GetComponent<Rigidbody>(),
+				Transform = m.GetComponent<Transform>(),
+				ConfigurableJoint = m,
+				Name = m.name,
+				Group = BodyConfig.GetMuscleGroup(m.name),
+				MaximumForce = maximumForce
+			};
+			if (muscle.Group == BodyConfig.GetRootMuscle())
+				rootConfigurableJoint = muscle.ConfigurableJoint;
+			muscle.RootConfigurableJoint = rootConfigurableJoint;
+			muscle.Init();
+
+			Muscles.Add(muscle);			
+		}
+		_startCount++;        
+    }
+
+
+
+	void HandleModelReset()
+	{
+		Transform[] allChildren = _agent.GetComponentsInChildren<Transform>();
+		if (transformsPosition != null)
+		{
+			foreach (var child in allChildren)
+			{
+				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;
+				}
+			}
+
+		}
+		else
+		{
+			startPosition = _agent.transform.position;
+			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;
+			}
+		}
+	}
+
+	public float GetHeightNormalizedReward(float maxHeight)
+	{
+		var height = GetHeight();
+		var heightPenality = maxHeight - height;
+		heightPenality = Mathf.Clamp(heightPenality, 0f, maxHeight);
+		var reward = 1f - heightPenality;
+		reward = Mathf.Clamp(reward, 0f, 1f);
+		return reward;
+	}
+	internal float GetHeight()
+	{
+		var feetYpos = BodyParts
+			.Where(x => x.Group == BodyPartGroup.Foot)
+			.Select(x => x.Transform.position.y)
+			.OrderBy(x => x)
+			.ToList();
+		float lowestFoot = 0f;
+		if (feetYpos != null && feetYpos.Count != 0)
+			lowestFoot = feetYpos[0];
+		var height = GetFirstBodyPart(BodyPartGroup.Head).Transform.position.y - lowestFoot;
+		return height;
+	}
+	public float GetDirectionNormalizedReward(BodyPartGroup bodyPartGroup, Vector3 direction)
+	{
+		BodyPart002 bodyPart = GetFirstBodyPart(bodyPartGroup);
+		float maxBonus = 1f;
+		var toFocalAngle = bodyPart.ToFocalRoation * 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;
+	}
+
+	public float GetUprightNormalizedReward(BodyPartGroup bodyPartGroup)
+	{
+		BodyPart002 bodyPart = GetFirstBodyPart(bodyPartGroup);
+		float maxBonus = 1f;
+		var toFocalAngle = bodyPart.ToFocalRoation * -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;
+	}
+	public float GetEffortNormalized(string[] ignorJoints = null)
+	{
+		double effort = 0;
+		double jointEffort = 0;
+		double joints = 0;
+		foreach (var muscle in Muscles)
+		{
+			if(muscle.Parent == null)
+				continue;
+			var name = muscle.Name;
+			if (ignorJoints != null && ignorJoints.Contains(name))
+				continue;
+			if (muscle.ConfigurableJoint.angularXMotion != ConfigurableJointMotion.Locked) {
+				jointEffort = Mathf.Pow(Mathf.Abs(muscle.TargetNormalizedRotationX),2);
+				effort += jointEffort;
+				joints++;
+			}
+			if (muscle.ConfigurableJoint.angularYMotion != ConfigurableJointMotion.Locked) {
+				jointEffort = Mathf.Pow(Mathf.Abs(muscle.TargetNormalizedRotationY),2);
+				effort += jointEffort;
+				joints++;
+			}
+			if (muscle.ConfigurableJoint.angularZMotion != ConfigurableJointMotion.Locked) {
+				jointEffort = Mathf.Pow(Mathf.Abs(muscle.TargetNormalizedRotationZ),2);
+				effort += jointEffort;
+				joints++;
+			}
+		}
+
+		return (float) (effort / joints);
+	}
+	public void OnSensorCollisionEnter(Collider sensorCollider, GameObject other) {
+		// if (string.Compare(other.name, "Terrain", true) !=0)
+		if (other.GetComponent<Terrain>() == null)
+			return;
+		var sensor = Sensors
+			.FirstOrDefault(x=>x == sensorCollider.gameObject);
+		if (sensor != null) {
+			var idx = Sensors.IndexOf(sensor);
+			SensorIsInTouch[idx] = 1f;
+		}
+	}
+	public void OnSensorCollisionExit(Collider sensorCollider, GameObject other)
+	{
+		// if (string.Compare(other.gameObject.name, "Terrain", true) !=0)
+		if (other.GetComponent<Terrain>() == null)
+			return;
+		var sensor = Sensors
+			.FirstOrDefault(x=>x == sensorCollider.gameObject);
+		if (sensor != null) {
+			var idx = Sensors.IndexOf(sensor);
+			SensorIsInTouch[idx] = 0f;
+		}
+	}  
+	public Vector3 GetLocalCenterOfMass()
+    {
+        var centerOfMass = GetCenterOfMass();
+		centerOfMass -= transform.position;
+        return centerOfMass;
+    }
+	public Vector3 GetCenterOfMass()
+	{
+		var centerOfMass = Vector3.zero;
+		float totalMass = 0f;
+		var bodies = BodyParts
+			.Select(x=>x.Rigidbody)
+			.Where(x=>x!=null)
+			.ToList();
+		foreach (Rigidbody rb in bodies)
+		{
+			centerOfMass += rb.worldCenterOfMass * rb.mass;
+			totalMass += rb.mass;
+		}
+		centerOfMass /= totalMass;
+		return centerOfMass;
+	}
+
+    public Vector3 GetNormalizedVelocity()
+    {
+        var pelvis = GetFirstBodyPart(BodyConfig.GetRootBodyPart()); 
+        Vector3 metersPerSecond = pelvis.Rigidbody.velocity;
+        var n = GetNormalizedVelocity(metersPerSecond);
+        return n;
+    }
+
+    public Vector3 GetNormalizedPosition()
+    {
+		// var position = GetCenterOfMass();
+        var pelvis = GetFirstBodyPart(BodyConfig.GetRootBodyPart()); 
+		var position = pelvis.Transform.position;
+		var normalizedPosition = GetNormalizedPosition(position - startPosition);
+        return normalizedPosition;
+    }
+    public void SetDebugFrameReward(float reward)
+	{
+		FrameReward = reward;
+		var stepCount = _agent.StepCount > 0 ? _agent.StepCount : 1;
+		if (_decisionRequester?.DecisionPeriod > 1)
+			stepCount /= _decisionRequester.DecisionPeriod;
+		AverageReward = _agent.GetCumulativeReward() / (float) stepCount;		
+	}
+
+
+    public List<float> GetSensorIsInTouch()
+    {
+        return SensorIsInTouch;
+    }
+    // public List<float> GetBodyPartsObservations()
+    // {
+    //     List<float> vectorObservation = new List<float>();
+	// 	foreach (var bodyPart in BodyParts)
+	// 	{
+	// 		bodyPart.UpdateObservations();
+	// 		// _agent.sensor.AddObservation(bodyPart.ObsRotation);
+    //         vectorObservation.Add(bodyPart.ObsRotation.x);
+    //         vectorObservation.Add(bodyPart.ObsRotation.y);
+    //         vectorObservation.Add(bodyPart.ObsRotation.z);
+    //         vectorObservation.Add(bodyPart.ObsRotation.w);
+
+	// 		// _agent.sensor.AddObservation(bodyPart.ObsRotationVelocity);
+    //         vectorObservation.Add(bodyPart.ObsRotationVelocity.x);
+    //         vectorObservation.Add(bodyPart.ObsRotationVelocity.y);
+    //         vectorObservation.Add(bodyPart.ObsRotationVelocity.z);
+
+	// 		// _agent.sensor.AddObservation(GetNormalizedVelocity(bodyPart.ObsVelocity));
+    //         var normalizedVelocity = GetNormalizedVelocity(bodyPart.ObsVelocity);
+    //         vectorObservation.Add(normalizedVelocity.x);
+    //         vectorObservation.Add(normalizedVelocity.y);
+    //         vectorObservation.Add(normalizedVelocity.z);
+	// 	}
+    //     return vectorObservation;
+    // }
+    public List<float> GetMusclesObservations()
+    {
+        List<float> vectorObservation = new List<float>();
+		foreach (var muscle in Muscles)
+		{
+			muscle.UpdateObservations();
+			if (muscle.ConfigurableJoint.angularXMotion != ConfigurableJointMotion.Locked)
+				vectorObservation.Add(muscle.TargetNormalizedRotationX);
+			if (muscle.ConfigurableJoint.angularYMotion != ConfigurableJointMotion.Locked)
+				vectorObservation.Add(muscle.TargetNormalizedRotationY);
+			if (muscle.ConfigurableJoint.angularZMotion != ConfigurableJointMotion.Locked)
+				vectorObservation.Add(muscle.TargetNormalizedRotationZ);
+        }        
+        return vectorObservation;
+    }
+	[Obsolete("use GetSensorObservations()")]
+    public List<float> GetSensorYPositions()
+    {
+		var sensorYpositions = Sensors
+			.Select(x=> this.GetNormalizedPosition(x.transform.position - startPosition))
+			.Select(x=>x.y)
+			.ToList();
+        return sensorYpositions;
+    }
+	[Obsolete("use GetSensorObservations()")]
+	public List<float> GetSensorZPositions()
+    {
+		var sensorYpositions = Sensors
+			.Select(x=> this.GetNormalizedPosition(x.transform.position - startPosition))
+			.Select(x=>x.z)
+			.ToList();
+        return sensorYpositions;
+    }
+
+	public List<float> GetSensorObservations()
+	{
+		var localSensorsPos = new Vector3[Sensors.Count];
+		var globalSensorsPos = new Vector3[Sensors.Count];
+		for (int i = 0; i < Sensors.Count; i++) {
+			globalSensorsPos[i] = sensorColliders[i].transform.TransformPoint(sensorColliders[i].center);
+			localSensorsPos[i] = globalSensorsPos[i] - startPosition;
+		}
+
+		// get heights based on global senor position
+		var sensorsPos = Sensors
+			.Select(x=>x.transform.position).ToList();
+		var senorHeights = _terrainGenerator != null
+			? _terrainGenerator.GetDistances2d(globalSensorsPos)
+			: Enumerable.Range(0, globalSensorsPos.Length).Select(x=>0f).ToList();
+		for (int i = 0; i < Sensors.Count; i++) {
+			senorHeights[i] -= sensorColliders[i].radius;
+			if (senorHeights[i] >= 1f)
+				senorHeights[i] = 1f;
+		}
+			
+		// get z positions based on local positions
+		var bounds = _spawnableEnv.bounds;
+		var normalizedZ = localSensorsPos
+			.Select(x=>x.z / (bounds.extents.z))
+			.ToList();
+		var observations = senorHeights
+			.Concat(normalizedZ)
+			.ToList();
+		return observations;
+	}
+
+    // public void OnCollectObservationsHandleDebug(AgentInfo info)
+    // {
+	// 	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;  
+
+    //     var pelvis = GetFirstBodyPart(BodyPartGroup.Hips);
+	// 	DistanceTraveled = pelvis.Transform.position.x;
+	// 	MaxDistanceTraveled = Mathf.Max(MaxDistanceTraveled, DistanceTraveled);
+    //     Vector3 metersPerSecond = pelvis.Rigidbody.velocity;
+	// 	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", MaxDistanceTraveled.ToString());
+	// 		Monitor.Log("NormalizedPos", GetNormalizedPosition().ToString());
+	// 		Monitor.Log("MPH: ", (aveMph).ToString());
+	// 	}            
+    // }  
+
+	float NextGaussian(float mu = 0, float sigma = 1)
+	{
+		var u1 = UnityEngine.Random.value;
+		var u2 = UnityEngine.Random.value;
+
+		var rand_std_normal = Mathf.Sqrt(-2.0f * Mathf.Log(u1)) *
+							Mathf.Sin(2.0f * Mathf.PI * u2);
+
+		var rand_normal = mu + sigma * rand_std_normal;
+
+		return rand_normal;
+	}
+	public Vector3 GetNormalizedVelocity(Vector3 metersPerSecond)
+	{
+		var maxMetersPerSecond = _spawnableEnv.bounds.size
+			/ _agent.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;
+	}
+	public Vector3 GetNormalizedPosition(Vector3 pos)
+	{
+		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;
+	}
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyManager002.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyPart002.cs

@@ -0,0 +1,239 @@
+// A class that describes a body part of an Agent or an Animator. The object
+// stores the rigid body associated with an Agent's body part. Along with some
+// important data like root bone, i. e. 'butt' for a marathonMan. Implments methods
+// to calculate distance to the characteristics of an animation being mimiced.
+
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+using Unity.MLAgents;
+
+
+[System.Serializable]
+public class BodyPart002
+{
+    
+    public string Name;
+    public BodyHelper002.BodyPartGroup Group;
+
+    public Vector3 ObsLocalPosition;
+    public Quaternion ObsRotation;
+    public Quaternion ObsRotationFromBase;
+    public Vector3 ObsRotationVelocity;
+    public Vector3 ObsVelocity;
+    public float ObsAngleDeltaFromAnimationRotation;
+    public Vector3 ObsDeltaFromAnimationPosition;
+
+    public Vector3 ObsDeltaFromAnimationVelocity;
+    public Vector3 ObsDeltaFromAnimationAngularVelocity;
+    public Vector3 ObsDeltaFromAnimationAngularVelocityWorld;
+    public Vector3 DebugMaxRotationVelocity;
+    public Vector3 DebugMaxVelocity;
+
+    public Quaternion DefaultLocalRotation;
+    public Quaternion ToJointSpaceInverse;
+    public Quaternion ToJointSpaceDefault;
+
+    public Rigidbody Rigidbody;
+    public Transform Transform;
+    public BodyPart002 Root;
+    public Quaternion InitialRootRotation;
+    public Vector3 InitialRootPosition;
+
+    // base = from where to measure rotation and position from
+    public Quaternion BaseRotation;
+    public Vector3 BasePosition;
+    //
+    public Quaternion ToFocalRoation;
+
+
+    Quaternion _lastObsRotation;
+    Quaternion _lastWorldRotation;
+    Vector3 _lastLocalPosition;
+    Vector3 _lastWorldPosition;
+    Vector3 _animationAngularVelocity;
+    Vector3 _animationAngularVelocityWorld;
+    Vector3 _animationVelocityWorld;
+
+    DecisionRequester _decisionRequester;
+
+    float _lastUpdateObsTime;
+    bool _firstRunComplete;
+    bool _hasRanVeryFirstInit;
+    private Vector3 _animationPositionWorld;
+    private Quaternion _animationRotation;
+
+    static Vector3 Vector3Max (Vector3 a, Vector3 b)
+    {
+        var answer = new Vector3(
+            Mathf.Max(Mathf.Abs(a.x), Mathf.Abs(b.x)),
+            Mathf.Max(Mathf.Abs(a.y), Mathf.Abs(b.y)),
+            Mathf.Max(Mathf.Abs(a.z), Mathf.Abs(b.z)));
+        return answer;
+    }
+
+    // Initialize
+    public void Init()
+    {
+        _decisionRequester = GameObject.Find("MarathonMan").GetComponent<DecisionRequester>();
+
+        _firstRunComplete = false;
+        if (Rigidbody != null){
+            Rigidbody.angularVelocity = Vector3.zero;
+            Rigidbody.velocity = Vector3.zero;
+        }
+
+        if (!_hasRanVeryFirstInit) {
+
+            InitialRootRotation = Root.Transform.transform.rotation;
+            InitialRootPosition = Root.Transform.transform.position;
+            BaseRotation = Root.Transform.transform.rotation;
+            BasePosition = Root.Transform.transform.position;
+
+			DefaultLocalRotation = LocalRotation;
+            Vector3 forward = this.Transform.forward;
+            Vector3 up = this.Transform.up;
+			Quaternion toJointSpace = Quaternion.LookRotation(forward, up);
+			
+			ToJointSpaceInverse = Quaternion.Inverse(toJointSpace);
+			ToJointSpaceDefault = DefaultLocalRotation * toJointSpace;
+
+    		// set body part direction
+			Vector3 focalOffset = new Vector3(10,0,0);
+            if (Rigidbody != null){
+                var focalPoint = Rigidbody.position + focalOffset;
+                ToFocalRoation = Rigidbody.rotation;
+                ToFocalRoation.SetLookRotation(focalPoint - Rigidbody.position);
+            }
+
+            _hasRanVeryFirstInit = true;
+
+        }
+    }
+
+    // Update values like ObsLocalPosition, ObsRotation, ... that will be accessed
+    // by the agent as observations for the neural network. Also calculates distances
+    // to an animation being mimicked
+    public void UpdateObservations()
+    {
+        Quaternion rotation;
+        Vector3 position;
+        if (this == Root) {
+            rotation = Quaternion.Inverse(InitialRootRotation) * Transform.rotation;
+            position =  Transform.position - InitialRootPosition;
+        }
+        else {
+            rotation = Quaternion.Inverse(Root.Transform.rotation) * Transform.rotation;
+            position =  Transform.position - Root.Transform.position;
+        }
+        
+        if (_firstRunComplete == false){
+            _lastUpdateObsTime = Time.time;
+            _lastLocalPosition = position;
+            _lastWorldPosition = Transform.position;
+            _lastObsRotation = rotation;
+            _lastWorldRotation = Transform.rotation;
+        }
+
+        var dt = Time.fixedDeltaTime * _decisionRequester.DecisionPeriod;
+
+        var velocity = (position - _lastLocalPosition)/dt;
+        var velocityWorld = (Transform.position - _lastWorldPosition)/dt;
+        var angularVelocity = JointHelper002.CalcDeltaRotationNormalizedEuler(_lastObsRotation, rotation)/dt;
+        var angularVelocityWorld = JointHelper002.CalcDeltaRotationNormalizedEuler(_lastWorldRotation, Transform.rotation)/dt;
+
+        // old calulation for observation vector
+        //angularVelocity = NormalizedEulerAngles(rotationVelocity.eulerAngles);
+        //angularVelocity /= 128f;
+        // old calculation end
+
+        _lastUpdateObsTime = Time.time;
+        _lastLocalPosition = position;
+        _lastWorldPosition = Transform.position;
+        _lastObsRotation = rotation;
+        _lastWorldRotation = Transform.rotation;
+
+        //if (Name == "right_right_foot") {
+        //    Debug.Log("^^^^^^^^^^^^");
+        //    Debug.Log("body part name: " + Name);
+        //    Debug.Log("animation angular velocity:" + _animationAngularVelocity);
+        //    Debug.Log("angular velocity:" + angularVelocity);
+        //    Debug.Log("animation angular velocity world:" + _animationAngularVelocityWorld);
+        //    Debug.Log("angular velocity world:" + angularVelocityWorld);
+        //    Debug.Log("rotation local:" + rotation.eulerAngles);
+        //    Debug.Log("animation rotation local: " + _animationRotation.eulerAngles);
+        //    Debug.Log("velocity world: " + velocityWorld);
+        //    Debug.Log("animation velocity world:" + _animationVelocityWorld);
+        //    Debug.Log("transform position:" + Transform.position);
+        //    Debug.Log("animation position world: " + _animationPositionWorld);
+        //    Debug.Log("dt:" + dt);
+        //}
+
+        ObsLocalPosition = position;
+        ObsRotation = rotation;
+        ObsRotationVelocity = angularVelocity;
+        ObsVelocity = velocity;
+
+        ObsDeltaFromAnimationPosition = _animationPositionWorld - Transform.position;
+
+        ObsAngleDeltaFromAnimationRotation = Quaternion.Angle(_animationRotation, rotation);
+        ObsAngleDeltaFromAnimationRotation = JointHelper002.NormalizedAngle(ObsAngleDeltaFromAnimationRotation);  
+
+        ObsDeltaFromAnimationVelocity = _animationVelocityWorld - velocityWorld;
+        ObsDeltaFromAnimationAngularVelocity = (_animationAngularVelocity - angularVelocity);
+        ObsDeltaFromAnimationAngularVelocityWorld = (_animationAngularVelocityWorld - angularVelocityWorld);
+
+        DebugMaxRotationVelocity = Vector3Max(DebugMaxRotationVelocity, angularVelocity);
+        DebugMaxVelocity = Vector3Max(DebugMaxVelocity, velocity);
+
+        _firstRunComplete = true;
+    }
+
+    // returns local rotation
+    public Quaternion LocalRotation {
+        get {
+            return Quaternion.Inverse(RootRotation) * Transform.rotation;
+        }
+    }
+
+    // retuns root rotation
+    public Quaternion RootRotation{
+        get {
+            return InitialRootRotation;
+        }
+    }
+
+    // Set the position, rotation of a body part to match animation's position and
+    // rotation. Also sets the rigid body's position and velocity, which is used
+    // in angular momentum calculation
+    public void MoveToAnim(Vector3 animPosition, Quaternion animRotation, Vector3 angularVelocity, Vector3 velocity)
+    {
+        Transform.position = animPosition;
+        Transform.rotation = animRotation;
+        if (Rigidbody != null){
+            foreach (var childRb in Rigidbody.GetComponentsInChildren<Rigidbody>())
+            {
+                if (childRb == Rigidbody)
+                    continue;
+                childRb.transform.localPosition = Vector3.zero;
+                childRb.transform.localEulerAngles = Vector3.zero;
+                childRb.angularVelocity = Vector3.zero;
+                childRb.velocity = Vector3.zero;
+            }
+            Rigidbody.angularVelocity = angularVelocity;
+            Rigidbody.velocity = velocity;
+        }
+    }
+
+    // Set the position of the animation being mimicked
+    public void SetAnimationPosition(Vector3 animPositionWorld, Quaternion animRotationLocal, Vector3 animVelocityWorld, Vector3 animAngularVelocityLocal, Vector3 animAngularVelocityWorld)
+    {
+        _animationPositionWorld = animPositionWorld;
+        _animationRotation = animRotationLocal;
+
+        _animationVelocityWorld = animVelocityWorld;
+        _animationAngularVelocity = animAngularVelocityLocal;
+        _animationAngularVelocityWorld = animAngularVelocityWorld;
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/BodyPart002.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/DomainRandomization.cs

@@ -0,0 +1,18 @@
+using System.Collections;
+using System.Collections.Generic;
+using Unity.MLAgents;
+using UnityEngine;
+
+public class DomainRandomization : MonoBehaviour
+{
+    // Start is called before the first frame update
+    void Start()
+    {
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/DomainRandomization.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/JointHelper002.cs

@@ -0,0 +1,107 @@
+// Helper Utilities to work with agent's rigid bodies charateristics. Allows to
+// calculate Angles between rotations in radians, find center of mass of an agent,
+// and find Angular Momentum of an agent. 
+
+using UnityEngine;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using System;
+
+public static class JointHelper002 {
+
+    // Return rotation from one rotation to another
+    public static Quaternion FromToRotation(Quaternion from, Quaternion to) {
+        if (to == from) return Quaternion.identity;
+
+        return to * Quaternion.Inverse(from);
+    }
+
+    // Calculate rotation between two rotations in radians. Adjusts the value to lie within [-pi, +pi].
+    public static Vector3 NormalizedEulerAngles(Vector3 eulerAngles) {
+        var x = eulerAngles.x < 180f ?
+            eulerAngles.x :
+            -360 + eulerAngles.x;
+        var y = eulerAngles.y < 180f ?
+            eulerAngles.y :
+            -360 + eulerAngles.y;
+        var z = eulerAngles.z < 180f ?
+            eulerAngles.z :
+            -360 + eulerAngles.z;
+        x = x * Mathf.Deg2Rad;
+        y = y * Mathf.Deg2Rad;
+        z = z * Mathf.Deg2Rad;
+        return new Vector3(x, y, z);
+    }
+
+    // Adjust the value of an angle to lie within [-pi, +pi].
+    public static float NormalizedAngle(float angle) {
+        if (angle < 180) {
+            return angle * Mathf.Deg2Rad;
+        }
+        return (angle - 360) * Mathf.Deg2Rad;
+    }
+
+    // Find rotation and convert to radians within [-pi, +pi].
+    public static Vector3 CalcDeltaRotationNormalizedEuler(Quaternion from, Quaternion to) {
+        var rotationVelocity = FromToRotation(from, to);
+        var angularVelocity = NormalizedEulerAngles(rotationVelocity.eulerAngles);
+        return angularVelocity;
+    }
+
+    // Find the center of mass of a list of Body Parts beloning to an agent. Relative to the root bone, i. e. "butt" for humanoid. 
+    public static Vector3 GetCenterOfMassRelativeToRoot(List<BodyPart002> BodyParts) {
+        var centerOfMass = Vector3.zero;
+        float totalMass = 0f;
+        var bodies = BodyParts
+            .Select(x => x.Rigidbody)
+            .Where(x => x != null)
+            .ToList();
+        var rootBone = BodyParts[0];
+        foreach (Rigidbody rb in bodies) {
+            centerOfMass += rb.worldCenterOfMass * rb.mass;
+            totalMass += rb.mass;
+        }
+        centerOfMass /= totalMass;
+        centerOfMass -= rootBone.InitialRootPosition;
+        return centerOfMass;
+    }
+
+    // Find the center of mass of a List of Body Parts relative to the world coordinate system. 
+    public static Vector3 GetCenterOfMassWorld(List<BodyPart002> BodyParts) {
+        var centerOfMass = GetCenterOfMassRelativeToRoot(BodyParts) + BodyParts[0].InitialRootPosition;
+        return centerOfMass;
+    }
+
+    // Calculate Angular Momentum of a List of Body Parts. In the world coordinate system about the center
+    // of mass of the Body Parts. Formulas at https://ocw.mit.edu/courses/aeronautics-and-astronautics/16-07-dynamics-fall-2009/lecture-notes/MIT16_07F09_Lec11.pdf
+    public static Vector3 GetAngularMoment(List<BodyPart002> BodyParts) {
+        var centerOfMass = GetCenterOfMassWorld(BodyParts);
+        var bodies = BodyParts
+            .Select(x => x.Rigidbody)
+            .Where(x => x != null)
+            .ToList();
+        Vector3 totalAngularMoment = Vector3.zero;
+        foreach (Rigidbody rb in bodies) {
+
+            var w_local = rb.transform.rotation * rb.angularVelocity;
+            var w_inertiaFrame = rb.inertiaTensorRotation * w_local;
+
+            Vector3 L_inertiaFrame = Vector3.zero;
+            L_inertiaFrame[0] = w_inertiaFrame[0] * rb.inertiaTensor[0];
+            L_inertiaFrame[1] = w_inertiaFrame[1] * rb.inertiaTensor[1];
+            L_inertiaFrame[2] = w_inertiaFrame[2] * rb.inertiaTensor[2];
+
+            Vector3 L_world = Quaternion.Inverse(rb.transform.rotation) * Quaternion.Inverse(rb.inertiaTensorRotation) * L_inertiaFrame;
+
+            Vector3 bodyPartCenterOfMassRelativeTobodyPartsCenterOfMass = rb.worldCenterOfMass - centerOfMass;
+            Vector3 LofBodyPartCenterOfMass = rb.mass * Vector3.Cross(bodyPartCenterOfMassRelativeTobodyPartsCenterOfMass, rb.velocity);
+
+            totalAngularMoment += L_world + LofBodyPartCenterOfMass;
+
+        }
+        return totalAngularMoment;
+    }
+}
+
+

Assets/3_MarathonEnvs/Scripts/Ragdoll002/JointHelper002.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonHelper.cs

@@ -0,0 +1,195 @@
+using System.Linq;
+using UnityEngine;
+
+namespace Unity.MLAgents
+{
+    public static class MarathonHelper
+    {
+        static readonly bool FlipMarathonX = false;
+
+        public static Vector3 RightToLeft(Vector3 rightHanded, bool hackFlipZ = false)
+        {
+            if (FlipMarathonX)
+                return new Vector3(rightHanded.x, rightHanded.z,
+                    rightHanded.y); // use if fliping marathon's X direction
+            return new Vector3(-rightHanded.x, rightHanded.z, -rightHanded.y); // use to maintain marathon's X direction
+        }
+
+        static char[] _delimiterChars = {' ', ',', ':', '\t'};
+
+        static string RemoveDuplicateWhitespace(string input)
+        {
+            while (input.Contains("  "))
+                input = input.Replace("  ", " ");
+            while (input.Contains("\t\t"))
+                input = input.Replace("\t\t", "\t");
+            return input;
+        }
+
+        static float Evaluate(string expression)
+        {
+            var doc = new System.Xml.XPath.XPathDocument(new System.IO.StringReader("<r/>"));
+            var nav = doc.CreateNavigator();
+            var newString = expression;
+            newString = (new System.Text.RegularExpressions.Regex(@"([\+\-\*])")).Replace(newString, " ${1} ");
+            newString = newString.Replace("/", " div ").Replace("%", " mod ");
+            var res = nav.Evaluate("number(" + newString + ")");
+            double d = (double) res;
+            return (float) d;
+        }
+
+
+        static public Vector3 ParseVector3NoFlipYZ(string str)
+        {
+            str = RemoveDuplicateWhitespace(str);
+            string[] words = str.Split(_delimiterChars);
+            float x = Evaluate(words[0]);
+            float y = Evaluate(words[1]);
+            float z = Evaluate(words[2]);
+            var vec3 = new Vector3(x, y, z);
+            return vec3;
+        }
+
+        static public Quaternion ParseQuaternion(string str)
+        {
+            str = RemoveDuplicateWhitespace(str);
+            string[] words = str.Split(_delimiterChars);
+            float w = Evaluate(words[0]);
+            float x = Evaluate(words[1]);
+            float y = Evaluate(words[2]);
+            float z = Evaluate(words[3]);
+            var q = new Quaternion(x, y, z, w);
+            return q;
+        }
+
+        static public Vector3 ParseAxis(string str)
+        {
+            var axis = MarathonHelper.ParseVector3NoFlipYZ(str);
+            if (FlipMarathonX)
+                axis = new Vector3(-axis.x, -axis.z, -axis.y); // use if fliping marathon's X direction
+            else
+                axis = new Vector3(axis.x, -axis.z, axis.y); // use to maintain marathon's X direction
+            return axis;
+        }
+
+        static public Vector3 JointParsePosition(string str, bool hackFlipZ)
+        {
+            str = RemoveDuplicateWhitespace(str);
+            string[] words = str.Split(_delimiterChars);
+            float x = Evaluate(words[0]);
+            float y = Evaluate(words[1]);
+            float z = Evaluate(words[2]);
+            Vector3 vec3 = new Vector3(x, y, z);
+            return RightToLeft(vec3, hackFlipZ);
+        }
+
+        public static Vector3 ParsePosition(string str)
+        {
+            str = RemoveDuplicateWhitespace(str);
+            string[] words = str.Split(_delimiterChars);
+            float x = Evaluate(words[0]);
+            float y = Evaluate(words[1]);
+            float z = Evaluate(words[2]);
+            Vector3 vec3 = new Vector3(x, y, z);
+            return RightToLeft(vec3);
+        }
+
+        public static Vector3 ParseFrom(string fromTo)
+        {
+            return ParseVector3NoFlipYZ(fromTo);
+        }
+
+        public static Vector3 ParseTo(string fromTo)
+        {
+            fromTo = RemoveDuplicateWhitespace(fromTo);
+            string[] words = fromTo.Split(_delimiterChars);
+            float x = Evaluate(words[3]);
+            float y = Evaluate(words[4]);
+            float z = Evaluate(words[5]);
+            Vector3 vec3 = new Vector3(x, y, z);
+            return vec3;
+        }
+
+        public static Vector2 ParseVector2(string str)
+        {
+            str = RemoveDuplicateWhitespace(str);
+            string[] words = str.Split(_delimiterChars);
+            float x = Evaluate(words[0]);
+            float y = Evaluate(words[1]);
+            var vec2 = new Vector2(x, y);
+            return vec2;
+        }
+
+        public static float ParseGetMin(string rangeAsText)
+        {
+            rangeAsText = RemoveDuplicateWhitespace(rangeAsText);
+            string[] words = rangeAsText.Split(_delimiterChars);
+            var range = words.Select(x => Evaluate(x));
+            return range.Min();
+        }
+
+        public static float ParseGetMax(string rangeAsText)
+        {
+            rangeAsText = RemoveDuplicateWhitespace(rangeAsText);
+            string[] words = rangeAsText.Split(_delimiterChars);
+            var range = words.Select(x => Evaluate(x));
+            return range.Max();
+        }
+
+
+        public static GameObject CreateBetweenPoints(this GameObject parent, Vector3 start, Vector3 end, float width,
+            bool useWorldSpace, GameObject root)
+        {
+            start = RightToLeft(start);
+            end = RightToLeft(end);
+            var instance = new GameObject();
+            var procCap = instance.AddComponent<ProceduralCapsule>();
+            var handleOverlap = instance.AddComponent<HandleOverlap>();
+            handleOverlap.Parent = root;
+            var collider = instance.AddComponent<CapsuleCollider>();
+            var offset = start - end;
+            var position = start - (offset / 2.0f);
+            var height = offset.magnitude;
+            collider.height = height + (width * 2) * .90f;
+            collider.radius = width * .90f;
+            procCap.height = height + (width);
+            procCap.radius = width;
+            procCap.CreateMesh();
+
+            instance.transform.parent = root.transform;
+            instance.transform.up = offset;
+            if (useWorldSpace)
+            {
+                instance.transform.position = position;
+            }
+            else
+            {
+                instance.transform.position = position + parent.transform.position;
+                instance.transform.rotation = instance.transform.rotation * parent.transform.rotation;
+            }
+
+            // UnityEngine.GameObject.Destroy(handleOverlap);
+            return instance;
+        }
+
+        public static GameObject CreateAtPoint(this GameObject parent, Vector3 position, float width,
+            bool useWorldSpace, GameObject root)
+        {
+            var scale = new Vector3(width, width, width);
+            var instance = GameObject.CreatePrimitive(PrimitiveType.Sphere);
+            instance.transform.parent = root.transform;
+            instance.transform.localScale = scale * 2;
+            if (useWorldSpace)
+            {
+                instance.transform.position = position;
+            }
+            else
+            {
+                instance.transform.position = position + parent.transform.position;
+                instance.transform.rotation = instance.transform.rotation * parent.transform.rotation;
+            }
+
+            return instance;
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonHelper.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonJoint.cs

@@ -0,0 +1,18 @@
+using UnityEngine;
+
+namespace Unity.MLAgents
+{
+    [System.Serializable]
+    public class MarathonJoint
+    {
+        public Joint Joint;
+        public string Name;
+        public string JointName;
+        public Vector2 CtrlRange;
+        public bool? CtrlLimited;
+        public float? Gear;
+        public ConfigurableJoint TrueBase;
+        public Transform TrueTarget;
+        public float MaximumForce;
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonJoint.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonSensor.cs

@@ -0,0 +1,12 @@
+using UnityEngine;
+
+namespace Unity.MLAgents
+{
+    [System.Serializable]
+    public class MarathonSensor
+    {
+        public Collider SiteObject;
+        public string Name;
+        public string SiteName;
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonSensor.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonSpawner.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonSupport.cs

@@ -0,0 +1,18 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class MarathonSupport : MonoBehaviour
+{
+    // Start is called before the first frame update
+    void Start()
+    {
+        
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/MarathonSupport.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/Muscle002.cs

@@ -0,0 +1,206 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+
+[System.Serializable]
+public class Muscle002
+{
+    public string Name;
+    public BodyHelper002.MuscleGroup Group;
+
+    [Range(-1,1)]
+    public float TargetNormalizedRotationX;
+    [Range(-1,1)]
+    public float TargetNormalizedRotationY;
+    [Range(-1,1)]
+    public float TargetNormalizedRotationZ;
+    public Vector3 MaximumForce;
+
+    public Vector3 ObsLocalPosition;
+    public Quaternion ObsRotation;
+    public Vector3 ObsNormalizedRotation;
+    public Vector3 ObsNormalizedDeltaFromTargetRotation;
+    public Vector3 ObsRotationVelocity;
+    public Vector3 ObsVelocity;
+
+    public Vector3 DebugMaxRotationVelocity;
+    public Vector3 DebugMaxVelocity;
+
+
+    public Quaternion DefaultLocalRotation;
+    public Quaternion ToJointSpaceInverse;
+    public Quaternion ToJointSpaceDefault;
+
+    public Rigidbody Rigidbody;
+    public Transform Transform;
+    public ConfigurableJoint ConfigurableJoint;
+    public Rigidbody Parent;
+    public ConfigurableJoint RootConfigurableJoint;
+    public Quaternion InitialRootRotation;
+    public Vector3 InitialRootPosition;
+
+    Quaternion _lastObsRotation;
+    Vector3 _lastLocalPosition;
+    float _lastUpdateObsTime;
+    bool _firstRunComplete;
+    bool _hasRanVeryFirstInit;
+
+
+    public void UpdateMotor()
+    {
+        float powerMultiplier = 2.5f;
+		var t = ConfigurableJoint.targetAngularVelocity;
+		t.x = TargetNormalizedRotationX * MaximumForce.x;
+		t.y = TargetNormalizedRotationY * MaximumForce.y;
+		t.z = TargetNormalizedRotationZ * MaximumForce.z;
+		ConfigurableJoint.targetAngularVelocity = t;
+
+		var angX = ConfigurableJoint.angularXDrive;
+		angX.positionSpring = 1f;
+		var scale = MaximumForce.x * Mathf.Pow(Mathf.Abs(TargetNormalizedRotationX), 3);
+		angX.positionDamper = Mathf.Max(1f, scale);
+		angX.maximumForce = Mathf.Max(1f, MaximumForce.x * powerMultiplier);
+		ConfigurableJoint.angularXDrive = angX;
+
+        var maxForce = Mathf.Max(MaximumForce.y, MaximumForce.z);
+		var angYZ = ConfigurableJoint.angularYZDrive;
+		angYZ.positionSpring = 1f;
+        var maxAbsRotXY = Mathf.Max(Mathf.Abs(TargetNormalizedRotationY) + Mathf.Abs(TargetNormalizedRotationZ));
+		scale = maxForce * Mathf.Pow(maxAbsRotXY, 3);
+		angYZ.positionDamper = Mathf.Max(1f, scale);
+		angYZ.maximumForce = Mathf.Max(1f, maxForce * powerMultiplier);
+		ConfigurableJoint.angularYZDrive = angYZ;
+	}    
+
+    static Vector3 NormalizedEulerAngles(Vector3 eulerAngles)
+    {
+        var x = eulerAngles.x < 180f ?
+            eulerAngles.x :
+            - 360 + eulerAngles.x;
+        var y = eulerAngles.y < 180f ?
+            eulerAngles.y :
+            - 360 + eulerAngles.y;
+        var z = eulerAngles.z < 180f ?
+            eulerAngles.z :
+            - 360 + eulerAngles.z;
+        x = x / 180f;
+        y = y / 180f;
+        z = z / 180f;
+        return new Vector3(x,y,z);
+    }
+    static Vector3 ScaleNormalizedByJoint(Vector3 normalizedRotation, ConfigurableJoint configurableJoint)
+    {
+        var x = normalizedRotation.x > 0f ?
+            (normalizedRotation.x * 180f) / configurableJoint.highAngularXLimit.limit :
+            (-normalizedRotation.x * 180f) / configurableJoint.lowAngularXLimit.limit;
+        var y = (normalizedRotation.y * 180f) / configurableJoint.angularYLimit.limit;
+        var z = (normalizedRotation.z * 180f) / configurableJoint.angularZLimit.limit;
+        var scaledNormalizedRotation = new Vector3(x,y,z);
+        return scaledNormalizedRotation;
+    }
+
+    static Vector3 Vector3Max (Vector3 a, Vector3 b)
+    {
+        var answer = new Vector3(
+            Mathf.Max(Mathf.Abs(a.x), Mathf.Abs(b.x)),
+            Mathf.Max(Mathf.Abs(a.y), Mathf.Abs(b.y)),
+            Mathf.Max(Mathf.Abs(a.z), Mathf.Abs(b.z)));
+        return answer;
+    }
+
+    public void Init()
+    {
+        _firstRunComplete = false;
+        Rigidbody.angularVelocity = Vector3.zero;
+        Rigidbody.velocity = Vector3.zero;
+
+
+        if (!_hasRanVeryFirstInit) {
+			Parent = ConfigurableJoint.connectedBody;
+			
+            InitialRootRotation = RootConfigurableJoint.transform.rotation;
+            InitialRootPosition = RootConfigurableJoint.transform.position;
+
+			DefaultLocalRotation = LocalRotation;
+			// Vector3 forward = Vector3.Cross (ConfigurableJoint.axis, ConfigurableJoint.secondaryAxis).normalized;
+			//Vector3 up = Vector3.Cross (forward, ConfigurableJoint.axis).normalized;
+            Vector3 forward = this.Transform.forward;
+            Vector3 up = this.Transform.forward;
+			Quaternion toJointSpace = Quaternion.LookRotation(forward, up);
+			
+			ToJointSpaceInverse = Quaternion.Inverse(toJointSpace);
+			ToJointSpaceDefault = DefaultLocalRotation * toJointSpace;
+            _hasRanVeryFirstInit = true;
+
+        }
+    }
+
+    public void UpdateObservations()
+    {
+        ObsRotation = this.LocalRotation;
+        ObsRotation = (ToJointSpaceInverse * UnityEngine.Quaternion.Inverse(this.LocalRotation) * this.ToJointSpaceDefault);
+        var r2 = (ToJointSpaceInverse * UnityEngine.Quaternion.Inverse(this.Transform.rotation) * this.ToJointSpaceDefault);
+        var s1 = ScaleNormalizedByJoint(NormalizedEulerAngles((this.LocalRotation * ToJointSpaceDefault).eulerAngles), ConfigurableJoint);
+        var s2 = ScaleNormalizedByJoint(NormalizedEulerAngles((Transform.localRotation * ToJointSpaceDefault).eulerAngles), ConfigurableJoint);
+        var s3 = ScaleNormalizedByJoint(NormalizedEulerAngles((this.LocalRotation * ToJointSpaceInverse).eulerAngles), ConfigurableJoint);
+        var s4 = ScaleNormalizedByJoint(NormalizedEulerAngles((Transform.localRotation * ToJointSpaceInverse).eulerAngles), ConfigurableJoint);
+        var s5 = ScaleNormalizedByJoint(NormalizedEulerAngles((UnityEngine.Quaternion.Inverse(this.LocalRotation) * ToJointSpaceDefault).eulerAngles), ConfigurableJoint);
+        
+        var normalizedRotation = NormalizedEulerAngles(ObsRotation.eulerAngles);
+        // var normalizedRotation = NormalizedEulerAngles(this.LocalRotation.eulerAngles);
+        ObsNormalizedRotation = ScaleNormalizedByJoint(normalizedRotation, ConfigurableJoint);
+        ObsNormalizedDeltaFromTargetRotation = 
+            new Vector3(TargetNormalizedRotationX, TargetNormalizedRotationY, TargetNormalizedRotationZ) - ObsNormalizedRotation;
+
+        // Debug code 
+        // if (Group == BodyHelper002.MuscleGroup.Head){
+        //     var debug = 1;
+        // }
+
+        if (_firstRunComplete == false){
+            _lastUpdateObsTime = Time.time;
+            _lastObsRotation = ObsRotation;
+            _lastLocalPosition = Transform.localPosition;
+        }
+        var dt = Time.time - _lastUpdateObsTime;
+        _lastUpdateObsTime = Time.time;
+        var rotationVelocity = ObsRotation.eulerAngles - _lastObsRotation.eulerAngles;
+        rotationVelocity = NormalizedEulerAngles(rotationVelocity);
+        rotationVelocity /= 128f;
+        if (dt > 0f)
+            rotationVelocity /= dt;
+        ObsRotationVelocity = rotationVelocity;
+        _lastObsRotation = ObsRotation;
+        var rootBone = RootConfigurableJoint.transform;
+        var toRootSpace = Quaternion.Inverse(RootConfigurableJoint.transform.rotation) * rootBone.rotation;
+        Quaternion rootRotation = Quaternion.Inverse(rootBone.rotation * toRootSpace) * Transform.rotation;
+        ObsLocalPosition = Transform.position - RootConfigurableJoint.transform.position;
+        var velocity = ObsLocalPosition - _lastLocalPosition;
+        ObsVelocity = velocity;
+        if (dt > 0f)
+            velocity /= dt;
+        _lastLocalPosition = ObsLocalPosition;
+
+        DebugMaxRotationVelocity = Vector3Max(DebugMaxRotationVelocity, rotationVelocity);
+        DebugMaxVelocity = Vector3Max(DebugMaxVelocity, velocity);
+
+        _firstRunComplete = true;
+    }
+    public Quaternion LocalRotation {
+        get {
+            // around root Rotation 
+            return Quaternion.Inverse(RootRotation) * Transform.rotation;
+
+            // around parent space
+            // return Quaternion.Inverse(ParentRotation) * transform.rotation;
+        }
+    }
+
+    public Quaternion RootRotation {
+        get {
+            return InitialRootRotation;
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/Muscle002.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/ProceduralCapsule.cs

@@ -0,0 +1,158 @@
+//------------------------------//
+//  ProceduralCapsule.cs        //
+//  Written by Jay Kay          //
+//  2016/05/27                  //
+//------------------------------//
+
+
+using UnityEngine;
+using System.Collections;
+
+
+[RequireComponent(typeof(MeshFilter), typeof(MeshRenderer))]
+public class ProceduralCapsule : MonoBehaviour
+{
+#if UNITY_EDITOR
+    [ContextMenu("Generate Procedural Capsule")]
+    public void GenerateProceduralCapsule()
+    {
+        // GenerateMesh();
+    }
+#endif
+
+
+    public float height = 2f;
+    public float radius = 0.5f;
+
+    public int segments = 24;
+
+
+    // void GenerateMesh() 
+    void Start()
+    {
+    }
+
+    public void CreateMesh()
+    {
+        // make segments an even number
+        if (segments % 2 != 0)
+            segments++;
+
+        // extra vertex on the seam
+        int points = segments + 1;
+
+        // calculate points around a circle
+        float[] pX = new float[points];
+        float[] pZ = new float[points];
+        float[] pY = new float[points];
+        float[] pR = new float[points];
+
+        float calcH = 0f;
+        float calcV = 0f;
+
+        for (int i = 0; i < points; i++)
+        {
+            pX[i] = Mathf.Sin(calcH * Mathf.Deg2Rad);
+            pZ[i] = Mathf.Cos(calcH * Mathf.Deg2Rad);
+            pY[i] = Mathf.Cos(calcV * Mathf.Deg2Rad);
+            pR[i] = Mathf.Sin(calcV * Mathf.Deg2Rad);
+
+            calcH += 360f / (float) segments;
+            calcV += 180f / (float) segments;
+        }
+
+
+        // - Vertices and UVs -
+
+        Vector3[] vertices = new Vector3[points * (points + 1)];
+        Vector2[] uvs = new Vector2[vertices.Length];
+        int ind = 0;
+
+        // Y-offset is half the height minus the diameter
+        // float yOff = ( height - ( radius * 2f ) ) * 0.5f;
+        float yOff = (height - (radius)) * 0.5f;
+        if (yOff < 0)
+            yOff = 0;
+
+        // uv calculations
+        float stepX = 1f / ((float) (points - 1));
+        float uvX, uvY;
+
+        // Top Hemisphere
+        int top = Mathf.CeilToInt((float) points * 0.5f);
+
+        for (int y = 0; y < top; y++)
+        {
+            for (int x = 0; x < points; x++)
+            {
+                vertices[ind] = new Vector3(pX[x] * pR[y], pY[y], pZ[x] * pR[y]) * radius;
+                vertices[ind].y = yOff + vertices[ind].y;
+
+                uvX = 1f - (stepX * (float) x);
+                uvY = (vertices[ind].y + (height * 0.5f)) / height;
+                uvs[ind] = new Vector2(uvX, uvY);
+
+                ind++;
+            }
+        }
+
+        // Bottom Hemisphere
+        int btm = Mathf.FloorToInt((float) points * 0.5f);
+
+        for (int y = btm; y < points; y++)
+        {
+            for (int x = 0; x < points; x++)
+            {
+                vertices[ind] = new Vector3(pX[x] * pR[y], pY[y], pZ[x] * pR[y]) * radius;
+                vertices[ind].y = -yOff + vertices[ind].y;
+
+                uvX = 1f - (stepX * (float) x);
+                uvY = (vertices[ind].y + (height * 0.5f)) / height;
+                uvs[ind] = new Vector2(uvX, uvY);
+
+                ind++;
+            }
+        }
+
+
+        // - Triangles -
+
+        int[] triangles = new int[(segments * (segments + 1) * 2 * 3)];
+
+        for (int y = 0, t = 0; y < segments + 1; y++)
+        {
+            for (int x = 0; x < segments; x++, t += 6)
+            {
+                triangles[t + 0] = ((y + 0) * (segments + 1)) + x + 0;
+                triangles[t + 1] = ((y + 1) * (segments + 1)) + x + 0;
+                triangles[t + 2] = ((y + 1) * (segments + 1)) + x + 1;
+
+                triangles[t + 3] = ((y + 0) * (segments + 1)) + x + 1;
+                triangles[t + 4] = ((y + 0) * (segments + 1)) + x + 0;
+                triangles[t + 5] = ((y + 1) * (segments + 1)) + x + 1;
+            }
+        }
+
+
+        // - Assign Mesh -
+
+        MeshFilter mf = gameObject.GetComponent<MeshFilter>();
+        Mesh mesh = mf.sharedMesh;
+        if (!mesh)
+        {
+            mesh = new Mesh();
+            mf.sharedMesh = mesh;
+        }
+
+        mesh.Clear();
+
+        mesh.name = "ProceduralCapsule";
+
+        mesh.vertices = vertices;
+        mesh.uv = uvs;
+        mesh.triangles = triangles;
+
+        mesh.RecalculateBounds();
+        mesh.RecalculateNormals();
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/ProceduralCapsule.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/RagDoll002.cs

@@ -0,0 +1,66 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class RagDoll002 : MonoBehaviour {
+
+    [System.Serializable]
+    public class MusclePower
+    {
+        public string Muscle;
+        public Vector3 PowerVector;
+    }
+
+    public List<MusclePower> MusclePowers;
+
+    public float MotorScale = 1f;
+
+	// Use this for initialization
+	void Start () {
+		Setup();
+	}
+	
+	// Update is called once per frame
+	void Update () {
+		
+	}
+
+	void Setup () {
+		// handle collision overlaps
+        IgnoreCollision("torso", new []{"left_upper_arm", "right_upper_arm"});
+        IgnoreCollision("butt", new []{"left_thigh", "right_thigh"});
+
+        IgnoreCollision("left_larm", new []{"left_upper_arm"});
+        IgnoreCollision("right_larm", new []{"right_upper_arm"});
+        IgnoreCollision("left_shin", new []{"left_thigh"});
+        IgnoreCollision("right_shin", new []{"right_thigh"});
+
+        IgnoreCollision("right_shin", new []{"right_right_foot"});
+        IgnoreCollision("left_shin", new []{"left_left_foot"});
+
+
+        //
+        var joints = GetComponentsInChildren<Joint>().ToList();
+        foreach (var joint in joints)
+            joint.enablePreprocessing = false;
+	}
+	void IgnoreCollision(string first, string[] seconds)
+    {
+        foreach (var second in seconds)
+        {
+            IgnoreCollision(first, second);
+        }
+    }
+    void IgnoreCollision(string first, string second)
+    {
+        var rigidbodies = GetComponentsInChildren<Rigidbody>().ToList();
+        var colliderOnes = rigidbodies.FirstOrDefault(x=>x.name.Contains(first))?.GetComponents<Collider>();
+        var colliderTwos = rigidbodies.FirstOrDefault(x=>x.name.Contains(second))?.GetComponents<Collider>();
+        if (colliderOnes == null || colliderTwos == null)
+            return;
+        foreach (var c1 in colliderOnes)
+            foreach (var c2 in colliderTwos)
+                Physics.IgnoreCollision(c1, c2);
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/RagDoll002.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/ScoreHistogramData.cs

@@ -0,0 +1,48 @@
+
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class ScoreHistogramData
+{
+    int _columnCount;
+    int _historyDepth;
+
+    public List<List<double>> Items;
+
+
+    public ScoreHistogramData (int columnCount, int historyDepth)
+    {
+        _columnCount = columnCount;
+        _historyDepth = historyDepth;
+        ReCreateScoreHistogramData();
+    }
+
+    public void ReCreateScoreHistogramData ()
+    {
+        Items = new List<List<double>>();
+        for (int i = 0; i < _columnCount; i++)
+        {
+            // var item = Enumerable.Range(0,_historyDepth).Select(x=>default(T)).ToList();
+            var item = new List<double>();
+            Items.Add(item);
+        }
+    }
+    public void SetItem(int column, double value)
+    {
+        Items[column].Add(value);
+        if (Items[column].Count > _historyDepth)
+            Items[column].RemoveAt(0);
+    }
+    public double GetAverage(int column)
+    {
+        double average = Items[column].Average();
+        return average;
+    }
+    public List<double> GetAverages()
+    {
+        List<double> averages = Items.Select(x=>x.Count > 0 ? x.Average() : 0).ToList();
+        return averages;
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/ScoreHistogramData.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/Scorer.cs

@@ -0,0 +1,50 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+using ManyWorlds;
+
+public class Scorer : MonoBehaviour
+{
+    public int TotalEpisodesToScore = 100;
+    public int EpisodesScored;
+
+    public float AverageScore;
+    public float StdDiv;
+    public string ScoreInfo;
+    public List<float> scores;
+    public List<string> scoreInfos;
+    [TextArea]
+    public string ScoreReport;
+    // Start is called before the first frame update
+    void Start()
+    {
+        scores = new List<float>(TotalEpisodesToScore);
+        scoreInfos = new List<string>(TotalEpisodesToScore);
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        
+    }
+
+    public void ReportScore(float score, string scoreInfo)
+    {
+        if (EpisodesScored >= TotalEpisodesToScore)
+            return;
+        scores.Add(score);
+        scoreInfos.Add(scoreInfo);
+        ScoreInfo = scoreInfo;
+        EpisodesScored = scores.Count;
+        AverageScore = scores.Average();
+        var sum = scores.Sum(d => (d - AverageScore) * (d - AverageScore));
+        StdDiv = Mathf.Sqrt(sum / EpisodesScored);
+        string name = string.Empty;
+        var spawnEnv = FindObjectOfType<SpawnableEnv>();
+        if (spawnEnv != null)
+            name = $"{spawnEnv.gameObject.name} ";
+        ScoreReport = $"{name}AveScore:{AverageScore}, StdDiv:{StdDiv} over {EpisodesScored} episodes using {scoreInfo}";
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/Scorer.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/SendOnCollisionTrigger.cs

@@ -0,0 +1,19 @@
+using UnityEngine;
+
+namespace Unity.MLAgents
+{
+    public class SendOnCollisionTrigger : MonoBehaviour
+    {
+        void OnCollisionEnter(Collision other)
+        {
+            // Messenger.
+            var otherGameobject = other.gameObject;
+            var marathonAgent = otherGameobject.GetComponentInParent<MarathonAgent>();
+            if (marathonAgent != null)
+                marathonAgent.OnTerrainCollision(otherGameobject, this.gameObject);
+            var iOnTerrainCollision = otherGameobject.GetComponentInParent<IOnTerrainCollision>();
+            if (iOnTerrainCollision != null)
+                iOnTerrainCollision.OnTerrainCollision(otherGameobject, this.gameObject);
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/SendOnCollisionTrigger.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll002/TerrainSetup.cs

@@ -0,0 +1,20 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class TerrainSetup : MonoBehaviour
+{
+    public Vector3 TerrainSize;
+    // Start is called before the first frame update
+    void Start()
+    {
+        var terrain = GetComponent<Terrain>();
+        terrain.terrainData.size = TerrainSize;
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll002/TerrainSetup.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 1d7031116ac22cf46809cbc8678f3b54
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/3_MarathonEnvs/Scripts/Ragdoll004/BodyPartDifferenceStats.cs

@@ -0,0 +1,14 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+[System.Serializable]
+
+public class BodyPartDifferenceStats
+{
+   public string Name;
+    public Vector3 Position;
+    public Vector3 Rotation;
+    public Vector3 Velocity;
+    public Vector3 AngualrVelocity;    
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/BodyPartDifferenceStats.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservationStats.cs

@@ -0,0 +1,357 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+using ManyWorlds;
+using UnityEngine.Assertions;
+
+public class DReConObservationStats : MonoBehaviour
+{
+    [System.Serializable]
+    public class Stat
+    {
+        public string Name;
+        public Vector3 Position;
+        public Quaternion Rotation;
+        public Vector3 Velocity;
+        public Vector3 AngualrVelocity;
+        [HideInInspector]
+        public Vector3 LastLocalPosition;
+        [HideInInspector]
+        public Quaternion LastLocalRotation;
+        [HideInInspector]
+        public bool LastIsSet;
+    }
+
+    public MonoBehaviour ObjectToTrack;
+    List<string> _bodyPartsToTrack;
+
+    [Header("Anchor stats")]
+    public Vector3 HorizontalDirection; // Normalized vector in direction of travel (assume right angle to floor)
+    // public Vector3 CenterOfMassInWorldSpace; 
+    public Vector3 AngualrVelocity;
+
+    [Header("Stats, relative to HorizontalDirection & Center Of Mass")]
+    public Vector3 CenterOfMassVelocity;
+    public Vector3 CenterOfMassHorizontalVelocity;
+    public float CenterOfMassVelocityMagnitude;
+    public float CenterOfMassHorizontalVelocityMagnitude;
+    public Vector3 DesiredCenterOfMassVelocity;
+    public Vector3 CenterOfMassVelocityDifference;
+    public List<Stat> Stats;
+
+    // [Header("... for debugging")]
+    [Header("Gizmos")]
+    public bool VelocityInWorldSpace = true;
+    public bool HorizontalVelocity = true;
+
+    [HideInInspector]
+    public Vector3 LastCenterOfMassInWorldSpace;
+    [HideInInspector]
+    public Quaternion LastRotation;
+    [HideInInspector]
+    public bool LastIsSet;
+
+
+    SpawnableEnv _spawnableEnv;
+    List<Collider> _bodyParts;
+    internal List<Rigidbody> _rigidbodyParts;
+    internal List<ArticulationBody> _articulationBodyParts;
+    GameObject _root;
+    InputController _inputController;
+    bool _hasLazyInitialized;
+
+    string rootName = "articulation:Hips";
+
+    public void setRootName(string s) {
+        rootName = s;
+    
+    }
+
+
+
+    public void OnAgentInitialize(List<string> bodyPartsToTrack, Transform defaultTransform)
+    {
+        Assert.IsFalse(_hasLazyInitialized);
+        _hasLazyInitialized = true;
+
+        _bodyPartsToTrack = bodyPartsToTrack;
+        _spawnableEnv = GetComponentInParent<SpawnableEnv>();
+        _inputController = _spawnableEnv.GetComponentInChildren<InputController>();
+        _rigidbodyParts = ObjectToTrack.GetComponentsInChildren<Rigidbody>().ToList();
+        _articulationBodyParts = ObjectToTrack.GetComponentsInChildren<ArticulationBody>().ToList();
+
+
+        if (_rigidbodyParts?.Count > 0)
+            _bodyParts = _rigidbodyParts
+                .SelectMany(x=>x.GetComponentsInChildren<Collider>())
+                .Distinct()
+                .ToList();
+        else
+            _bodyParts = _articulationBodyParts
+                .SelectMany(x=>x.GetComponentsInChildren<Collider>())
+                .Distinct()
+                .ToList();
+        // if (_rigidbodyParts?.Count > 0)
+        //     _bodyParts = _rigidbodyParts
+        //         .SelectMany(x => x.GetComponentsInChildren<Transform>())
+        //         .Distinct()
+        //         .ToList();
+        // else
+        //     _bodyParts = _articulationBodyParts
+        //         .SelectMany(x => x.GetComponentsInChildren<Transform>())
+        //         .Distinct()
+        //         .ToList();
+
+        var bodyPartNames = _bodyParts.Select(x=>x.name);
+        if (_bodyPartsToTrack?.Count > 0)
+            _bodyParts = _bodyPartsToTrack
+                .Where(x=>bodyPartNames.Contains(x))
+                .Select(x=>_bodyParts.First(y=>y.name == x))
+                .ToList();
+        Stats = _bodyParts
+            .Select(x=> new Stat{Name = x.name})
+            .ToList();
+        if (_root == null)
+        {
+           // Debug.Log("in game object: " + name + "my rootname is: " + rootName);
+            _root = _bodyParts.First(x=>x.name== rootName).gameObject;
+        }             
+        transform.position = defaultTransform.position;
+        transform.rotation = defaultTransform.rotation;
+    }
+
+    public void OnReset()
+    {
+        Assert.IsTrue(_hasLazyInitialized);
+        ResetStatus();
+        foreach (var bodyPart in Stats)
+        {
+            bodyPart.LastIsSet = false;
+        }
+        LastIsSet = false;
+    }
+    void ResetStatus()
+    {
+        foreach (var bodyPart in Stats)
+        {
+            bodyPart.LastIsSet = false;
+        }
+        LastIsSet = false;
+        var timeDelta = float.MinValue;
+        SetStatusForStep(timeDelta);
+    }
+
+
+    // Return rotation from one rotation to another
+    public static Quaternion FromToRotation(Quaternion from, Quaternion to) {
+        if (to == from) return Quaternion.identity;
+
+        return to * Quaternion.Inverse(from);
+    }
+
+    // Adjust the value of an angle to lie within [-pi, +pi].
+    public static float NormalizedAngle(float angle) {
+        if (angle < 180) {
+            return angle * Mathf.Deg2Rad;
+        }
+        return (angle - 360) * Mathf.Deg2Rad;
+    }
+
+    // Calculate rotation between two rotations in radians. Adjusts the value to lie within [-pi, +pi].
+    public static Vector3 NormalizedEulerAngles(Vector3 eulerAngles) {
+        var x = NormalizedAngle(eulerAngles.x);
+        var y = NormalizedAngle(eulerAngles.y);
+        var z = NormalizedAngle(eulerAngles.z);
+        return new Vector3(x, y, z);
+    }
+
+    // Find angular velocity. The delta rotation is converted to radians within [-pi, +pi].
+    public static Vector3 GetAngularVelocity(Quaternion from, Quaternion to, float timeDelta) {
+        var rotationVelocity = FromToRotation(from, to);
+        var angularVelocity = NormalizedEulerAngles(rotationVelocity.eulerAngles) / timeDelta;
+        return angularVelocity;
+    }
+
+    public void SetStatusForStep(float timeDelta)
+    {
+        // find Center Of Mass
+        Vector3 newCOM;
+        if (_rigidbodyParts?.Count > 0)
+            newCOM = GetCenterOfMass(_rigidbodyParts);
+        else
+            newCOM = GetCenterOfMass(_articulationBodyParts);
+        if (!LastIsSet)
+        {
+            LastCenterOfMassInWorldSpace = newCOM;
+        }
+
+        // generate Horizontal Direction
+        var newHorizontalDirection = new Vector3(0f, _root.transform.eulerAngles.y, 0f);
+        HorizontalDirection = newHorizontalDirection / 180f;
+
+        // set this object to be f space
+        transform.position = newCOM;
+        transform.rotation = Quaternion.Euler(newHorizontalDirection);
+
+        // get Center Of Mass velocity in f space
+        var velocity = transform.position - LastCenterOfMassInWorldSpace;
+        velocity /= timeDelta;
+        CenterOfMassVelocity = transform.InverseTransformVector(velocity);
+        CenterOfMassVelocityMagnitude = CenterOfMassVelocity.magnitude;
+
+        // get Center Of Mass horizontal velocity in f space
+        var comHorizontalDirection = new Vector3(velocity.x, 0f, velocity.z);
+        CenterOfMassHorizontalVelocity = transform.InverseTransformVector(comHorizontalDirection);
+        CenterOfMassHorizontalVelocityMagnitude = CenterOfMassHorizontalVelocity.magnitude;
+
+        // get Desired Center Of Mass horizontal velocity in f space
+        Vector3 desiredCom = new Vector3(
+            _inputController.DesiredHorizontalVelocity.x,
+            0f,
+            _inputController.DesiredHorizontalVelocity.y);
+        DesiredCenterOfMassVelocity = transform.InverseTransformVector(desiredCom);
+            
+        // get Desired Center Of Mass horizontal velocity in f space
+        CenterOfMassVelocityDifference = DesiredCenterOfMassVelocity-CenterOfMassHorizontalVelocity;
+        
+        if (!LastIsSet)
+        {
+            LastRotation = transform.rotation;
+        }
+        AngualrVelocity = GetAngularVelocity(LastRotation, transform.rotation, timeDelta);
+        LastRotation = transform.rotation;
+        LastCenterOfMassInWorldSpace = newCOM;
+        LastIsSet = true;
+
+        // get bodyParts stats in local space
+        foreach (var bodyPart in _bodyParts)
+        {
+            Stat bodyPartStat = Stats.First(x=>x.Name == bodyPart.name);
+
+            Vector3 c = Vector3.zero;
+            CapsuleCollider capsule = bodyPart as CapsuleCollider;
+            BoxCollider box = bodyPart as BoxCollider;
+            SphereCollider sphere = bodyPart as SphereCollider;
+            if (capsule != null)
+                c = capsule.center;
+            else if (box != null)
+                c = box.center;
+            else if (sphere != null)
+                c = sphere.center;
+            Vector3 worldPosition = bodyPart.transform.TransformPoint(c);
+            // Vector3 worldPosition = transform.position;
+
+            Quaternion worldRotation = bodyPart.transform.rotation;
+            Vector3 localPosition = transform.InverseTransformPoint(worldPosition);
+            Quaternion localRotation = FromToRotation(transform.rotation, worldRotation);
+            if (!bodyPartStat.LastIsSet)
+            {
+                bodyPartStat.LastLocalPosition = localPosition;
+                bodyPartStat.LastLocalRotation = localRotation;
+            }
+
+            bodyPartStat.Position = localPosition;
+            bodyPartStat.Rotation = localRotation;
+            bodyPartStat.Velocity = (localPosition - bodyPartStat.LastLocalPosition)/timeDelta;
+            bodyPartStat.AngualrVelocity = GetAngularVelocity(bodyPartStat.LastLocalRotation, localRotation, timeDelta);
+            bodyPartStat.LastLocalPosition = localPosition;
+            bodyPartStat.LastLocalRotation = localRotation;
+            bodyPartStat.LastIsSet = true;
+        }
+    }
+
+	Vector3 GetCenterOfMass(IEnumerable<Rigidbody> bodies)
+	{
+		var centerOfMass = Vector3.zero;
+		float totalMass = 0f;
+		foreach (Rigidbody ab in bodies)
+		{
+			centerOfMass += ab.worldCenterOfMass * ab.mass;
+			totalMass += ab.mass;
+		}
+		centerOfMass /= totalMass;
+		// centerOfMass -= _spawnableEnv.transform.position;
+		return centerOfMass;
+	}
+	Vector3 GetCenterOfMass(IEnumerable<ArticulationBody> bodies)
+	{
+		var centerOfMass = Vector3.zero;
+		float totalMass = 0f;
+		foreach (ArticulationBody ab in bodies)
+		{
+			centerOfMass += ab.worldCenterOfMass * ab.mass;
+			totalMass += ab.mass;
+		}
+		centerOfMass /= totalMass;
+		// centerOfMass -= _spawnableEnv.transform.position;
+		return centerOfMass;    
+    }
+
+    void OnDrawGizmosSelected()
+    {
+        if (_bodyPartsToTrack == null)
+            return;           
+        // draw arrow for desired input velocity
+        // Vector3 pos = new Vector3(transform.position.x, transform.position.y, transform.position.z);
+        Vector3 pos = new Vector3(transform.position.x, .3f, transform.position.z);
+        Vector3 vector = DesiredCenterOfMassVelocity;
+        if (VelocityInWorldSpace)
+            vector = transform.TransformVector(vector);
+        DrawArrow(pos, vector, Color.green);
+        Vector3 desiredInputPos = pos+vector;
+
+        if (HorizontalVelocity)
+        {
+            // arrow for actual velocity
+            vector = CenterOfMassHorizontalVelocity;
+            if (VelocityInWorldSpace)
+                vector = transform.TransformVector(vector);
+            DrawArrow(pos, vector, Color.blue);
+            Vector3 actualPos = pos+vector;
+
+            // arrow for actual velocity difference
+            vector = CenterOfMassVelocityDifference;
+            if (VelocityInWorldSpace)
+                vector = transform.TransformVector(vector);
+            DrawArrow(actualPos, vector, Color.red);
+        }
+        else
+        {
+            vector = CenterOfMassVelocity;
+            if (VelocityInWorldSpace)
+                vector = transform.TransformVector(vector);
+            DrawArrow(pos, vector, Color.blue);
+            Vector3 actualPos = pos+vector;
+
+            // arrow for actual velocity difference
+            vector = DesiredCenterOfMassVelocity-CenterOfMassVelocity;
+            if (VelocityInWorldSpace)
+                vector = transform.TransformVector(vector);
+            DrawArrow(actualPos, vector, Color.red);
+
+        }
+    }
+    void DrawArrow(Vector3 start, Vector3 vector, Color color)
+    {
+        float headSize = 0.25f;
+        float headAngle = 20.0f;
+        Gizmos.color = color;
+		Gizmos.DrawRay(start, vector);
+ 
+        if (vector.magnitude > 0f)
+        { 
+            Vector3 right = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180+headAngle,0) * new Vector3(0,0,1);
+            Vector3 left = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180-headAngle,0) * new Vector3(0,0,1);
+            Gizmos.DrawRay(start + vector, right * headSize);
+            Gizmos.DrawRay(start + vector, left * headSize);
+        }
+    }
+    public void ShiftCOM (Vector3 snapDistance)
+    {
+        Vector3 newCOM = LastCenterOfMassInWorldSpace + snapDistance;
+        LastCenterOfMassInWorldSpace = newCOM;
+        transform.position = newCOM;
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservationStats.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservations.cs

@@ -0,0 +1,221 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+using ManyWorlds;
+using UnityEngine.Assertions;
+
+public class DReConObservations : MonoBehaviour
+{
+    [Header("Observations")]
+
+    [Tooltip("Kinematic character center of mass velocity, Vector3")]
+    public Vector3 MocapCOMVelocity;
+
+    [Tooltip("RagDoll character center of mass velocity, Vector3")]
+    public Vector3 RagDollCOMVelocity;
+
+    [Tooltip("User-input desired horizontal CM velocity. Vector2")]
+    public Vector2 InputDesiredHorizontalVelocity;
+
+    [Tooltip("User-input requests jump, bool")]
+    public bool InputJump;
+
+    [Tooltip("User-input requests backflip, bool")]
+    public bool InputBackflip;
+
+    [Tooltip("Difference between RagDoll character horizontal CM velocity and user-input desired horizontal CM velocity. Vector2")]
+    public Vector2 HorizontalVelocityDifference;
+
+    [Tooltip("Positions and velocities for subset of bodies")]
+    public List<BodyPartDifferenceStats> BodyPartDifferenceStats;
+    public List<DReConObservationStats.Stat> MocapBodyStats;
+    public List<DReConObservationStats.Stat> RagDollBodyStats;
+
+    [Tooltip("Smoothed actions produced in the previous step of the policy are collected in t −1")]
+    public float[] PreviousActions;
+
+    [Header("Settings")]
+    public List<string> BodyPartsToTrack;
+
+    [Header("Gizmos")]
+    public bool VelocityInWorldSpace = true;
+    public bool PositionInWorldSpace = true;
+
+
+    public string targetedRootName = "articulation:Hips";
+
+
+
+    InputController _inputController;
+    SpawnableEnv _spawnableEnv;
+    DReConObservationStats _mocapBodyStats;
+    DReConObservationStats _ragDollBodyStats;
+    bool _hasLazyInitialized;
+
+    public void OnAgentInitialize()
+    {
+        Assert.IsFalse(_hasLazyInitialized);
+        _hasLazyInitialized = true;
+
+        _spawnableEnv = GetComponentInParent<SpawnableEnv>();
+        _inputController = _spawnableEnv.GetComponentInChildren<InputController>();
+        BodyPartDifferenceStats = BodyPartsToTrack
+            .Select(x=> new BodyPartDifferenceStats{Name = x})
+            .ToList();
+
+        _mocapBodyStats= new GameObject("MocapDReConObservationStats").AddComponent<DReConObservationStats>();
+        _mocapBodyStats.setRootName(targetedRootName);
+
+
+
+        _mocapBodyStats.ObjectToTrack = _spawnableEnv.GetComponentInChildren<MocapControllerArtanim>();
+
+        _mocapBodyStats.transform.SetParent(_spawnableEnv.transform);
+        _mocapBodyStats.OnAgentInitialize(BodyPartsToTrack, _mocapBodyStats.ObjectToTrack.transform);
+
+        _ragDollBodyStats = new GameObject("RagDollDReConObservationStats").AddComponent<DReConObservationStats>();
+        _ragDollBodyStats.setRootName(targetedRootName);
+
+        _ragDollBodyStats.ObjectToTrack = this;
+        _ragDollBodyStats.transform.SetParent(_spawnableEnv.transform);
+        _ragDollBodyStats.OnAgentInitialize(BodyPartsToTrack, transform);
+    }
+
+
+
+    public void OnStep(float timeDelta)
+    {
+        Assert.IsTrue(_hasLazyInitialized);
+        _mocapBodyStats.SetStatusForStep(timeDelta);
+        _ragDollBodyStats.SetStatusForStep(timeDelta);
+        UpdateObservations(timeDelta);
+    }
+    public void OnReset()
+    {
+        Assert.IsTrue(_hasLazyInitialized);
+        _mocapBodyStats.OnReset();
+        _ragDollBodyStats.OnReset();
+        _ragDollBodyStats.transform.position = _mocapBodyStats.transform.position;
+        _ragDollBodyStats.transform.rotation = _mocapBodyStats.transform.rotation;
+        var timeDelta = float.MinValue;
+        UpdateObservations(timeDelta);
+    }
+
+    public void UpdateObservations(float timeDelta)
+    {
+
+        MocapCOMVelocity = _mocapBodyStats.CenterOfMassVelocity;
+        RagDollCOMVelocity = _ragDollBodyStats.CenterOfMassVelocity;
+        InputDesiredHorizontalVelocity = new Vector2(
+            _ragDollBodyStats.DesiredCenterOfMassVelocity.x, 
+            _ragDollBodyStats.DesiredCenterOfMassVelocity.z);
+        InputJump = _inputController.Jump;
+        InputBackflip = _inputController.Backflip;
+        HorizontalVelocityDifference = new Vector2(
+            _ragDollBodyStats.CenterOfMassVelocityDifference.x,
+            _ragDollBodyStats.CenterOfMassVelocityDifference.z);
+
+        MocapBodyStats = BodyPartsToTrack
+            .Select(x=>_mocapBodyStats.Stats.First(y=>y.Name == x))
+            .ToList();
+        RagDollBodyStats = BodyPartsToTrack
+            .Select(x=>_ragDollBodyStats.Stats.First(y=>y.Name == x))
+            .ToList();
+        // BodyPartStats = 
+        foreach (var differenceStats in BodyPartDifferenceStats)
+        {
+            var mocapStats = _mocapBodyStats.Stats.First(x=>x.Name == differenceStats.Name);
+            var ragDollStats = _ragDollBodyStats.Stats.First(x=>x.Name == differenceStats.Name);
+
+            differenceStats.Position = mocapStats.Position - ragDollStats.Position;
+            differenceStats.Velocity = mocapStats.Velocity - ragDollStats.Velocity;
+            differenceStats.AngualrVelocity = mocapStats.AngualrVelocity - ragDollStats.AngualrVelocity;
+            differenceStats.Rotation = DReConObservationStats.GetAngularVelocity(mocapStats.Rotation, ragDollStats.Rotation, timeDelta);
+        }
+    }
+    public Transform GetRagDollCOM()
+    {
+        return _ragDollBodyStats.transform;
+    }
+    public void ShiftMocapCOM(Vector3 snapDistance)
+    {
+        _ragDollBodyStats.ShiftCOM(snapDistance);
+    }
+    void OnDrawGizmos()
+    {
+        if (_mocapBodyStats == null)
+            return;        
+        // MocapCOMVelocity
+        Vector3 pos = new Vector3(transform.position.x, .3f, transform.position.z);
+        Vector3 vector = MocapCOMVelocity;
+        if (VelocityInWorldSpace)
+            vector = _mocapBodyStats.transform.TransformVector(vector);
+        DrawArrow(pos, vector, Color.grey);
+
+        // RagDollCOMVelocity;
+        vector = RagDollCOMVelocity;
+        if (VelocityInWorldSpace)
+            vector = _ragDollBodyStats.transform.TransformVector(vector);
+        DrawArrow(pos, vector, Color.blue);
+        Vector3 actualPos = pos+vector;
+
+        // InputDesiredHorizontalVelocity;
+        vector = new Vector3(InputDesiredHorizontalVelocity.x, 0f, InputDesiredHorizontalVelocity.y);
+        if (VelocityInWorldSpace)
+            vector = _ragDollBodyStats.transform.TransformVector(vector);
+        DrawArrow(pos, vector, Color.green);
+
+        // HorizontalVelocityDifference;
+        vector = new Vector3(HorizontalVelocityDifference.x, 0f, HorizontalVelocityDifference.y);
+        if (VelocityInWorldSpace)
+            vector = _ragDollBodyStats.transform.TransformVector(vector);
+        DrawArrow(actualPos, vector, Color.red);
+
+        for (int i = 0; i < RagDollBodyStats.Count; i++)
+        {
+            var stat = RagDollBodyStats[i];
+            var differenceStat = BodyPartDifferenceStats[i];
+            pos = stat.Position;
+            vector = stat.Velocity;
+            if (PositionInWorldSpace)
+                pos = _ragDollBodyStats.transform.TransformPoint(pos);
+            if (VelocityInWorldSpace)
+                vector = _ragDollBodyStats.transform.TransformVector(vector);
+            DrawArrow(pos, vector, Color.cyan);
+            Vector3 velocityPos = pos+vector;
+
+            pos = stat.Position;
+            vector = differenceStat.Position;
+            if (PositionInWorldSpace)
+                pos = _ragDollBodyStats.transform.TransformPoint(pos);
+            if (VelocityInWorldSpace)
+                vector = _ragDollBodyStats.transform.TransformVector(vector);
+            Gizmos.color = Color.magenta;
+            Gizmos.DrawRay(pos, vector);
+            Vector3 differencePos = pos+vector;
+
+            vector = differenceStat.Velocity;
+            if (VelocityInWorldSpace)
+                vector = _ragDollBodyStats.transform.TransformVector(vector);
+            DrawArrow(velocityPos, vector, Color.red);
+        }
+        
+    }
+    void DrawArrow(Vector3 start, Vector3 vector, Color color)
+    {
+        float headSize = 0.25f;
+        float headAngle = 20.0f;
+        Gizmos.color = color;
+		Gizmos.DrawRay(start, vector);
+ 
+        if (vector.magnitude > 0f)
+        { 
+            Vector3 right = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180+headAngle,0) * new Vector3(0,0,1);
+            Vector3 left = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180-headAngle,0) * new Vector3(0,0,1);
+            Gizmos.DrawRay(start + vector, right * headSize);
+            Gizmos.DrawRay(start + vector, left * headSize);
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservations.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewardStats.cs

@@ -0,0 +1,392 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+using UnityEngine.Assertions;
+using ManyWorlds;
+
+public class DReConRewardStats : MonoBehaviour
+{
+    [Header("Settings")]
+
+    public MonoBehaviour ObjectToTrack;
+
+    [Header("Stats")]
+    public Vector3 CenterOfMassVelocity;
+    public float CenterOfMassVelocityMagnitude;
+
+    // [Header("debug")]
+    // public Vector3 debugA;
+    // public Vector3 debugB;
+    // public Vector3 debugC;
+
+    [HideInInspector]
+    public Vector3 LastCenterOfMassInWorldSpace;
+    [HideInInspector]
+    public bool LastIsSet;
+
+    SpawnableEnv _spawnableEnv;
+    List<Collider> _colliders;
+    List<Rigidbody> _rigidbodyParts;
+    List<ArticulationBody> _articulationBodyParts;
+    List<GameObject> _bodyParts;
+    GameObject _root;
+    List<GameObject> _trackRotations;
+    public List<Quaternion> Rotations;
+    public Vector3[] Points;
+    Vector3[] _lastPoints;
+    public Vector3[] PointVelocity;
+
+    [Header("Stats")]
+    public List<string> ColliderNames;
+    public List<string> RotationNames;
+    public List<string> BodyPartNames;
+    bool _hasLazyInitialized;
+
+
+    string rootName = "articulation:Hips";
+
+    public void setRootName(string s)
+    {
+        rootName = s;
+
+    }
+
+
+    public void OnAgentInitialize(Transform defaultTransform, DReConRewardStats orderToCopy = null)
+    {
+        Assert.IsFalse(_hasLazyInitialized);
+        _hasLazyInitialized = true;
+                
+        _spawnableEnv = GetComponentInParent<SpawnableEnv>();
+        _articulationBodyParts = ObjectToTrack
+            .GetComponentsInChildren<ArticulationBody>()
+            .Distinct()
+            .ToList();
+        _rigidbodyParts = ObjectToTrack
+            .GetComponentsInChildren<Rigidbody>()
+            .Distinct()
+            .ToList();
+        if (_rigidbodyParts?.Count>0)
+            _bodyParts = _rigidbodyParts.Select(x=>x.gameObject).ToList();
+        else
+            _bodyParts = _articulationBodyParts.Select(x=>x.gameObject).ToList();
+        _trackRotations = _bodyParts
+            .SelectMany(x=>x.GetComponentsInChildren<Transform>())
+            .Select(x=>x.gameObject)
+            .Distinct()
+            .Where(x=>x.GetComponent<Rigidbody>() != null || x.GetComponent<ArticulationBody>() != null)
+            // TODO: figure out how to not hard code this:
+            .Where(x=>x.name.StartsWith("articulation:") || x.name == "head")
+            .ToList();
+        _colliders = _bodyParts
+            .SelectMany(x=>x.GetComponentsInChildren<Collider>())
+            .Where(x=>x.enabled)
+            .Where(x=>!x.name.Contains("senor"))
+            .Distinct()
+            .ToList();
+        if (orderToCopy != null)
+        {
+            _bodyParts = orderToCopy._bodyParts
+                .Select(x=>_bodyParts.First(y=>y.name == x.name))
+                .ToList();
+            _trackRotations = orderToCopy._trackRotations
+                .Select(x=>_trackRotations.First(y=>y.name == x.name))
+                .ToList();
+            _colliders = orderToCopy._colliders
+                .Select(x=>_colliders.First(y=>y.name == x.name))
+                .ToList();
+        }
+        Points = Enumerable.Range(0,_colliders.Count * 6)
+            .Select(x=>Vector3.zero)
+            .ToArray();
+        _lastPoints = Enumerable.Range(0,_colliders.Count * 6)
+            .Select(x=>Vector3.zero)
+            .ToArray();            
+        PointVelocity = Enumerable.Range(0,_colliders.Count * 6)
+            .Select(x=>Vector3.zero)
+            .ToArray();
+        Rotations = Enumerable.Range(0,_trackRotations.Count)
+            .Select(x=>Quaternion.identity)
+            .ToList();
+        if (_root == null)
+        {
+            _root = _bodyParts.First(x=>x.name== rootName);
+        }        
+        transform.position = defaultTransform.position;
+        transform.rotation = defaultTransform.rotation;
+        ColliderNames = _colliders
+            .Select(x=>x.name)
+            .ToList();
+        RotationNames = _trackRotations
+            .Select(x=>x.name)
+            .ToList();
+        BodyPartNames = _bodyParts
+            .Select(x=>x.name)
+            .ToList();
+    }
+    public void OnReset()
+    {
+        Assert.IsTrue(_hasLazyInitialized);
+        ResetStatus();
+        LastIsSet = false;
+    }
+    public void ResetStatus()
+    {
+        CenterOfMassVelocity = Vector3.zero;
+        CenterOfMassVelocityMagnitude = 0f;
+        LastCenterOfMassInWorldSpace = transform.position;
+        GetAllPoints(Points);
+        Array.Copy(Points, 0, _lastPoints, 0, Points.Length);
+        for (int i = 0; i < Points.Length; i++)
+        {
+            PointVelocity[i] = Vector3.zero;
+        }
+        for (int i = 0; i < _trackRotations.Count; i++)
+        {
+            Quaternion localRotation = _trackRotations[i].transform.localRotation;
+            if (_trackRotations[i].gameObject == _root)
+                localRotation = Quaternion.Inverse(transform.rotation) * _trackRotations[i].transform.rotation;
+            Rotations[i] = localRotation;
+        }
+    }
+
+    public void SetStatusForStep(float timeDelta)
+    {
+        // find Center Of Mass and velocity
+        Vector3 newCOM;
+        if (_rigidbodyParts?.Count > 0)
+            newCOM = GetCenterOfMass(_rigidbodyParts);
+        else
+            newCOM = GetCenterOfMass(_articulationBodyParts);
+        if (!LastIsSet)
+        {
+            LastCenterOfMassInWorldSpace = newCOM;
+        }
+
+        // generate Horizontal Direction
+        var newHorizontalDirection = new Vector3(0f, _root.transform.eulerAngles.y, 0f);
+
+        // set this object to be f space
+        transform.position = newCOM;
+        transform.rotation = Quaternion.Euler(newHorizontalDirection);
+
+        // get Center Of Mass velocity in f space
+        var velocity = transform.position - LastCenterOfMassInWorldSpace;
+        velocity /= timeDelta;
+        CenterOfMassVelocity = transform.InverseTransformVector(velocity);
+        CenterOfMassVelocityMagnitude = CenterOfMassVelocity.magnitude;
+
+        LastCenterOfMassInWorldSpace = newCOM;
+        
+        GetAllPoints(Points);
+        if (!LastIsSet)
+        {
+            Array.Copy(Points, 0, _lastPoints, 0, Points.Length);
+        }
+        for (int i = 0; i < Points.Length; i++)
+        {
+            PointVelocity[i] = (Points[i] - _lastPoints[i]) / timeDelta;
+        }
+        Array.Copy(Points, 0, _lastPoints, 0, Points.Length);
+
+        for (int i = 0; i < _trackRotations.Count; i++)
+        {
+            Quaternion localRotation = _trackRotations[i].transform.localRotation;
+            if (_trackRotations[i].gameObject == _root)
+                localRotation = Quaternion.Inverse(transform.rotation) * _trackRotations[i].transform.rotation;
+            Rotations[i] = localRotation;
+
+        }
+
+        LastIsSet = true;
+    }
+
+    public List<float> GetPointDistancesFrom(DReConRewardStats target)
+    {
+        List<float> distances = new List<float>();
+        for (int i = 0; i < Points.Length; i++)
+        {
+            float distance = (Points[i] - target.Points[i]).magnitude;
+            distances.Add(distance);
+        }
+        return distances;
+    }
+
+    public List<float> GetPointVelocityDistancesFrom(DReConRewardStats target) {
+        List<float> distances = new List<float>();
+        for (int i = 0; i < PointVelocity.Length; i++) {
+            float distance = (PointVelocity[i] - target.PointVelocity[i]).magnitude;
+            distances.Add(distance);
+        }
+        return distances;
+    }
+
+    public void AssertIsCompatible(DReConRewardStats target)
+    {
+        Assert.AreEqual(Points.Length, target.Points.Length);
+        Assert.AreEqual(_lastPoints.Length, target._lastPoints.Length);
+        Assert.AreEqual(PointVelocity.Length, target.PointVelocity.Length);
+        Assert.AreEqual(Points.Length, _lastPoints.Length);
+        Assert.AreEqual(Points.Length, PointVelocity.Length);
+        Assert.AreEqual(_colliders.Count, target._colliders.Count);
+        for (int i = 0; i < _colliders.Count; i++)
+        {
+            string debugStr = $" _colliders.{_colliders[i].name} vs target._colliders.{target._colliders[i].name}";
+            Assert.AreEqual(_colliders[i].name, target._colliders[i].name, $"name:{debugStr}");
+            // Assert.AreEqual(_colliders[i].direction, target._colliders[i].direction, $"direction:{debugStr}");
+            // Assert.AreEqual(_colliders[i].height, target._colliders[i].height, $"height:{debugStr}");
+            // Assert.AreEqual(_colliders[i].radius, target._colliders[i].radius, $"radius:{debugStr}");
+        }
+        Assert.AreEqual(ColliderNames.Count, target.ColliderNames.Count);
+        Assert.AreEqual(RotationNames.Count, target.RotationNames.Count);
+        Assert.AreEqual(BodyPartNames.Count, target.BodyPartNames.Count);
+        for (int i = 0; i < ColliderNames.Count; i++)
+            Assert.AreEqual(ColliderNames[i], target.ColliderNames[i]);
+        for (int i = 0; i < RotationNames.Count; i++)
+            Assert.AreEqual(RotationNames[i], target.RotationNames[i]);
+        for (int i = 0; i < BodyPartNames.Count; i++)
+            Assert.AreEqual(BodyPartNames[i], target.BodyPartNames[i]);
+    }
+
+    void GetAllPoints(Vector3[] pointBuffer)
+    {
+        int idx = 0;
+        foreach (var collider in _colliders)
+        {
+            CapsuleCollider capsule = collider as CapsuleCollider;
+            BoxCollider box = collider as BoxCollider;
+            SphereCollider sphere = collider as SphereCollider;
+            Vector3 c = Vector3.zero;
+            Bounds b = new Bounds(c,c);
+
+            if (collider.name=="head")
+            {
+                c = c;
+            }
+
+            if (capsule != null)
+            {
+                c = capsule.center;
+                var r = capsule.radius*2;
+                var h = capsule.height;
+                h = Mathf.Max(r,h); // capsules height is clipped at r
+                if (capsule.direction == 0)
+                    b = new Bounds(c, new Vector3(h,r,r));
+                else if (capsule.direction == 1)
+                    b = new Bounds(c, new Vector3(r,h,r));
+                else if (capsule.direction == 2)
+                    b = new Bounds(c, new Vector3(r,r,h));
+                else throw new NotImplementedException();
+            }
+            else if (box != null)
+            {
+                c = box.center;
+                b = new Bounds(c, box.size);
+            }
+            else if (sphere != null)
+            {
+                c = sphere.center;
+                var r = sphere.radius*2;
+                b = new Bounds(c, new Vector3(r,r,r));
+            }
+            else
+                throw new NotImplementedException();
+
+            Vector3 point1, point2, point3, point4, point5, point6;
+            point1 = new Vector3(b.max.x, c.y, c.z);
+            point2 = new Vector3(b.min.x, c.y, c.z);
+            point3 = new Vector3(c.x, b.max.y, c.z);
+            point4 = new Vector3(c.x, b.min.y, c.z);
+            point5 = new Vector3(c.x, c.y, b.max.z);
+            point6 = new Vector3(c.x, c.y, b.min.z);
+            // from local collider space to world space
+            point1 = collider.transform.TransformPoint(point1);
+            point2 = collider.transform.TransformPoint(point2);
+            point3 = collider.transform.TransformPoint(point3);
+            point4 = collider.transform.TransformPoint(point4);
+            point5 = collider.transform.TransformPoint(point5);
+            point6 = collider.transform.TransformPoint(point6);
+            // transform from world space, into local space for COM 
+            point1 = this.transform.InverseTransformPoint(point1);
+            point2 = this.transform.InverseTransformPoint(point2);
+            point3 = this.transform.InverseTransformPoint(point3);
+            point4 = this.transform.InverseTransformPoint(point4);
+            point5 = this.transform.InverseTransformPoint(point5);
+            point6 = this.transform.InverseTransformPoint(point6);
+
+            pointBuffer[idx++] = point1;
+            pointBuffer[idx++] = point2;
+            pointBuffer[idx++] = point3;
+            pointBuffer[idx++] = point4;
+            pointBuffer[idx++] = point5;
+            pointBuffer[idx++] = point6;
+        }
+    }
+	Vector3 GetCenterOfMass(IEnumerable<ArticulationBody> bodies)
+	{
+		var centerOfMass = Vector3.zero;
+		float totalMass = 0f;
+		foreach (ArticulationBody ab in bodies)
+		{
+			centerOfMass += ab.worldCenterOfMass * ab.mass;
+			totalMass += ab.mass;
+		}
+		centerOfMass /= totalMass;
+		// centerOfMass -= _spawnableEnv.transform.position;
+		return centerOfMass;        
+	}    
+	Vector3 GetCenterOfMass(IEnumerable<Rigidbody> bodies)
+	{
+		var centerOfMass = Vector3.zero;
+		float totalMass = 0f;
+		foreach (Rigidbody ab in bodies)
+		{
+			centerOfMass += ab.worldCenterOfMass * ab.mass;
+			totalMass += ab.mass;
+		}
+		centerOfMass /= totalMass;
+		// centerOfMass -= _spawnableEnv.transform.position;
+		return centerOfMass;
+	}    
+    public void DrawPointDistancesFrom(DReConRewardStats target, int objIdex)
+    {
+        int start = 0;
+        int end = Points.Length-1;
+        if (objIdex >=0)
+        {
+            start = objIdex*6;
+            end = (objIdex*6)+6;
+        }
+        for (int i = start; i < end; i++)
+        {
+            Gizmos.color = Color.white;
+            var from = Points[i];
+            var to = target.Points[i];
+            var toTarget = target.Points[i];
+            // transform to this object's world space
+            from = this.transform.TransformPoint(from);
+            to = this.transform.TransformPoint(to);
+            // transform to target's world space
+            toTarget = target.transform.TransformPoint(toTarget);
+            Gizmos.color = Color.white;
+            Gizmos.DrawLine(from, toTarget);
+            // show this objects velocity
+            Vector3 velocity = PointVelocity[i];
+            Gizmos.color = Color.blue;
+            Gizmos.DrawRay(from, velocity);
+            // show targets velocity
+            Vector3 velocityTarget = target.PointVelocity[i];
+            Gizmos.color = Color.green;
+            Gizmos.DrawRay(toTarget, velocityTarget);
+        }
+    }
+    public void ShiftCOM (Vector3 snapDistance)
+    {
+        Vector3 newCOM = LastCenterOfMassInWorldSpace + snapDistance;
+        LastCenterOfMassInWorldSpace = newCOM;
+        transform.position = newCOM;
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewardStats.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewards.cs

@@ -0,0 +1,217 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+using UnityEngine.Assertions;
+using ManyWorlds;
+
+public class DReConRewards : MonoBehaviour
+{
+    [Header("Reward")]
+    public float SumOfSubRewards;
+    public float Reward;
+
+    [Header("Position Reward")]
+    public float SumOfDistances;
+    public float SumOfSqrDistances;
+    public float PositionReward;
+
+    [Header("Velocity Reward")]
+    public float PointsVelocityDifferenceSquared;
+    public float PointsVelocityReward;
+
+    [Header("Local Pose Reward")]
+    public List<float> RotationDifferences;
+    public float SumOfRotationDifferences;
+    public float SumOfRotationSqrDifferences;
+    public float LocalPoseReward;
+
+   
+    
+    [Header("Center of Mass Velocity Reward")]
+    public Vector3 MocapCOMVelocity;
+    public Vector3 RagDollCOMVelocity;
+
+    public float COMVelocityDifference;
+    public float ComReward;
+
+
+    [Header("Distance Factor")]
+    public float ComDistance;
+    public float DistanceFactor;
+
+    // [Header("Direction Factor")]
+    // public float DirectionDistance;
+    // public float DirectionFactor;
+
+
+    [Header("Misc")]
+    public float HeadHeightDistance;
+
+    [Header("Gizmos")]
+    public int ObjectForPointDistancesGizmo;
+
+    SpawnableEnv _spawnableEnv;
+    MocapControllerArtanim _mocap;
+    GameObject _ragDoll;
+    InputController _inputController;
+
+    internal DReConRewardStats _mocapBodyStats;
+    internal DReConRewardStats _ragDollBodyStats;
+
+    // List<ArticulationBody> _mocapBodyParts;
+    // List<ArticulationBody> _ragDollBodyParts;
+    Transform _mocapHead;
+    Transform _ragDollHead;
+
+    bool _hasLazyInitialized;
+
+    [Header("Things to check for rewards")]
+    public string headname = "head";
+
+    public string targetedRootName = "articulation:Hips";
+
+
+    public void OnAgentInitialize()
+    {
+        Assert.IsFalse(_hasLazyInitialized);
+
+        _hasLazyInitialized = true;
+
+        _spawnableEnv = GetComponentInParent<SpawnableEnv>();
+        Assert.IsNotNull(_spawnableEnv);
+
+        _mocap = _spawnableEnv.GetComponentInChildren<MocapControllerArtanim>();
+
+        _ragDoll = _spawnableEnv.GetComponentInChildren<RagDollAgent>().gameObject;
+        Assert.IsNotNull(_mocap);
+        Assert.IsNotNull(_ragDoll);
+        _inputController = _spawnableEnv.GetComponentInChildren<InputController>();
+        // _mocapBodyParts = _mocap.GetComponentsInChildren<ArticulationBody>().ToList();
+        // _ragDollBodyParts = _ragDoll.GetComponentsInChildren<ArticulationBody>().ToList();
+        // Assert.AreEqual(_mocapBodyParts.Count, _ragDollBodyParts.Count);
+        _mocapHead = _mocap
+            .GetComponentsInChildren<Transform>()
+            .First(x => x.name == headname);
+        _ragDollHead = _ragDoll
+            .GetComponentsInChildren<Transform>()
+            .First(x => x.name == headname);
+        _mocapBodyStats = new GameObject("MocapDReConRewardStats").AddComponent<DReConRewardStats>();
+        _mocapBodyStats.setRootName(targetedRootName);
+
+        _mocapBodyStats.ObjectToTrack = _mocap;
+
+        _mocapBodyStats.transform.SetParent(_spawnableEnv.transform);
+        _mocapBodyStats.OnAgentInitialize(_mocapBodyStats.ObjectToTrack.transform);
+
+        _ragDollBodyStats= new GameObject("RagDollDReConRewardStats").AddComponent<DReConRewardStats>();
+        _ragDollBodyStats.setRootName(targetedRootName);
+
+
+        _ragDollBodyStats.ObjectToTrack = this;
+        _ragDollBodyStats.transform.SetParent(_spawnableEnv.transform);
+        _ragDollBodyStats.OnAgentInitialize(transform, _mocapBodyStats);      
+
+        _mocapBodyStats.AssertIsCompatible(_ragDollBodyStats);    
+    }
+
+    // Update is called once per frame
+    public void OnStep(float timeDelta)
+    {
+        _mocapBodyStats.SetStatusForStep(timeDelta);
+        _ragDollBodyStats.SetStatusForStep(timeDelta);
+
+        // position reward
+        List<float> distances = _mocapBodyStats.GetPointDistancesFrom(_ragDollBodyStats);
+        PositionReward = -7.37f/(distances.Count/6f);
+        List<float> sqrDistances = distances.Select(x=> x*x).ToList();
+        SumOfDistances = distances.Sum();
+        SumOfSqrDistances = sqrDistances.Sum();
+        PositionReward *= SumOfSqrDistances;
+        PositionReward = Mathf.Exp(PositionReward);
+
+        // center of mass velocity reward
+        MocapCOMVelocity = _mocapBodyStats.CenterOfMassVelocity;
+        RagDollCOMVelocity = _ragDollBodyStats.CenterOfMassVelocity;
+        COMVelocityDifference = (MocapCOMVelocity-RagDollCOMVelocity).magnitude;
+        ComReward = -Mathf.Pow(COMVelocityDifference,2);
+        ComReward = Mathf.Exp(ComReward);
+
+        // points velocity
+        List<float> velocityDistances = _mocapBodyStats.GetPointVelocityDistancesFrom(_ragDollBodyStats);
+        List<float> sqrVelocityDistances = velocityDistances.Select(x=> x*x).ToList();
+        PointsVelocityDifferenceSquared = sqrVelocityDistances.Sum();
+        PointsVelocityReward = (-1f/_mocapBodyStats.PointVelocity.Length) * PointsVelocityDifferenceSquared;
+        PointsVelocityReward = Mathf.Exp(PointsVelocityReward);
+
+        // local pose reward
+        if (RotationDifferences == null || RotationDifferences.Count < _mocapBodyStats.Rotations.Count)
+            RotationDifferences = Enumerable.Range(0,_mocapBodyStats.Rotations.Count)
+            .Select(x=>0f)
+            .ToList();
+        SumOfRotationDifferences = 0f;
+        SumOfRotationSqrDifferences = 0f;
+        for (int i = 0; i < _mocapBodyStats.Rotations.Count; i++)
+        { 
+            var angle = Quaternion.Angle(_mocapBodyStats.Rotations[i], _ragDollBodyStats.Rotations[i]);
+            Assert.IsTrue(angle <= 180f);
+            angle = DReConObservationStats.NormalizedAngle(angle);
+            var sqrAngle = angle * angle;
+            RotationDifferences[i] = angle;
+            SumOfRotationDifferences += angle;
+            SumOfRotationSqrDifferences += sqrAngle;
+        }
+        LocalPoseReward = -6.5f/RotationDifferences.Count;
+        LocalPoseReward *= SumOfRotationSqrDifferences;
+        LocalPoseReward = Mathf.Exp(LocalPoseReward);
+
+        // distance factor
+        ComDistance = (_mocapBodyStats.transform.position - _ragDollBodyStats.transform.position).magnitude;
+        DistanceFactor = Mathf.Pow(ComDistance,2);
+        DistanceFactor = 1.4f*DistanceFactor;
+        DistanceFactor = 1.01f-DistanceFactor;
+        DistanceFactor = Mathf.Clamp(DistanceFactor, 0f, 1f);
+
+        // // direction factor
+        // Vector3 desiredDirection = _inputController.HorizontalDirection;
+        // var curDirection = _ragDollBodyStats.transform.forward;
+        // // cosAngle
+        // var directionDifference = Vector3.Dot(desiredDirection, curDirection);
+        // DirectionDistance = (1f + directionDifference) /2f; // normalize the error 
+        // DirectionFactor = Mathf.Pow(DirectionDistance,2);
+        // DirectionFactor = Mathf.Clamp(DirectionFactor, 0f, 1f);
+
+        // misc
+        HeadHeightDistance = (_mocapHead.position.y - _ragDollHead.position.y);
+        HeadHeightDistance = Mathf.Abs(HeadHeightDistance);
+
+        // reward
+        SumOfSubRewards = PositionReward+ComReward+PointsVelocityReward+LocalPoseReward;
+        Reward = DistanceFactor*SumOfSubRewards;
+        // Reward = (DirectionFactor*SumOfSubRewards) * DistanceFactor;
+    }
+    public void OnReset()
+    {
+        Assert.IsTrue(_hasLazyInitialized);
+       
+        _mocapBodyStats.OnReset();
+        _ragDollBodyStats.OnReset();
+        _ragDollBodyStats.transform.position = _mocapBodyStats.transform.position;
+        _ragDollBodyStats.transform.rotation = _mocapBodyStats.transform.rotation;
+    }
+    public void ShiftMocapCOM(Vector3 snapDistance)
+    {
+        _mocapBodyStats.ShiftCOM(snapDistance);
+    }
+
+    void OnDrawGizmos()
+    {
+        if (_ragDollBodyStats == null)
+            return;
+        var max = (_ragDollBodyStats.Points.Length/6)-1;
+        ObjectForPointDistancesGizmo = Mathf.Clamp(ObjectForPointDistancesGizmo, -1, max);
+        // _mocapBodyStats.DrawPointDistancesFrom(_ragDollBodyStats, ObjectForPointDistancesGizmo);
+        _ragDollBodyStats.DrawPointDistancesFrom(_mocapBodyStats, ObjectForPointDistancesGizmo);
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewards.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugAddOffset.cs

@@ -0,0 +1,29 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+
+//a class used to find out the offsets that we need to apply to the method Remap2Character, to go from a Physics-based character to a kinematic one
+//make sure you use the axis in local mode to find out rapidly the offset needed
+public class DebugAddOffset : MonoBehaviour
+{
+    /* [SerializeField]
+     Vector3 axis;
+     [SerializeField]
+     float angleDegrees;
+    */
+
+    [SerializeField]
+    Vector3 eulerAngles;
+
+    [SerializeField]
+    bool applyOffset = false;
+
+    // Update is called once per frame
+    void LateUpdate()
+    {
+        if (applyOffset)
+            // transform.localRotation =   transform.localRotation * Quaternion.AngleAxis(angleDegrees, axis);
+            transform.localRotation = Quaternion.Euler(eulerAngles.x, eulerAngles.y, eulerAngles.z) * transform.localRotation;
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugAddOffset.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugJoints.cs

@@ -0,0 +1,177 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class DebugJoints : MonoBehaviour
+{
+    public float SphereSize = 0.03f;
+
+    static Color[] _axisColor = { 
+        new Color(219f / 255, 62f / 255, 29f / 255, .93f), 
+        new Color(154f / 255, 243f / 255, 72f / 255, .93f), 
+        new Color(58f / 255, 122f / 255, 248f / 255, .93f)};
+    static Vector3[] _axisVector = { Vector3.right, Vector3.up, Vector3.forward };
+    ArticulationBody _body;
+    ArticulationBody _parentBody;
+    MarathonTestBedController _debugController;
+    // Start is called before the first frame update
+    ManyWorlds.SpawnableEnv _spawnableEnv;
+    MocapControllerArtanim _mocapController;
+    Rigidbody _target;
+    public Vector3 TargetRotationInJointSpace;
+    public Vector3 RotationInJointSpace;
+    public Vector3 RotationInJointSpaceError;
+    public Vector3 RotationInJointSpaceErrorRad;
+
+    public Vector3 JointPositionDeg;
+    public Vector3 JointTargetDeg;
+    public Vector3 JointPositionRad;
+    public Vector3 JointTargetRad;
+
+    void Start()
+    {
+        _body = GetComponent<ArticulationBody>();
+        _parentBody = _body.transform.parent.GetComponentInParent<ArticulationBody>();
+        _debugController = FindObjectOfType<MarathonTestBedController>();
+        _spawnableEnv = GetComponentInParent<ManyWorlds.SpawnableEnv>();
+        _mocapController = _spawnableEnv.GetComponentInChildren<MocapControllerArtanim>();
+        var mocapBodyParts = _mocapController.GetComponentsInChildren<Rigidbody>().ToList();
+        _target = mocapBodyParts.First(x=>x.name == _body.name);
+    }
+
+
+
+
+    // Update is called once per frame
+    void FixedUpdate()
+    {
+        if (_body == null)
+            return;
+        if (_body.jointType != ArticulationJointType.SphericalJoint)
+            return;
+
+        RotationInJointSpace = -(Quaternion.Inverse(_body.anchorRotation) * Quaternion.Inverse(_body.transform.localRotation) * _body.parentAnchorRotation).eulerAngles;
+        TargetRotationInJointSpace = -(Quaternion.Inverse(_body.anchorRotation) * Quaternion.Inverse(_target.transform.localRotation) * _body.parentAnchorRotation).eulerAngles;
+        RotationInJointSpaceError = TargetRotationInJointSpace-RotationInJointSpace;
+        RotationInJointSpace = new Vector3(
+            Mathf.DeltaAngle(0, RotationInJointSpace.x),
+            Mathf.DeltaAngle(0, RotationInJointSpace.y),
+            Mathf.DeltaAngle(0, RotationInJointSpace.z));
+        TargetRotationInJointSpace = new Vector3(
+            Mathf.DeltaAngle(0, TargetRotationInJointSpace.x),
+            Mathf.DeltaAngle(0, TargetRotationInJointSpace.y),
+            Mathf.DeltaAngle(0, TargetRotationInJointSpace.z));
+        RotationInJointSpaceError = new Vector3(
+            Mathf.DeltaAngle(0, RotationInJointSpaceError.x),
+            Mathf.DeltaAngle(0, RotationInJointSpaceError.y),
+            Mathf.DeltaAngle(0, RotationInJointSpaceError.z));
+        RotationInJointSpaceErrorRad = RotationInJointSpaceError * Mathf.Deg2Rad;
+        JointTargetDeg.x = TargetRotationInJointSpace.y;
+        JointTargetDeg.y = TargetRotationInJointSpace.z;
+        JointTargetDeg.z = TargetRotationInJointSpace.x;
+
+        var jointPosition = _body.jointPosition;
+        JointPositionDeg = Vector3.zero;
+        int i = 0;
+        if (_body.twistLock == ArticulationDofLock.LimitedMotion)
+            JointPositionDeg.x = jointPosition[i++];
+        if (_body.swingYLock == ArticulationDofLock.LimitedMotion)
+            JointPositionDeg.y = jointPosition[i++];
+        if (_body.swingZLock == ArticulationDofLock.LimitedMotion)
+            JointPositionDeg.z = jointPosition[i++];
+        float stiffness = 1000f;
+        float damping = 100f;
+        JointPositionDeg *= Mathf.Rad2Deg;
+
+        bool dontUpdateMotor = _debugController.DontUpdateMotor;
+        dontUpdateMotor &= _debugController.isActiveAndEnabled;
+        dontUpdateMotor &= _debugController.gameObject.activeInHierarchy;
+        if(dontUpdateMotor)
+        {
+    		// var drive = _body.yDrive;
+            // drive.stiffness = stiffness;
+            // drive.damping = damping;
+            // drive.target = JointTargetDeg.x;
+            // _body.yDrive = drive;
+            
+            // drive = _body.zDrive;
+            // drive.stiffness = stiffness;
+            // drive.damping = damping;
+            // drive.target = JointTargetDeg.y;
+            // _body.zDrive = drive;
+
+            // drive = _body.xDrive;
+            // drive.stiffness = stiffness;
+            // drive.damping = damping;
+            // drive.target = JointTargetDeg.z;
+            // _body.xDrive = drive;
+    		var drive = _body.xDrive;
+            drive.stiffness = stiffness;
+            drive.damping = damping;
+            drive.target = JointTargetDeg.x;
+            _body.xDrive = drive;
+            
+            drive = _body.yDrive;
+            drive.stiffness = stiffness;
+            drive.damping = damping;
+            drive.target = JointTargetDeg.y;
+            _body.yDrive = drive;
+
+            drive = _body.zDrive;
+            drive.stiffness = stiffness;
+            drive.damping = damping;
+            drive.target = JointTargetDeg.z;
+            _body.zDrive = drive;            
+        }
+        else
+        {
+    		// var drive = _body.yDrive;
+            // JointTargetDeg = Vector3.zero;
+            // JointTargetDeg.x = drive.target;
+            // drive = _body.zDrive;
+            // JointTargetDeg.y = drive.target;
+            // drive = _body.xDrive;
+            // JointTargetDeg.z = drive.target;
+    		var drive = _body.xDrive;
+            JointTargetDeg = Vector3.zero;
+            JointTargetDeg.x = drive.target;
+            drive = _body.yDrive;
+            JointTargetDeg.y = drive.target;
+            drive = _body.zDrive;
+            JointTargetDeg.z = drive.target;            
+        }
+        
+        JointPositionRad = JointPositionDeg * Mathf.Deg2Rad;
+        JointTargetRad = JointTargetDeg * Mathf.Deg2Rad;
+    }
+    public static Quaternion FromToRotation(Quaternion from, Quaternion to) {
+        if (to == from) return Quaternion.identity;
+
+        return to * Quaternion.Inverse(from);
+    }
+
+    // void OnDrawGizmos()
+    void OnDrawGizmosSelected()
+    {
+        if (_body == null)
+            return;
+        Gizmos.color = Color.white;
+        Vector3 position = _body.transform.TransformPoint(_body.anchorPosition);
+        Quaternion rotation = _body.transform.rotation * _body.anchorRotation;
+
+
+        for (int i = 0; i < _axisColor.Length; i++)
+        {
+            var axisColor = _axisColor[i];
+            var axis = _axisVector[i];
+            Gizmos.color = axisColor;
+            // Vector3 rotationEul = _body.transform.TransformDirection(_body.anchorRotation * axis);
+            Vector3 rotationEul = rotation * axis;
+            Gizmos.DrawSphere(position, SphereSize);
+            Vector3 direction = rotationEul;
+            Gizmos.DrawRay(position, direction);
+            
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugJoints.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MappingOffset.cs

@@ -0,0 +1,174 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+
+
+public class MappingOffset
+{
+    //string _tName;
+    Transform _t;
+    //string _rbName;
+    Rigidbody _rb;
+
+    //the ragdoll in the physics based anim uses articulated bodies, instead of rigidbodies:
+    ArticulationBody _ab;
+
+    Quaternion _offsetRB2Bone;
+
+
+
+    private bool _isRoot = false;
+    private Vector3 _debugDistance = Vector3.zero;
+    private bool _debugWithRigidBody;
+
+    //this variables define two completely different modes. In one it updates rigidbodies from transforms, in the other it updates transforms from articulationbodies. 
+    //see contstructors and function UpdateAnimation
+    private bool _updateRigidBodies = false;
+
+    Transform _tson = null;
+
+
+
+    public MappingOffset(Transform t, Rigidbody rb, Quaternion offset)
+    {
+
+        _t = t;
+        _rb = rb;
+        
+        //this causes trouble with mecanim, probably because when it is done there is already an animation pose loaded
+        //_offsetRB2Bone = offset;
+        _offsetRB2Bone = Quaternion.identity;
+
+
+        _ab = null;
+
+        _updateRigidBodies = true;
+
+
+
+
+    }
+
+
+    public MappingOffset(Transform t, ArticulationBody ab, Quaternion offset)
+    {
+
+        _t = t;
+        _rb = null;
+
+        //this causes trouble with mecanim, probably because when it is done there is already an animation pose loaded
+        //_offsetRB2Bone = offset;
+        _offsetRB2Bone = Quaternion.identity;
+
+        _ab = ab;
+
+        _updateRigidBodies = false;
+    }
+
+
+
+    public void SetAsRoot(bool b = true, float offset = 0.0f)
+    {
+
+        _isRoot = b;
+        _debugDistance.z = -offset;
+
+    }
+
+
+    //this is a special function used inside RagdollControllerArtanim, it is only used to check the mapping between physical and ragdoll characters works well
+    public void SetAsRagdollcontrollerDebug(bool debugWithRigidBody)
+    {
+        _debugWithRigidBody = debugWithRigidBody;
+
+
+    }
+
+
+    //public void SetSon(Transform son) {
+
+    //    if (!_updateRigidBodies)
+    //        Debug.LogError("using son transform only makes sense when we are in the mode that updates the rigidbodies form the transforms. Please check how you initialize this class");
+
+    //    _tson = son;
+    
+    
+    //}
+
+
+    public bool UpdateRigidBodies { get => _updateRigidBodies; set => _updateRigidBodies = value; }
+
+    public void UpdateRotation()
+    {
+
+        if (_updateRigidBodies)
+        {
+          
+           
+
+            if (_debugWithRigidBody)
+            {
+                _t.transform.localRotation = _offsetRB2Bone * _rb.transform.localRotation;
+
+                if (_isRoot)
+                {
+                    _t.transform.rotation = _rb.rotation;
+                    _t.transform.position = _rb.position + _debugDistance;
+
+
+                }
+
+
+
+            }
+            else
+            {
+                //THE MAIN OPERATION, used most frequently when called this function:
+                if (_isRoot)
+                {
+                    _rb.transform.rotation = _t.rotation;
+                    _rb.transform.position = _t.position + _debugDistance;
+
+
+                }
+                else if (_tson != null)
+                {
+
+                    //the center of this thing is in the wrong position
+                    //  Vector3 pos = (_tson.position - _t.position);
+                    //  _rb.position = _t.transform.position + (pos/2) +_debugDistance;
+
+                    //target.transform.rotation = animStartBone.transform.rotation* rotationOffset;
+
+                }
+                else { 
+                    // _rb.transform.rotation = _offsetRB2Bone * _t.rotation;
+
+                    //using the local rotation makes sure we do take into account rotation of their parents (for example, the call of this function for the arm, when rotating the spine)
+                    _rb.transform.localRotation = _offsetRB2Bone * _t.localRotation;
+                }
+            }
+        }
+        else
+        {
+            //_t.rotation = _offsetRB2Bone * _rb.transform.rotation;
+            _t.rotation = _offsetRB2Bone * _ab.transform.rotation;
+            if (_isRoot)
+            {
+                _t.position = _ab.transform.position + _debugDistance;
+
+                //TEST TEST TEST. we override the offset decided before to make it match
+                //_t.rotation = _ab.transform.rotation; 
+
+            }
+        }
+    }
+
+  
+
+
+
+}
+
+

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MappingOffset.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapAnimatorController.cs

@@ -0,0 +1,344 @@
+
+using Unity.MLAgents;
+using UnityEngine;
+using ManyWorlds;
+
+
+public class MocapAnimatorController : MonoBehaviour
+{
+    public float MaxForwardVelocity = 1f;        // Max run speed.
+    public float MinTurnVelocity = 400f;         // Turn velocity when moving at maximum speed.
+    public float MaxTurnVelocity = 1200f / 3;        // Turn velocity when stationary.
+    public float JumpSpeed = 10f;                // 
+    public bool debugForceJump;
+    Animator _anim;
+    CharacterController _characterController;
+    SpawnableEnv _spawnableEnv;
+    InputController _inputController;
+
+    bool _isGrounded;
+    bool _previouslyGrounded;
+    const float kAirborneTurnSpeedProportion = 5.4f;
+    const float kGroundTurnSpeedProportion = 200f/2;
+    const float kGroundedRayDistance = 1f;
+    const float kJumpAbortSpeed = 10f;
+    const float kMinEnemyDotCoeff = 0.2f;
+    const float kInverseOneEighty = 1f / 180f;
+    const float kStickingGravityProportion = 0.3f;
+
+    Material materialUnderFoot;
+    float _forwardVelocity;
+    Vector3 _lastGroundForwardVelocity;
+    float _desiredForwardSpeed;
+    float _verticalVelocity = -1f;
+    
+    Quaternion _targetDirection;    // direction we want to move towards
+    float _angleDiff;               // delta between targetRotation and current roataion
+    Quaternion _targetRotation;
+    bool _readyToJump;
+    bool _inCombo;
+    int _layerMask;
+
+    //for debugging, we disable this when setTpose in MarathonTestBedController is on
+    [HideInInspector]
+    public bool doFixedUpdate = true;
+
+
+    [SerializeField]
+    bool _isGeneratedProcedurally = false;
+
+    public bool IsGeneratedProcedurally { set => _isGeneratedProcedurally = value; }
+
+
+    protected bool IsMoveInput
+    {
+        get { return !Mathf.Approximately(_inputController.MovementVector.sqrMagnitude, 0f); }
+    }
+
+
+	public void OnAgentInitialize()
+    {
+        _anim = GetComponent<Animator>();
+        _characterController = GetComponent<CharacterController>();
+        _spawnableEnv = GetComponentInParent<SpawnableEnv>();
+        _inputController = _spawnableEnv.GetComponentInChildren<InputController>();
+        _targetDirection = Quaternion.Euler(0, 90, 0);
+        var ragDoll = _spawnableEnv.GetComponentInChildren<RagDollAgent>( true);//we include inactive childs
+        if (ragDoll)//in the ROM extraction case we do not have any ragdoll agent
+        {
+            _layerMask = 1 << ragDoll.gameObject.layer;
+            _layerMask |= 1 << this.gameObject.layer;
+            _layerMask = ~(_layerMask);
+        }
+    }
+
+    void Update()
+    {
+    }
+
+    void FixedUpdate()
+    {
+
+        if(doFixedUpdate)
+            OnFixedUpdate();
+    }
+
+
+    void OnFixedUpdate()
+    {
+        // RotateTarget(Time.fixedDeltaTime);
+        SetTargetFromMoveInput();
+        CalculateForwardMovement(Time.fixedDeltaTime);
+        CalculateVerticalMovement(Time.fixedDeltaTime);
+
+        if (this.IsMoveInput)
+            SetTargetRotation();
+
+        UpdateOrientation(Time.fixedDeltaTime);
+
+        // PlayAudio();
+
+        // TimeoutToIdle();
+
+        _previouslyGrounded = _isGrounded;
+    }
+
+    public void OnReset()
+    {
+
+        if (_isGeneratedProcedurally)
+            doFixedUpdate = false;
+
+
+
+            _isGrounded = true;
+        _previouslyGrounded = true;
+        _inCombo = false;
+        _readyToJump = false;
+        _forwardVelocity = 0f;
+        _lastGroundForwardVelocity = Vector3.zero;
+        _desiredForwardSpeed = 0f;
+        _verticalVelocity = 0f;
+        _angleDiff = 0f;
+
+        if (!doFixedUpdate)
+            return;
+
+
+
+        _anim.SetBool("onGround", _isGrounded);
+        // _anim.SetFloat("verticalVelocity", _verticalVelocity);
+        _anim.SetFloat("angleDeltaRad", _angleDiff * Mathf.Deg2Rad);
+        _anim.SetFloat("forwardVelocity", _forwardVelocity);
+        _anim.SetBool("backflip", false);
+        _anim.Rebind();
+        _anim.SetBool("onGround", _isGrounded);
+        // _anim.SetFloat("verticalVelocity", _verticalVelocity);
+        _anim.SetFloat("angleDeltaRad", _angleDiff * Mathf.Deg2Rad);
+        _anim.SetFloat("forwardVelocity", _forwardVelocity);
+        _anim.SetBool("backflip", false);
+            
+
+        OnFixedUpdate();
+        _anim.Update(0f);
+
+
+     
+
+    }
+
+
+    // Called each physics step (so long as the Animator component is set to Animate Physics) after FixedUpdate to override root motion.
+    void OnAnimatorMove()
+    {
+        if (_anim == null)
+            return;
+        Vector3 movement;
+        float verticalVelocity = _verticalVelocity;
+        if (_isGrounded)
+        {
+            // find ground
+            RaycastHit hit;
+            Ray ray = new Ray(transform.position + Vector3.up * kGroundedRayDistance * 0.5f, -Vector3.up);
+            if (Physics.Raycast(ray, out hit, kGroundedRayDistance, _layerMask, QueryTriggerInteraction.Ignore))
+            {
+                // project velocity on plane
+                movement = _anim.deltaPosition;
+                movement.y = 0f;
+                movement = Vector3.ProjectOnPlane(_anim.deltaPosition, hit.normal);
+                
+                // store material under foot
+                Renderer groundRenderer = hit.collider.GetComponentInChildren<Renderer>();
+                materialUnderFoot = groundRenderer ? groundRenderer.sharedMaterial : null;
+            }
+            else
+            {
+                // fail safe incase ray does not collide
+                movement = _anim.deltaPosition;
+                materialUnderFoot = null;
+            }
+            _lastGroundForwardVelocity = movement / Time.deltaTime;
+        }
+        else
+        {
+            movement = _lastGroundForwardVelocity * Time.deltaTime;
+        }
+        // Rotate the transform of the character controller by the animation's root rotation.
+        _characterController.transform.rotation *= _anim.deltaRotation;
+        // print ($"delta:{_anim.deltaPosition.magnitude} movement:{movement.magnitude} delta:{_anim.deltaPosition} movement:{movement}");
+
+        // Add to the movement with the calculated vertical speed.
+        movement += verticalVelocity * Vector3.up * Time.deltaTime;
+
+        // Move the character controller.
+        _characterController.Move(movement);
+
+        // After the movement store whether or not the character controller is grounded.
+        _isGrounded = _characterController.isGrounded;
+
+        // If Ellen is not on the ground then send the vertical speed to the animator.
+        // This is so the vertical speed is kept when landing so the correct landing animation is played.
+
+
+        if (!_isGeneratedProcedurally) { 
+            if (!_isGrounded)
+                _anim.SetFloat("verticalVelocity", verticalVelocity);
+
+            // Send whether or not Ellen is on the ground to the animator.
+            _anim.SetBool("onGround", _isGrounded);
+        }
+    }
+
+    void RotateTarget(float deltaTime)
+    {
+        if (!Mathf.Approximately(_inputController.CameraRotation.x*_inputController.CameraRotation.x, 0f))
+        {
+            float roation = _targetDirection.eulerAngles.y;
+            float delta = _inputController.CameraRotation.x * kGroundTurnSpeedProportion * deltaTime;
+            roation += delta;
+            // print($"{_targetDirection.eulerAngles.y} delta:{delta}, {roation}");
+            _targetDirection = Quaternion.Euler(0f, roation, 0f);
+        }
+    }
+    void SetTargetFromMoveInput()
+    {
+        if (!_inputController) //if it is used without a ragdoll agent (for example, for ROM extraction), we still need to initialize it
+            OnAgentInitialize();
+
+
+        Vector2 moveInput = _inputController.MovementVector;
+        Vector3 localMovementDirection = new Vector3(moveInput.x, 0f, moveInput.y).normalized;
+        _targetDirection = Quaternion.Euler(localMovementDirection);
+    }
+
+    void SetTargetRotation()
+    {
+        // Create three variables, move input local to the player, flattened forward direction of the camera and a local target rotation.
+        Vector2 moveInput = _inputController.MovementVector;
+        Vector3 localMovementDirection = new Vector3(moveInput.x, 0f, moveInput.y).normalized;
+        
+        Vector3 forward = _targetDirection * Vector3.forward;
+        forward.y = 0f;
+        forward.Normalize();
+
+        Quaternion targetRotation;
+        
+        // // If the local movement direction is the opposite of forward then the target rotation should be towards the camera.
+        // if (Mathf.Approximately(Vector3.Dot(localMovementDirection, Vector3.forward), -1.0f))
+        // {
+        //     targetRotation = Quaternion.LookRotation(-forward);
+        // }
+        // else
+        // {
+        //     // Otherwise the rotation should be the offset of the input from the camera's forward.
+        //     Quaternion cameraToInputOffset = Quaternion.FromToRotation(Vector3.forward, localMovementDirection);
+        //     targetRotation = Quaternion.LookRotation(cameraToInputOffset * forward);
+        // }
+        // targetRotation = Quaternion.LookRotation(-forward);
+        Quaternion cameraToInputOffset = Quaternion.FromToRotation(Vector3.forward, localMovementDirection);
+        targetRotation = Quaternion.LookRotation(cameraToInputOffset * forward);
+
+
+        // The desired forward direction.
+        Vector3 resultingForward = targetRotation * Vector3.forward;
+
+        // Find the difference between the current rotation of the player and the desired rotation of the player in radians.
+        float angleCurrent = Mathf.Atan2(transform.forward.x, transform.forward.z) * Mathf.Rad2Deg;
+        float targetAngle = Mathf.Atan2(resultingForward.x, resultingForward.z) * Mathf.Rad2Deg;
+
+        _angleDiff = Mathf.DeltaAngle(angleCurrent, targetAngle);
+        _targetRotation = targetRotation;
+    }    
+    void UpdateOrientation(float deltaTime)
+    {
+        _anim.SetFloat("angleDeltaRad", _angleDiff * Mathf.Deg2Rad);
+
+        Vector3 localInput = new Vector3(_inputController.MovementVector.x, 0f, _inputController.MovementVector.y);
+        float groundedTurnSpeed = Mathf.Lerp(MaxTurnVelocity, MinTurnVelocity, _forwardVelocity / _desiredForwardSpeed);
+        float actualTurnSpeed = _isGrounded ? groundedTurnSpeed : Vector3.Angle(transform.forward, localInput) * kInverseOneEighty * kAirborneTurnSpeedProportion * groundedTurnSpeed;
+        _targetRotation = Quaternion.RotateTowards(transform.rotation, _targetRotation, actualTurnSpeed * deltaTime);
+        bool hasNan = float.IsNaN(_targetRotation.x) || float.IsNaN(_targetRotation.y) ||float.IsNaN(_targetRotation.z);
+        if (!hasNan)
+            transform.rotation = _targetRotation;
+    }
+
+    void CalculateForwardMovement(float deltaTime)
+    {
+        // Cache the move input and cap it's magnitude at 1.
+        Vector2 moveInput = _inputController.MovementVector;
+        if (moveInput.sqrMagnitude > 1f)
+            moveInput.Normalize();
+
+        // Calculate the speed intended by input.
+        _desiredForwardSpeed = moveInput.magnitude * MaxForwardVelocity;
+
+        // Note: acceleration is handle in InputController
+        _forwardVelocity = _desiredForwardSpeed;
+
+        // Set the animator parameter to control what animation is being played.
+        _anim.SetFloat("forwardVelocity", _forwardVelocity);
+    }
+    void CalculateVerticalMovement(float deltaTime)
+    {
+        // If jump is not currently held and is on the ground then ready to jump.
+        if (!_inputController.Jump && _isGrounded)
+            _readyToJump = true;
+
+        _anim.SetBool("backflip", _inputController.Backflip);
+
+        if (_isGrounded)
+        {
+            // When grounded we apply a slight negative vertical speed to make Ellen "stick" to the ground.
+            _verticalVelocity = Physics.gravity.y * kStickingGravityProportion;
+
+            // If jump is held, Ellen is ready to jump and not currently in the middle of a melee combo...
+            if (_inputController.Jump && _readyToJump && !_inCombo)
+            {
+                // ... then override the previously set vertical speed and make sure she cannot jump again.
+                _verticalVelocity = JumpSpeed;
+                _isGrounded = false;
+                _readyToJump = false;
+                _anim.SetBool("onGround", false);
+            }
+        }
+        else 
+        {
+            // If Ellen is airborne, the jump button is not held and Ellen is currently moving upwards...
+            if (!_inputController.Jump && _verticalVelocity > 0.0f)
+            {
+                // ... decrease Ellen's vertical speed.
+                // This is what causes holding jump to jump higher that tapping jump.
+                _verticalVelocity -= kJumpAbortSpeed * deltaTime;
+            }
+
+            // If a jump is approximately peaking, make it absolute.
+            if (Mathf.Approximately(_verticalVelocity, 0f))
+            {
+                _verticalVelocity = 0f;
+            }
+            
+            // If Ellen is airborne, apply gravity.
+            _verticalVelocity += Physics.gravity.y * deltaTime;
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapAnimatorController.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapControllerArtanim.cs

@@ -0,0 +1,697 @@
+using System;
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+using UnityEngine.Assertions;
+
+using System.Linq.Expressions;
+
+public class MocapControllerArtanim : MonoBehaviour, IOnSensorCollision 
+{
+	public List<float> SensorIsInTouch;
+	List<GameObject> _sensors;
+
+	internal Animator anim;
+
+	[Range(0f,1f)]
+	public float NormalizedTime;    
+	public float Lenght;
+	public bool IsLoopingAnimation;
+
+	[SerializeField]
+	Rigidbody _rigidbodyRoot;
+
+	private List<Rigidbody> _rigidbodies;
+	private List<Transform> _transforms;
+
+	public bool RequestCamera;
+	public bool CameraFollowMe;
+	public Transform CameraTarget;
+
+	Vector3 _resetPosition;
+	Quaternion _resetRotation;
+
+
+	[SerializeField]
+	bool _isGeneratedProcedurally = false;
+
+	public bool IsGeneratedProcedurally { set => _isGeneratedProcedurally = value; }
+
+
+
+	[Space(20)]
+	//---------------------- piece added to deal with mixamo characters and mapping between skinned and physical characters
+	//[SerializeField]
+	//bool _usesMotionMatching = false;
+    private bool _usingMocapAnimatorController = false;
+	MocapAnimatorController _mocapAnimController;
+
+	[SerializeField]
+	float _debugDistance = 0.0f;
+
+
+	//I try to configure here, directly, the offsets.
+
+
+	// [SerializeField]
+	// string rigBaseName = "mixamorig";
+
+	//  private List<Transform> _targetPoseTransforms = null;
+	//[SerializeField]
+	//Transform _targetMocapCharacter;
+
+
+
+
+
+	private List<MappingOffset> _offsetsSource2RB = null;
+
+    //for debugging, we disable this when setTpose in MarathonTestBedController is on
+    [HideInInspector]
+    public bool doFixedUpdate = true;
+
+	bool _hasLazyInitialized;
+
+    private List<GameObject> allChildObjects = new List<GameObject>();
+
+
+
+    void SetOffsetSourcePose2RBInProceduralWorld() {
+
+		_transforms = GetComponentsInChildren<Transform>().ToList();
+
+		_offsetsSource2RB = new List<MappingOffset>();
+
+
+		if (_rigidbodies == null)
+		{
+			_rigidbodies = _rigidbodyRoot.GetComponentsInChildren<Rigidbody>().ToList();
+			// _transforms = GetComponentsInChildren<Transform>().ToList();
+		}
+
+
+
+		foreach (Rigidbody rb in _rigidbodies)
+		{
+
+			//ArticulationBody ab = _articulationbodies.First(x => x.name == abname);
+
+			string[] temp = rb.name.Split(':');
+
+			//string tname = temp[1];
+			string tname = rb.name.TrimStart(temp[0].ToArray<char>());
+
+			tname = tname.TrimStart(':');
+
+
+			//if structure is "articulation:" + t.name, it comes from a joint:
+
+			if (temp[0].Equals("articulation"))
+			{
+
+				Transform t = _transforms.First(x => x.name == tname);
+
+
+				//TODO: check these days if those values are different from 0, sometimes
+				Quaternion qoffset = rb.transform.rotation * Quaternion.Inverse(t.rotation);
+
+				MappingOffset r = new MappingOffset(t, rb, qoffset);
+
+				_offsetsSource2RB.Add(r);
+				r.UpdateRigidBodies = true;//TODO: check if really needed, probably the constructor already does it
+
+
+
+			}
+		}
+
+
+
+	}
+
+
+
+
+	MappingOffset SetOffsetSourcePose2RB(string rbname, string tname)
+	{
+		//here we set up:
+		// a. the transform of the rigged character input
+		// NO b. the rigidbody of the physical character
+		// c. the offset calculated between the rigged character INPUT, and the rigidbody
+
+
+		if (_transforms == null)
+		{
+			_transforms = GetComponentsInChildren<Transform>().ToList();
+			//Debug.Log("the number of transforms  in source pose is: " + _transforms.Count);
+
+		}
+
+
+		if (_offsetsSource2RB == null)
+		{
+			_offsetsSource2RB = new List<MappingOffset>();
+
+		}
+
+		if (_rigidbodies == null )
+		{
+			_rigidbodies = _rigidbodyRoot.GetComponentsInChildren<Rigidbody>().ToList();
+			// _transforms = GetComponentsInChildren<Transform>().ToList();
+		}
+
+
+
+
+		Rigidbody rb = null;
+
+
+		try
+		{
+			rb = _rigidbodies.First(x => x.name == rbname);
+
+		}
+		catch
+		{
+
+			Debug.LogError("no rigidbody with name " + rbname);
+
+		}
+
+
+	
+		Transform tref = null;
+		try
+		{
+
+			tref = _transforms.First(x => x.name == tname);
+
+		}
+		catch
+		{
+			Debug.LogError("no bone transform with name in input pose " + tname);
+
+		}
+
+		//from refPose to Physical body:
+		//q_{physical_body} = q_{offset} * q_{refPose}
+		//q_{offset} = q_{physical_body} * Quaternion.Inverse(q_{refPose})
+
+		//Quaternion qoffset = rb.transform.localRotation * Quaternion.Inverse(tref.localRotation);
+
+
+		//using the global rotation instead of the local one prevents from dependencies on bones that are not mapped to the rigid body (like the shoulder)
+		Quaternion qoffset = rb.transform.rotation * Quaternion.Inverse(tref.rotation);
+
+
+		MappingOffset r = new MappingOffset(tref, rb, qoffset);
+		r.UpdateRigidBodies = true;//not really needed, the constructor already does it
+
+		_offsetsSource2RB.Add(r);
+		return r;
+	}
+
+
+
+	//void SetSon(MappingOffset o, string tsonname) {
+
+	//	Transform tref = null;
+	//	try
+	//	{
+
+	//		tref = _transforms.First(x => x.name == tsonname);
+
+	//	}
+	//	catch (Exception e)
+	//	{
+	//		Debug.LogError("no bone transform with name in input pose " + tsonname);
+
+	//	}
+
+	//	o.SetSon(tref);
+
+	//}
+
+
+	//public bool UsingMocapAnimatorController { get => _usingMocapAnimatorController; set => _usingMocapAnimatorController = value; }
+
+	//public bool UsingMocapAnimatorController { get => _usingMocapAnimatorController;  }
+
+	public void OnAgentInitialize()
+	{
+		LazyInitialize();
+	}
+	void LazyInitialize()
+    {
+		if (_hasLazyInitialized)
+			return;
+		try
+		{
+			_mocapAnimController = GetComponent<MocapAnimatorController>();
+			string s = _mocapAnimController.name;//this should launch an exception if there is no animator
+			_usingMocapAnimatorController = true;
+		}
+		catch {
+			_usingMocapAnimatorController = false;
+			Debug.LogWarning("Mocap Controller is working WITHOUT MocapAnimatorController");
+		}
+
+
+		//we already created this in the procedural case:
+		if(! _isGeneratedProcedurally)
+			try
+			{
+                DynamicallyCreateRagdollForMocap();
+            }
+			catch(Exception e)
+			{
+				Debug.LogError(e);
+			}
+			
+
+        SetupSensors();
+
+		anim = GetComponent<Animator>();
+		if (_usingMocapAnimatorController && !_isGeneratedProcedurally)
+		{
+
+			// anim.Play("Record",0, NormalizedTime);
+			anim.Play("Idle", 0, NormalizedTime);
+			anim.Update(0f);
+		}
+
+			if (RequestCamera && CameraTarget != null)
+		{
+			var instances = FindObjectsOfType<MocapControllerArtanim>().ToList();
+			if (instances.Count(x=>x.CameraFollowMe) < 1)
+				CameraFollowMe = true;
+		}
+        if (CameraFollowMe){
+            var camera = FindObjectOfType<Camera>();
+            var follow = camera.GetComponent<SmoothFollow>();
+            follow.target = CameraTarget;
+        }
+		_resetPosition = transform.position;
+		_resetRotation = transform.rotation;
+
+		_hasLazyInitialized = true;
+    }
+
+
+	public void DynamicallyCreateRagdollForMocap()
+	{
+		// Find Ragdoll in parent
+		Transform parent = this.transform.parent;
+		RagDollAgent[] ragdolls = parent.GetComponentsInChildren<RagDollAgent>(true);
+		Assert.AreEqual(ragdolls.Length, 1, "code only supports one RagDollAgent");
+		RagDollAgent ragDoll = ragdolls[0];
+		var ragdollForMocap = new GameObject("RagdollForMocap");
+		ragdollForMocap.transform.SetParent(this.transform, false);
+		Assert.AreEqual(ragDoll.transform.childCount, 1, "code only supports 1 child");
+		var ragdollRoot = ragDoll.transform.GetChild(0);
+		// clone the ragdoll root
+		var clone = Instantiate(ragdollRoot);
+		// remove '(clone)' from names
+		foreach (var t in clone.GetComponentsInChildren<Transform>())
+		{
+			t.name = t.name.Replace("(Clone)", "");
+		}
+		clone.transform.SetParent(ragdollForMocap.transform, false);
+		// swap ArticulatedBody for RidgedBody, but delete abody first before attaching
+		foreach (var abody in clone.GetComponentsInChildren<ArticulationBody>())
+		{
+			var bodyGameobject = abody.gameObject;
+			//var rb = bodyGameobject.AddComponent<Rigidbody>();
+			var mass = abody.mass;
+			var gravity = abody.useGravity;
+			DestroyImmediate(abody);
+			var rb = bodyGameobject.AddComponent<Rigidbody>();
+			rb.mass = mass;
+			rb.useGravity = gravity;
+		}
+		// make Kinematic
+		foreach (var rb in clone.GetComponentsInChildren<Rigidbody>())
+		{
+			rb.isKinematic = true;
+		}
+		// set the root
+		this._rigidbodyRoot = clone.GetComponent<Rigidbody>();
+		// set the layers
+		ragdollForMocap.layer = this.gameObject.layer;
+
+		FindAllChildObjects(ragdollForMocap.transform);
+
+		foreach (var obj in allChildObjects)
+		{
+			obj.layer = this.gameObject.layer;
+		}
+
+		// setup HandleOverlap
+		foreach (var rb in clone.GetComponentsInChildren<Rigidbody>())
+		{
+			// remove cloned HandledOverlap
+			var oldHandleOverlap = rb.GetComponent<HandleOverlap>();
+			DestroyImmediate(oldHandleOverlap);
+			var handleOverlap = rb.gameObject.AddComponent<HandleOverlap>();
+			handleOverlap.Parent = clone.gameObject;
+
+		}
+
+        //var children = new List<Transform>();
+        //for (int i = 0; i < ragdollForMocap.transform.childCount; i++)
+        //{
+        //    children.Add(ragdollForMocap.transform.GetChild(i));
+        //}
+
+        //Debug.Log(children.Count);
+
+    }
+
+	// Function to recursively find all child game objects
+    private void FindAllChildObjects(Transform parent)
+    {
+        foreach (Transform child in parent)
+        {
+            // Add the child to the list
+            allChildObjects.Add(child.gameObject);
+
+            // Recursively call this function for each child's children
+            FindAllChildObjects(child);
+        }
+    }
+    void SetupSensors()
+	{
+		_sensors = GetComponentsInChildren<SensorBehavior>()
+			.Select(x=>x.gameObject)
+			.ToList();
+		SensorIsInTouch = Enumerable.Range(0,_sensors.Count).Select(x=>0f).ToList();
+	}
+
+    void FixedUpdate()
+    {
+		LazyInitialize();
+        if (doFixedUpdate)
+            OnFixedUpdate();
+	
+    }
+
+
+    void OnFixedUpdate() {
+		LazyInitialize();
+
+        //if (!_usesMotionMatching)
+        {
+            AnimatorStateInfo stateInfo = anim.GetCurrentAnimatorStateInfo(0);
+            AnimatorClipInfo[] clipInfo = anim.GetCurrentAnimatorClipInfo(0);
+            Lenght = stateInfo.length;
+            NormalizedTime = stateInfo.normalizedTime;
+            IsLoopingAnimation = stateInfo.loop;
+            var timeStep = stateInfo.length * stateInfo.normalizedTime;
+            //var endTime = 1f;
+            //if (IsLoopingAnimation)
+            //    endTime = 3f;
+            // if (NormalizedTime <= endTime) {
+            // }       
+        }
+
+		if (_isGeneratedProcedurally)
+			MimicAnimationArtanimInProceduralWorld();
+		else
+			MimicAnimationArtanim();
+
+
+    }
+
+
+	void MimicAnimationArtanimInProceduralWorld() {
+		if (!anim.enabled)
+			return;
+		else
+			SetOffsetSourcePose2RBInProceduralWorld();
+
+
+	}
+
+
+	void MimicAnimationArtanim() {
+		if (!anim.enabled)
+			return;
+
+		if (_offsetsSource2RB == null)
+		{
+			MappingOffset o = SetOffsetSourcePose2RB("articulation:Hips", "mixamorig:Hips");
+			
+
+			o.SetAsRoot(true, _debugDistance);
+			SetOffsetSourcePose2RB("articulation:Spine", "mixamorig:Spine");
+			SetOffsetSourcePose2RB("articulation:Spine1", "mixamorig:Spine1");
+			SetOffsetSourcePose2RB("articulation:Spine2", "mixamorig:Spine2");
+            SetOffsetSourcePose2RB("articulation:Neck", "mixamorig:Neck");
+
+            SetOffsetSourcePose2RB("head", "mixamorig:Head");
+
+
+			SetOffsetSourcePose2RB("articulation:LeftShoulder", "mixamorig:LeftShoulder");
+
+
+			SetOffsetSourcePose2RB("articulation:LeftArm", "mixamorig:LeftArm");
+			
+			SetOffsetSourcePose2RB("articulation:LeftForeArm", "mixamorig:LeftForeArm");
+
+			//	SetOffsetSourcePose2RB("left_hand", "mixamorig:LeftHand");
+			// no rigidbodies in hands, so far
+
+			SetOffsetSourcePose2RB("articulation:RightShoulder", "mixamorig:RightShoulder");
+
+
+			SetOffsetSourcePose2RB("articulation:RightArm", "mixamorig:RightArm");
+			SetOffsetSourcePose2RB("articulation:RightForeArm", "mixamorig:RightForeArm");
+			//	SetOffsetSourcePose2RB("right_hand", "mixamorig:RightHand");
+
+			SetOffsetSourcePose2RB("articulation:LeftUpLeg", "mixamorig:LeftUpLeg");
+			SetOffsetSourcePose2RB("articulation:LeftLeg", "mixamorig:LeftLeg");
+
+
+			SetOffsetSourcePose2RB("articulation:LeftToeBase", "mixamorig:LeftToeBase");
+	
+
+			SetOffsetSourcePose2RB("articulation:RightUpLeg", "mixamorig:RightUpLeg");
+			SetOffsetSourcePose2RB("articulation:RightLeg", "mixamorig:RightLeg");
+			SetOffsetSourcePose2RB("articulation:RightToeBase", "mixamorig:RightToeBase");
+		
+
+
+
+		}
+		else {
+			MimicCynematicChar();
+		}
+
+
+	}
+
+	
+
+
+	void MimicCynematicChar()
+	{
+
+		try
+		{
+			foreach (MappingOffset o in _offsetsSource2RB)
+			{
+				o.UpdateRotation();
+
+			}
+		}
+		catch
+		{
+			Debug.Log("not calibrated yet...");
+
+		}
+
+
+	}
+
+	/*
+	[Space(20)]
+	[Range(0f,1f)]
+	public float toePositionOffset = .3f;
+	[Range(0f,1f)]
+	public float toeRotationOffset = .7f;
+
+    void MimicLeftFoot(string name, Vector3 offset, Quaternion rotationOffset)
+	{
+		string animStartName = "mixamorig:LeftFoot";
+		// string animEndtName = "mixamorig:LeftToeBase";
+		string animEndtName = "mixamorig:LeftToe_End";
+		if (_rigidbodies == null || _transforms == null)
+		{
+			_rigidbodies = GetComponentsInChildren<Rigidbody>().ToList();
+			_transforms = GetComponentsInChildren<Transform>().ToList();
+		}
+
+		var animStartBone = _transforms.First(x=>x.name == animStartName);
+		var animEndBone = _transforms.First(x=>x.name == animEndtName);
+		var target = _rigidbodies.First(x=>x.name == name);
+
+		var rotation = Quaternion.Lerp(animStartBone.rotation, animEndBone.rotation, toeRotationOffset);
+		var skinOffset = (animEndBone.transform.position - animStartBone.transform.position);
+		target.transform.position = animStartBone.transform.position + (skinOffset * toePositionOffset) + offset;
+		target.transform.rotation = rotation * rotationOffset;
+	}
+	void MimicRightFoot(string name, Vector3 offset, Quaternion rotationOffset)
+	{
+		string animStartName = "mixamorig:RightFoot";
+		// string animEndtName = "mixamorig:RightToeBase";
+		string animEndtName = "mixamorig:RightToe_End";
+		if (_rigidbodies == null || _transforms == null)
+		{
+			_rigidbodies = GetComponentsInChildren<Rigidbody>().ToList();
+			_transforms = GetComponentsInChildren<Transform>().ToList();
+		}
+
+
+		var animStartBone = _transforms.First(x=>x.name == animStartName);
+		var animEndBone = _transforms.First(x=>x.name == animEndtName);
+		var target = _rigidbodies.First(x=>x.name == name);
+
+		var rotation = Quaternion.Lerp(animStartBone.rotation, animEndBone.rotation, toeRotationOffset);
+		var skinOffset = (animEndBone.transform.position - animStartBone.transform.position);
+		target.transform.position = animStartBone.transform.position + (skinOffset * toePositionOffset) + offset;
+		target.transform.rotation = rotation * rotationOffset;
+
+	}
+	*/
+
+	public void OnReset(Quaternion resetRotation)
+	{
+		LazyInitialize();
+
+        if (!doFixedUpdate)
+            return;
+
+            if (_usingMocapAnimatorController)
+		{
+			_mocapAnimController.OnReset();
+
+		}
+		else
+		{
+			Debug.Log("I am resetting the reference animation with MxMAnimator (no _mocapController)");
+
+			//GetComponent<MxMAnimator>().enabled = false;
+
+			//GetComponent<MxMAnimator>().enabled = true;
+
+
+
+		}
+
+
+
+
+
+		transform.position = _resetPosition;
+		// handle character controller skin width
+		var characterController = GetComponent<CharacterController>();
+		if (characterController != null)
+		{
+			var pos = transform.position;
+			pos.y += characterController.skinWidth;
+			transform.position = pos;
+		}
+		transform.rotation = resetRotation;
+		if (_isGeneratedProcedurally)
+			MimicAnimationArtanimInProceduralWorld();
+		else
+	        MimicAnimationArtanim();
+	}
+
+    public void OnSensorCollisionEnter(Collider sensorCollider, GameObject other)
+	{
+		LazyInitialize();
+
+		//if (string.Compare(other.name, "Terrain", true) !=0)
+		if (other.layer != LayerMask.NameToLayer("Ground"))
+				return;
+		var sensor = _sensors
+			.FirstOrDefault(x=>x == sensorCollider.gameObject);
+		if (sensor != null) {
+			var idx = _sensors.IndexOf(sensor);
+			SensorIsInTouch[idx] = 1f;
+		}
+	}
+	public void OnSensorCollisionExit(Collider sensorCollider, GameObject other)
+	{
+		LazyInitialize();
+
+		if (other.layer != LayerMask.NameToLayer("Ground"))
+				return;
+		var sensor = _sensors
+			.FirstOrDefault(x=>x == sensorCollider.gameObject);
+		if (sensor != null) {
+			var idx = _sensors.IndexOf(sensor);
+			SensorIsInTouch[idx] = 0f;
+		}
+	}   
+    public void CopyStatesTo(GameObject target)
+    {
+		LazyInitialize();
+
+        var targets = target.GetComponentsInChildren<ArticulationBody>().ToList();
+		if (targets?.Count == 0)
+			return;
+        var root = targets.First(x=>x.isRoot);
+		root.gameObject.SetActive(false);
+        foreach (var targetRb in targets)
+        {
+			var stat = GetComponentsInChildren<Rigidbody>().First(x=>x.name == targetRb.name);
+            targetRb.transform.position = stat.position;
+            targetRb.transform.rotation = stat.rotation;
+            if (targetRb.isRoot)
+            {
+                targetRb.TeleportRoot(stat.position, stat.rotation);
+            }
+			float stiffness = 0f;
+			float damping = 10000f;
+			if (targetRb.twistLock == ArticulationDofLock.LimitedMotion)
+			{
+				var drive = targetRb.xDrive;
+				drive.stiffness = stiffness;
+				drive.damping = damping;
+				targetRb.xDrive = drive;
+			}			
+            if (targetRb.swingYLock == ArticulationDofLock.LimitedMotion)
+			{
+				var drive = targetRb.yDrive;
+				drive.stiffness = stiffness;
+				drive.damping = damping;
+				targetRb.yDrive = drive;
+			}
+            if (targetRb.swingZLock == ArticulationDofLock.LimitedMotion)
+			{
+				var drive = targetRb.zDrive;
+				drive.stiffness = stiffness;
+				drive.damping = damping;
+				targetRb.zDrive = drive;
+			}
+        }
+		root.gameObject.SetActive(true);
+    }	   
+	public Vector3 SnapTo(Vector3 snapPosition)
+	{
+		snapPosition.y = transform.position.y;
+		var snapDistance = snapPosition-transform.position;
+		transform.position = snapPosition;
+		return snapDistance;
+	}
+
+	public List<Rigidbody> GetRigidBodies()
+	{
+		LazyInitialize();
+		return GetComponentsInChildren<Rigidbody>().ToList();
+	}
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapControllerArtanim.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapRangeOfMotionAnimatorController.cs

@@ -0,0 +1,43 @@
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class MocapRangeOfMotionAnimatorController : MonoBehaviour
+{
+    Animator _anim;
+    Animation _animation;
+    public Motion[] Motions;
+
+    // Start is called before the first frame update
+    void Start()
+    {
+        _anim = GetComponent<Animator>();
+        foreach (var motion in Motions)
+        {
+            // _anim.CrossFade()
+            // _anim.()
+            // _animation.CrossFadeQueued(motion.name);
+        }
+        // _animation = GetComponent<Animation>();
+        // // _characterController = GetComponent<CharacterController>();
+        // // _spawnableEnv = GetComponentInParent<SpawnableEnv>();
+        // // _inputController = _spawnableEnv.GetComponentInChildren<InputController>();
+        // // _targetDirection = Quaternion.Euler(0, 90, 0);
+        // // var ragDoll = _spawnableEnv.GetComponentInChildren<RagDollAgent>( true);//we include inactive childs
+        // _animation.CrossFadeQueued("jump");
+        // _animation.CrossFadeQueued("animation2");
+        // _animation.CrossFadeQueued("animation3");
+        // var anim = _animation["animation3"];
+    }
+
+    // Update is called once per frame
+    void Update()
+    {
+        
+    }
+    // Called each physics step (so long as the Animator component is set to Animate Physics) after FixedUpdate to override root motion.
+    void OnAnimatorMove()
+    {
+        // do nothing
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapRangeOfMotionAnimatorController.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDoll004.cs

@@ -0,0 +1,79 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class RagDoll004 : MonoBehaviour {
+
+    [System.Serializable]
+    public class MusclePower
+    {
+        public string Muscle;
+        public Vector3 PowerVector;
+    }
+
+    public List<MusclePower> MusclePowers;
+
+    public float MotorScale = 1f;
+    public float Stiffness = 100f;
+    public float Damping = 100f;
+    public float ForceLimit = float.MaxValue;
+
+
+    [Header("Debug Collisions")]
+    [SerializeField]
+    bool skipCollisionSetup;
+
+
+	// Use this for initialization
+	void Start () {
+		Setup();
+	}
+	
+	// Update is called once per frame
+	void Update () {
+		
+	}
+
+	void Setup () {
+
+        if (!skipCollisionSetup) { 
+
+		// handle collision overlaps
+        IgnoreCollision("articulation:Spine2", new []{ "LeftArm", "RightArm"});
+        IgnoreCollision("articulation:Hips", new []{ "RightUpLeg", "LeftUpLeg" });
+
+        IgnoreCollision("LeftForeArm", new []{ "LeftArm" });
+        IgnoreCollision("RightForeArm", new []{ "RightArm" });
+        IgnoreCollision("RightLeg", new []{ "RightUpLeg" });
+        IgnoreCollision("LeftLeg", new[] { "LeftUpLeg" });
+
+        IgnoreCollision("RightLeg", new []{"RightFoot"});
+        IgnoreCollision("LeftLeg", new []{"LeftFoot"});
+
+        }
+
+        //
+        var joints = GetComponentsInChildren<Joint>().ToList();
+        foreach (var joint in joints)
+            joint.enablePreprocessing = false;
+	}
+	void IgnoreCollision(string first, string[] seconds)
+    {
+        foreach (var second in seconds)
+        {
+            IgnoreCollision(first, second);
+        }
+    }
+    void IgnoreCollision(string first, string second)
+    {
+        var rigidbodies = GetComponentsInChildren<Rigidbody>().ToList();
+        var colliderOnes = rigidbodies.FirstOrDefault(x=>x.name.Contains(first))?.GetComponents<Collider>();
+        var colliderTwos = rigidbodies.FirstOrDefault(x=>x.name.Contains(second))?.GetComponents<Collider>();
+        if (colliderOnes == null || colliderTwos == null)
+            return;
+        foreach (var c1 in colliderOnes)
+            foreach (var c2 in colliderTwos)
+                Physics.IgnoreCollision(c1, c2);
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDoll004.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDollAgent.cs

@@ -0,0 +1,545 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using Unity.MLAgents.Actuators;
+using Unity.MLAgents.Sensors;
+using UnityEngine;
+using ManyWorlds;
+using UnityEngine.Assertions;
+
+using System;
+using Unity.Mathematics;
+
+public class RagDollAgent : Agent 
+{
+    [Header("Settings")]
+	public float FixedDeltaTime = 1f/60f;
+    public float SmoothBeta = 0.2f;
+
+    [Header("Camera")]
+
+    public bool RequestCamera;
+	public bool CameraFollowMe;
+	public Transform CameraTarget;    
+
+    [Header("... debug")]
+    public bool SkipRewardSmoothing;
+    public bool debugCopyMocap;
+    public bool ignorActions;
+    public bool dontResetOnZeroReward;
+    public bool dontSnapMocapToRagdoll;
+    public bool DebugPauseOnReset;
+    public bool UsePDControl = true;
+
+    List<Rigidbody> _mocapBodyParts;
+    List<ArticulationBody> _bodyParts;
+    SpawnableEnv _spawnableEnv;
+    DReConObservations _dReConObservations;
+    DReConRewards _dReConRewards;
+    RagDoll004 _ragDollSettings;
+    TrackBodyStatesInWorldSpace _trackBodyStatesInWorldSpace;
+    List<ArticulationBody> _motors;
+    MarathonTestBedController _debugController;  
+    InputController _inputController;
+    SensorObservations _sensorObservations;
+    DecisionRequester _decisionRequester;
+    MocapAnimatorController _mocapAnimatorController;
+
+
+    bool _hasLazyInitialized;
+    float[] _smoothedActions;
+    float[] _mocapTargets;
+
+    [Space(16)]
+    [SerializeField]
+    bool _hasAwake = false;
+    MocapControllerArtanim _mocapControllerArtanim;
+
+    private float timer = 0f;
+    private float waitTime = 3f;
+    private bool waitStarted;
+
+    private float x = 0;
+    private float y = 100;
+
+    void Awake()
+    {
+		if (RequestCamera && CameraTarget != null)
+		{
+            // Will follow the last object to be spawned
+            var camera = FindObjectOfType<Camera>();
+            if(camera != null) { 
+                var follow = camera.GetComponent<SmoothFollow>();
+                if (follow != null)
+                    follow.target = CameraTarget;
+            }
+        }
+        _hasAwake = true;        
+    }
+    void Update()
+    {
+        if (debugCopyMocap)
+        {
+            EndEpisode();
+        }
+
+        Assert.IsTrue(_hasLazyInitialized);
+
+        // hadle mocap going out of bounds
+        if (!_spawnableEnv.IsPointWithinBoundsInWorldSpace(_mocapControllerArtanim.transform.position))
+        {
+            _mocapControllerArtanim.transform.position = _spawnableEnv.transform.position;
+            _trackBodyStatesInWorldSpace.LinkStatsToRigidBodies();
+            EndEpisode();
+        }
+    }
+	override public void CollectObservations(VectorSensor sensor)
+    {
+        Assert.IsTrue(_hasLazyInitialized);
+
+        float timeDelta = Time.fixedDeltaTime * _decisionRequester.DecisionPeriod;
+        _dReConObservations.OnStep(timeDelta);
+
+        sensor.AddObservation(_dReConObservations.MocapCOMVelocity);
+        sensor.AddObservation(_dReConObservations.RagDollCOMVelocity);
+        sensor.AddObservation(_dReConObservations.RagDollCOMVelocity-_dReConObservations.MocapCOMVelocity);
+        sensor.AddObservation(_dReConObservations.InputDesiredHorizontalVelocity);
+        sensor.AddObservation(_dReConObservations.InputJump);
+        sensor.AddObservation(_dReConObservations.InputBackflip);
+        sensor.AddObservation(_dReConObservations.HorizontalVelocityDifference);
+        // foreach (var stat in _dReConObservations.MocapBodyStats)
+        // {
+        //     sensor.AddObservation(stat.Position);
+        //     sensor.AddObservation(stat.Velocity);
+        // }
+        foreach (var stat in _dReConObservations.RagDollBodyStats)
+        {
+            sensor.AddObservation(stat.Position);
+            sensor.AddObservation(stat.Velocity);
+        }                
+        foreach (var stat in _dReConObservations.BodyPartDifferenceStats)
+        {
+            sensor.AddObservation(stat.Position);
+            sensor.AddObservation(stat.Velocity);
+        }
+        sensor.AddObservation(_dReConObservations.PreviousActions);
+        
+        // add sensors (feet etc)
+        sensor.AddObservation(_sensorObservations.SensorIsInTouch);
+    }
+	public override void OnActionReceived(ActionBuffers actions)
+    {
+        float[] vectorAction = actions.ContinuousActions.Select(x=>x).ToArray();
+        
+        Assert.IsTrue(_hasLazyInitialized);
+
+        float timeDelta = Time.fixedDeltaTime;
+        if (!_decisionRequester.TakeActionsBetweenDecisions)
+            timeDelta = timeDelta*_decisionRequester.DecisionPeriod;
+        _dReConRewards.OnStep(timeDelta);        
+
+        bool shouldDebug = _debugController != null;
+        bool dontUpdateMotor = false;
+        if (_debugController != null)
+        {
+            dontUpdateMotor = _debugController.DontUpdateMotor;
+            dontUpdateMotor &= _debugController.isActiveAndEnabled;
+            dontUpdateMotor &= _debugController.gameObject.activeInHierarchy;
+            shouldDebug &= _debugController.isActiveAndEnabled;
+            shouldDebug &= _debugController.gameObject.activeInHierarchy;
+        }
+        if (shouldDebug)
+        {
+            vectorAction = GetDebugActions(vectorAction);
+        }
+        if (UsePDControl)
+        {
+            var targets = GetMocapTargets();
+            vectorAction = vectorAction
+                .Zip(targets, (action, target)=> Mathf.Clamp(target + action *2f, -1f, 1f))
+                .ToArray();
+        }
+        if (!SkipRewardSmoothing)
+            vectorAction = SmoothActions(vectorAction);
+        if (ignorActions)
+            vectorAction = vectorAction.Select(x=>0f).ToArray();
+		int i = 0;
+
+        var scale = 1f;
+
+        x += Time.fixedDeltaTime * scale;
+        y += Time.fixedDeltaTime * scale;
+       
+		foreach (var m in _motors)
+		{
+            if (m.isRoot)
+                continue;
+            if (dontUpdateMotor)
+                continue;
+            Vector3 targetNormalizedRotation = Vector3.zero;
+
+			if (m.twistLock == ArticulationDofLock.LimitedMotion && !waitStarted)
+            {
+                targetNormalizedRotation.x = vectorAction[i++];
+            }
+            else
+            {
+                var offset = 7.5f;
+                targetNormalizedRotation.x = (Mathf.PerlinNoise(x + offset, y + offset) * 2) - 1; ;
+            }
+
+            if (m.swingYLock == ArticulationDofLock.LimitedMotion && !waitStarted)
+            {
+                targetNormalizedRotation.y = vectorAction[i++];
+            }
+            else
+            {
+                var offset = 5f;
+                targetNormalizedRotation.y = (Mathf.PerlinNoise(x + offset, y + offset) * 2) - 1; ;
+            }
+
+            if (m.swingZLock == ArticulationDofLock.LimitedMotion && !waitStarted)
+            {
+                targetNormalizedRotation.z = vectorAction[i++];
+            } 
+            else
+            {
+                var offset = 22.3f;
+                targetNormalizedRotation.z = (Mathf.PerlinNoise(x + offset, y + offset) * 2) - 1; ;
+            }
+				
+            UpdateMotor(m, targetNormalizedRotation);
+        }
+        _dReConObservations.PreviousActions = vectorAction;
+
+        AddReward(_dReConRewards.Reward);
+
+        // if (_dReConRewards.HeadHeightDistance > 0.5f || _dReConRewards.Reward < 1f)
+        if (_dReConRewards.HeadHeightDistance > 0.5f || _dReConRewards.Reward <= 0f)
+
+        {
+            if (!dontResetOnZeroReward)
+            {
+                if (!waitStarted)
+                {
+                    waitStarted = true;
+                }
+
+                timer += Time.fixedDeltaTime;
+
+                if (timer >= waitTime)
+                {
+                    timer = 0;
+                    waitStarted = false;
+                    EndEpisode();
+                }
+            }
+
+                
+        }
+        // else if (_dReConRewards.HeadDistance > 1.5f)
+        else if (_dReConRewards.Reward <= 0.1f && !dontSnapMocapToRagdoll)
+        {
+            Transform ragDollCom = _dReConObservations.GetRagDollCOM();
+            Vector3 snapPosition = ragDollCom.position;
+            snapPosition.y = 0f;
+            _mocapControllerArtanim.SnapTo(snapPosition);
+            AddReward(-.5f);
+        }
+    }
+
+
+    float[] GetDebugActions(float[] vectorAction)
+    {
+        var debugActions = new List<float>();
+        foreach (var m in _motors)
+        {
+            if (m.isRoot)
+                continue;
+            DebugMotor debugMotor = m.GetComponent<DebugMotor>();
+            if (debugMotor == null)
+            {
+                debugMotor = m.gameObject.AddComponent<DebugMotor>();
+            }
+            // clip to -1/+1
+            debugMotor.Actions = new Vector3 (
+                Mathf.Clamp(debugMotor.Actions.x, -1f, 1f),
+                Mathf.Clamp(debugMotor.Actions.y, -1f, 1f),
+                Mathf.Clamp(debugMotor.Actions.z, -1f, 1f)
+            );
+            Vector3 targetNormalizedRotation = debugMotor.Actions;
+
+            if (m.twistLock == ArticulationDofLock.LimitedMotion)
+                debugActions.Add(targetNormalizedRotation.x);
+            if (m.swingYLock == ArticulationDofLock.LimitedMotion)
+                debugActions.Add(targetNormalizedRotation.y);
+            if (m.swingZLock == ArticulationDofLock.LimitedMotion)
+                debugActions.Add(targetNormalizedRotation.z);
+        }
+        
+        debugActions = debugActions.Select(x=>Mathf.Clamp(x,-1f,1f)).ToList();
+        _debugController.Actions = debugActions.ToArray();
+        return debugActions.ToArray();
+    }
+
+    float[] SmoothActions(float[] vectorAction)
+    {
+        // yt =β at +(1−β)yt−1
+        if (_smoothedActions == null)
+            _smoothedActions = vectorAction.Select(x=>0f).ToArray();
+        _smoothedActions = vectorAction
+            .Zip(_smoothedActions, (a, y)=> SmoothBeta * a + (1f-SmoothBeta) * y)
+            .ToArray();
+        return _smoothedActions;
+    }
+    public override void Initialize()
+    {
+        Assert.IsTrue(_hasAwake);
+        Assert.IsFalse(_hasLazyInitialized);
+        _hasLazyInitialized = true;
+
+        _decisionRequester = GetComponent<DecisionRequester>();
+        _debugController = FindObjectOfType<MarathonTestBedController>();
+        Time.fixedDeltaTime = FixedDeltaTime;
+        _spawnableEnv = GetComponentInParent<SpawnableEnv>();
+
+        if (_debugController != null)
+        {
+            dontResetOnZeroReward = true;
+            dontSnapMocapToRagdoll = true;
+            UsePDControl = false;
+        }
+
+        _mocapControllerArtanim = _spawnableEnv.GetComponentInChildren<MocapControllerArtanim>();
+        _mocapBodyParts = _mocapControllerArtanim.GetRigidBodies();
+
+        _bodyParts = GetComponentsInChildren<ArticulationBody>().ToList();
+        _dReConObservations = GetComponent<DReConObservations>();
+        _dReConRewards = GetComponent<DReConRewards>();
+
+        _trackBodyStatesInWorldSpace = _mocapControllerArtanim.GetComponent<TrackBodyStatesInWorldSpace>();
+
+        _ragDollSettings = GetComponent<RagDoll004>();
+        _inputController = _spawnableEnv.GetComponentInChildren<InputController>();
+        _sensorObservations = GetComponent<SensorObservations>();
+
+        foreach (var body in GetComponentsInChildren<ArticulationBody>())
+        {
+            body.solverIterations = 255;
+            body.solverVelocityIterations = 255;
+        }
+
+        _motors = GetComponentsInChildren<ArticulationBody>()
+            .Where(x=>x.jointType == ArticulationJointType.SphericalJoint)
+            .Where(x=>!x.isRoot)
+            .Distinct()
+            .ToList();
+        var individualMotors = new List<float>();
+        foreach (var m in _motors)
+        {
+            if (m.twistLock == ArticulationDofLock.LimitedMotion)
+                individualMotors.Add(0f);
+            if (m.swingYLock == ArticulationDofLock.LimitedMotion)
+                individualMotors.Add(0f);
+            if (m.swingZLock == ArticulationDofLock.LimitedMotion)
+                individualMotors.Add(0f);
+        }
+        _dReConObservations.PreviousActions = individualMotors.ToArray();
+
+        //_mocapAnimatorController = _mocapControllerArtanim.GetComponentInChildren<MocapAnimatorController>();
+        _mocapAnimatorController = _mocapControllerArtanim.GetComponent<MocapAnimatorController>();
+
+
+
+        _mocapControllerArtanim.OnAgentInitialize();
+        _dReConObservations.OnAgentInitialize();
+        _dReConRewards.OnAgentInitialize();
+        _trackBodyStatesInWorldSpace.OnAgentInitialize();
+        _mocapAnimatorController.OnAgentInitialize();
+        _inputController.OnReset();
+
+        _hasLazyInitialized = true;
+    }
+	public override void OnEpisodeBegin()
+	{
+        Assert.IsTrue(_hasAwake);
+        _smoothedActions = null;
+        debugCopyMocap = false;
+
+        _mocapAnimatorController.OnReset();
+        var angle = Vector3.SignedAngle(Vector3.forward, _inputController.HorizontalDirection, Vector3.up);
+        var rotation = Quaternion.Euler(0f, angle, 0f);
+        _mocapControllerArtanim.OnReset(rotation);
+        _mocapControllerArtanim.CopyStatesTo(this.gameObject);
+
+        // _trackBodyStatesInWorldSpace.CopyStatesTo(this.gameObject);
+        float timeDelta = float.MinValue;
+        _dReConObservations.OnReset();
+        _dReConRewards.OnReset();
+        _dReConObservations.OnStep(timeDelta);
+        _dReConRewards.OnStep(timeDelta);
+#if UNITY_EDITOR		
+		if (DebugPauseOnReset)
+		{
+	        UnityEditor.EditorApplication.isPaused = true;
+		}
+#endif	        
+        if (_debugController != null && _debugController.isActiveAndEnabled)
+        {
+            _debugController.OnAgentEpisodeBegin();
+        }
+    }   
+
+    float[] GetMocapTargets()
+    {
+        if (_mocapTargets == null)
+        {
+            _mocapTargets = _motors
+                .Where(x=>!x.isRoot)
+                .SelectMany(x => {
+                    List<float> list = new List<float>();
+                    if (x.twistLock == ArticulationDofLock.LimitedMotion)
+                        list.Add(0f);
+                    if (x.swingYLock == ArticulationDofLock.LimitedMotion)
+                        list.Add(0f);
+                    if (x.swingZLock == ArticulationDofLock.LimitedMotion)
+                        list.Add(0f);
+                    return list.ToArray();
+                })
+                .ToArray();
+        }
+        int i=0;
+        foreach (var joint in _motors)
+		{
+            if (joint.isRoot)
+                continue;
+            Rigidbody mocapBody = _mocapBodyParts.First(x=>x.name == joint.name);
+            Vector3 targetRotationInJointSpace = -(Quaternion.Inverse(joint.anchorRotation) * Quaternion.Inverse(mocapBody.transform.localRotation) * joint.parentAnchorRotation).eulerAngles;
+            targetRotationInJointSpace = new Vector3(
+                Mathf.DeltaAngle(0, targetRotationInJointSpace.x),
+                Mathf.DeltaAngle(0, targetRotationInJointSpace.y),
+                Mathf.DeltaAngle(0, targetRotationInJointSpace.z));
+            if (joint.twistLock == ArticulationDofLock.LimitedMotion)
+            {
+                var drive = joint.xDrive;
+                var scale = (drive.upperLimit-drive.lowerLimit) / 2f;
+                var midpoint = drive.lowerLimit + scale;
+                var target = (targetRotationInJointSpace.x -midpoint) / scale;
+                _mocapTargets[i] = target;
+                i++;
+            }
+            if (joint.swingYLock == ArticulationDofLock.LimitedMotion)
+            {
+                var drive = joint.yDrive;
+                var scale = (drive.upperLimit-drive.lowerLimit) / 2f;
+                var midpoint = drive.lowerLimit + scale;
+                var target = (targetRotationInJointSpace.y -midpoint) / scale;
+                _mocapTargets[i] = target;
+                i++;
+            }
+            if (joint.swingZLock == ArticulationDofLock.LimitedMotion)
+            {
+                var drive = joint.zDrive;
+                var scale = (drive.upperLimit-drive.lowerLimit) / 2f;
+                var midpoint = drive.lowerLimit + scale;
+                var target = (targetRotationInJointSpace.z -midpoint) / scale;
+                _mocapTargets[i] = target;
+                i++;
+            }
+        }
+        return _mocapTargets;
+    }
+
+    void UpdateMotor(ArticulationBody joint, Vector3 targetNormalizedRotation)
+    {
+        //Vector3 power = _ragDollSettings.MusclePowers.First(x=>x.Muscle == joint.name).PowerVector;
+
+        Vector3 power = Vector3.zero;
+        try
+        {
+            power = _ragDollSettings.MusclePowers.First(x => x.Muscle == joint.name).PowerVector;
+
+        }
+        catch
+        {
+            Debug.Log("there is no muscle for joint " + joint.name);
+
+        }
+
+
+        power *= _ragDollSettings.Stiffness;
+        float damping = _ragDollSettings.Damping;
+        float forceLimit = _ragDollSettings.ForceLimit;
+
+        if (joint.twistLock == ArticulationDofLock.LimitedMotion)
+        {
+            var drive = joint.xDrive;
+            var scale = (drive.upperLimit-drive.lowerLimit) / 2f;
+            var midpoint = drive.lowerLimit + scale;
+            var target = midpoint + (targetNormalizedRotation.x *scale);
+            drive.target = target;
+            drive.stiffness = power.x;
+            drive.damping = damping;
+            drive.forceLimit = forceLimit;
+            joint.xDrive = drive;
+        }
+
+        if (joint.swingYLock == ArticulationDofLock.LimitedMotion)
+        {
+            var drive = joint.yDrive;
+            var scale = (drive.upperLimit-drive.lowerLimit) / 2f;
+            var midpoint = drive.lowerLimit + scale;
+            var target = midpoint + (targetNormalizedRotation.y *scale);
+            drive.target = target;
+            drive.stiffness = power.y;
+            drive.damping = damping;
+            drive.forceLimit = forceLimit;
+            joint.yDrive = drive;
+        }
+
+        if (joint.swingZLock == ArticulationDofLock.LimitedMotion)
+        {
+            var drive = joint.zDrive;
+            var scale = (drive.upperLimit-drive.lowerLimit) / 2f;
+            var midpoint = drive.lowerLimit + scale;
+            var target = midpoint + (targetNormalizedRotation.z *scale);
+            drive.target = target;
+            drive.stiffness = power.z;
+            drive.damping = damping;
+            drive.forceLimit = forceLimit;
+            joint.zDrive = drive;
+        }
+	}
+
+    void FixedUpdate()
+    {
+        if (debugCopyMocap)
+        {
+            EndEpisode();
+        }
+    }
+    void OnDrawGizmos()
+    {
+        if (_dReConRewards == null)
+            return;
+        var comTransform = _dReConRewards._ragDollBodyStats.transform;
+        var vector = new Vector3( _inputController.MovementVector.x, 0f, _inputController.MovementVector.y);
+        var pos = new Vector3(comTransform.position.x, 0.001f, comTransform.position.z);
+        DrawArrow(pos, vector, Color.black);
+    }
+    void DrawArrow(Vector3 start, Vector3 vector, Color color)
+    {
+        float headSize = 0.25f;
+        float headAngle = 20.0f;
+        Gizmos.color = color;
+		Gizmos.DrawRay(start, vector);
+        if (vector != Vector3.zero)
+        { 
+            Vector3 right = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180+headAngle,0) * new Vector3(0,0,1);
+            Vector3 left = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180-headAngle,0) * new Vector3(0,0,1);
+            Gizmos.DrawRay(start + vector, right * headSize);
+            Gizmos.DrawRay(start + vector, left * headSize);
+        }
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDollAgent.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagdollControllerArtanim.cs

@@ -0,0 +1,350 @@
+using System;
+using System.Linq;
+using System.Collections;
+using System.Collections.Generic;
+using UnityEngine;
+
+public class RagdollControllerArtanim : MonoBehaviour
+
+    //this class does exactly the symetrical of MocapControllerArtanim: it maps animations from a ragdoll to a rigged character
+{
+
+    [SerializeField]
+	ArticulationBody _articulationBodyRoot;
+
+    //to generate an environment automatically from a rigged character and an animation (see folder ROM-extraction)
+    public ArticulationBody ArticulationBodyRoot { set => _articulationBodyRoot = value; 
+		get =>  _articulationBodyRoot; }
+  
+
+    private List<ArticulationBody> _articulationbodies = null;
+
+	private List<Transform> _targetPoseTransforms = null;
+
+    private List<MappingOffset> _offsetsRB2targetPoseTransforms = null;
+
+
+	private List<MappingOffset> _offsetsSource2RB = null;
+
+
+	[Space(20)]
+
+
+
+	//not used in 
+	[SerializeField]
+	float _debugDistance= 0.0f;
+
+
+	[SerializeField]
+	bool _isGeneratedProcedurally = false;
+
+	public bool IsGeneratedProcedurally { set => _isGeneratedProcedurally = value; }
+
+
+	
+	[SerializeField]
+	bool _debugWithRigidBody = false;
+	[SerializeField]
+	Rigidbody _rigidbodyRoot;
+
+
+	private List<Rigidbody> _rigidbodies = null;
+
+
+
+
+    // Start is called before the first frame update
+    public void Start()
+    {
+       
+    }
+
+
+
+	//this one is for the case where everything is generated procedurally
+	void  SetOffsetRB2targetPoseInProceduralWorld() {
+
+		if(_targetPoseTransforms == null)
+			_targetPoseTransforms = GetComponentsInChildren<Transform>().ToList();
+
+		if(_offsetsRB2targetPoseTransforms == null)
+			_offsetsRB2targetPoseTransforms = new List<MappingOffset>();
+
+
+		if(_articulationbodies == null)
+			_articulationbodies = _articulationBodyRoot.GetComponentsInChildren<ArticulationBody>().ToList();
+
+
+
+
+
+		foreach (ArticulationBody ab in _articulationbodies) {
+
+			//ArticulationBody ab = _articulationbodies.First(x => x.name == abname);
+
+			string[] temp = ab.name.Split(':');
+
+
+
+			//if it has another ":" in the name, it crashes miserably
+			//string tname = temp[1];
+			//instead, we do:
+			string tname = ab.name.TrimStart(temp[0].ToArray<char>());
+
+			tname = tname.TrimStart(':');
+			//Debug.Log("the full name is: " + ab.name + "  and the trimmed name is: " + tname);
+
+
+			//if structure is "articulation:" + t.name, it comes from a joint:
+
+			if (temp[0].Equals("articulation")) { 
+
+				Transform t = _targetPoseTransforms.First(x => x.name == tname);
+
+
+				//TODO: check these days if those values are different from 0, sometimes
+				Quaternion qoffset = ab.transform.rotation * Quaternion.Inverse(t.rotation);
+				MappingOffset r = new MappingOffset(t, ab, Quaternion.Inverse(qoffset));
+				if (ab.isRoot)
+				{
+					r.SetAsRoot(true, _debugDistance);
+
+				}
+
+				_offsetsRB2targetPoseTransforms.Add(r);
+
+			}
+		}
+
+
+
+	}
+
+
+
+
+
+
+	MappingOffset SetOffsetRB2targetPose(string rbname, string tname)
+	{
+		//here we set up:
+		// a. the transform of the rigged character output
+		// b. the rigidbody of the physical character
+		// c. the offset calculated between the rigged character INPUT, and the rigidbody
+
+
+		if (_targetPoseTransforms == null)
+		{
+			_targetPoseTransforms = GetComponentsInChildren<Transform>().ToList();
+			//	Debug.Log("the number of transforms  intarget pose is: " + _targetPoseTransforms.Count);
+
+		}
+
+
+		if (_offsetsRB2targetPoseTransforms == null)
+		{
+			_offsetsRB2targetPoseTransforms = new List<MappingOffset>();
+
+		}
+
+		if (_articulationbodies == null)
+		{
+            if (_debugWithRigidBody) { 
+			_rigidbodies = _rigidbodyRoot.GetComponentsInChildren<Rigidbody>().ToList();
+            }
+            else { 
+				_articulationbodies = _articulationBodyRoot.GetComponentsInChildren<ArticulationBody>().ToList();
+			}
+		}
+
+
+		Transform rb;
+		if (_debugWithRigidBody)
+		{
+			rb = _rigidbodies.First(x => x.name == rbname).transform;
+		}else
+		{
+            rb = null;
+          
+            try
+            {
+                rb = _articulationbodies.First(x => x.name == rbname).transform;
+                if (rb == null)
+                {
+                    Debug.LogError("no rigidbody with name " + rbname);
+
+                }
+
+            }
+            catch {
+
+                Debug.LogError("problem with finding rigidbody with a name like: " + rbname);
+
+            }
+
+
+
+
+          
+		}
+
+		Transform t = null;
+		try
+		{
+
+			t = _targetPoseTransforms.First(x => x.name == tname);
+
+		}
+		catch
+		{
+			Debug.LogError("no bone transform with name in target pose" + tname);
+
+		}
+
+		Transform tref = null;
+		try
+		{
+
+			tref = _targetPoseTransforms.First(x => x.name == tname);
+
+		}
+		catch
+		{
+			Debug.LogError("no bone transform with name in input pose " + tname);
+
+		}
+
+		//from refPose to Physical body:
+		//q_{physical_body} = q_{offset} * q_{refPose}
+		//q_{offset} = q_{physical_body} * Quaternion.Inverse(q_{refPose})
+
+		//Quaternion qoffset = rb.transform.localRotation * Quaternion.Inverse(tref.localRotation);
+
+		Quaternion qoffset = rb.transform.rotation * Quaternion.Inverse(tref.rotation);
+
+
+		//from physical body to targetPose:
+		//q_{target_pose} = q_{offset2} * q_{physical_body}
+		//q_{offset2} = Quaternion.Inverse(q_{offset})
+
+		MappingOffset r;
+		if (_debugWithRigidBody)
+		{
+			Rigidbody myrb = rb.GetComponent<Rigidbody>();
+			r = new MappingOffset(t, myrb, Quaternion.Inverse(qoffset));
+			r.SetAsRagdollcontrollerDebug(_debugWithRigidBody);
+		}
+		else 
+		{
+			ArticulationBody myrb = rb.GetComponent<ArticulationBody>();
+			r = new MappingOffset(t, myrb, Quaternion.Inverse(qoffset));
+		}
+
+
+
+
+		_offsetsRB2targetPoseTransforms.Add(r);
+		return r;
+	}
+
+
+	void MimicPhysicalChar()
+	{
+
+		try
+		{
+			foreach (MappingOffset o in _offsetsRB2targetPoseTransforms)
+			{
+				o.UpdateRotation();
+			}
+		}
+		catch
+		{
+			Debug.Log("not calibrated yet...");
+
+		}
+
+
+	}
+
+    private void FixedUpdate()
+    {
+		MimicAnimationArtanim();
+    }
+    void MimicAnimationArtanim()
+	{
+
+
+        if (_offsetsRB2targetPoseTransforms == null)
+		{
+
+            if (_isGeneratedProcedurally)
+            {
+
+				SetOffsetRB2targetPoseInProceduralWorld();
+            }
+            else { 
+
+				MappingOffset o = SetOffsetRB2targetPose("articulation:Hips", "mixamorig:Hips");
+				o.SetAsRoot(true, _debugDistance);
+				SetOffsetRB2targetPose("articulation:Spine", "mixamorig:Spine");
+				SetOffsetRB2targetPose("articulation:Spine1", "mixamorig:Spine1");
+				SetOffsetRB2targetPose("articulation:Spine2", "mixamorig:Spine2");
+				SetOffsetRB2targetPose("articulation:Neck", "mixamorig:Neck");
+				SetOffsetRB2targetPose("head", "mixamorig:Head");
+
+            
+
+				SetOffsetRB2targetPose("articulation:LeftShoulder", "mixamorig:LeftShoulder");
+
+				SetOffsetRB2targetPose("articulation:LeftArm", "mixamorig:LeftArm");
+				SetOffsetRB2targetPose("articulation:LeftForeArm", "mixamorig:LeftForeArm");
+
+				//	SetOffsetRB2targetPose("left_hand", "mixamorig:LeftHand");
+				// hands do not have rigidbodies
+
+
+
+
+				SetOffsetRB2targetPose("articulation:RightShoulder", "mixamorig:RightShoulder");
+
+				SetOffsetRB2targetPose("articulation:RightArm", "mixamorig:RightArm");
+				SetOffsetRB2targetPose("articulation:RightForeArm", "mixamorig:RightForeArm");
+			//	SetOffsetRB2targetPose("right_hand", "mixamorig:RightHand");
+
+
+				SetOffsetRB2targetPose("articulation:LeftUpLeg", "mixamorig:LeftUpLeg");
+
+
+				//			SetOffsetRB2targetPose("left_shin", "mixamorig:LeftLeg");
+				SetOffsetRB2targetPose("articulation:LeftLeg", "mixamorig:LeftLeg");
+				SetOffsetRB2targetPose("articulation:LeftFoot", "mixamorig:LeftFoot");
+				SetOffsetRB2targetPose("articulation:LeftToeBase", "mixamorig:LeftToeBase");
+				//	SetOffsetRB2targetPose("right_left_foot", "mixamorig:LeftToeBase");
+
+
+				SetOffsetRB2targetPose("articulation:RightUpLeg", "mixamorig:RightUpLeg");
+				//SetOffsetRB2targetPose("right_shin", "mixamorig:RightLeg");
+				SetOffsetRB2targetPose("articulation:RightLeg", "mixamorig:RightLeg");
+
+				SetOffsetRB2targetPose("articulation:RightFoot", "mixamorig:RightFoot");
+				SetOffsetRB2targetPose("articulation:RightToeBase", "mixamorig:RightToeBase");
+					//	SetOffsetRB2targetPose("left_right_foot", "mixamorig:RightToeBase");
+
+			}
+
+		}
+		else
+		{
+			MimicPhysicalChar();
+
+
+		}
+
+
+
+	}
+
+
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagdollControllerArtanim.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/SensorObservations.cs

@@ -0,0 +1,51 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using Unity.MLAgents;
+using UnityEngine;
+
+public class SensorObservations : MonoBehaviour, IOnSensorCollision 
+{
+	public List<float> SensorIsInTouch;
+
+	List<GameObject> _sensors;
+
+    // Start is called before the first frame update
+    void Start()
+    {
+        SetupSensors();
+    }
+
+    void SetupSensors()
+	{
+		_sensors = GetComponentsInChildren<SensorBehavior>()
+			.Select(x=>x.gameObject)
+			.ToList();
+		SensorIsInTouch = Enumerable.Range(0,_sensors.Count).Select(x=>0f).ToList();
+	}
+
+    public void OnSensorCollisionEnter(Collider sensorCollider, GameObject other)
+	{
+		//if (string.Compare(other.name, "Terrain", true) !=0)
+		if (other.layer != LayerMask.NameToLayer("Ground"))
+			return;
+		var sensor = _sensors
+			.FirstOrDefault(x=>x == sensorCollider.gameObject);
+		if (sensor != null) {
+			var idx = _sensors.IndexOf(sensor);
+			SensorIsInTouch[idx] = 1f;
+		}
+	}
+	public void OnSensorCollisionExit(Collider sensorCollider, GameObject other)
+	{
+		//if (string.Compare(other.gameObject.name, "Terrain", true) !=0)
+		if (other.layer != LayerMask.NameToLayer("Ground"))
+			return;
+		var sensor = _sensors
+			.FirstOrDefault(x=>x == sensorCollider.gameObject);
+		if (sensor != null) {
+			var idx = _sensors.IndexOf(sensor);
+			SensorIsInTouch[idx] = 0f;
+		}
+	}   
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/SensorObservations.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Ragdoll004/TrackBodyStatesInWorldSpace.cs

@@ -0,0 +1,114 @@
+using System.Collections;
+using System.Collections.Generic;
+using System.Linq;
+using UnityEngine;
+
+public class TrackBodyStatesInWorldSpace : MonoBehaviour
+{
+    [System.Serializable]
+    public class Stat
+    {
+        public string Name;
+        public Vector3 Position;
+        public Quaternion Rotation;
+        public Vector3 Velocity;
+        public Vector3 AngualrVelocity;
+        [HideInInspector]
+        public Vector3 LastPosition;
+        [HideInInspector]
+        public Quaternion LastRotation;
+        [HideInInspector]
+        public bool LastIsSet;
+    }
+    public List<TrackBodyStatesInWorldSpace.Stat> Stats;
+
+    internal List<Rigidbody> _rigidbodies;
+
+    // Start is called before the first frame update
+    public void OnAgentInitialize()
+    {
+        _rigidbodies = GetComponentsInChildren<Rigidbody>().ToList();
+        Stats = _rigidbodies
+            .Select(x=> new TrackBodyStatesInWorldSpace.Stat{Name = x.name})
+            .ToList();        
+    }
+
+    void FixedUpdate()
+    {
+        if (_rigidbodies == null)
+            OnAgentInitialize();
+            
+        float timeDelta = Time.fixedDeltaTime;
+
+        foreach (var rb in _rigidbodies)
+        {
+            Stat stat = Stats.First(x=>x.Name == rb.name);
+            if (!stat.LastIsSet)
+            {
+                stat.LastPosition = rb.transform.position;
+                stat.LastRotation = rb.transform.rotation;
+            }
+            stat.Position = rb.transform.position;
+            stat.Rotation = rb.transform.rotation;
+            stat.Velocity = rb.transform.position - stat.LastPosition;
+            stat.Velocity /= timeDelta;
+            stat.AngualrVelocity = DReConObservationStats.GetAngularVelocity(stat.LastRotation, rb.transform.rotation, timeDelta);
+            stat.LastPosition = rb.transform.position;
+            stat.LastRotation = rb.transform.rotation;
+            stat.LastIsSet = true;
+        }        
+    }
+
+    public void LinkStatsToRigidBodies()
+    {
+        foreach (var rb in _rigidbodies)
+        {
+            Stat stat = Stats.First(x=>x.Name == rb.name);
+            stat.LastPosition = rb.transform.position;
+            stat.LastRotation = rb.transform.rotation;
+            stat.Position = rb.transform.position;
+            stat.Rotation = rb.transform.rotation;
+            stat.Velocity = Vector3.zero;
+            stat.AngualrVelocity = Vector3.zero;
+            stat.LastPosition = rb.transform.position;
+            stat.LastRotation = rb.transform.rotation;
+            stat.LastIsSet = true;
+        }        
+        
+    }
+
+    public void CopyStatesTo(GameObject target)
+    {
+        var targets = target.GetComponentsInChildren<ArticulationBody>().ToList();
+        var root = targets.First(x=>x.isRoot);
+        root.gameObject.SetActive(false);
+        foreach (var stat in Stats)
+        {
+            var targetRb = targets.First(x=>x.name == stat.Name);
+            targetRb.transform.position = stat.Position;
+            targetRb.transform.rotation = stat.Rotation;
+            // targetRb.velocity = stat.Velocity;
+            // targetRb.angularVelocity = stat.AngualrVelocity;
+
+            // var drive = targetRb.yDrive;
+            // drive.targetVelocity = stat.AngualrVelocity.x;
+            // targetRb.yDrive = drive;
+
+            // drive = targetRb.zDrive;
+            // drive.targetVelocity = stat.AngualrVelocity.y;
+            // targetRb.zDrive = drive;
+
+            // drive = targetRb.xDrive;
+            // drive.targetVelocity = stat.AngualrVelocity.z;
+            // targetRb.xDrive = drive;
+
+            targetRb.inertiaTensor = stat.Velocity;
+            targetRb.inertiaTensorRotation = Quaternion.Euler(stat.AngualrVelocity);
+            if (targetRb.isRoot)
+            {
+                targetRb.TeleportRoot(stat.Position, stat.Rotation);
+            }
+        }
+        root.gameObject.SetActive(true);
+    }
+}

Assets/3_MarathonEnvs/Scripts/Ragdoll004/TrackBodyStatesInWorldSpace.cs.meta

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

Assets/3_MarathonEnvs/Scripts/Shared.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: 439894d914f161849839b9ed7ef6fc08
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

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: