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()
