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