Problem about position offset caused by rotation action

[WangJuan6]

Dear iGibson team: I'm trying to build the global point cloud graph. I use the position and orientation to calculate the rotation matrix and transformation matrix.

current_rotation = env.simulator.robots[0].eyes.get_orientation()
current_position = env.simulator.robots[0].eyes.get_position()
# rotation matrix
from scipy.spatial.transform import Rotation as R
r = R.from_quat(current_rotation)
r_matrix = r.as_matrix()
# transformation matrix
t_matrix = current_position.T @ r_matrix.T

When the agent performs the forward action, the result is as follows: When the agent performs the rotation action, the result is as follows: Obviously, the second graph has offset. I try to use rotation matrix only when the agent performs rotation action, the result is as follows: I guess the error is caused by the position offset of the rotation action. So, how can I eliminate this position offset? Thanks, Juan

[ChengshuLi]

I am not completely sure I understand your issue. The robot's action is being physically simulated, so if the robot uses a forward action at timestep 0 and a turn-left action at timestep 1, it will still inherit some forward momentum at timestep 1 (like in the real world). Is this the position offset you are referring to?

@mjlbach Would you mind sharing your approach or maybe code snippet to build a global point cloud?

[WangJuan6]

Hi @ChengshuLi : Thanks for your reply. For the first graph, I use the husky robot and set the robot using forward action at every timestep, the result seems correct. I just found if I use the turtlebot robot and set the robot using forward action at every timestep, the result is not correct:

The husky robot always collision with other objects and change the position of other objects. I can not initialize it to a good position by env.simulator.robots[0].set_position([10,10,0]). So, I use turtlebot robot in the second and third graphs, and setting the robot uses turn-left action at every timestep. The second graph is the result using the rotation matrix and transformation matrix. The third graph is the result of using the rotation matrix only. So, I guess there is some position offset of turtlebot robot. Thanks, Juan

[WangJuan6]

Hi @ChengshuLi: I have other questions about the environment, can you answer me?

1. I set the turtlebot's action to [0,1], the robot will rotate 0.15 radians. Now, I want the turtlebot robot to rotate 90°(from 0° to 90°), what should I do?
2. If I want the husky robot to rotate 90°(from 0° to 90°), what should I do?
3. When the robot has a collision with an object, the position of the object will change. I want to know how to judge the validation of the current action(If the robot performs this action, there will not have any collision)? and how to set the object's position not moving even though it has a collision?
4. How to initialize the position and orientation of the robot? I try to use env.simulator.robots[0].set_position([10,10,0]) and env.land(env.robots[0], [10, 10, 0], [0, 0, 0]), but it does not works.

Thanks, Juan