tz = 0 running = True while robot.step(timestep) != -1: t = translation.getSFVec3f() if running: percent = 1 - abs(0.25 - t[2]) / 0.25 if percent < 0: percent = 0 if t[2] > 0.01 and abs( tz - t[2] ) < 0.0001: # away from starting position and not moving any more message = "stop" running = False else: message = "percent" message += ":" + str(percent) robot.wwiSendText(message) tz = t[2] else: # wait for record message message = robot.wwiReceiveText() if message: if message.startswith("record:"): record = robotbenchmarkRecord(message, "robot_programming", percent) robot.wwiSendText(record) break elif message == "exit": break robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
# Detect status of inverted pendulum position = positionField.getSFFloat() if position < -1.58 or position > 1.58: # stop run = False robot.wwiSendText("stop") else: if forceStep <= 0: forceStep = 800 + random.randint(0, 400) force = force + 0.02 toSend = "%.2lf %d" % (force, seed) if sys.version_info.major > 2: toSend = bytes(toSend, "utf-8") emitter.send(toSend) else: forceStep = forceStep - 1 else: # wait for record message message = robot.wwiReceiveText() if message: if message.startswith("record:"): record = robotbenchmarkRecord(message, "inverted_pendulum", time) robot.wwiSendText(record) break elif message == "exit": break robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
running = True stopMessageSent = False while robot.step(timestep) != -1: if running: time = robot.getTime() # If some of the maze blocks have been moved immediately terminate the benchmark for i in range(0, mazeBlocksListCount): item = mazeBlocksList[i] if isPositionChanged(item['initialPosition'], item['node'].getPosition()): time = 60 robot.wwiSendText("time:%-24.3f" % time) break if time < 60 and not isMazeEndReached(thymio2.getPosition()): robot.wwiSendText("time:%-24.3f" % time) else: running = False timestep = int(timestep / 4) else: # Wait for record message. if not stopMessageSent: robot.wwiSendText("stop") stopMessageSent = True else: message = robot.wwiReceiveText() if message and message.startswith("record:"): record = robotbenchmarkRecord(message, "maze_runner", -time) robot.wwiSendText(record) break
enable_sensors_visualization(supervisor, False) elif message.endswith("true"): enable_sensors_visualization(supervisor, True) supervisor.wwiSendText("stop") if inEmergencyLane: supervisor.setLabel(0, "The vehicle should not enter in the emergency lane.", 0.01, 0.85, 0.09, 0xFF0000, 0, "Lucida Console") elif collided: supervisor.setLabel(0, "The vehicle should not collide.", 0.01, 0.85, 0.09, 0xFF0000, 0, "Lucida Console") elif sumoFailure: supervisor.setLabel( 0, "Problem with traffic generation using SUMO, we can't take this performance into account.", 0.01, 0.85, 0.09, 0xFF0000, 0, "Lucida Console") position = vehicleNode.getPosition() distance = round( roadPath.project(Point((position[0], position[2]))) - initialDistance, 3) while supervisor.step(timestep) != -1: # wait for record message message = supervisor.wwiReceiveText() if message and message.startswith("record:"): record = robotbenchmarkRecord(message, "highway_driving", distance) supervisor.wwiSendText(record) break
if i < cubeObstaclesCount: randomlyPlaceObject(cubeObstacles[i]) i += 1 if running: time = robot.getTime() # If the robot has collided with something that isn't the ground or has # reached the goal or has even run out of time, record final time and # terminate simulation. numberofContactPoints = thymio2.getNumberOfContactPoints() for x in range(0, numberofContactPoints): contactPoint = thymio2.getContactPoint(x) if contactPoint[1] > 0.02 or thymio2.getPosition( )[0] < -3.3 or time >= 80: if contactPoint[1] > 0.02: time = 80 running = False break robot.wwiSendText("time:%-24.3f" % time) else: # Wait for record message. if not stopMessageSent: robot.wwiSendText("stop") stopMessageSent = True else: message = robot.wwiReceiveText() if message and message.startswith("record:"): record = robotbenchmarkRecord(message, "obstacle_avoidance", -time) robot.wwiSendText(record) break
distance = round(math.sqrt(math.pow(targetPosition[0] - position[0], 2) + math.pow(targetPosition[2] - position[2], 2)), 4) time = robot.getTime() robot.wwiSendText("update: " + str(time) + " {0:.4f}".format(distance)) if boxPicked and distance < 0.036 and position[1] < 0.156: if distance == previousDistance: notMovingStepCount += 1 if notMovingStepCount > 10: break else: notMovingStepCount = 0 previousDistance = distance elif not boxPicked and position[1] > 0.21: boxPicked = True robot.wwiSendText("stop") while robot.step(timestep) != -1: # wait for record message message = robot.wwiReceiveText() if message: if message.startswith("record:"): record = robotbenchmarkRecord(message, "pick_and_place", -time) robot.wwiSendText(record) break elif message == "exit": break robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
hitsCount += target.hit(robotHead.getPosition(), [-R[2], -R[5], -R[8]], hitError) stepsCount += 1 robot.wwiSendText("hits:%d/%d" % (hitsCount, stepsCount)) # Update target object position: # return if the whole trajectory has been completed. isRunning = target.move(timestep) # Compute final grade and exit if stepsCount == 0: hitRate = 0.0 else: hitRate = float(hitsCount) / stepsCount robot.wwiSendText("stop") # Wait for record message. timestep = int(robot.getBasicTimeStep()) while robot.step(timestep) != -1: message = robot.wwiReceiveText() if message: if message.startswith("record:"): record = robotbenchmarkRecord(message, "visual_tracking", hitRate) robot.wwiSendText(record) break elif message == "exit": break robot.simulationSetMode(Supervisor.SIMULATION_MODE_PAUSE)
# Main loop starts here. while (supervisor.step(timestep) != -1 and not metric.isBenchmarkOver()): # Recovers current time and position/orientation of the robot. pos = pioneer.getPosition() pos2d = [pos[0], pos[2]] orientation = pioneer.getOrientation() time = supervisor.getTime() metric.update(pos2d, orientation, time) if ALLOW_LABELS: metric.updateLabels(0x0000ff, supervisor, time) supervisor.wwiSendText('update:' + metric.getWebMetricUpdate()) for pointMessage in metric.getWebNewPoints(): supervisor.wwiSendText(pointMessage) supervisor.wwiSendText('stop') # Wait for credentials sent by the robot window. while supervisor.step(timestep) != -1: message = supervisor.wwiReceiveText() if message and message.startswith("record:"): performance = metric.getPerformance() record = robotbenchmarkRecord(message, "square_path", performance) supervisor.wwiSendText(record) break