beep-final/Assets/7_Scripts/Agent/Walker.cs

using UnityEngine;
using Unity.MLAgents.Actuators;
using Unity.MLAgentsExamples;
using Unity.MLAgents.Sensors;
using Unity.MLAgents;
using BodyPart = Unity.MLAgentsExamples.BodyPart;
using Random = UnityEngine.Random;
using static UnityEngine.GraphicsBuffer;
using UnityEngine.UI;
using Unity.VisualScripting;
using System.Collections.Generic;
using System.Xml.Linq;
//using System.Diagnostics;

public class Walker : Agent
{



    public enum Brain
    {
        Walker,
        Getup,
        Climber,
        Balance,
        DMScrambler,
        Sitting,
        Treadmill
    }

    public Brain m_SelectedBrain;

    [Header("Walk Speed")]
    [Range(0.1f, 30)]
    [SerializeField]
    private float m_TargetWalkingSpeed = 30;

    public float MTargetWalkingSpeed
    {
        get { return m_TargetWalkingSpeed; }
        set { m_TargetWalkingSpeed = Mathf.Clamp(value, .1f, m_MaxWalkingSpeed); }
    }

    // The Max Walking Speed
    const float m_MaxWalkingSpeed = 30;

    [Header("Randomise")]
    // Randomise Walking Speed every Episode
    public bool rWalkSpeedEachEpisode;
    public bool m_RandomiseYRotation = true;
    public bool m_RandomiseXYZRotation = false;

    [Header("Swap Model Settings")]
    public bool m_ModelSwap = false;
    public bool m_ProximitySwapper = false;
    public bool m_SwitchModelAfterFalling = false;

    [Header("DM Monitor")]
    public int m_StepCountAtLastMeter = 0;
    public int m_LastXPosition = 0;
    private Terrain m_Terrain;

    [HideInInspector]
    public bool m_FinishedSwapGetup = false;
    public bool m_FinishedSwapSit = false;

    [HideInInspector]
    public ModelSwap m_ModelSwapper;

    // The direction the Agent will walk towards during training
    private Vector3 m_WorldDirectionToWalk = Vector3.right;

    // Specifies the Target the Agent will Walk towards
    [Header("Target to Walk Towards")]
    public Transform target;

    /* Orientation Cube Controller - a reference point for observations, as
     Ragdolls can be very erratic in their movements */
    OrientationCubeController m_OrientationCube;

    // Joint Drive Controller - Controls the params of Joints
    JointDriveController m_JointDriveController;

    // Specifies all the Body Parts (Configurable Joints)
    [Header("Body Parts")]
    public Transform rest;
    public Transform seat;

    public Transform FLT;
    public Transform FLL;
    public Transform FLF;

    public Transform FRT;
    public Transform FRL;
    public Transform FRF;

    public Transform BRT;
    public Transform BRL;
    public Transform BRF;

    public Transform BLT;
    public Transform BLL;
    public Transform BLF;

    // Setup
    public override void Initialize()
    {
        m_OrientationCube = GetComponentInChildren<OrientationCubeController>();
        m_JointDriveController = GetComponent<JointDriveController>();
        m_ModelSwapper = GetComponent<ModelSwap>();

        var parent = gameObject.transform.parent;

        try
        {
            m_Terrain = parent.GetComponentInChildren<Terrain>();
        }
        catch
        {
            m_Terrain = null;
        }

        // Setup each Body Part
        m_JointDriveController.SetupBodyPart(seat);
        m_JointDriveController.SetupBodyPart(rest);

        m_JointDriveController.SetupBodyPart(FRT);
        m_JointDriveController.SetupBodyPart(FRL);
        m_JointDriveController.SetupBodyPart(FRF);

        m_JointDriveController.SetupBodyPart(FLT);
        m_JointDriveController.SetupBodyPart(FLL);
        m_JointDriveController.SetupBodyPart(FLF);

        m_JointDriveController.SetupBodyPart(BRT);
        m_JointDriveController.SetupBodyPart(BRL);
        m_JointDriveController.SetupBodyPart(BRF);

        m_JointDriveController.SetupBodyPart(BLT);
        m_JointDriveController.SetupBodyPart(BLL);
        m_JointDriveController.SetupBodyPart(BLF);
    }

