Unity PID controller for position and rotation

PIDController.cs

using System.Collections;
using System.Collections.Generic;
// using Sirenix.OdinInspector;
using UnityEngine;
using UnityEngine.Serialization;

namespace BotaLab
{
    /**
    * @brief A Unity Component to control position and rotation with PID controller
    * An Object with this approaches target position and rotation according to PID Control Rule.
    * https://en.wikipedia.org/wiki/PID_controller
    **/
    [RequireComponent(typeof (Rigidbody))]
    public class PIDController : MonoBehaviour
    {
        // Transform of a target
        [SerializeField]
        protected Transform m_target = null;

        // class for pid gains
        [System.Serializable]
        public class Gain
        {
            public float p = 1.0f;

            public float i = 0.0f;

            public float d = 1.0f;
        }

        // [BoxGroup("PositionGain")]
        [SerializeField]
        // [SerializeField, InlineProperty, HideLabel]
        protected Gain m_posGain = new Gain();

        // [BoxGroup("RotationGain")]
        [SerializeField]
        // [SerializeField, InlineProperty, HideLabel]
        protected Gain m_rotGain = new Gain();

        [SerializeField]
        protected bool m_gravityCompensation = true;

        // rigidbody of an attached gameobject
        private Rigidbody rb_;

        // rigidbody of an attached gameobject
        private Transform tf_;

        // Position error
        private Vector3 error_ = Vector3.zero;

        private Vector3 prevError_ = Vector3.zero;

        private Vector3 diffError_ = Vector3.zero;

        private Vector3 intError_ = Vector3.zero;

        // Rotation error
        private Quaternion rotError_ = Quaternion.identity;

        private Quaternion prevRotError_ = Quaternion.identity;

        private Quaternion diffRotError_ = Quaternion.identity;

        private Quaternion intRotError_ = Quaternion.identity;

        private float angleError_ = 0f;

        private Vector3 errorAxis_;

        private Vector3 diffErrorAxis_;

        private Vector3 intErrorAxis_;

        // private float prevAngleError_ = 0f;
        private float diffAngleError_ = 0f;

        private float intAngleError_ = 0f;

        private Vector3 force_ = Vector3.zero;

        /**
        * @brief target transform
        * 
        */
        public Transform target
        {
            get
            {
                return m_target;
            }
        }

        /**
        * @brief Position gains
        * 
        */
        public Gain positionGains
        {
            get
            {
                return m_posGain;
            }
        }

        /**
        * @brief Rotation gains
        * 
        */
        public Gain rotationGains
        {
            get
            {
                return m_rotGain;
            }
        }

        /**
        * @brief rotation axis
        * 
        */
        Vector3 RotAxis(Quaternion q)
        {
            var n = new Vector3(q.x, q.y, q.z);
            return n.normalized;
        }

        /**
        *
        */
        protected Quaternion RotOptimize(Quaternion q)
        {
            if (q.w < 0.0f)
            {
                q.x *= -1.0f;
                q.y *= -1.0f;
                q.z *= -1.0f;
                q.w *= -1.0f;
            }

            return q;
        }

        /**
        * @brief Position error
        * 
        **/
        public Vector3 posError
        {
            get
            {
                return error_;
            }
        }

        /**
        * @brief Set a target object
        * 
        * @param target target transform
        */
        public void SetTarget(Transform target)
        {
            m_target = target;
        }

        /**
        * @brief Set pid gains for position control
        * 
        * @param p proportional gain
        * @param i integral gain
        * @param d differential gain
        */
        public void SetPositionGains(float p, float i, float d)
        {
            m_posGain.p = p;
            m_posGain.i = i;
            m_posGain.d = d;
        }

        /**
        * @brief Set pid gains for rotation control
        * 
        * @param p proportional gain
        * @param i integral gain
        * @param d differential gain
        */
        public void SetRotationGains(float p, float i, float d)
        {
            m_rotGain.p = p;
            m_rotGain.i = i;
            m_rotGain.d = d;
        }

        void Awake()
        {
            rb_ = GetComponent<Rigidbody>();
            tf_ = transform;
        }

        void FixedUpdate()
        {
            if (m_target == null)
            {
                return;
            }

            /// position ///
            error_ = m_target.position - tf_.position;
            intError_ += error_ * Time.deltaTime; // integral
            diffError_ = (error_ - prevError_) / Time.deltaTime;
            prevError_ = error_;

            force_ =
                m_posGain.p * error_ +
                m_posGain.i * intError_ +
                m_posGain.d * diffError_;

            if (m_gravityCompensation && rb_.useGravity)
            {
                force_ += -rb_.mass * Physics.gravity;
            }

            /// rotation ///
            rotError_ =
                RotOptimize(m_target.rotation *
                Quaternion.Inverse(tf_.rotation));

            // diffRotError_ = rotError_ * Quaternion.Inverse(prevRotError_);
            // intRotError_ = intRotError_ * rotError_;
            rotError_.ToAngleAxis(out angleError_, out errorAxis_);

            // diffRotError_.ToAngleAxis(out diffAngleError_, out diffErrorAxis_);
            // intRotError_.ToAngleAxis(out intAngleError_, out intErrorAxis_);
            // var trq = errorAxis_ * (m_rotGain.p*angleError_)
            //   +diffErrorAxis_*(m_rotGain.i*diffAngleError_)
            //   +intErrorAxis_*(m_rotGain.d*intAngleError_);
            angleError_ *= Mathf.Deg2Rad; // deg to rad
            var angVel_ = m_rotGain.p * angleError_ * errorAxis_;

            if (error_.sqrMagnitude > 0.1f)
            {
                rb_.AddForce (force_);
            }
            else if (m_gravityCompensation && rb_.useGravity)
            {
                rb_.AddForce(-rb_.mass * Physics.gravity);
            }

            if (angleError_ * angleError_ > 0.01f)
            {
                rb_.angularVelocity = angVel_;
            }
            prevRotError_ = rotError_;
        }

        // [Button]
        public void TeleportToTarget()
        {
            if (!tf_)
            {
                tf_ = transform;
            }
            if (m_target)
            {
                tf_
                    .SetPositionAndRotation(m_target.position,
                    m_target.rotation);
            }
        }
    }
}

Shouldn't

// var trq = errorAxis_ * (m_rotGain.p * angleError_)
// + diffErrorAxis_ * (m_rotGain.i * diffAngleError_)
// + intErrorAxis_ * (m_rotGain.d * intAngleError_);

be

// var trq = errorAxis_ * (m_rotGain.p*angleError_)
// + diffErrorAxis_ * (m_rotGain.d * diffAngleError_)
// + intErrorAxis_ * (m_rotGain.i * intAngleError_);