def get_robot_device(robot: Robot, name: str, kind: Type[TDevice]) -> TDevice: device: Optional[Device] = None try: # webots 2020b is buggy and always raises TypeError when passed a str, # however we're aiming to be forwards compatible with 2021a, so try this first. device = robot.getDevice(name) except TypeError: pass if device is None: # webots 2020b always returns None when not raising TypeError if kind is Emitter: device = robot.getEmitter(name) elif kind is Receiver: device = robot.getReceiver(name) elif kind is Motor: device = robot.getMotor(name) elif kind is LED: device = robot.getLED(name) elif kind is DistanceSensor: device = robot.getDistanceSensor(name) elif kind is TouchSensor: device = robot.getTouchSensor(name) elif kind is Compass: device = robot.getCompass(name) elif kind is Display: device = robot.getDisplay(name) if not isinstance(device, kind): raise TypeError return device
f.close() robot = Robot() timestep = int(robot.getBasicTimeStep() * 4) panHeadMotor = robot.getMotor('PRM:/r1/c1/c2-Joint2:12') tiltHeadMotor = robot.getMotor('PRM:/r1/c1/c2/c3-Joint2:13') panHeadMotor.setPosition(float('+inf')) tiltHeadMotor.setPosition(float('+inf')) panHeadMotor.setVelocity(0.0) tiltHeadMotor.setVelocity(0.0) camera = robot.getCamera('PRM:/r1/c1/c2/c3/i1-FbkImageSensor:F1') camera.enable(timestep) width = camera.getWidth() height = camera.getHeight() display = robot.getDisplay('display') display.attachCamera(camera) display.setColor(0xFF0000) targetPoint = [] targetRadius = 0 k = 0 flag_start_kalman = True sigmaMeasure = 3 sigmaNoise = 1 err = 10 while robot.step(timestep) != -1: if targetPoint: display.setAlpha(0.0) radius = targetRadius if radius < 5: radius = 5
left_motor.setPosition(float('inf')) right_motor.setPosition(float('inf')) #KEYBOARD INITIALIZATION keyboard = Keyboard() keyboard.enable(timestep) print("Select the 3D window and control the robot using the W, A, S, D keys.") #LIDAR INITIALIZATION lidar = robot.getLidar("lidar") lidar.enable(timestep) #DISPLAY INITIALIZATION i = 0 display = robot.getDisplay("extra_display") grapher = Grapher(display) while robot.step(timestep) != -1: i += 1 #HANDLING LIDAR IMAGE ACQUISITION imageArray = np.array(lidar.getRangeImageArray()).T #HANDLING KEYBOARD BEHAVIOUR ascii_val = keyboard.getKey() if (ascii_val == -1): left_motor.setVelocity(0) right_motor.setVelocity(0) else: key = chr(ascii_val).lower() if (key == 'w'): left_motor.setVelocity(VELOCITY)
#cozmoSpeaker.setEngine("microsoft") cozmoSpeaker.setLanguage("fr-FR") # "en-US" for American English (default value). "en-UK" for British English. "de-DE" for German. "es-ES" for Spanish. "fr-FR" for French. "it-IT" for Italian. ## Motors leftMotor = robot.getMotor("LeftWheelMotor") rightMotor = robot.getMotor("RightWheelMotor") liftMotor = robot.getMotor("LiftMotor") headMotor = robot.getMotor("HeadMotor") leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) liftMotor.setPosition(0.5) headMotor.setPosition(0.2) leftMotor.setPosition(-1) rightMotor.setPosition(-1) display = robot.getDisplay("face_display") awe = display.imageLoad("cozmo-awe.png") surprised = display.imageLoad("cozmo-surprised.png"); happy = display.imageLoad("cozmo-happy.png"); display.imagePaste(awe, 0, 0, False); # Controller welcome message in webots console print("Autre TechLab Cozmo controller") print("TIME STAMP = " +str(timestep)) # Main loop: # - perform simulation steps until Webots is stopping the controller left = 0.2 right = 0.2 flag = -2
from RobotControls import RobotControls from socket_client import SocketClient from VisionDisplay import VisionDisplay import Constants ENABLE_SOCKET = True ENABLE_VISION_DISPLAY = False VISION_DISPLAY_NAME = 'vision_display' # create the Robot instance. robot = Robot() # initialize vision display vision_display = False if ENABLE_VISION_DISPLAY: vision_display = VisionDisplay(robot.getDisplay(VISION_DISPLAY_NAME), robot.getCamera('camera'), Display.RGB) # get the time step of the current world. Constants.TIMESTEP = int(robot.getBasicTimeStep()) # get the robot movement logic rbc = RobotControls(robot) # get the keyboard for user input kb = robot.getKeyboard() kb.enable(Constants.TIMESTEP) socket = False if ENABLE_SOCKET: socket = SocketClient('localhost', 4444)