    /* Loop over Body Parts & Reset them to inital conditions
     */
    public override void OnEpisodeBegin()
    {
        // Set values of Joint Controller for exploring emrgent behvaiours
        if (m_SelectedBrain == Brain.DMScrambler)
        {
            //this.GetComponent<DMTerrain>().Reset();
        }

        foreach (var bodyPart in m_JointDriveController.bodyPartsDict.Values)
        {
            bodyPart.Reset(bodyPart);
        }

        // Apply Random Rotation to Seat
        if (m_SelectedBrain == Brain.Getup) { if (m_RandomiseXYZRotation) seat.rotation = Quaternion.Euler(Random.Range(0.0f, 360f), Random.Range(0.0f, 360f), Random.Range(0.0f, 360f)); }
        else { if (m_RandomiseYRotation) seat.rotation = Quaternion.Euler(0f, Random.Range(0.0f, 360f), 0f); }

        UpdateOrientationObject();

        // Set our Walking Speed goal
        MTargetWalkingSpeed =
            rWalkSpeedEachEpisode ? Random.Range(0.1f, m_MaxWalkingSpeed) : MTargetWalkingSpeed;

        // Check if model swapper is enabled, and set model if so
        if (m_ModelSwap)
        {
            m_ModelSwapper.SetInitialModel();
        }

        m_LastXPosition = (int)GetAverageXPositionFeet();

    }

    /* Add relevant information for each body part (observations)
     */
    public void CollectObservationsBP(BodyPart bp, VectorSensor sensor)
    {
        // Check if touching Ground
        sensor.AddObservation(bp.groundContact.touchingGround);
        sensor.AddObservation(bp.groundContact.touchingStairs);
        sensor.AddObservation(bp.groundContact.touchingObject);

        // Get Velocities in Context of the Orientation Cube Space
        sensor.AddObservation(m_OrientationCube.transform.InverseTransformDirection(bp.rb.velocity));
        sensor.AddObservation(m_OrientationCube.transform.InverseTransformDirection(bp.rb.angularVelocity));

        // Gets Position relative to the Seat in Context of our Orientation Cube
        sensor.AddObservation(m_OrientationCube.transform.InverseTransformDirection(bp.rb.position - seat.position));

        if (bp.rb.transform != seat)
        {
            sensor.AddObservation(bp.rb.transform.localRotation);
            sensor.AddObservation(bp.currentStrength / m_JointDriveController.maxJointForceLimit);
        }
    }

    /* Main method to Collect Observations about environemt
     */
    public override void CollectObservations(VectorSensor sensor)
    {
        // Direction Agent is Facing
        var cubeForward = m_OrientationCube.transform.forward;

        // Velocity we want to Match
        var velocityGoal = cubeForward * MTargetWalkingSpeed;

        // Ragdolls AVG Velocity
        var avgVelocity = GetAvgVelocity();

        // Current Ragdoll Vecloty, Normalised (How far away is the goal vs. current vel)
        sensor.AddObservation(Vector3.Distance(velocityGoal, avgVelocity));

        // Avgerage Body Velocity Relative to Orientation Cube
        sensor.AddObservation(m_OrientationCube.transform.InverseTransformDirection(avgVelocity));

        // Velocity Goal Relative to the Cube
        sensor.AddObservation(m_OrientationCube.transform.InverseTransformDirection(velocityGoal));

        // Rotation Delta
        sensor.AddObservation(Quaternion.FromToRotation(seat.forward, cubeForward));

        // Position of target relative to Cube
        sensor.AddObservation(m_OrientationCube.transform.InverseTransformDirection(target.transform.position));

        // Distance of the target relative to the Cube
        //sensor.AddObservation(Vector3.Distance(m_OrientationCube.transform.position, target.transform.position));

        foreach (var bodyPart in m_JointDriveController.bodyPartsList)
        {
            CollectObservationsBP(bodyPart, sensor);
        }
    }

