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