void UpdateAnimationStep(float timeStep)
{
// HACK deal with two of first frame
if (NormalizedTime == 0f && AnimationSteps.FirstOrDefault(x => x.NormalizedTime == 0f) != null)
{
return;
}
// var c = _master.Muscles.Count;
var c = BodyParts.Count;
var animStep = new AnimationStep();
animStep.TimeStep = timeStep;
animStep.NormalizedTime = NormalizedTime;
// animStep.RootPositions = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.Velocities = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.RotaionVelocities = Enumerable.Repeat(Quaternion.identity, c).ToList();
animStep.AngularVelocities = Enumerable.Repeat(Vector3.zero, c).ToList();
// animStep.NormalizedAngularVelocities = Enumerable.Repeat(Vector3.zero, c).ToList();
// animStep.RootRotations = Enumerable.Repeat(Quaternion.identity, c).ToList();
animStep.RootAngles = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.Positions = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.Rotaions = Enumerable.Repeat(Quaternion.identity, c).ToList();
animStep.Velocity = transform.position - _lastVelocityPosition;
animStep.Names = BodyParts.Select(x => x.Name).ToList();
animStep.SensorIsInTouch = new List <float>(SensorIsInTouch);
_lastVelocityPosition = transform.position;
var rootBone = BodyParts[0];
foreach (var bodyPart in BodyParts)
{
var i = BodyParts.IndexOf(bodyPart);
if (i == 0)
{
animStep.Rotaions[i] = Quaternion.Inverse(_baseRotation) * bodyPart.Transform.rotation;
animStep.Positions[i] = bodyPart.Transform.position - bodyPart.InitialRootPosition;
animStep.RootAngles[i] = animStep.Rotaions[i].eulerAngles;
}
else
{
animStep.Rotaions[i] = Quaternion.Inverse(_baseRotation) * bodyPart.Transform.rotation;
animStep.RootAngles[i] = animStep.Rotaions[i].eulerAngles;
animStep.Positions[i] = bodyPart.Transform.position - rootBone.Transform.position;
}
if (NormalizedTime != 0f)
{
animStep.Velocities[i] = bodyPart.Transform.position - _lastPosition[i];
animStep.RotaionVelocities[i] = JointHelper002.FromToRotation(_lastRotation[i], bodyPart.Transform.rotation);
animStep.AngularVelocities[i] = animStep.RotaionVelocities[i].eulerAngles;
}
_lastPosition[i] = bodyPart.Transform.position;
_lastRotation[i] = bodyPart.Transform.rotation;
}
animStep.CenterOfMass = GetCenterOfMass();
animStep.TransformPosition = transform.position;
animStep.TransformRotation = transform.rotation;
AnimationSteps.Add(animStep);
}
protected virtual void LateUpdate()
{
if (_resetCenterOfMassOnLastUpdate)
{
ObsCenterOfMass = JointHelper002.GetCenterOfMassRelativeToRoot(BodyParts);
//ObsCenterOfMass = GetCenterOfMass();
_lastCenterOfMass = ObsCenterOfMass;
_resetCenterOfMassOnLastUpdate = false;
}
#if UNITY_EDITOR
VisualizeTargetPose();
#endif
}
public void UpdateObservations()
{
Quaternion rotation;
Quaternion rotationFromBase;
Vector3 position;
Vector3 angle;
if (this == Root)
{
rotation = Quaternion.Inverse(InitialRootRotation) * Transform.rotation;
position = Transform.position - InitialRootPosition;
angle = rotation.eulerAngles;
}
else
{
rotation = Quaternion.Inverse(Root.Transform.rotation) * Transform.rotation;
angle = rotation.eulerAngles;
position = Transform.position - Root.Transform.position;
}
rotationFromBase = Quaternion.Inverse(BaseRotation) * Transform.rotation;
// Vector3 animPosition = bodyPart.InitialRootPosition + animStep.Positions[0];
// Quaternion animRotation = bodyPart.InitialRootRotation * animStep.Rotaions[0];
// if (i != 0) {
// animPosition += animStep.Positions[i];
// animRotation *= animStep.Rotaions[i];
// }
if (_firstRunComplete == false)
{
_lastUpdateObsTime = Time.time;
_lastObsRotation = Transform.rotation;
_lastLocalPosition = Transform.position;
}
var dt = Time.time - _lastUpdateObsTime;
_lastUpdateObsTime = Time.time;
var velocity = Transform.position - _lastLocalPosition;
var rotationVelocity = JointHelper002.FromToRotation(_lastObsRotation, Transform.rotation);
var angularVelocity = rotationVelocity.eulerAngles;
_lastLocalPosition = Transform.position;
_lastObsRotation = Transform.rotation;
angularVelocity = NormalizedEulerAngles(angularVelocity);
angularVelocity /= 128f;
if (dt > 0f)
{
angularVelocity /= dt;
}
if (dt > 0f)
{
velocity /= dt;
}
ObsDeltaFromAnimationPosition = _animationPosition - position;
ObsNormalizedDeltaFromAnimationRotation = _animationRotation * Quaternion.Inverse(rotationFromBase);
ObsAngleDeltaFromAnimationRotation = Mathf.Abs(Quaternion.Angle(_animationRotation, rotationFromBase) / 180f);
ObsLocalPosition = position;
ObsRotation = rotation;
ObsRotationFromBase = rotationFromBase;
ObsRotationVelocity = angularVelocity;
ObsVelocity = velocity;
// ObsRotation = this.LocalRotation;
// ObsRotation = (ToJointSpaceInverse * UnityEngine.Quaternion.Inverse(this.LocalRotation) * this.ToJointSpaceDefault);
// var normalizedRotation = NormalizedEulerAngles(ObsRotation.eulerAngles);
// Debug code
// if (Group == BodyHelper002.BodyPartGroup.Head){
// var debug = 1;
// }
// var dt = Time.time - _lastUpdateObsTime;
// _lastUpdateObsTime = Time.time;
// var rotationVelocity = ObsRotation.eulerAngles - _lastObsRotation.eulerAngles;
// rotationVelocity = NormalizedEulerAngles(rotationVelocity);
// rotationVelocity /= 128f;
// if (dt > 0f)
// rotationVelocity /= dt;
// ObsRotationVelocity = rotationVelocity;
// _lastObsRotation = ObsRotation;
// ObsLocalPosition = Transform.position - Root.Transform.position;
// var velocity = ObsLocalPosition - _lastLocalPosition;
// ObsVelocity = velocity;
// if (dt > 0f)
// velocity /= dt;
// _lastLocalPosition = ObsLocalPosition;
// // ObsDeltaFromAnimationPosition = _animationPosition - Transform.position;
// // ObsNormalizedDeltaFromAnimationRotation = _animationRotation * Quaternion.Inverse(Transform.rotation);
// // ObsAngleDeltaFromAnimationRotation = Quaternion.Angle(_animationRotation, Transform.rotation);
// // ObsNormalizedDeltaFromAnimationRotation = NormalizedEulerAngles(obsDeltaFromAnimationRotation.eulerAngles);
if (_firstRunComplete == false)
{
ObsDeltaFromAnimationPosition = Vector3.zero;
ObsNormalizedDeltaFromAnimationRotation = new Quaternion(0, 0, 0, 0);
ObsAngleDeltaFromAnimationRotation = 0f;
}
DebugMaxRotationVelocity = Vector3Max(DebugMaxRotationVelocity, angularVelocity);
DebugMaxVelocity = Vector3Max(DebugMaxVelocity, velocity);
_firstRunComplete = true;
}
// Prepares an animation step. Records the positions, rotations, velocities
// of the rigid bodies forming an animation into the animation step structure.
void UpdateAnimationStep(float timeStep)
{
// HACK deal with two of first frame
if (NormalizedTime == 0f && AnimationSteps.FirstOrDefault(x => x.NormalizedTime == 0f) != null)
{
return;
}
// var c = _master.Muscles.Count;
var c = BodyParts.Count;
var animStep = new AnimationStep();
animStep.TimeStep = timeStep;
animStep.NormalizedTime = NormalizedTime;
animStep.Velocities = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.VelocitiesLocal = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.AngularVelocities = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.AngularVelocitiesLocal = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.Positions = Enumerable.Repeat(Vector3.zero, c).ToList();
animStep.Rotations = Enumerable.Repeat(Quaternion.identity, c).ToList();
animStep.CenterOfMass = JointHelper002.GetCenterOfMassRelativeToRoot(BodyParts);
//animStep.CenterOfMass = GetCenterOfMass();
animStep.CenterOfMassVelocity = animStep.CenterOfMass - _lastCenterOfMass;
animStep.Names = BodyParts.Select(x => x.Name).ToList();
animStep.SensorIsInTouch = new List <float>(SensorIsInTouch);
_lastCenterOfMass = animStep.CenterOfMass;
var rootBone = BodyParts[0];
foreach (var bodyPart in BodyParts)
{
var i = BodyParts.IndexOf(bodyPart);
if (i == 0)
{
animStep.Rotations[i] = Quaternion.Inverse(bodyPart.InitialRootRotation) * bodyPart.Transform.rotation;
animStep.Positions[i] = bodyPart.Transform.position - bodyPart.InitialRootPosition;
}
else
{
animStep.Rotations[i] = Quaternion.Inverse(rootBone.Transform.rotation) * bodyPart.Transform.rotation;
animStep.Positions[i] = bodyPart.Transform.position - rootBone.Transform.position;
}
if (NormalizedTime != 0f)
{
animStep.Velocities[i] = (bodyPart.Transform.position - _lastPosition[i]) / (_decisionRequester.DecisionPeriod * Time.fixedDeltaTime);;
animStep.AngularVelocities[i] = JointHelper002.CalcDeltaRotationNormalizedEuler(_lastRotation[i], bodyPart.Transform.rotation) / (_decisionRequester.DecisionPeriod * Time.fixedDeltaTime);;
animStep.VelocitiesLocal[i] = (animStep.Positions[i] - _lastPositionLocal[i]) / (_decisionRequester.DecisionPeriod * Time.fixedDeltaTime);;
animStep.AngularVelocitiesLocal[i] = JointHelper002.CalcDeltaRotationNormalizedEuler(_lastRotationLocal[i], animStep.Rotations[i]) / (_decisionRequester.DecisionPeriod * Time.fixedDeltaTime);
}
if (bodyPart.Rigidbody != null)
{
bodyPart.Rigidbody.angularVelocity = JointHelper002.CalcDeltaRotationNormalizedEuler(bodyPart.Transform.rotation, _lastRotation[i]) / (_decisionRequester.DecisionPeriod * Time.fixedDeltaTime);
bodyPart.Rigidbody.velocity = (bodyPart.Transform.position - _lastPosition[i]) / (_decisionRequester.DecisionPeriod * Time.fixedDeltaTime);
bodyPart.Rigidbody.transform.position = bodyPart.Transform.position;
bodyPart.Rigidbody.transform.rotation = bodyPart.Transform.rotation;
}
_lastPosition[i] = bodyPart.Transform.position;
_lastRotation[i] = bodyPart.Transform.rotation;
_lastPositionLocal[i] = animStep.Positions[i];
_lastRotationLocal[i] = animStep.Rotations[i];
}
animStep.TransformPosition = transform.position;
animStep.TransformRotation = transform.rotation;
animStep.AngularMoment = JointHelper002.GetAngularMoment(BodyParts);
AnimationSteps.Add(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);
}
// Update values like ObsLocalPosition, ObsRotation, ... that will be accessed
// by the agent as observations for the neural network. Also calculates distances
// to an animation being mimicked
public void UpdateObservations()
{
Quaternion rotation;
Vector3 position;
if (this == Root)
{
rotation = Quaternion.Inverse(InitialRootRotation) * Transform.rotation;
position = Transform.position - InitialRootPosition;
}
else
{
rotation = Quaternion.Inverse(Root.Transform.rotation) * Transform.rotation;
position = Transform.position - Root.Transform.position;
}
if (_firstRunComplete == false)
{
_lastUpdateObsTime = Time.time;
_lastLocalPosition = position;
_lastWorldPosition = Transform.position;
_lastObsRotation = rotation;
_lastWorldRotation = Transform.rotation;
}
var dt = Time.fixedDeltaTime * _decisionRequester.DecisionPeriod;
var velocity = (position - _lastLocalPosition) / dt;
var velocityWorld = (Transform.position - _lastWorldPosition) / dt;
var angularVelocity = JointHelper002.CalcDeltaRotationNormalizedEuler(_lastObsRotation, rotation) / dt;
var angularVelocityWorld = JointHelper002.CalcDeltaRotationNormalizedEuler(_lastWorldRotation, Transform.rotation) / dt;
// old calulation for observation vector
//angularVelocity = NormalizedEulerAngles(rotationVelocity.eulerAngles);
//angularVelocity /= 128f;
// old calculation end
_lastUpdateObsTime = Time.time;
_lastLocalPosition = position;
_lastWorldPosition = Transform.position;
_lastObsRotation = rotation;
_lastWorldRotation = Transform.rotation;
//if (Name == "right_right_foot") {
// Debug.Log("^^^^^^^^^^^^");
// Debug.Log("body part name: " + Name);
// Debug.Log("animation angular velocity:" + _animationAngularVelocity);
// Debug.Log("angular velocity:" + angularVelocity);
// Debug.Log("animation angular velocity world:" + _animationAngularVelocityWorld);
// Debug.Log("angular velocity world:" + angularVelocityWorld);
// Debug.Log("rotation local:" + rotation.eulerAngles);
// Debug.Log("animation rotation local: " + _animationRotation.eulerAngles);
// Debug.Log("velocity world: " + velocityWorld);
// Debug.Log("animation velocity world:" + _animationVelocityWorld);
// Debug.Log("transform position:" + Transform.position);
// Debug.Log("animation position world: " + _animationPositionWorld);
// Debug.Log("dt:" + dt);
//}
ObsLocalPosition = position;
ObsRotation = rotation;
ObsRotationVelocity = angularVelocity;
ObsVelocity = velocity;
ObsDeltaFromAnimationPosition = _animationPositionWorld - Transform.position;
ObsAngleDeltaFromAnimationRotation = Quaternion.Angle(_animationRotation, rotation);
ObsAngleDeltaFromAnimationRotation = JointHelper002.NormalizedAngle(ObsAngleDeltaFromAnimationRotation);
ObsDeltaFromAnimationVelocity = _animationVelocityWorld - velocityWorld;
ObsDeltaFromAnimationAngularVelocity = (_animationAngularVelocity - angularVelocity);
ObsDeltaFromAnimationAngularVelocityWorld = (_animationAngularVelocityWorld - angularVelocityWorld);
DebugMaxRotationVelocity = Vector3Max(DebugMaxRotationVelocity, angularVelocity);
DebugMaxVelocity = Vector3Max(DebugMaxVelocity, velocity);
_firstRunComplete = true;
}