    public override void Heuristic(in ActionBuffers actionsOut)
    {
        ActionSegment<float> continuousActionsOut = actionsOut.ContinuousActions;
    }

    public override void OnActionReceived(ActionBuffers actions)
    {
        var bpDict = m_JointDriveController.bodyPartsDict;
        var element = -1;

        var continuousActions = actions.ContinuousActions;

        bpDict[rest].SetJointTargetRotation(continuousActions[++element], 0, 0);

        bpDict[FRT].SetJointTargetRotation(continuousActions[++element], 0, 0);
        bpDict[FRL].SetJointTargetRotation(continuousActions[++element], continuousActions[++element], 0);
        bpDict[FLT].SetJointTargetRotation(continuousActions[++element], 0, 0);
        bpDict[FLL].SetJointTargetRotation(continuousActions[++element], continuousActions[++element], 0);
        bpDict[BRT].SetJointTargetRotation(continuousActions[++element], 0, 0);
        bpDict[BRL].SetJointTargetRotation(continuousActions[++element], continuousActions[++element], 0);
        bpDict[BLT].SetJointTargetRotation(continuousActions[++element], 0, 0);
        bpDict[BLL].SetJointTargetRotation(continuousActions[++element], continuousActions[++element], 0);
        bpDict[BRF].SetJointTargetRotation(continuousActions[++element], 0, 0);
        bpDict[BLF].SetJointTargetRotation(continuousActions[++element], 0, 0);
        bpDict[FRF].SetJointTargetRotation(continuousActions[++element], 0, 0);
        bpDict[FLF].SetJointTargetRotation(continuousActions[++element], 0, 0);

        bpDict[rest].SetJointStrength(continuousActions[++element]);
        bpDict[FRT].SetJointStrength(continuousActions[++element]);
        bpDict[FRL].SetJointStrength(continuousActions[++element]);
        bpDict[FLT].SetJointStrength(continuousActions[++element]);
        bpDict[FLL].SetJointStrength(continuousActions[++element]);
        bpDict[BRT].SetJointStrength(continuousActions[++element]);
        bpDict[BRL].SetJointStrength(continuousActions[++element]);
        bpDict[BLT].SetJointStrength(continuousActions[++element]);
        bpDict[BLL].SetJointStrength(continuousActions[++element]);
        bpDict[BLF].SetJointStrength(continuousActions[++element]);
        bpDict[BRF].SetJointStrength(continuousActions[++element]);
        bpDict[FLF].SetJointStrength(continuousActions[++element]);
        bpDict[FRF].SetJointStrength(continuousActions[++element]);
    }

    void UpdateOrientationObject()
    {
        m_WorldDirectionToWalk = target.position - seat.position;
        m_OrientationCube.UpdateOrientation(seat, target);
    }

