def vec3_world_to_local_id(local_frame_id: int, world_vec: Vec3) -> Vec3:
from lobster_simulator.common.pybullet_api import PybulletAPI
pos, orn = PybulletAPI.getBasePositionAndOrientation(local_frame_id)
return vec3_world_to_local(pos, orn, world_vec)
def get_position_and_orientation(self) -> Tuple[Vec3, Quaternion]:
"""
Gets both the position and the orientation of the robot.
:return: Tuple with the position and the orientation.
"""
return p.getBasePositionAndOrientation(self._object_id)