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