Function pd_control

MujocoDeploy/mujoco_deploy_g1.py:27–29

Calculates torques from position commands

(target_q, q, kp, target_dq, dq, kd)

    return config

def pd_control(target_q, q, kp, target_dq, dq, kd):
    """Calculates torques from position commands"""
    return (target_q - q) * kp + (target_dq - dq) * kd

def quat_rotate_inverse(q, v):
    """Rotate vector v by the inverse of quaternion q"""