using System.Collections; using System.Collections.Generic; using System.Linq; using Unity.MLAgents; using Unity.MLAgents.Actuators; using Unity.MLAgents.Sensors; using UnityEngine; using ManyWorlds; using UnityEngine.Assertions; using System; using Unity.Mathematics; public class RagDollAgent : Agent { [Header("Settings")] public float FixedDeltaTime = 1f/60f; public float SmoothBeta = 0.2f; [Header("Camera")] public bool RequestCamera; public bool CameraFollowMe; public Transform CameraTarget; [Header("... debug")] public bool SkipRewardSmoothing; public bool debugCopyMocap; public bool ignorActions; public bool dontResetOnZeroReward; public bool dontSnapMocapToRagdoll; public bool DebugPauseOnReset; public bool UsePDControl = true; List _mocapBodyParts; List _bodyParts; SpawnableEnv _spawnableEnv; DReConObservations _dReConObservations; DReConRewards _dReConRewards; RagDoll004 _ragDollSettings; TrackBodyStatesInWorldSpace _trackBodyStatesInWorldSpace; List _motors; MarathonTestBedController _debugController; InputController _inputController; SensorObservations _sensorObservations; DecisionRequester _decisionRequester; MocapAnimatorController _mocapAnimatorController; bool _hasLazyInitialized; float[] _smoothedActions; float[] _mocapTargets; [Space(16)] [SerializeField] bool _hasAwake = false; MocapControllerArtanim _mocapControllerArtanim; private float timer = 0f; private float waitTime = 3f; private bool waitStarted; private float x = 0; private float y = 100; public TerrainCollider m_TerrainCollider; public LayerMask m_ExcludeLayer; public LayerMask m_Temp; public delegate void OnMoveToNextModelDelegate(); public static OnMoveToNextModelDelegate m_MoveToNextModel; public delegate void OnAgentFailDelegate(); public static OnAgentFailDelegate m_AgentFail; void Awake() { if (RequestCamera && CameraTarget != null) { // Will follow the last object to be spawned var camera = FindObjectOfType(); if(camera != null) { var follow = camera.GetComponent(); if (follow != null) follow.target = CameraTarget; } } _hasAwake = true; } void Update() { if (debugCopyMocap) { EndEpisode(); } Assert.IsTrue(_hasLazyInitialized); // hadle mocap going out of bounds if (!_spawnableEnv.IsPointWithinBoundsInWorldSpace(_mocapControllerArtanim.transform.position)) { _mocapControllerArtanim.transform.position = _spawnableEnv.transform.position; _trackBodyStatesInWorldSpace.LinkStatsToRigidBodies(); EndEpisode(); } } override public void CollectObservations(VectorSensor sensor) { Assert.IsTrue(_hasLazyInitialized); float timeDelta = Time.fixedDeltaTime * _decisionRequester.DecisionPeriod; _dReConObservations.OnStep(timeDelta); sensor.AddObservation(_dReConObservations.MocapCOMVelocity); sensor.AddObservation(_dReConObservations.RagDollCOMVelocity); sensor.AddObservation(_dReConObservations.RagDollCOMVelocity-_dReConObservations.MocapCOMVelocity); sensor.AddObservation(_dReConObservations.InputDesiredHorizontalVelocity); sensor.AddObservation(_dReConObservations.InputJump); sensor.AddObservation(_dReConObservations.InputBackflip); sensor.AddObservation(_dReConObservations.HorizontalVelocityDifference); // foreach (var stat in _dReConObservations.MocapBodyStats) // { // sensor.AddObservation(stat.Position); // sensor.AddObservation(stat.Velocity); // } foreach (var stat in _dReConObservations.RagDollBodyStats) { sensor.AddObservation(stat.Position); sensor.AddObservation(stat.Velocity); } foreach (var stat in _dReConObservations.BodyPartDifferenceStats) { sensor.AddObservation(stat.Position); sensor.AddObservation(stat.Velocity); } sensor.AddObservation(_dReConObservations.PreviousActions); // add sensors (feet etc) sensor.AddObservation(_sensorObservations.SensorIsInTouch); } public override void OnActionReceived(ActionBuffers actions) { float[] vectorAction = actions.ContinuousActions.Select(x=>x).ToArray(); Assert.IsTrue(_hasLazyInitialized); float timeDelta = Time.fixedDeltaTime; if (!_decisionRequester.TakeActionsBetweenDecisions) timeDelta = timeDelta*_decisionRequester.DecisionPeriod; _dReConRewards.OnStep(timeDelta); bool shouldDebug = _debugController != null; bool dontUpdateMotor = false; if (_debugController != null) { dontUpdateMotor = _debugController.DontUpdateMotor; dontUpdateMotor &= _debugController.isActiveAndEnabled; dontUpdateMotor &= _debugController.gameObject.activeInHierarchy; shouldDebug &= _debugController.isActiveAndEnabled; shouldDebug &= _debugController.gameObject.activeInHierarchy; } if (shouldDebug) { vectorAction = GetDebugActions(vectorAction); } if (UsePDControl) { var targets = GetMocapTargets(); vectorAction = vectorAction .Zip(targets, (action, target)=> Mathf.Clamp(target + action *2f, -1f, 1f)) .ToArray(); } if (!SkipRewardSmoothing) vectorAction = SmoothActions(vectorAction); if (ignorActions) vectorAction = vectorAction.Select(x=>0f).ToArray(); int i = 0; var scale = 1f; x += Time.fixedDeltaTime * scale; y += Time.fixedDeltaTime * scale; foreach (var m in _motors) { if (m.isRoot) continue; if (dontUpdateMotor) continue; Vector3 targetNormalizedRotation = Vector3.zero; if (m.twistLock == ArticulationDofLock.LimitedMotion && !waitStarted) { targetNormalizedRotation.x = vectorAction[i++]; } else { var offset = 7.5f; targetNormalizedRotation.x = (Mathf.PerlinNoise(x + offset, y + offset) * 2) - 1; ; } if (m.swingYLock == ArticulationDofLock.LimitedMotion && !waitStarted) { targetNormalizedRotation.y = vectorAction[i++]; } else { var offset = 5f; targetNormalizedRotation.y = (Mathf.PerlinNoise(x + offset, y + offset) * 2) - 1; ; } if (m.swingZLock == ArticulationDofLock.LimitedMotion && !waitStarted) { targetNormalizedRotation.z = vectorAction[i++]; } else { var offset = 22.3f; targetNormalizedRotation.z = (Mathf.PerlinNoise(x + offset, y + offset) * 2) - 1; ; } UpdateMotor(m, targetNormalizedRotation); } _dReConObservations.PreviousActions = vectorAction; AddReward(_dReConRewards.Reward); // if (_dReConRewards.HeadHeightDistance > 0.5f || _dReConRewards.Reward < 1f) if (_dReConRewards.HeadHeightDistance > 0.5f || _dReConRewards.Reward <= 0f) { if (!dontResetOnZeroReward) { if (!waitStarted) { waitStarted = true; //Disable Ground Collider if (m_TerrainCollider) { // Exclude Marathon from Collider m_Temp = m_TerrainCollider.excludeLayers; m_TerrainCollider.excludeLayers = m_ExcludeLayer; } m_AgentFail?.Invoke(); } timer += Time.fixedDeltaTime; if (timer >= waitTime) { timer = 0; waitStarted = false; m_TerrainCollider.excludeLayers = m_Temp; m_MoveToNextModel?.Invoke(); EndEpisode(); } } } // else if (_dReConRewards.HeadDistance > 1.5f) else if (_dReConRewards.Reward <= 0.1f && !dontSnapMocapToRagdoll) { Transform ragDollCom = _dReConObservations.GetRagDollCOM(); Vector3 snapPosition = ragDollCom.position; snapPosition.y = 0f; _mocapControllerArtanim.SnapTo(snapPosition); AddReward(-.5f); } } float[] GetDebugActions(float[] vectorAction) { var debugActions = new List(); foreach (var m in _motors) { if (m.isRoot) continue; DebugMotor debugMotor = m.GetComponent(); if (debugMotor == null) { debugMotor = m.gameObject.AddComponent(); } // clip to -1/+1 debugMotor.Actions = new Vector3 ( Mathf.Clamp(debugMotor.Actions.x, -1f, 1f), Mathf.Clamp(debugMotor.Actions.y, -1f, 1f), Mathf.Clamp(debugMotor.Actions.z, -1f, 1f) ); Vector3 targetNormalizedRotation = debugMotor.Actions; if (m.twistLock == ArticulationDofLock.LimitedMotion) debugActions.Add(targetNormalizedRotation.x); if (m.swingYLock == ArticulationDofLock.LimitedMotion) debugActions.Add(targetNormalizedRotation.y); if (m.swingZLock == ArticulationDofLock.LimitedMotion) debugActions.Add(targetNormalizedRotation.z); } debugActions = debugActions.Select(x=>Mathf.Clamp(x,-1f,1f)).ToList(); _debugController.Actions = debugActions.ToArray(); return debugActions.ToArray(); } float[] SmoothActions(float[] vectorAction) { // yt =β at +(1−β)yt−1 if (_smoothedActions == null) _smoothedActions = vectorAction.Select(x=>0f).ToArray(); _smoothedActions = vectorAction .Zip(_smoothedActions, (a, y)=> SmoothBeta * a + (1f-SmoothBeta) * y) .ToArray(); return _smoothedActions; } public override void Initialize() { Assert.IsTrue(_hasAwake); Assert.IsFalse(_hasLazyInitialized); _hasLazyInitialized = true; _decisionRequester = GetComponent(); _debugController = FindObjectOfType(); Time.fixedDeltaTime = FixedDeltaTime; _spawnableEnv = GetComponentInParent(); if (_debugController != null) { dontResetOnZeroReward = true; dontSnapMocapToRagdoll = true; UsePDControl = false; } _mocapControllerArtanim = _spawnableEnv.GetComponentInChildren(); _mocapBodyParts = _mocapControllerArtanim.GetRigidBodies(); _bodyParts = GetComponentsInChildren().ToList(); _dReConObservations = GetComponent(); _dReConRewards = GetComponent(); _trackBodyStatesInWorldSpace = _mocapControllerArtanim.GetComponent(); _ragDollSettings = GetComponent(); _inputController = _spawnableEnv.GetComponentInChildren(); _sensorObservations = GetComponent(); foreach (var body in GetComponentsInChildren()) { body.solverIterations = 255; body.solverVelocityIterations = 255; } _motors = GetComponentsInChildren() .Where(x=>x.jointType == ArticulationJointType.SphericalJoint) .Where(x=>!x.isRoot) .Distinct() .ToList(); var individualMotors = new List(); foreach (var m in _motors) { if (m.twistLock == ArticulationDofLock.LimitedMotion) individualMotors.Add(0f); if (m.swingYLock == ArticulationDofLock.LimitedMotion) individualMotors.Add(0f); if (m.swingZLock == ArticulationDofLock.LimitedMotion) individualMotors.Add(0f); } _dReConObservations.PreviousActions = individualMotors.ToArray(); //_mocapAnimatorController = _mocapControllerArtanim.GetComponentInChildren(); _mocapAnimatorController = _mocapControllerArtanim.GetComponent(); _mocapControllerArtanim.OnAgentInitialize(); _dReConObservations.OnAgentInitialize(); _dReConRewards.OnAgentInitialize(); _trackBodyStatesInWorldSpace.OnAgentInitialize(); _mocapAnimatorController.OnAgentInitialize(); _inputController.OnReset(); _hasLazyInitialized = true; } public override void OnEpisodeBegin() { Assert.IsTrue(_hasAwake); _smoothedActions = null; debugCopyMocap = false; _mocapAnimatorController.OnReset(); var angle = Vector3.SignedAngle(Vector3.forward, _inputController.HorizontalDirection, Vector3.up); var rotation = Quaternion.Euler(0f, angle, 0f); _mocapControllerArtanim.OnReset(rotation); _mocapControllerArtanim.CopyStatesTo(this.gameObject); // _trackBodyStatesInWorldSpace.CopyStatesTo(this.gameObject); float timeDelta = float.MinValue; _dReConObservations.OnReset(); _dReConRewards.OnReset(); _dReConObservations.OnStep(timeDelta); _dReConRewards.OnStep(timeDelta); #if UNITY_EDITOR if (DebugPauseOnReset) { UnityEditor.EditorApplication.isPaused = true; } #endif if (_debugController != null && _debugController.isActiveAndEnabled) { _debugController.OnAgentEpisodeBegin(); } } float[] GetMocapTargets() { if (_mocapTargets == null) { _mocapTargets = _motors .Where(x=>!x.isRoot) .SelectMany(x => { List list = new List(); if (x.twistLock == ArticulationDofLock.LimitedMotion) list.Add(0f); if (x.swingYLock == ArticulationDofLock.LimitedMotion) list.Add(0f); if (x.swingZLock == ArticulationDofLock.LimitedMotion) list.Add(0f); return list.ToArray(); }) .ToArray(); } int i=0; foreach (var joint in _motors) { if (joint.isRoot) continue; Rigidbody mocapBody = _mocapBodyParts.First(x=>x.name == joint.name); Vector3 targetRotationInJointSpace = -(Quaternion.Inverse(joint.anchorRotation) * Quaternion.Inverse(mocapBody.transform.localRotation) * joint.parentAnchorRotation).eulerAngles; targetRotationInJointSpace = new Vector3( Mathf.DeltaAngle(0, targetRotationInJointSpace.x), Mathf.DeltaAngle(0, targetRotationInJointSpace.y), Mathf.DeltaAngle(0, targetRotationInJointSpace.z)); if (joint.twistLock == ArticulationDofLock.LimitedMotion) { var drive = joint.xDrive; var scale = (drive.upperLimit-drive.lowerLimit) / 2f; var midpoint = drive.lowerLimit + scale; var target = (targetRotationInJointSpace.x -midpoint) / scale; _mocapTargets[i] = target; i++; } if (joint.swingYLock == ArticulationDofLock.LimitedMotion) { var drive = joint.yDrive; var scale = (drive.upperLimit-drive.lowerLimit) / 2f; var midpoint = drive.lowerLimit + scale; var target = (targetRotationInJointSpace.y -midpoint) / scale; _mocapTargets[i] = target; i++; } if (joint.swingZLock == ArticulationDofLock.LimitedMotion) { var drive = joint.zDrive; var scale = (drive.upperLimit-drive.lowerLimit) / 2f; var midpoint = drive.lowerLimit + scale; var target = (targetRotationInJointSpace.z -midpoint) / scale; _mocapTargets[i] = target; i++; } } return _mocapTargets; } void UpdateMotor(ArticulationBody joint, Vector3 targetNormalizedRotation) { //Vector3 power = _ragDollSettings.MusclePowers.First(x=>x.Muscle == joint.name).PowerVector; Vector3 power = Vector3.zero; try { power = _ragDollSettings.MusclePowers.First(x => x.Muscle == joint.name).PowerVector; } catch { Debug.Log("there is no muscle for joint " + joint.name); } power *= _ragDollSettings.Stiffness; float damping = _ragDollSettings.Damping; float forceLimit = _ragDollSettings.ForceLimit; if (joint.twistLock == ArticulationDofLock.LimitedMotion) { var drive = joint.xDrive; var scale = (drive.upperLimit-drive.lowerLimit) / 2f; var midpoint = drive.lowerLimit + scale; var target = midpoint + (targetNormalizedRotation.x *scale); drive.target = target; drive.stiffness = power.x; drive.damping = damping; drive.forceLimit = forceLimit; joint.xDrive = drive; } if (joint.swingYLock == ArticulationDofLock.LimitedMotion) { var drive = joint.yDrive; var scale = (drive.upperLimit-drive.lowerLimit) / 2f; var midpoint = drive.lowerLimit + scale; var target = midpoint + (targetNormalizedRotation.y *scale); drive.target = target; drive.stiffness = power.y; drive.damping = damping; drive.forceLimit = forceLimit; joint.yDrive = drive; } if (joint.swingZLock == ArticulationDofLock.LimitedMotion) { var drive = joint.zDrive; var scale = (drive.upperLimit-drive.lowerLimit) / 2f; var midpoint = drive.lowerLimit + scale; var target = midpoint + (targetNormalizedRotation.z *scale); drive.target = target; drive.stiffness = power.z; drive.damping = damping; drive.forceLimit = forceLimit; joint.zDrive = drive; } } void FixedUpdate() { if (debugCopyMocap) { EndEpisode(); } } void OnDrawGizmos() { if (_dReConRewards == null) return; var comTransform = _dReConRewards._ragDollBodyStats.transform; var vector = new Vector3( _inputController.MovementVector.x, 0f, _inputController.MovementVector.y); var pos = new Vector3(comTransform.position.x, 0.001f, comTransform.position.z); DrawArrow(pos, vector, Color.black); } void DrawArrow(Vector3 start, Vector3 vector, Color color) { float headSize = 0.25f; float headAngle = 20.0f; Gizmos.color = color; Gizmos.DrawRay(start, vector); if (vector != Vector3.zero) { Vector3 right = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180+headAngle,0) * new Vector3(0,0,1); Vector3 left = Quaternion.LookRotation(vector) * Quaternion.Euler(0,180-headAngle,0) * new Vector3(0,0,1); Gizmos.DrawRay(start + vector, right * headSize); Gizmos.DrawRay(start + vector, left * headSize); } } }