organisation & splat changes
This commit is contained in:
@@ -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;
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/BodyPartDifferenceStats.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/BodyPartDifferenceStats.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 2e418599cb06e4b48a6b848a526eabb1
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservationStats.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservationStats.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 57f67553e98604b56957f53986eb03ea
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
221
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservations.cs
Normal file
221
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservations.cs
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservations.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConObservations.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 81f30d0638fe74d3faa981d3a313928f
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
392
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewardStats.cs
Normal file
392
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewardStats.cs
Normal file
@@ -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;
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewardStats.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewardStats.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: a152576916e144ab68f92ab021188574
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
217
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewards.cs
Normal file
217
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewards.cs
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewards.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DReConRewards.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 1ec1e370542a14f3da19024594e4c394
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
29
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugAddOffset.cs
Normal file
29
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugAddOffset.cs
Normal file
@@ -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;
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugAddOffset.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugAddOffset.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 9d663abd7b0e14942907bf7b1557f8b6
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
177
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugJoints.cs
Normal file
177
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugJoints.cs
Normal file
@@ -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);
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugJoints.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/DebugJoints.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 9cdefdb10ded44c16a6a038694a4f530
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
174
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MappingOffset.cs
Normal file
174
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MappingOffset.cs
Normal file
@@ -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;
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MappingOffset.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MappingOffset.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 833d19473d96562409eca2917d253b6b
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapAnimatorController.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapAnimatorController.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: ddcd176a32835474d9ddbce9eb0273af
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -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();
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapControllerArtanim.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapControllerArtanim.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 5f59f726c3dcc1c4287d40a0e3156276
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -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
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapRangeOfMotionAnimatorController.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/MocapRangeOfMotionAnimatorController.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 46dc81c1b45ee42e5bc937f012b5d79e
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
79
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDoll004.cs
Normal file
79
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDoll004.cs
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDoll004.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDoll004.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: e2f99f5d487864414a2cc1394839d407
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
545
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDollAgent.cs
Normal file
545
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDollAgent.cs
Normal file
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDollAgent.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagDollAgent.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 0ddf48037c6c1455a8a163a00e342e36
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -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();
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagdollControllerArtanim.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/RagdollControllerArtanim.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 9cf757e03aeee494fb67ce8859dc8590
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: -1100
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/SensorObservations.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/SensorObservations.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 9ad1013fcfea94c78bd38ee533603b89
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
@@ -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);
|
||||
}
|
||||
}
|
||||
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/TrackBodyStatesInWorldSpace.cs.meta
generated
Normal file
11
Assets/3_MarathonEnvs/Scripts/Ragdoll004/TrackBodyStatesInWorldSpace.cs.meta
generated
Normal file
@@ -0,0 +1,11 @@
|
||||
fileFormatVersion: 2
|
||||
guid: 25f24ef536e54416c9c95e2d222138e5
|
||||
MonoImporter:
|
||||
externalObjects: {}
|
||||
serializedVersion: 2
|
||||
defaultReferences: []
|
||||
executionOrder: 0
|
||||
icon: {instanceID: 0}
|
||||
userData:
|
||||
assetBundleName:
|
||||
assetBundleVariant:
|
||||
Reference in New Issue
Block a user