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);
}
}
// Start is called before the first frame update
void Awake()
{
_spawnableEnv = GetComponentInParent <SpawnableEnv>();
_inputController = _spawnableEnv.GetComponentInChildren <InputController>();
BodyPartDifferenceStats = BodyPartsToTrack
.Select(x => new BodyPartDifferenceStats {
Name = x
})
.ToList();
_mocapBodyStats = new GameObject("MocapDReConObservationStats").AddComponent <DReConObservationStats>();
var mocapController = _spawnableEnv.GetComponentInChildren <MocapController>();
_mocapBodyStats.ObjectToTrack = mocapController;
_mocapBodyStats.transform.SetParent(_spawnableEnv.transform);
_mocapBodyStats.OnAwake(BodyPartsToTrack, _mocapBodyStats.ObjectToTrack.transform);
_ragDollBodyStats = new GameObject("RagDollDReConObservationStats").AddComponent <DReConObservationStats>();
_ragDollBodyStats.ObjectToTrack = this;
_ragDollBodyStats.transform.SetParent(_spawnableEnv.transform);
_ragDollBodyStats.OnAwake(BodyPartsToTrack, transform);
}
void FixedUpdate()
{
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;
}
}
// 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;
}