    void FixedUpdate()
    {
        // Check if model swapper is on
        if (m_ModelSwap)
        {
            //SwitchModelAfterFalling();
        }

        UpdateOrientationObject();

        var cubeForward = m_OrientationCube.transform.forward;

        var matchSpeedReward = GetMatchingVelocityReward(cubeForward * MTargetWalkingSpeed, GetAvgVelocity());

        var lookAtTargetReward = (Vector3.Dot(cubeForward, seat.forward) + 1) * .5f;

        // (3) Distance from ground
        // var distFromGround = Mathf.Pow(Mathf.Clamp(Vector3.Distance(seat.transform.position, GameObject.Find("Ground").transform.position) + 0.2f, 0, 1), 2);

        if (m_SelectedBrain == Brain.Getup)
        {

            Vector2 deltaAngle = GetAngleDeltaXZ();
            //Debug.Log($"{Mathf.Pow((deltaAngle.x * deltaAngle.y), 2)}");
            AddReward(Mathf.Pow((deltaAngle.x * deltaAngle.y), 2) * 0.02f);

        }
        else if (m_SelectedBrain == Brain.Balance)
        {
            // Sets reward for the agent based on its stableness, while moving towards a target
            Vector2 deltaAngle = GetAngleDeltaXZ();
            AddReward(matchSpeedReward * lookAtTargetReward * (deltaAngle.x * deltaAngle.y));

        }
        else if (m_SelectedBrain == Brain.DMScrambler)
        {
            // Normalised Velocity in a certain direciton.
            //var seatVelocity = GetAvgVelocitySeat();
            //float normalisedVelcoity = Mathf.Clamp(GetNormalizedVelocity(seatVelocity).z, 0f, 1f);
            //AddReward(normalisedVelcoity);
            //AddReward(matchSpeedReward * lookAtTargetReward);
            // If it hasnt moved forward in a certain amount of steps, end the episode.
            // (1) Get X Position of the Foot, or Avgerge of all Feet

            AddReward(matchSpeedReward * lookAtTargetReward * DistanceFromTarget(20f));

            float feetXPosition = GetAverageXPositionFeet();
            int newXPosition = (int)feetXPosition;



            // (2) Compare step count with highest X position
            if (newXPosition > m_LastXPosition)
            {
                m_LastXPosition = newXPosition;
                m_StepCountAtLastMeter = this.StepCount;
            }

            // If the agent goes 1000 steps w/o making any progress, we will end the episode.
            if (this.StepCount - m_StepCountAtLastMeter >= (1000))
            {
                SetReward(-1f);
                //EndEpisode();
            }

            //AddReward(-0.002f);

            //Debug.Log(DistanceFromTarget(20f));
        }
        else if (m_SelectedBrain == Brain.Walker)
        {
            AddReward(matchSpeedReward * lookAtTargetReward);
        }
        else if (m_SelectedBrain == Brain.Climber)
        {
            //AddReward(matchSpeedReward * lookAtTargetReward * DistanceFromTarget(20f));
            AddReward(-0.002f);
        }
        else if (m_SelectedBrain == Brain.Sitting)
        {
            var height = CheckHeightRaycast();
            var clampedHeight = Mathf.Pow((1 - height), 2);
            //Debug.Log($"Clamped Height: {Mathf.Pow(clampedHeight,2)}");
            AddReward(clampedHeight);
        }
        else if (m_SelectedBrain == Brain.Treadmill)
        {
            //Debug.Log(DistanceFromTarget(5f));
            AddReward(matchSpeedReward * lookAtTargetReward * DistanceFromTarget(5f));
        }
    }

    // Use regular update to listen for keypresses
    void Update()
    {
        if (m_ModelSwap)
        {
            InputSwitchModel();
        }

    }

    // Switch the model given the current input from the user
    // G -> Getup
    // S -> Sit
    // This is a simple toggle
    private void InputSwitchModel()
    {

        if (Input.GetKeyDown(KeyCode.G) && !m_ModelSwapper.m_currentModelName.Equals("Sitting"))
        {
            if( !m_FinishedSwapGetup)
            {
                m_FinishedSwapGetup = true;
                m_ModelSwapper.m_PastModel = m_ModelSwapper.m_currentModelName;
                m_ModelSwapper.SwitchModel("Getup", this);
            }
            else
            {
                m_FinishedSwapGetup = false;
                m_ModelSwapper.SwitchModel(m_ModelSwapper.m_PastModel, this);
            }
        }

        if ( Input.GetKeyDown(KeyCode.S) && !m_ModelSwapper.m_currentModelName.Equals("Getup"))
        {
            if (!m_FinishedSwapSit)
            {
                m_FinishedSwapSit = true;
                m_ModelSwapper.m_PastModel = m_ModelSwapper.m_currentModelName;
                m_ModelSwapper.SwitchModel("Sitting", this);
            }
            else 
            { 
                m_FinishedSwapSit = false;
                m_ModelSwapper.SwitchModel(m_ModelSwapper.m_PastModel, this);
            }            
        }

    }

