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