void MimicAnimationFrame(StyleTransfer002Animator.AnimationStep animStep, bool onlySetAnimation = false)
{
if (!onlySetAnimation)
{
foreach (var rb in GetComponentsInChildren<Rigidbody>())
{
rb.angularVelocity = Vector3.zero;
rb.velocity = Vector3.zero;
}
}
foreach (var bodyPart in BodyParts)
{
var i = animStep.Names.IndexOf(bodyPart.Name);
Vector3 animPosition = bodyPart.InitialRootPosition + animStep.Positions[0];
Quaternion animRotation = bodyPart.InitialRootRotation * animStep.Rotaions[0];
if (i != 0) {
animPosition += animStep.Positions[i];
animRotation = bodyPart.InitialRootRotation * animStep.Rotaions[i];
}
Vector3 angularVelocity = animStep.AngularVelocities[i] / Time.fixedDeltaTime;
Vector3 velocity = animStep.Velocities[i] / Time.fixedDeltaTime;
// angularVelocity = Vector3.zero;
// velocity = Vector3.zero;
bool setAnim = !onlySetAnimation;
if (bodyPart.Name.Contains("head") || bodyPart.Name.Contains("upper_waist"))
setAnim = false;
if (setAnim)
bodyPart.MoveToAnim(animPosition, animRotation, angularVelocity, velocity);
bodyPart.SetAnimationPosition(animStep.Positions[i], animStep.Rotaions[i]);
}
}
void Step(StyleTransfer002Animator.AnimationStep animStep)
{
if (_phaseIsRunning){
if (!DebugShowWithOffset)
AnimationIndex++;
if (AnimationIndex>=_muscleAnimator.AnimationSteps.Count) {
//ResetPhase();
Done();
AnimationIndex--;
}
}
if (_phaseIsRunning && IsInferenceMode && CameraFollowMe)
{
_muscleAnimator.anim.enabled = true;
_muscleAnimator.anim.Play("Record",0, animStep.NormalizedTime);
_muscleAnimator.anim.transform.position = animStep.TransformPosition;
_muscleAnimator.anim.transform.rotation = animStep.TransformRotation;
}
}
void CompareAnimationFrame(StyleTransfer002Animator.AnimationStep animStep)
{
MimicAnimationFrame(animStep, true);
}
StyleTransfer002Animator.AnimationStep UpdateObservations()
{
if (DebugMode)
{
AnimationIndex = 0;
}
var debugStepIdx = AnimationIndex;
StyleTransfer002Animator.AnimationStep animStep = null;
StyleTransfer002Animator.AnimationStep debugAnimStep = null;
if (_phaseIsRunning)
{
debugStepIdx += DebugAnimOffset;
if (DebugShowWithOffset)
{
debugStepIdx = Mathf.Clamp(debugStepIdx, 0, _muscleAnimator.AnimationSteps.Count);
debugAnimStep = _muscleAnimator.AnimationSteps[debugStepIdx];
}
animStep = _muscleAnimator.AnimationSteps[AnimationIndex];
}
EndEffectorDistance = 0f;
FeetRotationDistance = 0f;
EndEffectorVelocityDistance = 0;
RotationDistance = 0f;
VelocityDistance = 0f;
CenterOfMassDistance = 0f;
SensorDistance = 0f;
if (_phaseIsRunning && DebugShowWithOffset)
{
MimicAnimationFrame(debugAnimStep);
}
else if (_phaseIsRunning)
{
CompareAnimationFrame(animStep);
}
foreach (var muscle in Muscles)
{
var i = Muscles.IndexOf(muscle);
muscle.UpdateObservations();
if (!DebugShowWithOffset && !DebugDisableMotor)
{
muscle.UpdateMotor();
}
if (!muscle.Rigidbody.useGravity)
{
continue; // skip sub joints
}
}
foreach (var bodyPart in BodyParts)
{
if (_phaseIsRunning)
{
bodyPart.UpdateObservations();
var rotDistance = bodyPart.ObsAngleDeltaFromAnimationRotation;
var squareRotDistance = Mathf.Pow(rotDistance, 2);
RotationDistance += squareRotDistance;
// RotationDistance += rotDistance;
if (bodyPart.Group == BodyHelper002.BodyPartGroup.Hand ||
bodyPart.Group == BodyHelper002.BodyPartGroup.Torso ||
bodyPart.Group == BodyHelper002.BodyPartGroup.Foot)
{
EndEffectorDistance += bodyPart.ObsDeltaFromAnimationPosition.sqrMagnitude;
// EndEffectorDistance += bodyPart.ObsDeltaFromAnimationPosition.magnitude;
}
if (bodyPart.Group == BodyHelper002.BodyPartGroup.Foot)
{
FeetRotationDistance += squareRotDistance;
// EndEffectorRotDistance += rotDistance;
}
}
}
// RotationDistance *= RotationDistance; // take the square;
ObsCenterOfMass = GetCenterOfMass();
if (_phaseIsRunning)
{
CenterOfMassDistance = (animStep.CenterOfMass - ObsCenterOfMass).sqrMagnitude;
}
ObsVelocity = ObsCenterOfMass - _lastCenterOfMass;
if (_fakeVelocity)
{
ObsVelocity = animStep.Velocity;
}
_lastCenterOfMass = ObsCenterOfMass;
if (!_resetCenterOfMassOnLastUpdate)
{
_fakeVelocity = false;
}
if (_phaseIsRunning)
{
var animVelocity = animStep.Velocity / Time.fixedDeltaTime;
ObsVelocity /= Time.fixedDeltaTime;
var velocityDistance = ObsVelocity - animVelocity;
VelocityDistance = velocityDistance.sqrMagnitude;
var sensorDistance = 0.0;
var sensorDistanceStep = 1.0 / _agent.SensorIsInTouch.Count;
for (int i = 0; i < _agent.SensorIsInTouch.Count; i++)
{
if (animStep.SensorIsInTouch[i] != _agent.SensorIsInTouch[i])
{
sensorDistance += sensorDistanceStep;
}
}
SensorDistance = (float)sensorDistance;
}
// normalize distances
// VelocityDistance /= 10f;
// VelocityDistance = Mathf.Clamp(VelocityDistance, -1f, 1f);
// CenterOfMassDistance
if (!IgnorRewardUntilObservation)
{
MaxEndEffectorDistance = Mathf.Max(MaxEndEffectorDistance, EndEffectorDistance);
MaxFeetRotationDistance = Mathf.Max(MaxFeetRotationDistance, FeetRotationDistance);
MaxEndEffectorVelocityDistance = Mathf.Max(MaxEndEffectorVelocityDistance, EndEffectorVelocityDistance);
MaxRotationDistance = Mathf.Max(MaxRotationDistance, RotationDistance);
MaxVelocityDistance = Mathf.Max(MaxVelocityDistance, VelocityDistance);
MaxCenterOfMassDistance = Mathf.Max(MaxCenterOfMassDistance, CenterOfMassDistance);
MaxSensorDistance = Mathf.Max(MaxSensorDistance, SensorDistance);
}
if (IgnorRewardUntilObservation)
{
IgnorRewardUntilObservation = false;
}
ObsPhase = _muscleAnimator.AnimationSteps[AnimationIndex].NormalizedTime % 1f;
return(animStep);
}
// Calculate values like ObsCenterOfMass, ObsAngularMoment that will be used
// as observations fed into the neural network for training and inference. Also
// calculates the distance metrics between animation's and agents' body parts.
// The distances are used by the Agent to calculate rewards. The max distances
// can be used to manually tune the denominators in exp(-distance/denominator)
// when forming reward function.
StyleTransfer002Animator.AnimationStep UpdateObservations()
{
if (DebugMode)
{
AnimationIndex = 0;
}
var debugStepIdx = AnimationIndex;
StyleTransfer002Animator.AnimationStep animStep = null;
StyleTransfer002Animator.AnimationStep debugAnimStep = null;
if (_phaseIsRunning)
{
debugStepIdx += DebugAnimOffset;
if (DebugShowWithOffset)
{
debugStepIdx = Mathf.Clamp(debugStepIdx, 0, _muscleAnimator.AnimationSteps.Count);
debugAnimStep = _muscleAnimator.AnimationSteps[debugStepIdx];
}
animStep = _muscleAnimator.AnimationSteps[AnimationIndex];
}
EndEffectorDistance = 0f;
EndEffectorVelocityDistance = 0;
JointAngularVelocityDistance = 0;
JointAngularVelocityDistanceWorld = 0;
RotationDistance = 0f;
CenterOfMassVelocityDistance = 0f;
CenterOfMassDistance = 0f;
AngularMomentDistance = 0f;
SensorDistance = 0f;
if (_phaseIsRunning && DebugShowWithOffset)
{
MimicAnimationFrame(debugAnimStep);
}
else if (_phaseIsRunning)
{
CompareAnimationFrame(animStep);
}
foreach (var muscle in Muscles)
{
var i = Muscles.IndexOf(muscle);
muscle.UpdateObservations();
if (!DebugShowWithOffset && !DebugDisableMotor)
{
muscle.UpdateMotor();
}
if (!muscle.Rigidbody.useGravity)
{
continue; // skip sub joints
}
}
foreach (var bodyPart in BodyParts)
{
if (_phaseIsRunning)
{
bodyPart.UpdateObservations();
var rotDistance = bodyPart.ObsAngleDeltaFromAnimationRotation;
var squareRotDistance = Mathf.Pow(rotDistance, 2);
RotationDistance += squareRotDistance;
JointAngularVelocityDistance += bodyPart.ObsDeltaFromAnimationAngularVelocity.sqrMagnitude;
JointAngularVelocityDistanceWorld += bodyPart.ObsDeltaFromAnimationAngularVelocityWorld.sqrMagnitude;
if (bodyPart.Group == BodyHelper002.BodyPartGroup.Hand ||
bodyPart.Group == BodyHelper002.BodyPartGroup.Torso ||
bodyPart.Group == BodyHelper002.BodyPartGroup.Foot)
{
EndEffectorDistance += bodyPart.ObsDeltaFromAnimationPosition.sqrMagnitude;
EndEffectorVelocityDistance += bodyPart.ObsDeltaFromAnimationVelocity.sqrMagnitude;
}
}
}
ObsCenterOfMass = JointHelper002.GetCenterOfMassRelativeToRoot(BodyParts);
ObsAngularMoment = JointHelper002.GetAngularMoment(BodyParts);
if (_phaseIsRunning)
{
CenterOfMassDistance = (animStep.CenterOfMass - ObsCenterOfMass).sqrMagnitude;
AngularMomentDistance = (animStep.AngularMoment - ObsAngularMoment).sqrMagnitude;
}
ObsVelocity = ObsCenterOfMass - _lastCenterOfMass;
if (_fakeVelocity)
{
ObsVelocity = animStep.CenterOfMassVelocity;
}
_lastCenterOfMass = ObsCenterOfMass;
if (!_resetCenterOfMassOnLastUpdate)
{
_fakeVelocity = false;
}
if (_phaseIsRunning)
{
var animVelocity = animStep.CenterOfMassVelocity / (Time.fixedDeltaTime * _decisionRequester.DecisionPeriod);
ObsVelocity /= (Time.fixedDeltaTime * _decisionRequester.DecisionPeriod);
CenterOfMassVelocityDistance = (ObsVelocity - animVelocity).sqrMagnitude;
SensorDistance = 0.0f;
var sensorDistanceStep = 1.0f / _agent.SensorIsInTouch.Count;
for (int i = 0; i < _agent.SensorIsInTouch.Count; i++)
{
if (animStep.SensorIsInTouch[i] != _agent.SensorIsInTouch[i])
{
SensorDistance += sensorDistanceStep;
}
}
}
if (!IgnorRewardUntilObservation)
{
MaxEndEffectorDistance = Mathf.Max(MaxEndEffectorDistance, EndEffectorDistance);
MaxEndEffectorVelocityDistance = Mathf.Max(MaxEndEffectorVelocityDistance, EndEffectorVelocityDistance);
MaxRotationDistance = Mathf.Max(MaxRotationDistance, RotationDistance);
MaxCenterOfMassVelocityDistance = Mathf.Max(MaxCenterOfMassVelocityDistance, CenterOfMassVelocityDistance);
MaxEndEffectorVelocityDistance = Mathf.Max(MaxEndEffectorVelocityDistance, EndEffectorVelocityDistance);
MaxJointAngularVelocityDistance = Mathf.Max(MaxJointAngularVelocityDistance, JointAngularVelocityDistance);
MaxJointAngularVelocityDistanceWorld = Mathf.Max(MaxJointAngularVelocityDistanceWorld, JointAngularVelocityDistanceWorld);
MaxCenterOfMassDistance = Mathf.Max(MaxCenterOfMassDistance, CenterOfMassDistance);
MaxAngularMomentDistance = Mathf.Max(MaxAngularMomentDistance, AngularMomentDistance);
MaxSensorDistance = Mathf.Max(MaxSensorDistance, SensorDistance);
}
if (IgnorRewardUntilObservation)
{
IgnorRewardUntilObservation = false;
}
ObsPhase = _muscleAnimator.AnimationSteps[AnimationIndex].NormalizedTime % 1f;
return(animStep);
}
