Fix projected gravity calculation in legged_robot.py

[curieuxjy]

(Update legged_robot.py)

The misfunction for calculating projected gravity should be resolved by changing quat_rotate_inverse to the quat_rotate function for calculating the projected_gravity vector.

[SUZ-tsinghua]

No. It is quat_rotate_inverse.

[curieuxjy]

No. It is quat_rotate_inverse.

@SUZ-tsinghua, I wanted to bring your attention to a specific line of code in the NVIDIA-Omniverse IsaacGymEnvs repository. Have you had a chance to review it?

Here is the link for reference: https://github.com/NVIDIA-Omniverse/IsaacGymEnvs/blob/aeed298638a1f7b5421b38f5f3cc2d1079b6d9c3/isaacgymenvs/tasks/anymal.py#L372.

In this updated version of the repository, you'll notice that the function quat_rotate_inverse has been changed to quat_rotate. Additionally, if you implement a visualization for the projected gravity vector and observe the rendering window, the error becomes evident. This observation suggests that the initial implementation using quat_rotate_inverse was wrong.

[SUZ-tsinghua]

No. It is quat_rotate_inverse.

@SUZ-tsinghua, I wanted to bring your attention to a specific line of code in the NVIDIA-Omniverse IsaacGymEnvs repository. Have you had a chance to review it?

Here is the link for reference: https://github.com/NVIDIA-Omniverse/IsaacGymEnvs/blob/aeed298638a1f7b5421b38f5f3cc2d1079b6d9c3/isaacgymenvs/tasks/anymal.py#L372.

In this updated version of the repository, you'll notice that the function quat_rotate_inverse has been changed to ``quat_rotate. Additionally, if you implement a visualization for the projected gravity vector and observe the rendering window, the error becomes evident. This observation suggests that the initial implementation using quat_rotate_inverse` was wrong.

@curieuxjy But you can directly verify the correctness of the code by running
`from isaacgym.torch_utils import *

base_quat = torch.tensor([[0.0, 0.7071, 0.0, 0.7071]])
gravity_vec = torch.tensor([[0.0, 0.0, -1.]])

projected_gravity_inverse = quat_rotate_inverse(base_quat, gravity_vec)
projected_gravity = quat_rotate(base_quat, gravity_vec)

print("projected gravity inverse",projected_gravity_inverse)
print("projected gravity", projected_gravity)``

It rotates the base frame so the x axis of the base frame is the -z axis of the env frame. And projected_gravity_inverse is [1,0,0] but projected_gravity is [-1,0,0].

[luoye2333]

@SUZ-tsinghua @curieuxjy
quat_rotate obtains the coordinates of the robot's normal vector (the vector [0,0,-1] in the robot's coordinate system, pointing downward from the body and moving with the robot) in the global coordinate system.
quat_rotate_inverse obtains the coordinates of the global gravity direction vector [0,0,-1] in the robot's local coordinate system.

The x and y components of the former are related to the yaw rotation, while the x and y components of the latter are unrelated to yaw. Additionally, the z components of both are actually equal.

It should be more reasonable to exclude the yaw component because locomotion is essentially unrelated to yaw.

In the latest IsaacLab implementation, quat_rotate_inverse is used. As for why IsaacGymEnvs can also use quat_rotate—adding yaw merely introduces irrelevant information, which may make training slightly more difficult, but it still works.

quat_rotate得到的是全局坐标系下 机器人法线(机器人坐标系中的[0,0,-1]指向身体下方的向量, 跟随机器人运动)的坐标
quat_rotate_inverse得到的是全局重力方向向量[0,0,-1]在机器人局部坐标系下的坐标
前一个x,y分量和yaw旋转有关 后一个x,y分量和yaw没关系 另外这两个的z分量其实是相等的
应该是不要yaw分量更合理一些 因为locomotion和yaw其实没啥关系
现在最新的IsaacLab实现里面用的是quat_rotate_inverse 至于为什么IsaacGymEnvs用quat_rotate也行 加了yaw以后只是多了无关的信息 可能会训练困难一点 但是也能work

[luoye2333]

@SUZ-tsinghua @curieuxjy use this to test

import isaacgym
from isaacgym.torch_utils import *

yaw=torch.tensor([0,35.]).deg2rad()
roll=torch.tensor([10,10.]).deg2rad()
pitch=torch.tensor([15,15.]).deg2rad()
zero=torch.zeros(2)
q1=quat_from_euler_xyz(zero,zero,yaw)
q2=quat_from_euler_xyz(roll,pitch,zero)
q=quat_mul(q1,q2)
g = torch.tensor([0,0,-1.]).unsqueeze(0).repeat(2,1)
print(quat_rotate(q,g))
print(quat_rotate_inverse(q,g))

output

tensor([[-0.2549,  0.1736, -0.9513],
        [-0.3084, -0.0040, -0.9513]])
tensor([[ 0.2588, -0.1677, -0.9513],
        [ 0.2588, -0.1677, -0.9513]])

[SUZ-tsinghua]

@luoye2333 This vector will be input into the observation vector. Guess how you get this vector during deployment?

[luoye2333]

@SUZ-tsinghua Do you mean yaw? Yaw is available in most imu. though this may accumlate error when time increases.

[SUZ-tsinghua]

@luoye2333 I mean projected_gravity. If you use quat_rotate_inverse(), this vector can be directly obtained by imu. That's why we use quat_rotate_inverse() instead of quat_rotate().

[SUZ-tsinghua]

@luoye2333 I just want to say, quat_rotate() might work when training, but quat_rotate_inverse() is more intuitive considering deployment.

[luoye2333]

@luoye2333 I just want to say, quat_rotate() might work when training, but quat_rotate_inverse() is more intuitive considering deployment.

I agree.