The issue about the orientation get from getLinkState().

[daodreamer]

Hi, there.

I'm using panda-gym to train a reinforcement learning model that makes Panda robot do some tasks. During the training I need to get the orientation of end-effector. Therefore, I call the function of PyBullet: getLinkState(panda_robot, ee_link, computeForwardKinematics=True)[1], but I found when the ee has the angle in the range (-120, 120) (after convert) w.r.t. world frame. This function returns always positive value. That means I cannot distinguish the ee has rotated itself e.g. +90 degrees or -90 degrees, because the returned values are the same equal to 90 degrees after converting to degrees from quaternion. I expect it will return e.g. -90 degrees or 270 degrees.

Thanks for your help.

[qgallouedec]

I'm not sure to get your point. Next time, provide a code example, it makes things easier to understand and reproduce. From my understanding, I wrote the following, and everything seems to work fine.

import time

import numpy as np

from panda_gym.envs.robots.panda import Panda
from panda_gym.pybullet import PyBullet

# Instantiate a PyBullet simulation
sim = PyBullet(render_mode="human")

# Instantiate a Panda robot
panda_robot = Panda(sim, block_gripper=False, base_position=np.zeros(3), control_type="joints")
panda_robot.reset()

# Rotate right
action = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0])
for _ in range(100):
    panda_robot.set_action(action)
    time.sleep(sim.dt)  # for real-time rendering
    sim.step()

    quaternion = sim.physics_client.getLinkState(0, 11)[1]
    euler = sim.physics_client.getEulerFromQuaternion(quaternion)
    print(np.rad2deg(euler[2]))  # z angle in degrees

# -1.5028899203810255
# -2.7614755594932308
# -4.020060248582
# -5.278644606738675
# ...

panda_robot.reset()

# Rotate left
action = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0])
for _ in range(100):
    panda_robot.set_action(action)
    time.sleep(sim.dt)  # for real-time rendering
    sim.step()

    quaternion = sim.physics_client.getLinkState(0, 11)[1]
    euler = sim.physics_client.getEulerFromQuaternion(quaternion)
    print(np.rad2deg(euler[2]))  # z angle in degrees

# 0.9748766047523295
# 2.2327720627833294
# 3.4906664168441783
# 4.748560201445388
# ...

[daodreamer]

I'm not sure to get your point. Next time, provide a code example, it makes things easier to understand and reproduce. From my understanding, I wrote the following, and everything seems to work fine.

import time

import numpy as np

from panda_gym.envs.robots.panda import Panda
from panda_gym.pybullet import PyBullet

# Instantiate a PyBullet simulation
sim = PyBullet(render_mode="human")

# Instantiate a Panda robot
panda_robot = Panda(sim, block_gripper=False, base_position=np.zeros(3), control_type="joints")
panda_robot.reset()

# Rotate right
action = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0])
for _ in range(100):
    panda_robot.set_action(action)
    time.sleep(sim.dt)  # for real-time rendering
    sim.step()

    quaternion = sim.physics_client.getLinkState(0, 11)[1]
    euler = sim.physics_client.getEulerFromQuaternion(quaternion)
    print(np.rad2deg(euler[2]))  # z angle in degrees

# -1.5028899203810255
# -2.7614755594932308
# -4.020060248582
# -5.278644606738675
# ...

panda_robot.reset()

# Rotate left
action = np.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.5, 0.0])
for _ in range(100):
    panda_robot.set_action(action)
    time.sleep(sim.dt)  # for real-time rendering
    sim.step()

    quaternion = sim.physics_client.getLinkState(0, 11)[1]
    euler = sim.physics_client.getEulerFromQuaternion(quaternion)
    print(np.rad2deg(euler[2]))  # z angle in degrees

# 0.9748766047523295
# 2.2327720627833294
# 3.4906664168441783
# 4.748560201445388
# ...

Thanks a lot!

This helped me to fix the issue. In my scripts I used pyquaternion to convert quaternion to degrees, and I just tried again following your code, using sim.physics_client.getEulerFromQuaternion(quaternion). It works well. XD