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

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;
}
}
}
}