right_ir = robot.getDistanceSensor('ir1')
right_ir.enable(time_step)
while robot.step(time_step) != -1:
left_ir_value = left_ir.getValue()
right_ir_value = right_ir.getValue()
print("left: {} right: {}".format(left_ir_value, right_ir_value))
left_speed = max_speed
right_speed = max_speed
if (left_ir_value > right_ir_value) and (6 < left_ir_value < 15):
print("GoLeft")
left_speed = -max_speed
elif (right_ir_value > left_ir_value) and (6 < right_ir_value < 15):
print("Go Right")
right_speed = -max_speed
left_motor.setVelocity(left_speed)
right_motor.setVelocity(right_speed)
if __name__ == "__main__":
my_Robot = Robot()
run_robot(my_Robot)
def __init__(self):
self.robot = Robot()
self.timeStep = int(4 * self.robot.getBasicTimeStep())
self.keyboard = self.robot.getKeyboard()
self.init_servos()
self.init_positional_sensors()
back_left_speed = -max_speed
back_right_speed = max_speed
print("Turn Left")
#if all four sensor reads black then stop
elif( left_ir_value >999 ) and ( right_ir_value>999 ) and ( middle_ir_value >999 )and ( stop_ir_value>999 ) :
left_speed = max_speed*0
right_speed = max_speed*0
back_left_speed = max_speed*0
back_right_speed = max_speed*0
print("Stop")
#Passing values to motors
left_motor.setVelocity(left_speed)
right_motor.setVelocity(right_speed)
back_left_motor.setVelocity(back_left_speed)
back_right_motor.setVelocity(back_right_speed)
# Enter here exit cleanup code.
if __name__ == "__main__":
my_robot = Robot()
run_robot(my_robot)
#Jai hind Dosto
def __init__(self):
print('Initializing robot...')
# Create the Robot instance.
self.robot = Robot()
# distance sensors
self.ps = []
ps_names = [
'ps0', 'ps1', 'ps2', 'ps3',
'ps4', 'ps5', 'ps6', 'ps7'
]
for i in range(8):
self.ps.append(self.robot.getDistanceSensor(ps_names[i]))
self.ps[i].enable(TIME_STEP)
self.leds = []
led_names = [
'led0', 'led1', 'led2', 'led3', 'led4',
'led5', 'led6', 'led7', 'led8', 'led9'
]
for i in range(10):
self.leds.append(self.robot.getLED(led_names[i]))
self.compass = self.robot.getCompass('compass')
self.compass.enable(TIME_STEP)
self.leftMotor = self.robot.getMotor('left wheel motor')
self.rightMotor = self.robot.getMotor('right wheel motor')
self.left_encoder = self.robot.getPositionSensor('left wheel sensor')
self.right_encoder = self.robot.getPositionSensor('right wheel sensor')
self.left_encoder.enable(TIME_STEP)
self.right_encoder.enable(TIME_STEP)
self.leftMotor.setPosition(float('inf'))
self.rightMotor.setPosition(float('inf'))
self.leftMotor.setVelocity(0.0)
self.rightMotor.setVelocity(0.0)
self.touch_sensor = self.robot.getTouchSensor('touch sensor')
self.touch_sensor.enable(TIME_STEP)
self.left_vel = None
self.right_vel = None
# Current absolute direction
self.facing = None
self.facing_angle = None
self.turning = None
# start values for encoder tracking
self.left_pos_start = None
self.right_pos_start = None
# Led for search animation
self.search_led = 0
self.led_timer = 0
self.initialize_sensors()
# Current movement state
self.state = None
self.best_path = None
self.path_logger = PathLogger()
if use_file and run == 3:
self.path_logger.load(last_best_path)
self.best_path = iter(self.path_logger)
# Load initial samples in each sensor window
self.ps_windows = []
for i in range(0, 8):
self.ps_windows.append(list(repeat(self.ps[i].getValue(), WINDOW_SIZE)))
""" maze_mapper controller. """ import copy import pickle import math import numpy as np import cv2 from controller import Robot, Motor, DistanceSensor """ Initial setup up and global variables. """ robot = Robot() # initialize the robot state = 'get_target' sub_state = 'bearing' LIDAR_SENSOR_MAX_RANGE = 0.25 # Meters LIDAR_ANGLE_BINS = 21 # 21 Bins to cover the angular range of the lidar, centered at 10 LIDAR_ANGLE_RANGE = 3.14159 # 180 degrees, 3.14159 radians # These are your pose values that you will update by solving the odometry equations pose_x = None pose_y = None pose_theta = None # get the time step of the current world. SIM_TIMESTEP = int(robot.getBasicTimeStep()) # Initialize Motors
from RL_brain import DeepQNetwork
from controller import Robot
import numpy as np
import sys
import rospy
from std_msgs.msg import Bool
from geometry_msgs.msg import Vector3
import time
import tensorflow
front = "Front"
back = "Back"
# prepare for controllers
robot1 = Robot(front)
robot2 = Robot(back)
class Policy:
def __init__(self):
# define publisher to control start or stop vrep
self.pub_start_signal = rospy.Publisher("/startSimulation",
Bool,
queue_size=1)
self.pub_stop_signal = rospy.Publisher("/stopSimulation",
Bool,
queue_size=1)
# maybe start the simulation with hand would be a good way
time.sleep(2)
class Vehicle:
stage = 0
MAX_SPEED = 2
robot = Robot()
motors = []
armMotors = []
armSensors = []
armPs = []
camera = Camera('camera')
compass = Compass('compass')
ds = robot.getDistanceSensor('distance sensor')
ds2 = robot.getDistanceSensor('distance sensor2')
HEIGHT = camera.getHeight()
WIDTH = camera.getWidth()
foodStage = 0
p0 = WIDTH / 2
leftSpeed = 0.0
rightSpeed = 0.0
leftSum = 0
rightSum = 0
HALF = int(WIDTH / 2)
frame = np.zeros((HEIGHT, WIDTH))
blur_im = np.zeros((HEIGHT, WIDTH))
position = WIDTH / 2
def __init__(self):
#get motors, camera and initialise them.
motorNames = ['left motor', 'right motor',
'tower rotational motor'] #, 'trigger motor']
for i in range(3):
self.motors.append(self.robot.getMotor(motorNames[i]))
self.motors[i].setPosition(float('inf'))
if i <= 1:
self.motors[i].setVelocity(0)
else:
self.motors[i].setVelocity(0.8)
self.motors[i].setPosition(0)
armMotorNames = [
"arm_motor_1", "arm_motor_2", "arm_motor_3", "arm_motor_4",
"arm_motor_5", "arm_motor_6", "arm_motor_7", "arm_motor_8"
]
for i in range(len(armMotorNames)):
self.armMotors.append(self.robot.getMotor(armMotorNames[i]))
self.armMotors[i].setPosition(0)
self.armMotors[i].setVelocity(0.3)
armSensorNames = [
"arm_position_sensor_1", "arm_position_sensor_2",
"arm_position_sensor_3", "arm_position_sensor_4",
"arm_position_sensor_5", "arm_position_sensor_6",
"arm_position_sensor_7", "arm_position_sensor_8"
]
for i in range(len(armSensorNames)):
self.armSensors.append(
self.robot.getPositionSensor(armSensorNames[i]))
self.armSensors[i].enable(int(self.robot.getBasicTimeStep()))
self.camera.enable(int(self.robot.getBasicTimeStep()))
self.compass.enable(int(self.robot.getBasicTimeStep()))
self.ds.enable(int(self.robot.getBasicTimeStep()))
self.ds2.enable(int(self.robot.getBasicTimeStep()))
def getStage(self):
return self.stage
def setStage(self, stage_num):
self.stage = stage_num
def getCompass(self):
return np.angle(
complex(self.compass.getValues()[0],
self.compass.getValues()[2]))
def getDistanceValue(self, input_ds=1):
if input_ds == 1:
return self.ds.getValue()
if input_ds == 2:
return self.ds2.getValue()
def towerSeeLeft(self):
self.motors[2].setPosition(np.pi / 2)
@staticmethod
def towerSeeRight():
Vehicle.motors[2].setPosition(-np.pi / 2)
def towerRestore(self):
self.motors[2].setPosition(0)
def pickFood(self):
if self.foodStage == 0:
self.armMotors[0].setPosition(0.039) #upper-arm extend
self.armMotors[1].setPosition(0.93) #upper-arm rotate
if (self.armSensors[1].getValue() > 0.92):
self.armMotors[2].setPosition(0.038) #mid-arm extend
#print(sensor3.getValue())
if (self.armSensors[2].getValue() > 0.0379):
self.armMotors[4].setPosition(0.005) #grab the box
self.armMotors[5].setPosition(0.005)
if ((self.armSensors[4].getValue() >= 0.00269)
or (self.armSensors[5].getValue() >= 0.0031)):
self.foodStage = 1
elif self.foodStage == 1:
self.armMotors[2].setPosition(0) #raise the box
self.armMotors[2].setVelocity(.1)
self.armMotors[1].setPosition(0)
self.armMotors[6].setPosition(0.9)
if (self.armSensors[1].getValue() < 0.002):
self.armMotors[0].setPosition(0)
self.armMotors[0].setVelocity(0.01) #grabbing task finished
if (self.armSensors[0].getValue() < 0.003):
return True
return False
def releaseFood(self):
if (self.armSensors[6].getValue() > 0.899):
self.armMotors[3].setPosition(0.06)
self.armMotors[7].setPosition(0.06)
if ((self.armSensors[3].getValue() > 0.059)
and (self.armSensors[7].getValue() > 0.059)):
self.armMotors[4].setPosition(0)
self.armMotors[5].setPosition(0)
if ((self.armSensors[4].getValue() <= 0.00269)
or (self.armSensors[5].getValue() >= 0.0031)):
self.armMotors[3].setPosition(0)
self.armMotors[7].setPosition(0)
self.armMotors[6].setPosition(0)
return True
#Img Processing related
@staticmethod
def get_frame():
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
# get image and process it
frame = np.zeros((HEIGHT, WIDTH))
for x in range(0, Vehicle.WIDTH):
for y in range(0, HEIGHT):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
frame[y][x] = gray
return frame
@staticmethod
def edge_detect(frame,
threshold=254,
maxVal=255,
kernel_1=20,
kernel_2=15):
# threshold
# Edge_LineTracking_T1:254; Edge_Bridge_detection_T3:180; Edge_Arch_detection_T4:100;Edge_Color_detect_T5:254
# maxVal
# Edge_LineTracking_T1:255; Edge_Bridge_detection_T3:255; Edge_Arch_detection_T4:255;Edge_Color_detect_T5:255
# kernel_1
# Edge_LineTracking_T1:20; Edge_Bridge_detection_T3:20; Edge_Arch_detection_T4:20;Edge_Color_detect_T5:20
# kernel_2
# Edge_LineTracking_T1:15; Edge_Bridge_detection_T3:15; Edge_Arch_detection_T4:15;Edge_Color_detect_T5:15
# edge_detect Sobel
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absx = cv2.convertScaleAbs(x)
absy = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absx, 0.5, absy, 0.5, 0)
#cv2.imwrite("dst.jpg", dst)
# to binary
ret, binary = cv2.threshold(dst, threshold, maxVal, cv2.THRESH_BINARY)
#cv2.imwrite("binary.jpg", binary)
# Smooth
kernel1 = np.ones((kernel_1, kernel_1), np.float) / 25
smooth = cv2.filter2D(binary, -1, kernel1)
# blur
blur_im = cv2.boxFilter(smooth, -1, (kernel_2, kernel_2), normalize=1)
#cv2.imwrite("blur.jpg", blur_im)
return blur_im
@staticmethod
def positioning(blur_im,
type_select=0,
height_per_1=0.7,
height_per_2=0.8):
# height_per_1
# Edge_LineTracking_T1:0.7; Edge_Bridge_detection_T3:0.3; Edge_Arch_detection_T4:0.3;Edge_Color_detect_T5:0.6
# height_per_2
# Edge_LineTracking_T1:0.8; Edge_Bridge_detection_T3:0.6; Edge_Arch_detection_T4:0.8;Edge_Color_detect_T5:0.7
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
HALF = Vehicle.HALF
axis = 0
num = 0
position = 0
axis_l = 0
axis_r = 0
num_l = 0
num_r = 0
position_l = 0
position_r = WIDTH - 1
if type_select == 0:
for axis_v in range(int(HEIGHT * height_per_1),
int(HEIGHT * height_per_2)):
for axis_h in range(WIDTH * 0, WIDTH):
if blur_im[axis_v][axis_h] != 0:
axis = axis + axis_h
num = num + 1
if num:
position = axis / num + 1
elif type_select == 1:
for axis_v in range(int(HEIGHT * height_per_1),
int(HEIGHT * height_per_2)):
for axis_h in range(WIDTH * 0, HALF):
if blur_im[axis_v][axis_h] != 0:
axis_l = axis_l + axis_h
num_l = num_l + 1
if blur_im[axis_v][axis_h + HALF] != 0:
axis_r = axis_r + axis_h + HALF
num_r = num_r + 1
if num_l:
position_l = axis_l / num_l + 1
if num_r:
position_r = axis_r / num_r + 1
position = (position_l + position_r) / 2
else:
print('INPUT ERROR!:positioning:type_select')
return position
@staticmethod
def steering(position,
type_select=0,
rectify_pixel=0,
base_speed=3.0,
straight_speed=2.0):
# rectify_pixel
# Edge_LineTracking_T1:7; Edge_Bridge_detection_T3:5; Edge_Arch_detection_T4:5;Edge_Color_detect_T5:3
# base_speed
# Edge_LineTracking_T1:3; Edge_Bridge_detection_T3:5; Edge_Arch_detection_T4:5;Edge_Color_detect_T5:20
# straight_speed
# Edge_LineTracking_T1:2; Edge_Bridge_detection_T3:3; Edge_Arch_detection_T4:2;Edge_Color_detect_T5:2
WIDTH = Vehicle.WIDTH
if type_select == 0:
if abs(position - WIDTH / 2) > rectify_pixel:
leftSpeed = (position / WIDTH) * base_speed
rightSpeed = (1 - position / WIDTH) * base_speed
else:
leftSpeed = straight_speed
rightSpeed = straight_speed
elif type_select == 1:
if abs(position - WIDTH / 2) > rectify_pixel:
leftSpeed = (position / WIDTH - 0.5) * base_speed
rightSpeed = (0.5 - position / WIDTH) * base_speed
else:
leftSpeed = straight_speed
rightSpeed = straight_speed
else:
print('INPUT ERROR!:steering:type_select')
return leftSpeed, rightSpeed
#Speed setting functions
def setSpeed(self, left, right):
#set speed for four tracks
self.motors[0].setVelocity(left)
self.motors[1].setVelocity(right)
def turnRound(self, diff):
#set speed for turning left or right
if abs(diff) < 0.001:
self.setSpeed(0, 0)
return False
else:
self.setSpeed(-0.3 * diff, 0.3 * diff)
return True
def linePatrol(self):
#get camera image, process it and set speed based on line patrolling algorithm
#return False if there is no line
image = self.camera.getImage()
leftSum = 0
rightSum = 0
leftSpeed = 0
rightSpeed = 0
cameraData = self.camera.getImage()
HEIGHT = self.camera.getHeight()
WIDTH = self.camera.getWidth()
frame = np.zeros((HEIGHT, WIDTH))
for x in range(0, HEIGHT):
for y in range(0, WIDTH):
frame[y][x] = int(
self.camera.imageGetGray(cameraData, WIDTH, x, y))
absX = cv2.convertScaleAbs(cv2.Sobel(frame, cv2.CV_16S, 1, 0))
absY = cv2.convertScaleAbs(cv2.Sobel(frame, cv2.CV_16S, 0, 1))
# to binary
ret, binary = cv2.threshold(cv2.addWeighted(absX, 0.5, absY, 0.5, 0),
254, 255, cv2.THRESH_BINARY)
binaryImg = cv2.boxFilter(cv2.filter2D(
binary, -1,
np.ones((20, 20), np.float) / 25),
-1, (15, 15),
normalize=1)
positionSum = 0
positionCount = 0
for i in range(int(HEIGHT * 0.75), HEIGHT):
for j in range(WIDTH * 0, WIDTH):
if binaryImg[i][j] != 0:
positionSum += j
positionCount += 1
farpositionSum = 0
farpositionCount = 0
for i in range(int(HEIGHT * 0.32), int(HEIGHT * 0.44)):
for j in range(WIDTH * 0, WIDTH):
if binaryImg[i][j] != 0:
farpositionSum += j
farpositionCount += 1
if farpositionCount == 0:
farcenter = 0
else:
farcenter = farpositionSum / farpositionCount
if abs(farcenter - WIDTH / 2) < 0.1:
self.motors[0].setAcceleration(0.3)
self.motors[1].setAcceleration(0.3)
#("straight")
else:
self.motors[0].setAcceleration(20)
self.motors[1].setAcceleration(20)
#print("not straight")
if positionCount or farpositionCount:
if positionCount == 0:
center = 80
else:
center = positionSum / positionCount
diff = 4 * (0.5 - center / WIDTH)
if diff > 0.01:
leftSpeed = (1 - 0.6 * diff) * self.MAX_SPEED
rightSpeed = (1 - 0.3 * diff) * self.MAX_SPEED
elif diff < -0.01:
leftSpeed = (1 + 0.3 * diff) * self.MAX_SPEED
rightSpeed = (1 + 0.6 * diff) * self.MAX_SPEED
else:
leftSpeed = self.MAX_SPEED
rightSpeed = self.MAX_SPEED
self.setSpeed(leftSpeed, rightSpeed)
return True
else:
return False
@staticmethod
def linePatrol2():
# Task 1
# get_frame
frame = Vehicle.get_frame()
# edge_detect
blur_im = Vehicle.edge_detect(frame, 100, 255, 20, 15)
# positioning
position = Vehicle.positioning(blur_im, 0, 0.7, 0.8)
# steering
leftSpeed, rightSpeed = Vehicle.steering(position, 0, 4, 0.8, 0.8)
return leftSpeed, rightSpeed
@staticmethod
def box_found():
# Task 2
HALF = Vehicle.HALF
frame = Vehicle.get_frame()
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absx = cv2.convertScaleAbs(x)
absy = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absx, 0.5, absy, 0.5, 0)
cv2.imwrite("dst.jpg", dst)
# to binary
ret, filtered = cv2.threshold(dst, 50, 255, cv2.THRESH_TOZERO)
cv2.imwrite("binary.jpg", filtered)
position_i = Vehicle.positioning(filtered, 0, 0.32, 0.35)
if abs(position_i - HALF) < 10:
position = Vehicle.positioning(filtered, 0, 0.25, 0.3)
if abs(
position - HALF
) < 10: # this is a predict letting it stop exactly at the box center
return True
else:
pass
@staticmethod
def bridge_found():
# Task 3
HALF = Vehicle.HALF
WIDTH = Vehicle.WIDTH
HEIGHT = Vehicle.HEIGHT
count = 0
num1 = 0
num = 0
axis = WIDTH
position_i = 0
frame = Vehicle.get_frame()
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absx = cv2.convertScaleAbs(x)
absy = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absx, 0.5, absy, 0.5, 0)
# cv2.imwrite("dst.jpg", dst)
# to binary
ret, filtered = cv2.threshold(dst, 50, 255, cv2.THRESH_TOZERO)
# cv2.imwrite("filtered.jpg", filtered)
for axis_v in range(int(HEIGHT * 0.8), int(HEIGHT * 0.87)):
for axis_h in range(HALF, WIDTH):
if filtered[axis_v][axis_h] != 0:
axis = axis + axis_h
num1 = num1 + 1
if num1:
position_i = axis / num1 + 1
if abs(position_i - 1.5 * HALF) < 10:
# print('pi = ', position_i)
for axis_v in range(int(HEIGHT * 0.82), int(HEIGHT * 0.85)):
for axis_h in range(WIDTH * 0, int(WIDTH * 1)):
if filtered[axis_v][axis_h] != 0:
axis_i = axis_h
axis = min(axis_i, axis)
# print('axis = ', axis)
if abs(axis - 0.5 * WIDTH) < 5:
return True
else:
return False
def arch_found(self, a, b):
# Task 4
global count_arch
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
frame = np.zeros((HEIGHT, WIDTH))
Q = 0
position = 0
for x in range(0, WIDTH):
for y in range(0, HEIGHT):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
if 110 < gray < 130:
frame[y][x] = 255
Q = Q + 1
else:
pass
#print(i)
# cv2.imwrite('frame.jpg', frame)
#blur_im = self.edge_detect(frame, 254, 255)
#cv2.imwrite('blur.jpg', blur_im)
if Q > 50:
position = self.positioning(frame, 0, a, b)
#print(i, position)
if count_arch == 0:
if int(position) < WIDTH / 2:
count_arch = 1
for i in range(2):
self.motors[i].setVelocity(0)
return 1
else:
if int(position) > WIDTH / 2:
for i in range(2):
self.motors[i].setVelocity(0)
return 1
else:
return 0
else:
return 0
def count(self):
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
i = 0
for x in range(0, WIDTH):
for y in range(0, HEIGHT):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
if 110 < gray < 130:
i = i + 1
else:
pass
#print(i)
return i
@staticmethod
def colourPatrol():
# Task 5
HEIGHT = Vehicle.HEIGHT
WIDTH = Vehicle.WIDTH
camera = Vehicle.camera
cameraData = camera.getImage()
frame = np.zeros((HEIGHT, WIDTH))
color_kernel = np.zeros((3, 3))
compass = Vehicle.getCompass(Vehicle)
position = HEIGHT / 2
leftSpeed = 0
rightSpeed = 0
global flag
if (flag == -1) and (3.12 < abs(compass) < 3.15):
for x in range(0, 3):
for y in range(0, 3):
gray = int(
camera.imageGetGray(cameraData, WIDTH,
x + int(0.5 * WIDTH),
y + int(0.80 * HEIGHT)))
color_kernel[y][x] = gray
gray_average = np.mean(color_kernel)
# color classification
if 100 < gray_average < 130:
flag = 0 # red
elif 175 < gray_average < 200:
flag = 1 # yellow
elif 140 < gray_average < 170:
flag = 2 # purple
else:
pass
# print('color flag=', flag)
leftSpeed = 2.0
rightSpeed = 2.0
elif flag == 0 or flag == 1 or flag == 2:
for x in range(0, 3):
for y in range(0, 3):
gray = int(
camera.imageGetGray(cameraData, WIDTH,
x + int(0.5 * WIDTH),
y + int(0.95 * HEIGHT)))
color_kernel[y][x] = gray
gray_average = np.mean(color_kernel)
if 90 < gray_average < 100:
flag = 3 # finished
for y in range(0, HEIGHT):
for x in range(0, WIDTH):
gray = int(camera.imageGetGray(cameraData, WIDTH, x, y))
# color classification
if flag == 0:
if 100 < gray < 130:
frame[y][x] = 255
else:
pass
elif flag == 1:
if 175 < gray < 200:
frame[y][x] = 255
else:
pass
elif flag == 2:
if 140 < gray < 170:
frame[y][x] = 255
else:
pass
# edge_detect Sobel
x = cv2.Sobel(frame, cv2.CV_16S, 1, 0)
y = cv2.Sobel(frame, cv2.CV_16S, 0, 1)
absX = cv2.convertScaleAbs(x)
absY = cv2.convertScaleAbs(y)
dst = cv2.addWeighted(absX, 0.5, absY, 0.5, 0)
# to binary
ret, binary = cv2.threshold(dst, 254, 255, cv2.THRESH_BINARY)
# Smooth
kernel1 = np.ones((20, 20), np.float) / 25
smooth = cv2.filter2D(binary, -1, kernel1)
# blur
blur_im1 = cv2.boxFilter(smooth, -1, (15, 15), normalize=1)
# cv2.imwrite('smooth_blur.jpg', blur_im1)
# cv2.imwrite('gray.jpg', frame)
# cv2.imwrite('frame.jpg', frame)
# video.write(blur_im1)
# Locate
axis = 0
num = 0
for axis_v in range(int(HEIGHT * 0.8), int(HEIGHT * 0.9)):
for axis_h in range(WIDTH * 0, WIDTH):
if blur_im1[axis_v][axis_h] != 0:
axis = axis + axis_h
num = num + 1
if num:
position = axis / num + 1
# Steering
if 5 <= abs(position - WIDTH / 2):
leftSpeed = (position / WIDTH) * 1.5
rightSpeed = (1 - position / WIDTH) * 1.5
elif 2.5 < abs(position - WIDTH / 2) < 5:
leftSpeed = (position / WIDTH) * 0.9
rightSpeed = (1 - position / WIDTH) * 0.9
# if lines are losted in the view of camera, try adjust parameters here
else:
leftSpeed = 1
rightSpeed = 1
else:
pass
return leftSpeed, rightSpeed
"""twolink controller."""
from controller import Robot
from controller import Motor
from controller import PositionSensor
from controller import InertialUnit
import math
#create a robot instace
arm = Robot()
target1 = 0
target2 = 0
#target2=0
#kp=50
#kd=5
count = 0
r = 0
timestep = int(arm.getBasicTimeStep())
dt = 0.001 * timestep
#print(timestep)
logfile_name = "twolink_pd.log"
#initialise every component
motor1 = arm.getMotor("rm@flt")
motor2 = arm.getMotor("rm@flk")
motor3 = arm.getMotor("rm@blt")
motor4 = arm.getMotor("rm@blk")
motor5 = arm.getMotor("rm@frt")
motor6 = arm.getMotor("rm@frk")
motor7 = arm.getMotor("rm@brt")
motor8 = arm.getMotor("rm@brk")
ps1 = arm.getPositionSensor("ps@flt")
def __init__(self):
# Initialize TurtleBot specifics
self.maxAngSpeed = 1.5 # 2.84 max
self.maxLinSpeed = 0.1 # 0.22 max
# Initialize reward parameters:
self.moveTowardsParam = 1
self.moveAwayParam = 1.1
self.safetyLimit = 0.75
self.obsProximityParam = 1
self.EOEPunish = -300
self.EOEReward = 600
# Total reward counter for each component
self.moveTowardGoalTotalReward = 0
self.obsProximityTotalPunish = 0
# Initialize RL specific variables
self.rewardInterval = 1 # How often do you want to get rewards
self.epConc = '' # Conculsion of episode [Collision, Success, TimeOut]
self.reward = 0 # Reward gained in the timestep
self.totalreward = 0 # Reward gained throughout the episode
self.counter = 0 # Timestep counter
self.needReset = True #
self.state = [] # State of the environment
self.done = False # If the episode is done
self.action = [0, 0] # Current action chosen
self.prevAction = [0, 0] # Past action chosen
self.goal = [] # Goal
self.goals = [x / 100 for x in range(-450, 451, 150)]
#self.goals = [[0.72, 6.78],[5.03, 6.78],[12.7, 6.78], [10.9, -1.68], [3, 0],[-11.5, -2.5]]
self.seeded = False
self.dist = 0 # Distance to the goal
self.prevDist = 0 # Previous distance to the goal
# Logging variables
self.currentEpisode = 0 # Current episode number
self.start = 0 # Timer for starting
self.duration = 0 # Episode duration in seconds
self.epInfo = [] # Logging info for the episode
self.startPosition = []
self.prevMax = 0 # temporary variable
# Initialize a supervisor
self.supervisor = Robot()
# Initialize robot
self.robot = self.supervisor.getFromDef('TB')
self.translationField = self.robot.getField('translation')
self.orientationField = self.robot.getField('rotation')
# Initialize lidar
self.lidar = self.supervisor.getLidar('LDS-01')
self.lidarDiscretization = 30
self.lidar.enable(self.timeStep)
self.lidarRanges = []
# Initialize Motors
self.motors.append(self.supervisor.getMotor("right wheel motor"))
self.motors.append(self.supervisor.getMotor("left wheel motor"))
self.motors[0].setPosition(float("inf"))
self.motors[1].setPosition(float("inf"))
self.motors[0].setVelocity(0.0)
self.motors[1].setVelocity(0.0)
self.direction = []
self.position = []
self.orientation = []
# Initialize Goal Object
self.goalObject = self.supervisor.getFromDef('GOAL')
self.goalTranslationField = self.goalObject.getField('translation')
# Initialize action-space and observation-space
self.action_space = spaces.Box(low=np.array(
[-self.maxLinSpeed, -self.maxAngSpeed]),
high=np.array([0, self.maxAngSpeed]),
dtype=np.float32)
#TODO: MAKE POSTIVE LINEAR SPEED A FORWARD MOTION
self.observation_space = spaces.Box(low=-10,
high=10,
shape=(14, ),
dtype=np.float16)
def __init__(self):
self.robot = Robot()
def get_robot():
return Robot()
from controller import Robot
TIME_STEP = 1000
robot = Robot() #Creación del robot
ds = [] #Sensores de distancia
dsNames = ['ds_right', 'ds_left']
for i in range(2): #inicializar sensores
ds.append(robot.getDistanceSensor(dsNames[i]))
ds[i].enable(TIME_STEP)
wheels = [] # inicializar llantas
wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4']
for i in range(4):
wheels.append(robot.getMotor(wheelsNames[i]))
wheels[i].setPosition(float('inf'))
wheels[i].setVelocity(0.0)
avoidObstacleCounter = 0
while robot.step(TIME_STEP) != -1:
leftSpeed = 1.0 # [rad/s]
rightSpeed = 1.0 # [rad/s]
if avoidObstacleCounter > 0: #girar
avoidObstacleCounter -= 1
leftSpeed = 1.0 # [rad/s]
rightSpeed = -1.0 # [rad/s]
else: # Leer sensores
for i in range(2):
if ds[i].getValue() < 950.0: #Distancia max
avoidObstacleCounter = 100
wheels[0].setVelocity(leftSpeed)
wheels[1].setVelocity(rightSpeed)
wheels[2].setVelocity(leftSpeed)
wheels[3].setVelocity(rightSpeed)
def main():
global robot, _timestep, _pop, _gbest, _w, _wdamp, _c1, _c2, iter
ph = 0.0
chi = 0.0
outbuffer = [0.0 for i in range(0, 3)]
# setting parameters for PSO
_w = _IW
_wdamp = _IW_Damp
_c1 = _CT1
_c2 = _CT2
# If you would like to use Constriction Coefficients for PSO,
# uncomment the following block and comment the above set of parameters.
# Constriction Coefficients
# -------------------------------------------------------
ph = _PH1 + _PH2 # was _PH1+_PH1
chi = 2.0 / (ph - 2 + math.sqrt(ph * ph - 4 * ph)
) # velocity constrain factor
_w = chi # inertia Weight
_wdamp = 1 # inertia Weight Damping Ratio
_c1 = chi * _PH1 # personal Learning Coefficient
_c2 = chi * _PH2 # global Learning Coefficient was _c1=chi*_PH2
# -------------------------------------------------------
# create the Robot instance.
robot = Robot()
# get the time step of the current world.
timestep = int(robot.getBasicTimeStep())
_timestep = timestep
# 每16个基本步算一步
InitRobot()
# Main loop:
InitSwarm()
Report(0)
for iter in range(1, _MaxIter + 1): # 开始 实验 _MaxIter 次看看会不会成功
for i in range(1,
_SwarmSize + 1): # 我们每一次Iter 其实都有_SwarmSize 个 particle
_pop[i].update_velocity()
# 公式15
_pop[i].update_position()
# 公式16
# Evaluation
_pop[i].fitness = Evaluation(_pop[i])
# evaluate 获取fitness
# Update Global Best
if _pop[i].fitness < _gbest.fitness:
_gbest.fitness = _pop[i].fitness
for j in range(1, _LenChrom + 1):
_gbest.pos[j] = _pop[i].pos[j]
else: #如果globle fitness没有变化或者还变差了，那就用firework炸一波
# 用FA更新swarm
FA_results = FWA(_pop, 5)
for i in range(len(_pop) - 1):
_pop[i + 1].FA_pop(FA_results['fireworks'][i],
FA_results['new_fitness'][i])
_gbest.fitness = _pop[1].fitness
for j in range(1, _LenChrom + 1):
_gbest.pos[j] = _pop[1].pos[j]
# update inertia weight with damping ratio
_w = _w * _wdamp
# update inertia weight with damping ratio
# save and report information of gbest
Report(iter)
# If the robot is capable of following the boundary more than one round,
# then PSO terminates.
if _gbest.fitness <= 1.0 / _MaxTimestep:
iter = _MaxIter + 1
outbuffer[0] = 100
# terminating simulation command
outbuffer[1] = _gbest.fitness
# fitness value
robot_send_data(outbuffer, 2)
break
# loop for iteration ends
# reload the environment.
outbuffer[0] = -1
# reset command
outbuffer[1] = 0
# fitness value
robot_send_data(outbuffer, 2)
return 0
from controller import Gyro
from controller import Motor
from controller import GPS
from controller import Camera
from controller import CameraRecognitionObject
import math
#Function definitions
def clamp(n, smallest, largest):
return max(smallest, min(n, largest))
# create the Robot instance.
agro_drone = Robot()
# get the time step of the current world.
timestep = int(agro_drone.getBasicTimeStep())
camera = agro_drone.getCamera("camera")
# Initialise devices
imu = agro_drone.getInertialUnit('inertial unit')
imu.enable(timestep)
gps = agro_drone.getGPS('gps')
gps.enable(timestep)
gyro = agro_drone.getGyro('gyro')
gyro.enable(timestep)
#returns list in string form with number rounded to 2 decimals
def roundedList(myList):
string = "[ "
for i in myList:
string += str(round(i, 2)) + ", "
string += "]"
return string
recognitionColor = [
0, 0, 0
] #Recognition Color of a human (the object we are looking for)
timeStep = 32
myRobot = Robot()
camera = myRobot.getCamera("camera") #Creates Camera object called "camera"
#Use this object to call camera related methods
camera.recognitionEnable(
timeStep) #recognitionEnable allows camera to detect Recognition Colors
while myRobot.step(timeStep) != -1:
objects = camera.getRecognitionObjects(
) #returns list of all objects in camera's view with a Recognition Color (walls, humans, etc.)
for obj in objects:
if obj.get_colors(
) == recognitionColor: #checks if Recognition Color of obj matches Recognition Color of a human
print("Recognized object!")
print(
"Position: " + roundedList(obj.get_position())
def main():
robot = Robot()
assert robot.getSynchronization()
name = robot.getName()
channel = grpc.insecure_channel(BROKER_ADDRESS)
stub = wb_controller_pb2_grpc.WbControllerStub(channel)
sendQueue = Queue(32)
handshakeMsg = wb_controller_pb2.WbControllerMessage.ClientMessage()
handshakeMsg.wb_controller_handshake.robot_name = name
# TODO: fill robot_info
sendQueue.put(handshakeMsg)
timestep = float('NaN')
isIdle = True
ticker = None
syncChannel = None
try:
call = stub.Session(iter(sendQueue.get, None), wait_for_ready=True)
def cancel():
nonlocal call
nonlocal sendQueue
call.cancel()
sendQueue.put(None)
for serverMsg in call:
if timestep != timestep:
# handshake response not received (timestep is nan)
if serverMsg.HasField('wb_controller_handshake_response'):
if serverMsg.wb_controller_handshake_response \
.HasField('error'):
raise RuntimeError('Failed to handshake: {}'.format(
serverMsg.wb_controller_handshake_response.error))
print('Robot connected to broker')
timestep = \
serverMsg.wb_controller_handshake_response.ok.timestep
isIdle = True
ticker = WbtIdleTicker(robot, doneFunc=cancel)
ticker.start()
else:
if serverMsg.HasField('ping'):
nonce = serverMsg.ping.nonce
pong = \
wb_controller_pb2.WbControllerMessage.ClientMessage()
pong.pong.nonce = nonce
sendQueue.put(pong)
if isIdle:
if serverMsg.HasField('wb_controller_bound'):
ticker.isRunning = False
ticker.join()
isIdle = False
isSync = serverMsg.wb_controller_bound.is_sync
syncChannel = Queue() if isSync else None
ticker = WbtTicker(robot,
timestep,
dataChannel=sendQueue,
syncChannel=syncChannel,
doneFunc=cancel)
ticker.start()
print('Robot bound')
else:
if serverMsg.HasField('wb_controller_unbound'):
ticker.isRunning = False
if syncChannel is not None:
syncChannel.put(True)
ticker.join()
print('Robot unbound')
isIdle = True
ticker = WbtIdleTicker(robot, doneFunc=cancel)
ticker.start()
if serverMsg.HasField('commands'):
applyCommands(robot, serverMsg.commands)
if syncChannel is not None:
syncChannel.put(True)
finally:
if ticker is not None:
ticker.isRunning = False
ticker.join()
# Import Webots libraries from controller import Robot import numpy as np import pickle # Import evalution functions from eval import showPlots # Import functions from other scripts in controller folder from lqr_controller import LQRController from adaptive_controller import AdaptiveController #from lqr_controller import CustomController # Instantiate dron driver supervisor driver = Robot() # Get the time step of the current world timestep = int(driver.getBasicTimeStep()) # Set your percent loss of thrust lossOfThust = 0.5 # Instantiate controller and start sensors customController = AdaptiveController(driver, lossOfThust) customController.initializeMotors() customController.startSensors(timestep) # Initialize state storage vectors stateHistory = [] referenceHistory = []
def __init__(self,
ros_active=False,
robot='wolfgang',
do_ros_init=True,
robot_node=None,
base_ns='',
recognize=False,
camera_active=True,
foot_sensors_active=True):
"""
The RobotController, a Webots controller that controls a single robot.
The environment variable WEBOTS_ROBOT_NAME should be set to "amy", "rory", "jack" or "donna" if used with
4_bots.wbt or to "amy" if used with 1_bot.wbt.
:param ros_active: Whether ROS messages should be published
:param robot: The name of the robot to use, currently one of wolfgang, darwin, nao, op3
:param do_ros_init: Whether to call rospy.init_node (only used when ros_active is True)
:param external_controller: Whether an external controller is used, necessary for RobotSupervisorController
:param base_ns: The namespace of this node, can normally be left empty
"""
self.ros_active = ros_active
self.recognize = recognize
self.camera_active = camera_active
self.foot_sensors_active = foot_sensors_active
if robot_node is None:
self.robot_node = Robot()
else:
self.robot_node = robot_node
self.walkready = [0] * 20
self.time = 0
self.motors = []
self.sensors = []
# for direct access
self.motors_dict = {}
self.sensors_dict = {}
self.timestep = int(self.robot_node.getBasicTimeStep())
self.switch_coordinate_system = True
self.is_wolfgang = False
self.pressure_sensors = None
if robot == 'wolfgang':
self.is_wolfgang = True
self.proto_motor_names = [
"RShoulderPitch [shoulder]", "LShoulderPitch [shoulder]",
"RShoulderRoll", "LShoulderRoll", "RElbow", "LElbow",
"RHipYaw", "LHipYaw", "RHipRoll [hip]", "LHipRoll [hip]",
"RHipPitch", "LHipPitch", "RKnee", "LKnee", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadPan",
"HeadTilt"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.initial_joint_positions = {
"LAnklePitch": -30,
"LAnkleRoll": 0,
"LHipPitch": 30,
"LHipRoll": 0,
"LHipYaw": 0,
"LKnee": 60,
"RAnklePitch": 30,
"RAnkleRoll": 0,
"RHipPitch": -30,
"RHipRoll": 0,
"RHipYaw": 0,
"RKnee": -60,
"LShoulderPitch": 75,
"LShoulderRoll": 0,
"LElbow": 36,
"RShoulderPitch": -75,
"RShoulderRoll": 0,
"RElbow": -36,
"HeadPan": 0,
"HeadTilt": 0
}
self.sensor_suffix = "_sensor"
accel_name = "imu accelerometer"
gyro_name = "imu gyro"
camera_name = "camera"
self.pressure_sensor_names = []
if self.foot_sensors_active:
self.pressure_sensor_names = [
"llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"
]
self.pressure_sensors = []
for name in self.pressure_sensor_names:
sensor = self.robot_node.getDevice(name)
sensor.enable(self.timestep)
self.pressure_sensors.append(sensor)
elif robot == 'darwin':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
elif robot == 'nao':
self.proto_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch",
"LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch",
"LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw",
"HeadPitch"
]
self.external_motor_names = self.proto_motor_names
self.sensor_suffix = "S"
accel_name = "accelerometer"
gyro_name = "gyro"
camera_name = "CameraTop"
self.switch_coordinate_system = False
elif robot == 'op3':
self.proto_motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll",
"r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll",
"l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee",
"r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll",
"head_pan", "head_tilt"
]
self.sensor_suffix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
self.switch_coordinate_system = False
self.motor_names_to_external_names = {}
self.external_motor_names_to_motor_names = {}
for i in range(len(self.proto_motor_names)):
self.motor_names_to_external_names[
self.proto_motor_names[i]] = self.external_motor_names[i]
self.external_motor_names_to_motor_names[
self.external_motor_names[i]] = self.proto_motor_names[i]
self.current_positions = {}
self.joint_limits = {}
for motor_name in self.proto_motor_names:
motor = self.robot_node.getDevice(motor_name)
motor.enableTorqueFeedback(self.timestep)
self.motors.append(motor)
self.motors_dict[
self.motor_names_to_external_names[motor_name]] = motor
sensor = self.robot_node.getDevice(motor_name + self.sensor_suffix)
sensor.enable(self.timestep)
self.sensors.append(sensor)
self.sensors_dict[
self.motor_names_to_external_names[motor_name]] = sensor
self.current_positions[self.motor_names_to_external_names[
motor_name]] = sensor.getValue()
# min, max and middle position (precomputed since it will be used at each step)
self.joint_limits[
self.motor_names_to_external_names[motor_name]] = (
motor.getMinPosition(), motor.getMaxPosition(),
0.5 * (motor.getMinPosition() + motor.getMaxPosition()))
self.accel = self.robot_node.getDevice(accel_name)
self.accel.enable(self.timestep)
self.gyro = self.robot_node.getDevice(gyro_name)
self.gyro.enable(self.timestep)
if self.is_wolfgang:
self.accel_head = self.robot_node.getDevice(
"imu_head accelerometer")
self.accel_head.enable(self.timestep)
self.gyro_head = self.robot_node.getDevice("imu_head gyro")
self.gyro_head.enable(self.timestep)
self.camera = self.robot_node.getDevice(camera_name)
self.camera_counter = 0
if self.camera_active:
self.camera.enable(self.timestep * CAMERA_DIVIDER)
if self.recognize:
self.camera.recognitionEnable(self.timestep)
self.last_img_saved = 0.0
self.img_save_dir = "/tmp/webots/images" + \
time.strftime("%Y-%m-%d-%H-%M-%S") + \
os.getenv('WEBOTS_ROBOT_NAME')
if not os.path.exists(self.img_save_dir):
os.makedirs(self.img_save_dir)
self.imu_frame = rospy.get_param("~imu_frame", "imu_frame")
if self.ros_active:
if base_ns == "":
clock_topic = "/clock"
else:
clock_topic = base_ns + "clock"
if do_ros_init:
rospy.init_node("webots_ros_interface",
argv=['clock:=' + clock_topic])
self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole")
self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole")
self.camera_optical_frame = rospy.get_param(
"~camera_optical_frame", "camera_optical_frame")
self.head_imu_frame = rospy.get_param("~head_imu_frame",
"imu_frame_2")
self.pub_js = rospy.Publisher(base_ns + "joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher(base_ns + "imu/data_raw",
Imu,
queue_size=1)
self.pub_imu_head = rospy.Publisher(base_ns + "imu_head/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher(base_ns + "camera/image_proc",
Image,
queue_size=1)
self.pub_cam_info = rospy.Publisher(base_ns + "camera/camera_info",
CameraInfo,
queue_size=1,
latch=True)
self.pub_pres_left = rospy.Publisher(base_ns +
"foot_pressure_left/raw",
FootPressure,
queue_size=1)
self.pub_pres_right = rospy.Publisher(base_ns +
"foot_pressure_right/raw",
FootPressure,
queue_size=1)
self.cop_l_pub_ = rospy.Publisher(base_ns + "cop_l",
PointStamped,
queue_size=1)
self.cop_r_pub_ = rospy.Publisher(base_ns + "cop_r",
PointStamped,
queue_size=1)
rospy.Subscriber(base_ns + "DynamixelController/command",
JointCommand, self.command_cb)
# publish camera info once, it will be latched
self.cam_info = CameraInfo()
self.cam_info.header.stamp = rospy.Time.from_seconds(self.time)
self.cam_info.header.frame_id = self.camera_optical_frame
self.cam_info.height = self.camera.getHeight()
self.cam_info.width = self.camera.getWidth()
f_y = self.mat_from_fov_and_resolution(
self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height,
self.cam_info.width), self.cam_info.height)
f_x = self.mat_from_fov_and_resolution(self.camera.getFov(),
self.cam_info.width)
self.cam_info.K = [
f_x, 0, self.cam_info.width / 2, 0, f_y,
self.cam_info.height / 2, 0, 0, 1
]
self.cam_info.P = [
f_x, 0, self.cam_info.width / 2, 0, 0, f_y,
self.cam_info.height / 2, 0, 0, 0, 1, 0
]
self.pub_cam_info.publish(self.cam_info)
if robot == "op3":
# start pose
command = JointCommand()
command.joint_names = ["r_sho_roll", "l_sho_roll"]
command.positions = [-math.tau / 8, math.tau / 8]
self.command_cb(command)
# needed to run this one time to initialize current position, otherwise velocity will be nan
self.get_joint_values(self.external_motor_names)
"""pioneer_test controller"""
from controller import Robot, Motor, Lidar
import time
import pandas as pd
TIME_STEP = 32
robot = Robot() #Pioneer 3
#inicializar os dispositivos
lidar = robot.getLidar(
"Sick LMS 291") #Alcance de 80 metros e pega os dados em 180 graus
lidar.enable(
TIME_STEP) #O Lidar mede informações em metros da renderização do sensor
lidar.enablePointCloud()
left_wheel = robot.getMotor("left wheel")
left_wheel.setPosition(float('inf'))
left_wheel.setVelocity(3.0)
right_wheel = robot.getMotor("right wheel")
right_wheel.setPosition(float('inf'))
right_wheel.setVelocity(3.0)
robot.step(TIME_STEP)
rangeImageComplete = []
#retorna -1 quando o Webots finalizar o controlador
'--recognize',
action='store_true',
help="if true, recognition is active (for training data collection)")
args, unknown = parser.parse_known_args()
rospy.init_node("webots_ros_interface", argv=['clock:=/clock'])
pid_param_name = "/webots_pid" + args.sim_id
while not rospy.has_param(pid_param_name):
rospy.logdebug("Waiting for parameter " + pid_param_name + " to be set..")
time.sleep(2.0)
webots_pid = rospy.get_param(pid_param_name)
os.environ["WEBOTS_PID"] = str(webots_pid)
os.environ["WEBOTS_ROBOT_NAME"] = args.robot_name
if args.void_controller:
rospy.logdebug("Starting void interface for " + args.robot_name)
from controller import Robot
r = Robot()
while not rospy.is_shutdown():
r.step(int(r.getBasicTimeStep()))
else:
rospy.logdebug("Starting ros interface for " + args.robot_name)
r = RobotController(ros_active=True,
do_ros_init=False,
recognize=args.recognize,
camera_active=(not args.disable_camera))
while not rospy.is_shutdown():
r.step()
#########################################
##
## CONSTANT DECLARATION
##
#########################################
##VALUES DEFINED BELOW SHOULD ONLY BE ALTERED UNDER THE FOLLOWING CONDITIONS:
##
## 1. There are physical design changes to the machine, such as wheel
## positioning.
## 2. Testing is performed and determines other default values are more
## practical.
##Global objects for the LIDAR sensor and PWM driver breakout board.
ROBOT = Robot()
SENSOR = ROBOT.getDevice('lidar')
WHEELS = [ROBOT.getDevice('wheel1_joint'), ROBOT.getDevice('wheel2_joint'),
ROBOT.getDevice('wheel0_joint')]
##Any distance measurements are in inches.
##Any angular measurements are in degrees.
##File directories for indication sounds used throughout the project.
BEGIN_SOUND = "sounds/begin.wav"
FIN_SOUND = "sounds/finished.wav"
BLOKD_SOUND = "sounds/blocked.wav"
STNDBY_SOUND = "sounds/standby.wav"
##Time delay time used before starting and exiting the program.
SLEEP_TIME = 1.33
def __init__(self, ros_active=False, robot='wolfgang', do_ros_init=True):
# requires WEBOTS_ROBOT_NAME to be set to "amy" or "rory"
self.ros_active = ros_active
self.robot_node = Robot()
self.walkready = [0] * 20
self.time = 0
self.motors = []
self.sensors = []
self.timestep = int(self.robot_node.getBasicTimeStep())
self.switch_coordinate_system = True
self.is_wolfgang = False
self.pressure_sensors = None
if robot == 'wolfgang':
self.is_wolfgang = True
self.motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
self.external_motor_names = self.motor_names
sensor_postfix = "_sensor"
accel_name = "imu accelerometer"
gyro_name = "imu gyro"
camera_name = "camera"
pressure_sensor_names = [
"llb", "llf", "lrf", "lrb", "rlb", "rlf", "rrf", "rrb"
]
self.pressure_sensors = []
for name in pressure_sensor_names:
sensor = self.robot_node.getDevice(name)
sensor.enable(30)
self.pressure_sensors.append(sensor)
elif robot == 'darwin':
self.motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbow", "LElbow", "RHipYaw", "LHipYaw",
"RHipRoll", "LHipRoll", "RHipPitch", "LHipPitch", "RKnee",
"LKnee", "RAnklePitch", "LAnklePitch", "RAnkleRoll",
"LAnkleRoll", "HeadPan", "HeadTilt"
]
sensor_postfix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
elif robot == 'nao':
self.motor_names = [
"RShoulderPitch", "LShoulderPitch", "RShoulderRoll",
"LShoulderRoll", "RElbowYaw", "LElbowYaw", "RHipYawPitch",
"LHipYawPitch", "RHipRoll", "LHipRoll", "RHipPitch",
"LHipPitch", "RKneePitch", "LKneePitch", "RAnklePitch",
"LAnklePitch", "RAnkleRoll", "LAnkleRoll", "HeadYaw",
"HeadPitch"
]
self.external_motor_names = self.motor_names
sensor_postfix = "S"
accel_name = "accelerometer"
gyro_name = "gyro"
camera_name = "CameraTop"
self.switch_coordinate_system = False
elif robot == 'op3':
self.motor_names = [
"ShoulderR", "ShoulderL", "ArmUpperR", "ArmUpperL",
"ArmLowerR", "ArmLowerL", "PelvYR", "PelvYL", "PelvR", "PelvL",
"LegUpperR", "LegUpperL", "LegLowerR", "LegLowerL", "AnkleR",
"AnkleL", "FootR", "FootL", "Neck", "Head"
]
self.external_motor_names = [
"r_sho_pitch", "l_sho_pitch", "r_sho_roll", "l_sho_roll",
"r_el", "l_el", "r_hip_yaw", "l_hip_yaw", "r_hip_roll",
"l_hip_roll", "r_hip_pitch", "l_hip_pitch", "r_knee", "l_knee",
"r_ank_pitch", "l_ank_pitch", "r_ank_roll", "l_ank_roll",
"head_pan", "head_tilt"
]
sensor_postfix = "S"
accel_name = "Accelerometer"
gyro_name = "Gyro"
camera_name = "Camera"
self.switch_coordinate_system = False
# self.robot_node = self.supervisor.getFromDef(self.robot_node_name)
for motor_name in self.motor_names:
self.motors.append(self.robot_node.getDevice(motor_name))
self.motors[-1].enableTorqueFeedback(self.timestep)
self.sensors.append(
self.robot_node.getDevice(motor_name + sensor_postfix))
self.sensors[-1].enable(self.timestep)
self.accel = self.robot_node.getDevice(accel_name)
self.accel.enable(self.timestep)
self.gyro = self.robot_node.getDevice(gyro_name)
self.gyro.enable(self.timestep)
if self.is_wolfgang:
self.accel_head = self.robot_node.getDevice(
"imu_head accelerometer")
self.accel_head.enable(self.timestep)
self.gyro_head = self.robot_node.getDevice("imu_head gyro")
self.gyro_head.enable(self.timestep)
self.camera = self.robot_node.getDevice(camera_name)
self.camera.enable(self.timestep)
if self.ros_active:
if do_ros_init:
rospy.init_node("webots_ros_interface", argv=['clock:=/clock'])
self.l_sole_frame = rospy.get_param("~l_sole_frame", "l_sole")
self.r_sole_frame = rospy.get_param("~r_sole_frame", "r_sole")
self.camera_optical_frame = rospy.get_param(
"~camera_optical_frame", "camera_optical_frame")
self.head_imu_frame = rospy.get_param("~head_imu_frame",
"imu_frame_2")
self.imu_frame = rospy.get_param("~imu_frame", "imu_frame")
self.pub_js = rospy.Publisher("joint_states",
JointState,
queue_size=1)
self.pub_imu = rospy.Publisher("imu/data_raw", Imu, queue_size=1)
self.pub_imu_head = rospy.Publisher("imu_head/data",
Imu,
queue_size=1)
self.pub_cam = rospy.Publisher("camera/image_proc",
Image,
queue_size=1)
self.pub_cam_info = rospy.Publisher("camera/camera_info",
CameraInfo,
queue_size=1,
latch=True)
self.pub_pres_left = rospy.Publisher("foot_pressure_left/filtered",
FootPressure,
queue_size=1)
self.pub_pres_right = rospy.Publisher(
"foot_pressure_right/filtered", FootPressure, queue_size=1)
self.cop_l_pub_ = rospy.Publisher("cop_l",
PointStamped,
queue_size=1)
self.cop_r_pub_ = rospy.Publisher("cop_r",
PointStamped,
queue_size=1)
rospy.Subscriber("DynamixelController/command", JointCommand,
self.command_cb)
# publish camera info once, it will be latched
self.cam_info = CameraInfo()
self.cam_info.header.stamp = rospy.Time.from_seconds(self.time)
self.cam_info.header.frame_id = self.camera_optical_frame
self.cam_info.height = self.camera.getHeight()
self.cam_info.width = self.camera.getWidth()
f_y = self.mat_from_fov_and_resolution(
self.h_fov_to_v_fov(self.camera.getFov(), self.cam_info.height,
self.cam_info.width), self.cam_info.height)
f_x = self.mat_from_fov_and_resolution(self.camera.getFov(),
self.cam_info.width)
self.cam_info.K = [
f_x, 0, self.cam_info.width / 2, 0, f_y, self.cam_info.height / 2,
0, 0, 1
]
self.cam_info.P = [
f_x, 0, self.cam_info.width / 2, 0, 0, f_y,
self.cam_info.height / 2, 0, 0, 0, 1, 0
]
if self.ros_active:
self.pub_cam_info.publish(self.cam_info)
from skimage.draw import line, rectangle, rectangle_perimeter, ellipse, polygon
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)
def setup():
robot = Robot()
beacons = gen_beacons()
emitters_list = set_beacons(robot, beacons)
return robot, emitters_list
from controller import Robot, Motor, Keyboard
from pathfinding import Pathfind
robot = Robot()
timestep = int(robot.getBasicTimeStep())
keyboard = Keyboard()
keyboard.enable(timestep)
# ----- Inicializa os motores -----
motor_l = robot.getMotor('left wheel motor')
motor_r = robot.getMotor('right wheel motor')
motor_l.setPosition(float('inf'))
motor_r.setPosition(float('inf'))
motor_l.setVelocity(0.0)
motor_r.setVelocity(0.0)
# ----- Funções de controle do robo -----
# Funções criadas de forma empírica
def RobotTurnRight():
motor_r.setVelocity(-2.197)
motor_l.setVelocity(2.197)
robot.step(1000)
motor_l.setVelocity(0.0)
robot.step(timestep)
def RobotTurnLeft():
from controller import Robot, Motor, DistanceSensor
from ikpy.chain import Chain
import numpy as np
import time
from controller import Camera, Device
robot = Robot("E-puck")
