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)