timestep = int(robot.getBasicTimeStep()) stopped = False burger = robot.getFromDef('Burger') # Supervisor translationField = robot.getFromDef('ROBOT_POSITION') rotationField = burger.getField('rotation') groundtruth = np.zeros((1, 3)) burgerData = np.zeros((1, 4)) lidarData = np.zeros((1, 128)) freq = 0 keyboard = robot.getKeyboard() keyboard.enable(timestep) receiver = robot.getReceiver("receiver") receiver.enable(timestep) lastDist = 0 np.set_printoptions(suppress=True) while robot.step(timestep) != -1: if freq == 10: translation = translationField.getPosition() orientation = translationField.getOrientation() zOri = np.array([orientation[6], orientation[8]])
MAX_MOTOR_VELOCITY = 50 # [rpm] WHEEL_RADIUS = 0.025 # [m] WHEEL_BASE = 0.165 # [m] MAX_LINEAR_SPEED = 1 # [m.s-1] MAX_ANGULAR_SPEED = 1.5708 # [rad.s-1] LINEAR_ACCEL = 0.5 # [m.s-2] ANGULAR_ACCEL = LINEAR_ACCEL / WHEEL_BASE # [rad.s-2] # time in [ms] of a simulation step TIME_STEP = 32 # create the Robot instance. supervisor = Supervisor() # Init keyboard keyboard = supervisor.getKeyboard() keyboard.enable(TIME_STEP) # Init motors leftMotor = supervisor.getDevice('left wheel motor') rightMotor = supervisor.getDevice('right wheel motor') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) linearVelocity = 0 angularVelocity = 0 def check_keyboard_inputs():