def __init__(self, robot: Robot, timestep=128): # Initialize the arm motors and encoders. self.arm_chain = self.get_armchain() self.suction_cups = [ robot.getDevice("robot_box_connector(1)"), robot.getDevice("robot_box_connector(2)"), robot.getDevice("robot_box_connector(3)") ] for suction_cup in self.suction_cups: suction_cup.enablePresence(timestep) self.motors = [] for link in self.arm_chain.links: if any(joint_name in link.name for joint_name in ["arm", "linear_actuator"]): motor = robot.getDevice(link.name) motor.setVelocity(.4) position_sensor = motor.getPositionSensor() position_sensor.enable(timestep) motor.setPosition(0.0) self.motors.append(motor) self.pickup_x = INITIAL_PICKUP_X # used to try different pickup locations if pickup fails # Deactivate fixed joints for i in [0, 1, 2]: self.arm_chain.active_links_mask[i] = False
def __init__(self, robot: Robot, timestep=128): self.max_val = 150 self.low_val = 120 self.high_val = 140 # Front distance sensor self.ds_front = robot.getDevice('ds_front') self.ds_front.enable(timestep) #Specifys how far away the bot must be from the object infront of it in order to move forward (in CM) self.safety_distance = 100 #IR sensors IR = {} for name in ["ds_left", "ds_right", "ds_mid","ds_back"]: IR[name[3:]] = robot.getDevice(name) IR[name[3:]].enable(timestep) self.IR = SimpleNamespace(**IR) # Wheels # Front Left, Back Left, Front Right, Back Right Wheels = {} for name in ['wheel_FL', 'wheel_FR', 'wheel_BL', 'wheel_BR']: Wheels[name[6:]] = robot.getDevice(name) # self.Wheels[name[6:]].enable(timestep) Wheels[name[6:]].setPosition(float('inf')) Wheels[name[6:]].setVelocity(0.0) self.wheels = SimpleNamespace(**Wheels)
def main(): print("Initializing Olympiad...") robot = Robot() # get the time step of the current world. global TIMESTEP TIMESTEP = int(robot.getBasicTimeStep()) # gyroscope, accelorometer gyro = robot.getDevice("torso_gyro") accel = robot.getDevice("torso_accelerometer") gyro.enable(TIMESTEP) accel.enable(TIMESTEP) # If the ZMP is stable then there will be no trajectory # Assumes that the base-frame-origin is in between the two feet since it's standing # This stays under the x and y coor of the COM (assuming standing straight) desiredXZMP = 0 desiredYZMP = 0 priorError = [0, 0] priorIntegral = [0, 0] while robot.step(TIMESTEP) != -1: xObs, yObs = calculateZMP(gyro, accel) error = [desiredXZMP - xObs, desiredYZMP - yObs] controllerPID(robot, error, priorError, priorIntegral)
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
def getDevice(self, name: str): # here to make sure no device is retrieved this way if name in [ 'gps', 'compass', 'wheel1', 'wheel2', 'ultrasonic_left', 'ultrasonic_right', 'infrared', "red_light_sensor", "green_light_sensor" ]: raise RuntimeError( 'Please use the corresponding properties instead') return Robot.getDevice(self, name)
def main(): # create the Robot instance. print("Initializing world...") robot = Robot() timestep = 16 curr_time = 0 lfootsensor = robot.getDevice("torso_gyro") while robot.step(timestep) != -1: # print(calculateCOM(robot)) curr_time += timestep/1000.0 # head_motor = robot.getDevice("torso_yaw") motor2 = robot.getDevice("neck_roll") lknee = robot.getDevice("left_knee_pitch") rknee = robot.getDevice("right_knee_pitch") lhip = robot.getDevice("left_hip_pitch") rhip = robot.getDevice("right_hip_pitch") lfootsensor.enable(10) # print sensor data every second if curr_time % 1 < 0.01: print(lfootsensor.getValues())
def __init__(self, robot: controller.Robot, robotData: RobotData): """ Intializer :param robot: Webots robot object :param robotData: RobotData object to update """ self._robot = robot # type: controller.Robot self._timestep = int(robot.getBasicTimeStep()) self.robotData = robotData self.leftMotor = robot.getDevice('wheel1') # type: controller.Motor self.rightMotor = robot.getDevice('wheel2') # type: controller.Motor self.leftGate = robot.getDevice('gate_left') # type: controller.Motor self.rightGate = robot.getDevice( 'gate_right') # type: controller.Motor self._maxMotorVelocity = self.leftMotor.getMaxVelocity() self._motorVelocityControl = False self.distanceLong = robot.getDevice( 'ds_long') # type: controller.DistanceSensor self.colorSensor = robot.getDevice('camera') # type: controller.Camera self.gps = robot.getDevice('gps') # type: controller.GPS self.radio = Radio(robot.getDevice('emitter'), robot.getDevice('receiver')) self.compass = robot.getDevice('compass') # type: controller.Compass self.leftMotor.enableForceFeedback(self._timestep) self.rightMotor.enableForceFeedback(self._timestep) self.distanceLong.enable(self._timestep) self.colorSensor.enable(self._timestep) self.gps.enable(self._timestep) self.radio.enable(self._timestep) self.compass.enable(self._timestep) self._turnController = PIDController(0.02, 0.002, 0.05, 0, 2) self._pathController = PIDController(0.01, 0.002, 0, 0, 3) self._pointController = PIDController(1 / 0.1, 0, 0, 0, 0) self.depositBox = None
from controller import Robot TIME_STEP = 64 robot = Robot() ds = [] dsNames = ['ds_right', 'ds_left'] for i in range(2): ds.append(robot.getDevice(dsNames[i])) ds[i].enable(TIME_STEP) wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for i in range(4): wheels.append(robot.getDevice(wheelsNames[i])) wheels[i].setPosition(float('inf')) wheels[i].setVelocity(0.0) avoidObstacleCounter = 0 while robot.step(TIME_STEP) != -1: leftSpeed = 1.0 rightSpeed = 1.0 if avoidObstacleCounter > 0: avoidObstacleCounter -= 1 leftSpeed = 1.0 rightSpeed = -1.0 else: # read sensors for i in range(2): if ds[i].getValue() < 950.0: avoidObstacleCounter = 100 wheels[0].setVelocity(leftSpeed) wheels[1].setVelocity(rightSpeed) wheels[2].setVelocity(leftSpeed) wheels[3].setVelocity(rightSpeed)
port = robot.getCustomData() print("Port: ", port) # initialize sensors # ps = [] # psNames = [ # 'ps0', 'ps1', 'ps2', 'ps3', # 'ps4', 'ps5', 'ps6', 'ps7' # ] # for i in range(8): # ps.append(robot.getDevice(psNames[i])) # ps[i].enable(TIME_STEP) # initialize and set motor devices leftMotor = robot.getDevice('left wheel') rightMotor = robot.getDevice('right wheel') leftSensor = robot.getDevice("left wheel sensor") leftSensor.enable(TIME_STEP) rightSensor = robot.getDevice("right wheel sensor") rightSensor.enable(TIME_STEP) # Set to endless rotational motion leftMotor.setPosition(float('+inf')) rightMotor.setPosition(float('+inf')) leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) class MainHandler(tornado.web.RequestHandler): def get(self): self.write("Robot Ready!")
u = 0.0 # linear speed [m/s] w = 0.0 # angular speed [rad/s] # Physical parameters of the robot for the kinematics model R = 0.0205 # radius of the wheels: 20.5mm [m] D = 0.0565 # distance between the wheels: 52mm [m] A = 0.05 # distance from the center of the wheels to the point of interest [m] #------------------------------------------------------- # Initialize devices # distance sensors ps = [] psNames = ['ps0', 'ps1', 'ps2', 'ps3', 'ps4', 'ps5', 'ps6', 'ps7'] for i in range(8): ps.append(robot.getDevice(psNames[i])) ps[i].enable(timestep) # ground sensors gs = [] gsNames = ['gs0', 'gs1', 'gs2'] for i in range(3): gs.append(robot.getDevice(gsNames[i])) gs[i].enable(timestep) # encoders encoder = [] encoderNames = ['left wheel sensor', 'right wheel sensor'] for i in range(2): encoder.append(robot.getDevice(encoderNames[i])) encoder[i].enable(timestep)
timestep = int(robot.getBasicTimeStep()) # The Tiago robot has multiple motors, each identified by their names below part_names = ("head_2_joint", "head_1_joint", "torso_lift_joint", "arm_1_joint", "arm_2_joint", "arm_3_joint", "arm_4_joint", "arm_5_joint", "arm_6_joint", "arm_7_joint", "wheel_left_joint", "wheel_right_joint") # All motors except the wheels are controlled by position control. The wheels # are controlled by a velocity controller. We therefore set their position to infinite. target_pos = (0.0, 0.0, 0.09, 0.07, 1.02, -3.16, 1.27, 1.32, 0.0, 1.41, 'inf', 'inf') robot_parts = [] for i in range(N_PARTS): robot_parts.append(robot.getDevice(part_names[i])) robot_parts[i].setPosition(float(target_pos[i])) robot_parts[i].setVelocity(robot_parts[i].getMaxVelocity() / 2.0) # The Tiago robot has a couple more sensors than the e-Puck # Some of them are mentioned below. We will use its LiDAR for Lab 5 # range = robot.getDevice('range-finder') # range.enable(timestep) # camera = robot.getDevice('camera') # camera.enable(timestep) # camera.recognitionEnable(timestep) lidar = robot.getDevice('Hokuyo URG-04LX-UG01') lidar.enable(timestep) lidar.enablePointCloud()
class RobotController: def __init__(self, max_steps=2, init_position=(0, 0, 0), final_position=(-0.3, 0, 0.3), max_speed=3, ): self.robot = Robot() self.timestep = int(self.robot.getBasicTimeStep()) self.max_steps = max_steps self.max_speed = max_speed self.setupDevice() self.init_position = init_position self.current_position = init_position self.final_position = final_position self.done = False # Interactive self.feedbackAmount = 0 # self.policy_reuse = PPR() def setupDevice(self): self.leftMotor = self.robot.getDevice('left wheel motor') self.rightMotor = self.robot.getDevice('right wheel motor') self.leftMotor.setPosition(float('inf')) self.rightMotor.setPosition(float('inf')) self.rightDistanceSensor = self.robot.getDevice('ds1') self.leftDistanceSensor = self.robot.getDevice('ds0') self.rightDistanceSensor.enable(self.timestep) self.leftDistanceSensor.enable(self.timestep) self.gps = self.robot.getDevice('gps') self.touchSensor1 = self.robot.getDevice('touch_sensor1') self.touchSensor2 = self.robot.getDevice('touch_sensor2') self.touchSensor3 = self.robot.getDevice('touch_sensor3') self.touchSensor4 = self.robot.getDevice('touch_sensor4') self.touchSensor5 = self.robot.getDevice('touch_sensor5') self.gps.enable(self.timestep) self.touchSensor1.enable(self.timestep) self.touchSensor2.enable(self.timestep) self.touchSensor3.enable(self.timestep) self.touchSensor4.enable(self.timestep) self.touchSensor5.enable(self.timestep) self.camera = Camera('camera') self.camera.enable(self.timestep) self.leftMotor.setVelocity(0) self.rightMotor.setVelocity(0) self.init_leftValue = self.leftDistanceSensor.getValue() self.init_rightValue = self.rightDistanceSensor.getValue() self.receiver = Receiver('receiver') self.emitter = Emitter('emitter') self.receiver.enable(self.timestep) def is_collised(self): if (self.touchSensor1.getValue() + self.touchSensor2.getValue() + self.touchSensor3.getValue() + self.touchSensor4.getValue() + self.touchSensor5.getValue() > 0): print(1, self.touchSensor1.getValue()) print(2, self.touchSensor2.getValue()) print(3, self.touchSensor3.getValue()) print(4, self.touchSensor4.getValue()) print(5, self.touchSensor5.getValue()) return True gpsValue = self.gps.getValues() self.current_position = gpsValue if (self.current_position[0] < - 0.5 or self.current_position[0] > 0.5 or self.current_position[2] < - 0.5 or self.current_position[2] > 0.5): return True return False def step(self, a): if not self.done: self.robot.step(self.timestep) if not self.is_collised(): leftValue = self.leftDistanceSensor.getValue() rightValue = self.rightDistanceSensor.getValue() reward = -0.1 leftSpeed, rightSpeed = 0, 0 if a == 0: leftSpeed, rightSpeed = self.turnLeft(leftValue, rightValue) elif a == 1: leftSpeed, rightSpeed = self.turnRight(leftValue, rightValue) elif a == 2: leftSpeed, rightSpeed = self.goStraight() reward = 0 # elif a == 3: # leftSpeed, rightSpeed = self.goSlow() self.leftMotor.setVelocity(leftSpeed) self.rightMotor.setVelocity(rightSpeed) # set observation ............. observations = leftValue, rightValue # set reward ............. # set done ......... r = self.set_done() return observations, reward + r, self.done, False else: observations = self.reset() reward = -100 return observations, reward, False, True return None, None, self.done, False def set_done(self): gpsValue = self.gps.getValues() self.current_position = gpsValue if abs(self.current_position[0] - self.final_position[0]) <= 0.08 and \ abs(self.current_position[2] - self.final_position[2]) <= 0.08: self.done = True return 1000 return 0 def random_action(self): return random.choice(self.action_space()) def goStraight(self): return self.max_speed, self.max_speed def goSlow(self): return self.max_speed / 4, self.max_speed / 4 def turnLeft(self, leftDistance, rightDistance): return -(leftDistance / 100), (rightDistance / 100) + 0.5 def turnRight(self, leftDistance, rightDistance): return (leftDistance / 100) + 0.5, -(rightDistance / 100) def reset(self): self.done = False return self.init_leftValue, self.init_rightValue def send_to_super(self, message, data): data = message, data dataSerialized = pickle.dumps(data) self.emitter.send(dataSerialized) def receive_handle(self): if self.receiver.getQueueLength() > 0: data = self.receiver.getData() message, action, step = pickle.loads(data) self.receiver.nextPacket() if message == 'step': obs, r, d, i = self.step(action) data = obs, r, d, i, step self.send_to_super('step_done', data) if message == 'reset': obs = self.reset() self.send_to_super('reset_done', obs) return -1 def start(self): while self.robot.step(self.timestep) != -1: self.receive_handle()
from controller import Robot, Camera import cv2 as cv # Include OpenCV library import numpy as np # For python, include numpy as well robot = Robot() camera = robot.getDevice("camera1") timestep = int(robot.getBasicTimeStep()) camera.enable(timestep) def empty(a): pass def controllPanel(): cv.namedWindow("parameters") cv.resizeWindow("parameters", 640, 240) cv.createTrackbar("Threshold_1", "parameters", 500, 500, empty) cv.createTrackbar("Threshold_", "parameters", 500, 500, empty) cv.createTrackbar("area", "parameters", 10000, 10000, empty) print("area", "parameters") def showAllImages(img1, img2, img3): cv.imshow("img1", img1) cv.imshow("img2", img2) cv.imshow("img3", img3) def findContoursMethod(source, sourceToProcess, areaFinder): shapeType = ""
from skimage.feature import blob_dog from skimage.filters import gaussian from pathfinder import findpath, h from math import hypot import scipy.interpolate as si from scipy.signal import find_peaks from scipy.stats import entropy from robot_manager import * from environment_manager import * from utils import visualiser from state_manager import robot_state_manager ### master controller initialisation: TIME_STEP = 64 master_robot = Robot() receiver = master_robot.getDevice('receiver') emitter = master_robot.getDevice('emitter') receiver.enable(TIME_STEP) emitter.setChannel(Emitter.CHANNEL_BROADCAST) receiver.setChannel(Receiver.CHANNEL_BROADCAST) red_bot = robot_manager(0, emitter) blue_bot = robot_manager(1, emitter) environment = environment_manager() visualiser1 = visualiser(2, ["Occupancy Grid", "Block Grid"], k=1) visualiser2 = visualiser(2, ["Blue bot", "Red bot"], k=2) red_bot_state_manager = robot_state_manager(master_robot) blue_bot_state_manager = robot_state_manager(master_robot)
def __init__(self, robot: controller.Robot, colour='red'): """ initialize robot and its components, colour can be green or red """ self._robot = robot self.colour = colour self.field = Field(colour) self.infrared_vref = 4.3 # queue of tuple (i, pos) where i is 0 if initial, 1 if added and pos is position of the box self.box_list = [] self.other_box_list = [] self.sweep_locations = np.array([]) self.other_sweep_locations = np.array([]) self.unique_boxes = [] self.position = np.array([]) self.other_position = np.array([]) self.other_bearing = 0 # this one is 0-360 self.current_target = [] self.stop = False self.other_stop = False self.parked = False self.sweep_ready = False self.other_parked = False self.other_blocked = False self.carrying = False self.other_available = False self.other_sweep_ready = False self.second_sweep_locations_ready = False self.other_second_sweep_locations_ready = False self.left_wheel = robot.getDevice(Robot.left_wheel_name) self.right_wheel = robot.getDevice(Robot.right_wheel_name) self.box_claw = robot.getDevice(Robot.box_claw_name) self.left_claw = robot.getDevice(Robot.left_claw_name) self.right_claw = robot.getDevice(Robot.right_claw_name) self.box_claw_sensor = robot.getDevice(Robot.box_claw_sensor_name) self.left_claw_sensor = robot.getDevice(Robot.left_claw_sensor_name) self.right_claw_sensor = robot.getDevice(Robot.right_claw_sensor_name) self.infrared = robot.getDevice(Robot.infrared_name) self.dsUltrasonic = robot.getDevice(Robot.dsUltrasonic_name) self.lightsensorRed = robot.getDevice(Robot.lightSensorRed_name) self.lightsensorGreen = robot.getDevice(Robot.lightSensorGreen_name) self.emitter = robot.getDevice(Robot.emitter_name) self.receiver = robot.getDevice(Robot.receiver_name) self.gps = robot.getDevice(Robot.gps_name) self.compass = robot.getDevice(Robot.compass_name) self.compass1 = robot.getDevice(Robot.compass1_name) # Device.enable() takes the sampling period in milliseconds TIME_STEP = self.TIME_STEP # 38.3 ms ± 9.6 ms, choose the upper bound (worst-case scenario) self.infrared.enable(48) # 150 μs to 25 ms, 38 ms if no obstacle # We're only using it at small distances, so it should be well under one TIME_STEP (16 ms) self.dsUltrasonic.enable(TIME_STEP) self.box_claw_sensor.enable(TIME_STEP) self.left_claw_sensor.enable(TIME_STEP) self.right_claw_sensor.enable(TIME_STEP) # ATmega4809's ADC samples really quickly self.lightsensorRed.enable(TIME_STEP) self.lightsensorGreen.enable(TIME_STEP) self.receiver.enable(TIME_STEP) # Assume computer vision is close to real time self.gps.enable(TIME_STEP) self.compass.enable(TIME_STEP) self.compass1.enable(TIME_STEP) self.emitter.setChannel(Robot.COMMUNICATION_CHANNEL) self.receiver.setChannel(Robot.COMMUNICATION_CHANNEL) self.box_claw.setPosition(0.0) self.left_claw.setPosition(0.0) self.right_claw.setPosition(0.0) self.left_wheel.setPosition(float('inf')) self.left_wheel.setVelocity(0.0) self.right_wheel.setPosition(float('inf')) self.right_wheel.setVelocity(0.0) self.green_analogue = hardware.ADCInput( lambda: hardware.PhototransistorCircuit(self.lightsensorGreen ).voltage()) self.red_analogue = hardware.ADCInput( lambda: hardware.PhototransistorCircuit(self.lightsensorRed ).voltage()) self.infrared_analogue = hardware.ADCInput( lambda: self.infrared.getValue(), self.infrared_vref)
"""PAUL_demo1 controller.""" from controller import Robot, Motor, DistanceSensor # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # initialise the 3 wheels wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3'] for i in range(3): wheels.append(robot.getDevice(wheelsNames[i])) wheels[i].setPosition(float('inf')) wheels[i].setVelocity(0.0) # unit: rad/s # initialise distance sensors ds_top = robot.getDevice('ds_top') ds_bottom = robot.getDevice('ds_bottom') ds_top.enable(timestep) ds_bottom.enable(timestep) # initialise LED uv_led = robot.getDevice('uv_led') # stores current direction & state goingUp = True # changingDir = False def setSpeed(v):
class RCJSoccerRobot: def __init__(self): # create the Robot instance. self.robot = Robot() self.name = self.robot.getName() self.team = self.name[0] self.player_id = int(self.name[1]) self.receiver = self.robot.getDevice("receiver") self.receiver.enable(TIME_STEP) self.left_motor = self.robot.getDevice("left wheel motor") self.right_motor = self.robot.getDevice("right wheel motor") self.left_motor.setPosition(float('+inf')) self.right_motor.setPosition(float('+inf')) self.left_motor.setVelocity(0.0) self.right_motor.setVelocity(0.0) def parse_supervisor_msg(self, packet: str) -> dict: """Parse message received from supervisor Returns: dict: Location info about each robot and the ball. Example: { 'B1': {'x': 0.0, 'y': 0.2, 'orientation': 1}, 'B2': {'x': 0.4, 'y': -0.2, 'orientation': 1}, ... 'ball': {'x': -0.7, 'y': 0.3} } """ # X, Z and rotation for each robot # plus X and Z for ball struct_fmt = 'ddd' * 6 + 'dd' unpacked = struct.unpack(struct_fmt, packet) data = {} for i, r in enumerate(ROBOT_NAMES): data[r] = { "x": unpacked[3 * i], "y": unpacked[3 * i + 1], "orientation": unpacked[3 * i + 2] } data["ball"] = { "x": unpacked[3 * N_ROBOTS], "y": unpacked[3 * N_ROBOTS + 1] } return data def get_new_data(self) -> dict: """Read new data from supervisor Returns: dict: See `parse_supervisor_msg` method """ packet = self.receiver.getData() self.receiver.nextPacket() return self.parse_supervisor_msg(packet) def is_new_data(self) -> bool: """Check if there are new data to be received Returns: bool: Whether there is new data received from supervisor. """ return self.receiver.getQueueLength() > 0 def get_angles(self, ball_pos: dict, robot_pos: dict) -> Tuple[float, float]: """Get angles in degrees. Args: ball_pos (dict): Dict containing info about position of the ball robot_pos (dict): Dict containing info about position and rotation of the robot Returns: :rtype: (float, float): Angle between the robot and the ball Angle between the robot and the north """ robot_angle: float = robot_pos['orientation'] # Get the angle between the robot and the ball angle = math.atan2( ball_pos['y'] - robot_pos['y'], ball_pos['x'] - robot_pos['x'], ) if angle < 0: angle = 2 * math.pi + angle if robot_angle < 0: robot_angle = 2 * math.pi + robot_angle robot_ball_angle = math.degrees(angle + robot_angle) # Axis Z is forward # TODO: change the robot's orientation so that X axis means forward robot_ball_angle -= 90 if robot_ball_angle > 360: robot_ball_angle -= 360 # elif robot_ball_angle < 0: # robot_ball_angle += 360 return robot_ball_angle, robot_angle def get_NZangles(self, robot_pos: dict, Neu_posx, Neu_posy) -> Tuple[float]: """encontrar el angulo del robot con la zona neutral """ NeuZone_angle: float = robot_pos['orientation'] # Get the angle between the robot and the ball angle = math.atan2( Neu_posy - robot_pos['y'], Neu_posx - robot_pos['x'], ) if angle < 0: angle = 2 * math.pi + angle if NeuZone_angle < 0: NeuZone_angle = 2 * math.pi + NeuZone_angle NeuZone_angle = math.degrees(angle + NeuZone_angle) # Axis Z is forward # TODO: change the robot's orientation so that X axis means forward NeuZone_angle -= 90 if NeuZone_angle > 360: NeuZone_angle -= 360 return NeuZone_angle def run(self): raise NotImplementedError
#state = "Path_follower" #needs to be set by the supervisor #state = "caught" #set from the caught state #state = "flee" # ePuck Constants EPUCK_AXLE_DIAMETER = 0.053 # ePuck's wheels are 53mm apart. MAX_SPEED = 6.28 # create the Robot instance. robot = Robot() # Initialize Motors leftMotor = robot.getDevice('left wheel motor') rightMotor = robot.getDevice('right wheel motor') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) led = robot.getDevice('led0') receiver = robot.getDevice('receiver') # get the time step of the current world. SIM_TIMESTEP = int(robot.getBasicTimeStep()) # Initialize the Display display = robot.getDevice("display")
from controller import Emitter from controller import Receiver import numpy as np import struct import matplotlib.pyplot as plt from skimage.draw import line, rectangle, rectangle_perimeter, ellipse from skimage.feature import blob_dog from skimage.filters import gaussian from pathfinder import findpath import scipy.interpolate as si TIME_STEP = 64 robot = Robot() receiver = robot.getDevice('receiver') emitter = robot.getDevice('emitter') receiver.enable(TIME_STEP) emitter.setChannel(Emitter.CHANNEL_BROADCAST) receiver.setChannel(Receiver.CHANNEL_BROADCAST) update = False ### useful defs def log_odds(map): return np.log(np.divide(map, np.subtract(1, map))) def inv_lodds(lmap):
# ratio between the value reported by the wheel sensor and the actual # angle traveled by the robot while turning, in radian. ROTATE_FACTOR = 0.566 # maximum speed for the velocity value of the wheels. MAX_SPEED = 5.24 # minimum speed used while slowing down and trying to stop at a precise # location. MIN_SPEED = 0.1 robot = Robot() timestep = int(robot.getBasicTimeStep()) leftWheel = robot.getDevice('left wheel') rightWheel = robot.getDevice('right wheel') leftWheel.setVelocity(0) rightWheel.setVelocity(0) leftWheel.setPosition(float('inf')) rightWheel.setPosition(float('inf')) leftWheelSensor = robot.getDevice('left wheel sensor') leftWheelSensor.enable(timestep) rightWheelSensor = robot.getDevice('right wheel sensor') rightWheelSensor.enable(timestep)
pose_x = 0.197 pose_y = 0.678 pose_theta = 0 # ePuck Constants EPUCK_AXLE_DIAMETER = 0.053 # ePuck's wheels are 53mm apart. MAX_SPEED = 6.28 # create the Robot instance. robot = Robot() # get the time step of the current world. SIM_TIMESTEP = int(robot.getBasicTimeStep()) # Initialize Motors leftMotor = robot.getDevice('left wheel motor') rightMotor = robot.getDevice('right wheel motor') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) # Initialize and Enable the Ground Sensors gsr = [0, 0, 0] ground_sensors = [ robot.getDevice('gs0'), robot.getDevice('gs1'), robot.getDevice('gs2') ] for gs in ground_sensors: gs.enable(SIM_TIMESTEP)
"""Sample Webots controller for the pit escape benchmark.""" from controller import Robot robot = Robot() timestep = int(robot.getBasicTimeStep()) # Max possible speed for the motor of the robot. maxSpeed = 8.72 # Configuration of the main motor of the robot. pitchMotor = robot.getDevice("body pitch motor") pitchMotor.setPosition(float('inf')) pitchMotor.setVelocity(0.0) # This is the time interval between direction switches. # The robot will start by going forward and will go backward after # this time interval, and so on. timeInterval = 1.5 # At first we go forward. pitchMotor.setVelocity(maxSpeed) forward = True lastTime = 0 while robot.step(timestep) != -1: now = robot.getTime() # We check if enough time has elapsed. if now - lastTime > timeInterval: # If yes, then we switch directions.
bridge = CvBridge() rosmsg = bridge.cv2_to_imgmsg(rotatedImg, encoding="passthrough") pub.publish(rosmsg) rospy.init_node('camera_test_node') robot = Robot() global camPub global rangePub camPub = rospy.Publisher('/camera/image', Image, queue_size=20) rangePub = rospy.Publisher('/range_finder/image', Image, queue_size=20) # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) SAMPLE_TIME = 100 camera = robot.getDevice('camera') depth = robot.getDevice('range-finder') global rscam global depthcam rscam = Camera('camera') depthcam = RangeFinder('range-finder') depthcam.enable(SAMPLE_TIME) rscam.enable(SAMPLE_TIME) rospy.Subscriber('publish_images', Bool, camera_CB) clockPublisher = rospy.Publisher('clock', Clock, queue_size=1) if not rospy.get_param('use_sim_time', False): rospy.logwarn('use_sim_time is not set!') # Main loop: # - perform simulation steps until Webots is stopping the controller
"""Controller for our Robot""" __author__ = "Neeraj Chatterjee and Dennis Thomas" from controller import Robot # Created instance of Robot class which will help us interact with the environment robot = Robot() timestep = 32 max_speed = 10 max_ir = 1000 # Setting up indentifiers for our motors right_motor = robot.getDevice('motor1') left_motor = robot.getDevice('motor2') # Setting up indentifiers for our infrared sensors # left, straight and right ir will be used to detect the line the robot has to follow # top ir will be used in detecting if we have reached the END or not straight_ir = robot.getDevice('ir0') left_ir = robot.getDevice('ir1') right_ir = robot.getDevice('ir2') top_ir = robot.getDevice('ir3') # We are going to use flag variable to check if the robot has reached the end or not flag = False # Even if we reached the end, we want the robot to move a bit forward so that it sits nicely # in the centre of the END box. So we will use counter to setup a pseudo loop, so that it # moves for a "while" counter = 13
"""Move the conveyor belt until the cube reaches the desired position.""" from controller import Robot robot = Robot() motor = robot.getDevice("belt motor") motor.setVelocity(0.2) motor.setPosition(0.75)
class RobotLayer: def __init__(self, timeStep): self.maxWheelSpeed = 6.28 self.timeStep = timeStep self.robot = Robot() self.prevRotation = 0 self.rotation = 0 self.globalPosition = [0, 0] self.prevGlobalPosition = [0, 0] self.positionOffsets = [0, 0] self.__useGyroForRotation = True self.time = 0 self.rotateToDegsFirstTime = True self.delayFirstTime = True self.gyroscope = Gyroscope(self.robot.getDevice("gyro"), 1, self.timeStep) self.gps = Gps(self.robot.getDevice("gps"), self.timeStep) self.lidar = Lidar(self.robot.getDevice("lidar"), self.timeStep) self.leftWheel = Wheel(self.robot.getDevice("wheel1 motor"), self.maxWheelSpeed) self.rightWheel = Wheel(self.robot.getDevice("wheel2 motor"), self.maxWheelSpeed) self.colorSensor = ColourSensor(self.robot.getDevice("colour_sensor"), 0.037, 32) self.leftGroundSensor = DistanceSensor( 0.04, 0.0523, 45, self.robot.getDevice("distance sensor2"), self.timeStep) self.rightGroundSensor = DistanceSensor( 0.04, 0.0523, -45, self.robot.getDevice("distance sensor1"), self.timeStep) self.comunicator = Comunicator(self.robot.getDevice("emitter"), self.robot.getDevice("receiver"), self.timeStep) self.rightCamera = Camera(self.robot.getDevice("camera2"), self.timeStep) self.leftCamera = Camera(self.robot.getDevice("camera1"), self.timeStep) self.victimClasifier = VictimClassifier() def getVictims(self): poses = [] imgs = [] for camera in (self.rightCamera, self.leftCamera): cposes, cimgs = self.victimClasifier.getVictimImagesAndPositions( camera.getImg()) poses += cposes imgs += cimgs print("Victim Poses: ", poses) for img in imgs: print("Victim shape:", img.shape) closeVictims = self.victimClasifier.getCloseVictims(poses, imgs) finalVictims = [] for closeVictim in closeVictims: finalVictims.append( self.victimClasifier.classifyVictim(closeVictim)) return finalVictims def reportVictims(self, letter): self.comunicator.sendVictim(self.globalPosition, letter) def sendArray(self, array): self.comunicator.sendMap(array) def sendEnd(self): print("End sended") self.comunicator.sendEndOfPlay() # Decides if the rotation detection is carried out by the gps or gyro @property def rotationDetectionType(self): if self.__useGyroForRotation: return "gyroscope" else: return "gps" @rotationDetectionType.setter def rotationDetectionType(self, rotationType): if rotationType == "gyroscope": self.__useGyroForRotation = True elif rotationType == "gps": self.__useGyroForRotation = False else: raise ValueError("Invalid rotation detection type inputted") def delaySec(self, delay): if self.delayFirstTime: self.delayStart = self.robot.getTime() self.delayFirstTime = False else: if self.time - self.delayStart >= delay: self.delayFirstTime = True return True return False # Moves the wheels at the specified ratio def moveWheels(self, leftRatio, rightRatio): self.leftWheel.move(leftRatio) self.rightWheel.move(rightRatio) def rotateToDegs(self, degs, orientation="closest", maxSpeed=0.5): accuracy = 2 if self.rotateToDegsFirstTime: #print("STARTED ROTATION") self.rotateToDegsFirstTime = False self.seqRotateToDegsInitialRot = self.rotation self.seqRotateToDegsinitialDiff = round( self.seqRotateToDegsInitialRot - degs) diff = self.rotation - degs moveDiff = max(round(self.rotation), degs) - min(self.rotation, degs) if diff > 180 or diff < -180: moveDiff = 360 - moveDiff speedFract = min(mapVals(moveDiff, accuracy, 90, 0.2, 0.8), maxSpeed) if accuracy * -1 < diff < accuracy or 360 - accuracy < diff < 360 + accuracy: self.rotateToDegsFirstTime = True return True else: if orientation == "closest": if 180 > self.seqRotateToDegsinitialDiff > 0 or self.seqRotateToDegsinitialDiff < -180: direction = "right" else: direction = "left" elif orientation == "farthest": if 180 > self.seqRotateToDegsinitialDiff > 0 or self.seqRotateToDegsinitialDiff < -180: direction = "left" else: direction = "right" else: direction = orientation if moveDiff > 10: if direction == "right": self.moveWheels(speedFract * -1, speedFract) elif direction == "left": self.moveWheels(speedFract, speedFract * -1) else: if direction == "right": self.moveWheels(speedFract * -0.5, speedFract) elif direction == "left": self.moveWheels(speedFract, speedFract * -0.5) #print("speed fract: " + str(speedFract)) #print("target angle: " + str(degs)) #print("moveDiff: " + str(moveDiff)) #print("diff: " + str(diff)) #print("orientation: " + str(orientation)) #print("direction: " + str(direction)) #print("initialDiff: " + str(self.seqRotateToDegsinitialDiff)) #print("ROT IS FALSE") return False def rotateSmoothlyToDegs(self, degs, orientation="closest", maxSpeed=0.5): accuracy = 2 seqRotateToDegsinitialDiff = round(self.rotation - degs) diff = self.rotation - degs moveDiff = max(round(self.rotation), degs) - min(self.rotation, degs) if diff > 180 or diff < -180: moveDiff = 360 - moveDiff speedFract = min(mapVals(moveDiff, accuracy, 90, 0.2, 0.8), maxSpeed) if accuracy * -1 < diff < accuracy or 360 - accuracy < diff < 360 + accuracy: self.rotateToDegsFirstTime = True return True else: if orientation == "closest": if 180 > seqRotateToDegsinitialDiff > 0 or seqRotateToDegsinitialDiff < -180: direction = "right" else: direction = "left" elif orientation == "farthest": if 180 > seqRotateToDegsinitialDiff > 0 or seqRotateToDegsinitialDiff < -180: direction = "left" else: direction = "right" else: direction = orientation if direction == "right": self.moveWheels(speedFract * -0.5, speedFract) elif direction == "left": self.moveWheels(speedFract, speedFract * -0.5) #print("speed fract: " + str(speedFract)) #print("target angle: " + str(degs)) #print("moveDiff: " + str(moveDiff)) #print("diff: " + str(diff)) #print("orientation: " + str(orientation)) #print("direction: " + str(direction)) #print("initialDiff: " + str(seqRotateToDegsinitialDiff)) #print("ROT IS FALSE") return False def moveToCoords(self, targetPos): errorMargin = 0.01 descelerationStart = 0.5 * 0.12 diffX = targetPos[0] - self.globalPosition[0] diffY = targetPos[1] - self.globalPosition[1] #print("Target Pos: ", targetPos) #print("Used global Pos: ", self.globalPosition) #print("diff in pos: " + str(diffX) + " , " + str(diffY)) dist = getDistance((diffX, diffY)) #print("Dist: "+ str(dist)) if errorMargin * -1 < dist < errorMargin: #self.robot.move(0,0) #print("FinisehedMove") return True else: ang = getDegsFromCoords((diffX, diffY)) ang = normalizeDegs(ang) #print("traget ang: " + str(ang)) ratio = min(mapVals(dist, 0, descelerationStart, 0.1, 1), 1) ratio = max(ratio, 0.8) if self.rotateToDegs(ang): self.moveWheels(ratio, ratio) #print("Moving") return False # Gets a point cloud with all the detections from lidar and distance sensorss def getDetectionPointCloud(self): rawPointCloud = self.lidar.getPointCloud(layers=(2, 3)) processedPointCloud = [] for point in rawPointCloud: procPoint = [ point[0] + self.globalPosition[0], point[1] + self.globalPosition[1] ] #procPoint = [procPoint[0] + procPoint[0] * 0.1, procPoint[1] + procPoint[1] * 0.1] processedPointCloud.append(procPoint) return processedPointCloud def getColorDetection(self): pos = self.colorSensor.getPosition(self.globalPosition, self.rotation) detection = self.colorSensor.getTileType() return pos, detection def trapsAtSides(self): sides = [] if self.leftGroundSensor.isFar(): sides.append(self.leftGroundSensor.position) if self.rightGroundSensor.isFar(): sides.append(self.rightGroundSensor.position) return sides # Returns True if the simulation is running def doLoop(self): return self.robot.step(self.timeStep) != -1 def getWheelDirection(self): if self.rightWheel.velocity + self.leftWheel.velocity == 0: return 0 return (self.rightWheel.velocity + self.leftWheel.velocity) / 2 # Must run every TimeStep def update(self): # Updates the current time self.time = self.robot.getTime() # Updates the gps, gyroscope self.gps.update() self.gyroscope.update(self.time) # Gets global position self.prevGlobalPosition = self.globalPosition self.globalPosition = self.gps.getPosition() self.globalPosition[0] += self.positionOffsets[0] self.globalPosition[1] += self.positionOffsets[1] if self.gyroscope.getDiff() < 0.00001 and self.getWheelDirection( ) >= 0: self.rotationDetectionType = "gps" else: self.rotationDetectionType = "gyroscope" self.prevRotation = self.rotation # Gets global rotation if self.__useGyroForRotation: self.rotation = self.gyroscope.getDegrees() print("USING GYRO") else: print("USING GPS") val = self.gps.getRotation() if val is not None: self.rotation = val self.gyroscope.setDegrees(self.rotation) # Sets lidar rotation self.lidar.setRotationDegrees(self.rotation + 0) self.rightGroundSensor.setPosition(self.globalPosition, self.rotation) self.leftGroundSensor.setPosition(self.globalPosition, self.rotation) self.comunicator.update()
# Devices robot = Robot() # --Motors # left_wheel = robot.getDevice("left_wheel") # right_wheel = robot.getDevice("right_wheel") # left_door = robot.getDevice("left_door") # right_door = robot.getDevice("right_door") # --Sensors # ds_top = robot.getDevice('ds_short') # ds_bottom = robot.getDevice('ds_long') # ls_red = robot.getDevice('ls_red') # ls_green = robot.getDevice('ls_green') # gps = robot.getDevice("gps") # compass = robot.getDevice("compass") emitter = robot.getDevice("emitter") receiver = robot.getDevice("receiver") # Initial setup # --Set maximum velocities # left_wheel.setVelocity(MAX_SPEED) # right_wheel.setVelocity(MAX_SPEED) # --Enable sensors # ds_top.enable(TIME_STEP) # ds_bottom.enable(TIME_STEP) # ls_red.enable(TIME_STEP) # ls_green.enable(TIME_STEP) # gps.enable(TIME_STEP) # compass.enable(TIME_STEP) receiver.enable(TIME_STEP)
pose_x = 1 pose_y = 1 pose_theta = 0 #Controls the state machine for the epuck state = "seek" # ePuck Constants EPUCK_AXLE_DIAMETER = 0.053 # ePuck's wheels are 53mm apart. MAX_SPEED = 6.28 # create the Robot instance. robot = Robot() # Initialize Motors leftMotor = robot.getDevice('left wheel motor') rightMotor = robot.getDevice('right wheel motor') leftMotor.setPosition(float('inf')) rightMotor.setPosition(float('inf')) leftMotor.setVelocity(0.0) rightMotor.setVelocity(0.0) led = robot.getDevice('led0') emitter = robot.getDevice('emitter1') # get the time step of the current world. SIM_TIMESTEP = int(robot.getBasicTimeStep()) # get and enable lidar lidar = robot.getDevice("LDS-01") lidar.enable(SIM_TIMESTEP)
signum = lambda x: -1 if x < 0 else 1 # create the Robot instance. robot = Robot() # get the time step of the current world. timestep = int(robot.getBasicTimeStep()) # The left knee joint is not being actuated currently # leftKneeMotor = robot.getDevice('left_knee_motor') # leftKneeMotor.enableTorqueFeedback(timestep) # leftEncoder = robot.getDevice('left_knee_sensor') # leftEncoder.enable(timestep) # Commands for the right knee joint rightKneeMotor = robot.getDevice('right_knee_motor') rightKneeMotor.enableTorqueFeedback(timestep) rightEncoder = robot.getDevice('right_knee_sensor') rightEncoder.enable(timestep) counter = -100 direction = -1 stepSize = constants.MAX_STEP_SIZE tolerance = constants.MAX_TOLERANCE lowerLimit = 0 upperLimit = -round(pi/2, constants.PRECISION) setPoint = upperLimit tracking = pd.DataFrame([], columns=[ "right_knee_angular_position",
from controller import Robot TIME_STEP = 64 robot = Robot() receiver = robot.getDevice("receiver") receiver.enable(TIME_STEP) left_motor = robot.getDevice("left wheel motor") right_motor = robot.getDevice("right wheel motor") left_motor.setPosition(float('+inf')) right_motor.setPosition(float('+inf')) left_motor.setVelocity(0.0) right_motor.setVelocity(0.0) while robot.step(TIME_STEP) != -1: if receiver.getQueueLength() > 0: packet = receiver.getData() receiver.nextPacket()