    private void SwitchModelAfterFalling()
    {
        if (m_SwitchModelAfterFalling)
        {
            // If on, check its rotation, and if certain parts are touching the floor
            Vector2 deltaAngle = GetAngleDeltaXZ();

            if ((deltaAngle.y < 0.3 || deltaAngle.x < 0.3) && !m_FinishedSwapGetup)
            {
                // Swap Model
                m_FinishedSwapGetup = true;
                m_ModelSwapper.m_PastModel = m_ModelSwapper.m_currentModelName;
                m_ModelSwapper.SwitchModel("Getup", this);
            }
            else if (deltaAngle.y > 0.8 && deltaAngle.x > 0.8 && m_FinishedSwapGetup)
            {
                // Swap to Original Model
                m_FinishedSwapGetup = false;
                m_ModelSwapper.SwitchModel(m_ModelSwapper.m_PastModel, this);
            }

        }
    }
    float DistanceFromTarget(float maxDistance)
    {
        float dist = Vector3.Distance(seat.transform.position, target.transform.position);
        float normalisedValue = 1 - Mathf.InverseLerp(0f, maxDistance, dist);
        return Mathf.Pow(normalisedValue, 2);
    }

    Vector3 GetAvgVelocity()
    {
        Vector3 velSum = Vector3.zero;

        int rbCount = 0;
        foreach (var item in m_JointDriveController.bodyPartsList)
        {
            rbCount++;
            velSum += item.rb.velocity;
        }

        var avgVelocity = velSum / rbCount;
        return avgVelocity;
    }

    Vector3 GetAvgVelocitySeat()
    {
        Vector3 velSum = Vector3.zero;


        foreach (var item in m_JointDriveController.bodyPartsList)
        {
            if (item.rb.transform == seat)
            {
                velSum = item.rb.velocity;
                break;
            }
        }

        return velSum;
    }

    public float GetMatchingVelocityReward(Vector3 velGoal, Vector3 currentVel)
    {
        var velDeltaMag = Mathf.Clamp(Vector3.Distance(currentVel, velGoal), 0, MTargetWalkingSpeed);
        return Mathf.Pow(1 - Mathf.Pow(velDeltaMag / MTargetWalkingSpeed, 2), 2);
    }

    float DeltaAngle(float angle)
    {
        var currentZRot = angle;
        float zRotDist = Mathf.Abs(Mathf.DeltaAngle(0f, currentZRot));
        float normalizedDistance = 1f - Mathf.InverseLerp(0f, 180f, zRotDist);
        float expZDist = Mathf.Pow(normalizedDistance, 2);
        return expZDist;
    }

    Vector2 GetAngleDeltaXZ()
    {
        return new Vector2(DeltaAngle(seat.eulerAngles.x), DeltaAngle(seat.eulerAngles.z));
    }

    float CheckHeightRaycast()
    {
        RaycastHit hit;

        if (Physics.Raycast(seat.transform.position, transform.TransformDirection(Vector3.down), out hit, Mathf.Infinity))
        {
            Debug.DrawRay(transform.position, transform.TransformDirection(Vector3.forward) * hit.distance, Color.red);
            return hit.distance / 2;
        }
        else
        {
            Debug.DrawRay(transform.position, transform.TransformDirection(Vector3.forward), Color.blue);
            return 1;
        }
    }

    float GetAverageXPositionFeet()
    {
        float average = (FLF.position.z + FRF.position.z + BLF.position.z + BRF.position.z) / 4.0f;
        return average;
    }

    Vector3 GetNormalizedVelocity(Vector3 metersPerSecond)
    {
        var maxMetersPerSecond = m_Terrain.terrainData.bounds.size
            / this.MaxStep
            / Time.fixedDeltaTime;

        var maxXZ = Mathf.Max(maxMetersPerSecond.x, maxMetersPerSecond.z);
        maxMetersPerSecond.x = maxXZ;
        maxMetersPerSecond.z = maxXZ;
        maxMetersPerSecond.y = 53; // override with
        float x = metersPerSecond.x / maxMetersPerSecond.x;
        float y = metersPerSecond.y / maxMetersPerSecond.y;
        float z = metersPerSecond.z / maxMetersPerSecond.z;
        // clamp result
        x = Mathf.Clamp(x, -1f, 1f);
        y = Mathf.Clamp(y, -1f, 1f);
        z = Mathf.Clamp(z, -1f, 1f);
        Vector3 normalizedVelocity = new Vector3(x, y, z);
        return normalizedVelocity;
    }
}