def robotCode(self):
self.path = np.array([[0.2, 0.2, -0.2, -0.2], [0.4, -0.4, -0.4, 0.4],
[15, 15, 15, 15]])
# dash = IDash(framerate=0.1)
# plt.plot(self.path[0,:], self.path[1,:])
# dash.add(plt)
#k=0.0345 # ball model: d(t) = vmot*k*(1-exp(-t/T))
goal = (0.0, 0.0)
self.path, status = passPath(self.getRobotConf(self.bot),
self.ballEngine.getBallPose(),
goal,
kick=True)
print self.path
dash = IDash(framerate=0.1)
dash.add(lambda: plt.plot(-self.path[1, :], self.path[0, :], 'b-*') and
plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
dash.plotframe()
print 'estimated time path'
print calculatePathTime(self.path)
t = self.ballEngine.getSimTime() # time in seconds
# while (self.ballEngine.getSimTime()-t)<10: #End program after 30sec
cc = 1
while cc:
robotConf = self.getRobotConf(self.bot)
cc = self.followPath(robotConf, status, rb=0.05)
self.ballEngine.update()
nextBallPos = self.ballEngine.getNextRestPos()
# robotConf = self.getRobotConf(self.bot)
# ballPos = self.ballEngine.getBallPose() # (x, y)
# vRobot = v2Pos(robotConf, ballPos)
self.setMotorVelocities(0, 0)
print 'real time path'
print(self.ballEngine.getSimTime() - t)
def robotCode(self):
self.path = np.array([[0.2, 0.2, -0.2, -0.2],
[0.4, -0.4, -0.4, 0.4],
[15, 15, 15, 15]])
# dash = IDash(framerate=0.1)
# plt.plot(self.path[0,:], self.path[1,:])
# dash.add(plt)
#k=0.0345 # ball model: d(t) = vmot*k*(1-exp(-t/T))
goal = (0.0, 0.0)
self.path, status = passPath(self.getRobotConf(self.bot), self.ballEngine.getBallPose(), goal, kick=True)
print self.path
dash = IDash(framerate=0.1)
dash.add(lambda: plt.plot(-self.path[1,:], self.path[0,:], 'b-*') and
plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
dash.plotframe()
print 'estimated time path'
print calculatePathTime(self.path)
t = self.ballEngine.getSimTime() # time in seconds
# while (self.ballEngine.getSimTime()-t)<10: #End program after 30sec
cc=1
while cc:
robotConf = self.getRobotConf(self.bot)
cc = self.followPath(robotConf, status, rb=0.05)
self.ballEngine.update()
nextBallPos = self.ballEngine.getNextRestPos()
# robotConf = self.getRobotConf(self.bot)
# ballPos = self.ballEngine.getBallPose() # (x, y)
# vRobot = v2Pos(robotConf, ballPos)
self.setMotorVelocities(0,0)
print 'real time path'
print (self.ballEngine.getSimTime()-t)
#SOM visualization of MNIST digits
image_template = np.zeros((28,28))
map_image = np.tile(image_template, map_shape)
map_image_w = map_image.copy()
lattice_predictions = np.zeros(map_size)
for node_count in range(map_size):
m_row, n_col = idx1D_to_idx2D(node_count, map_shape)
closest_x_idx = np.argmin(cdist(w[:,node_count].reshape(1, -1), trX))
image_of_closest_x = trX[closest_x_idx].reshape((28,28))
lattice_predictions[node_count] = trY[closest_x_idx] # prediction mapping
image_of_w = w[:,node_count].reshape((28,28))
map_image[m_row*28:(m_row+1)*28, n_col*28:(n_col+1)*28] = image_of_closest_x
map_image_w[m_row*28:(m_row+1)*28, n_col*28:(n_col+1)*28] = image_of_w
dash.add(lambda: plt.imshow(map_image, cmap='gray'))
dash.add(lambda: plt.imshow(map_image_w, cmap='gray'))
# predictions
def predict(X, w, lattice_predictions):
n_examples = X.shape[0]
Ypred = np.zeros(n_examples)
for row in range(n_examples):
closest_w_idx = np.argmin(cdist(X[row].reshape(1,-1), w.T))
Ypred[row] = lattice_predictions[closest_w_idx]
return Ypred
trYpred = predict(trX, w, lattice_predictions)
teYpred = predict(teX, w, lattice_predictions)
# evaluate
train_acc = accuracy_score(trY, trYpred)
class MyRobotRunner(base_robot.BaseRobotRunner):
def __init__(self, *args, **kwargs):
super(MyRobotRunner, self).__init__(*args, **kwargs)
self.idash = IDash(0.005)
def robotCode(self):
#waypoint = [(0.2, 0.4), (0, 0.4), (-0.2, 0.4),(-0.2, 0.2), (-0.2, 0),(0, 0), (0.2, 0),(0.2, -0.2), (0.2, -0.4),(0, -0.4), (-0.2, -0.4)]
waypoint = [(0, 0.4), (-0.2, 0.4),(-0.2, 0.2), (-0.2, 0),(0, 0), (0.2, 0),(0.2, -0.2), (0.2, -0.4),
(0, -0.4),(-0.05, -0.4), (-0.1, -0.4), (-0.15, -0.4), (-0.2, -0.4), (-0.2, -0.4)]
# waypoint = []
# r = 0.2
# increments = 5
# xCenter = 0
# yCenter = 0.2
# for i in range(increments-1):
# # circle center: (0, 0.2)
# x = r*math.sin(i*math.pi/increments) + xCenter
# y = r*math.cos(i*math.pi/increments) + yCenter
# waypoint.append((x,y))
# xCenter = 0
# yCenter = -0.2
# for i in range(increments):
# # circle center: (0, -0.2)
# x = -r*math.sin(i*math.pi/increments) + xCenter
# y = r*math.cos(i*math.pi/increments) + yCenter
# waypoint.append((x,y))
# for i in range(3):
# waypoint.append((-0.2/4*i,-0.4))
#k=0.0345 # ball model: d(t) = vmot*k*(1-exp(-t/T))
for i in range(len(waypoint)):
p0 = self.ballEngine.getBallPose()
curr_bot_pose = list(self.getRobotConf(self.bot))
curr_bot_pose[1] -= 0.05
self.path, status = passPath(curr_bot_pose, p0, waypoint[i],vmax=10,vr=7, kq= 0.0035)
print self.path
# dash = IDash(framerate=0.1)
# dash.add(lambda: plt.plot(-self.path[1,:], self.path[0,:], 'b-*') and
# plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
# dash.plotframe()
print 'estimated time path'
# print calculatePathTime(self.path)
t = self.ballEngine.getSimTime()
t = self.ballEngine.getSimTime() # time in seconds
while (self.ballEngine.getSimTime()-t)<10: #End program after 30sec
# cc=1
# while cc:
def vizBots():
actx, acty = self.getRobotConf()[:2]
ballx, bally = self.ballEngine.getBallPose()[:2]
plt.hold('on')
plt.plot(-waypoint[i][1], waypoint[i][0], 'k*')
plt.plot(-bally, ballx, 'b.')
plt.plot(-acty, actx, 'g+')
plt.plot(-self.path[1,:], self.path[0,:], 'r.')
plt.xlim([-0.9, 0.9]) # y axis in the field
plt.ylim([-0.65, 0.65]) # x axis in the field
plt.xlabel('active path length: {}'.format(self.path.shape[1]))
self.idash.add(vizBots)
robotConf = self.getRobotConf(self.bot)
cc = self.followPath(robotConf, rb=0.05)
self.ballEngine.update()
p1 = self.ballEngine.getBallPose()
p3 = self.ballEngine.getNextRestPos()
dist_from_start = np.sqrt((p1[0] - p0[0])**2 + (p1[1] - p0[1])**2)
velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
#self.idash.plotframe()
if dist_from_start > 0.01: # the ball has been touched
if velocity_measure < 0.003:
break# wait til velocity reaches zero
# robotConf = self.getRobotConf(self.bot)
# ballPos = self.ballEngine.getBallPose() # (x, y)
# vRobot = v2Pos(robotConf, ballPos)
self.setMotorVelocities(0,0)
print 'real time path'
print (self.ballEngine.getSimTime()-t)
class Lab2Program:
def __init__(self):
self.initialize_vrep_client()
self.initilize_vrep_api()
self.define_constants()
self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initialize_vrep_client(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID!=-1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initilize_vrep_api(self):
# initialize ePuck handles and variables
_, self.bodyelements=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
_, self.ePuck=vrep.simxGetObjectHandle(
self.clientID, 'ePuck', vrep.simx_opmode_oneshot_wait)
_, self.ePuckBase=vrep.simxGetObjectHandle(
self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of ePuck
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
# initialize overhead cam
_, self.overheadCam=vrep.simxGetObjectHandle(
self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
_, self.resolution, self.image = vrep.simxGetVisionSensorImage(
self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# initialize goal handle + odom
_, self.goalHandle=vrep.simxGetObjectHandle(
self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
_, self.goalPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# STOP Motor Velocities. Not sure if this is necessary
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,0,vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,0,vrep.simx_opmode_oneshot_wait)
def define_constants(self):
self.map_side_length = 2.55
# FIMXE: hard coded goals
# self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22), (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)]
self.GOALS = None
self.worldNorthTheta = None
self.maxVelocity = 2.0
self.history_length = 5
self.theta_history = RingBuffer(self.history_length)
self.e_theta_h = RingBuffer(self.history_length)
self.blurred_paths = None
self.path_skip = 8
def global_map_preprocess(self, resolution, image):
im = format_vrep_image(resolution, image) # original image
im = image_vert_flip(im)
resize_length = int(im.shape[0]/2)
im = skimage.transform.resize(im, (resize_length, resize_length, 3))
return im
def global_map_process(self, im):
walls = im[:,:,0] > 0.25
paths = walls < 0.15
if self.blurred_paths is None:
print "Computed"
blurred_map = skimage.filters.gaussian_filter(walls, sigma=2)
blurred_paths = blurred_map < 0.15
# np.save('binary_grid.npy', blurred_paths)
return paths, blurred_paths
else:
return paths, self.blurred_paths
def robot_pose_get(self):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
self.theta_history.append(theta)
return (x, y, theta)
def lidar_scan_get(self, window_size=21):
""" gets lidar scan range values """
fr = (window_size - 1) / 2 # fire range
lidarValues = pseudoLidarSensor(self.paths, self.pose[0], self.pose[1], fr, original_four=False)
return lidarValues
def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
numLidarValues = len(lidarValues)
lidarAngles = [np.pi / numLidarValues * index for index in range(numLidarValues)]
repulsionVectors = [np.array(pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle)) for val, angle in zip(lidarValues, lidarAngles)]
return repulsionVectors
def attraction_vector_compute(self, k_attract=100.0):
self.attractionVal, self.attractionAngle = cart2pol(
self.goal_pose_pixel[1] - self.pose_pixel[1], # cols counts same to normal horz axes
self.pose_pixel[0] - self.goal_pose_pixel[0] # rows counts opposite to normal vert axes
)
attractionVector = np.array(pol2cart(k_attract*self.attractionVal, self.attractionAngle))
return attractionVector
def robot_code(self):
t = time.time()
self.curr_goal = 0
while (time.time() - t) < 200:
# global map
_,resolution,image = vrep.simxGetVisionSensorImage(self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait) # Get image
im = self.global_map_preprocess(resolution, image)
self.pose = self.robot_pose_get()
x, y, theta = self.pose
# theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x))
# initialize worldNorthTheta for the first time
if self.worldNorthTheta is None:
self.worldNorthTheta = self.pose[2]
_ = [self.theta_history.append(self.pose[2]) for _ in range(self.history_length)]
self.pose_pixel = np.array(odom2pixelmap(x, y, self.map_side_length, im.shape[0]))
m, n = self.pose_pixel
self.paths, self.blurred_paths = self.global_map_process(im)
# calculate intermediate goals once
if self.GOALS is None:
raw_input("")
# acquire pixel location of goal
_, finishPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_buffer)
self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1], self.map_side_length, im.shape[0])
contiguousPath = AStarBinaryGrid(self.blurred_paths).calculate_path(self.pose_pixel, self.finish_pixel)
self.GOALS = contiguousPath[::self.path_skip]
# SKIP THIS FIRST LOOP AND CONTINUE
continue
else:
self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal])
goal_m, goal_n = self.goal_pose_pixel
lidarValues = self.lidar_scan_get(window_size=21)
#############################
# Potential Field Algorithm #
#############################
repulsionVectors = self.repulsion_vectors_compute(lidarValues, k_repulse=10.0)
attractionVector = self.attraction_vector_compute(k_attract=100.0)
finalVector = np.sum(np.vstack((repulsionVectors, attractionVector)), axis=0)
finalUnitVector = finalVector / np.linalg.norm(finalVector)
print "finalUnitVector: ", finalUnitVector
# TODO: do we use the unit vector (for direction) only, or the finalVector?
finalValue, finalAngle = cart2pol(finalUnitVector[0], finalUnitVector[1])
error_theta = angle_diff(mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta)
self.e_theta_h.append(error_theta)
k_angular_p = 2.0 * self.maxVelocity
k_angular_D = 0.25
k_angular_S = 1.0
k_angular_I = 2.5
omega = k_angular_p * (
error_theta
+ k_angular_D / k_angular_S * (self.e_theta_h[-1] - self.e_theta_h[-2])
+ k_angular_S / k_angular_I * sum(self.e_theta_h)
)
print "Omega: ", round(omega,1)
#############################
# StateController Algorithm #
#############################
# if desired heading is not directly in front
if np.abs(error_theta) > np.pi / 3:
# turn in place
forward_vel = self.maxVelocity
# omega *= 0.25
else:
# Direct yourself to the goal
goal_distance = np.linalg.norm(self.goal_pose_pixel - self.pose_pixel)
print "distance_from_goal: ", goal_distance
if goal_distance <= 4:
# Achieved Goal!
forward_vel = self.maxVelocity * 0.25 # Slow down to prepare for the next one
self.curr_goal += 1
else:
forward_vel = self.maxVelocity
# control the motors
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
self.idash.add(lambda: self.plot_maze(self.blurred_paths*1.0, m, n, goal_m, goal_n))
self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n))
def plot_current_and_desired_heading():
self.plot_unit_quiver(finalUnitVector, 'r')
self.plot_unit_quiver(pol2cart(1, worldTheta2mapTheta(theta, self.worldNorthTheta)), 'k')
plt.title("Error Theta: %f" % error_theta)
self.idash.add(plot_current_and_desired_heading)
self.idash.add(self.plot_theta_history)
self.idash.plotframe()
if self.killer.kill_now:
self.clean_exit()
def plot_maze(self, im, m, n, goal_m, goal_n):
""" plots the maze, with robot pose and goal pose visualized """
if len(im.shape) == 3:
goal_pixel_values = np.array((1.0, 1.0, 1.0))
robot_pixel_values = np.array((255.0/255.0,192/255.0,203/255.0))
elif len(im.shape) == 2:
goal_pixel_values = 0.5
robot_pixel_values = 0.5
im[goal_m,goal_n] = goal_pixel_values
im[m,n] = robot_pixel_values
plt.imshow(im)
def plot_unit_quiver(self, vector, color):
X, Y = (0, 0)
U, V = (vector[0], vector[1])
plt.quiver(X,Y,U,V,angles='xy',scale_units='xy',scale=1,color=color)
plt.xlim([-1.1,1.1])
plt.ylim([-1.1,1.1])
def plot_theta_history(self, expansion=5):
plt.plot([theta for theta in self.theta_history])
if len(self.theta_history) < expansion:
plt.xlim([0, expansion])
else:
plt.xlim([0, len(self.theta_history)])
ylim = np.pi + 0.5
plt.ylim([-ylim, ylim])
def plot_all_goals(self, im):
# display all goals
for goal_idx in range(len(self.GOALS)):
im[self.GOALS[goal_idx][0], self.GOALS[goal_idx][1]] = np.array((1.0, 1.0, 1.0))
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
sys.exit(0)
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
self.robot_code()
self.clean_exit()
class FinalProjectProgram():
def __init__(self, color, number):
# parameter init
self.bot_name = '%s%d' % (color, number)
self.bot_nameStriker = 'Red1'
# run startup methods
self.initializeVrepClient()
self.initializeVrepApi()
# self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initializeVrepClient(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000,
5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID != -1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor = vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.rightMotor = vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.bot = vrep.simxGetObjectHandle(self.clientID, self.bot_name,
vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.bot, -1,
vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
# # initialize overhead cam
# _, self.overheadCam=vrep.simxGetObjectHandle(
# self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
# _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
# self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# # initialize goal handle + odom
# _, self.goalHandle=vrep.simxGetObjectHandle(
# self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
# _, self.goalPose = vrep.simxGetObjectPosition(
# self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# initialize bot handles and variables for Red1
_, self.leftMotorStriker = vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker = vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(self.clientID,
self.botStriker, -1,
vrep.simx_opmode_streaming)
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
# count = 0
# while True:
# self.unittestTurnSideways(count)
# count += 1
xGoalie = self.getRobotPose(self.bot)[0]
yGoalie = self.getRobotPose(self.bot)[1]
thetaGoalie = self.getRobotPose(self.bot)[2]
thetaStriker = self.getRobotPose(self.botStriker)[2]
# ball pose
ballPose = (0, 0) # (x, y)
goalie2Ball = abs(yGoalie -
ballPose[1]) # distance between ball and goalkeeper
goalieDistance = abs(goalie2Ball * math.tan(thetaStriker))
if thetaStriker < 0:
xSaving = xGoalie + goalieDistance
else:
xSaving = xGoalie - goalieDistance
savingPosition = [xSaving, yGoalie,
z] # position in which the the goalie saves the ball
# desiredHeading = -math.pi/2
# tolerance = 5*math.pi/360
# error = []
# Kp = 2
# Tdiff = 0.01
# Tint = 0.1
# Tsample = 0.02
# i = 0
#
# while ((thetaGoalie < (desiredHeading - tolerance) or thetaGoalie > (desiredHeading + tolerance))) and i <= 20:
# error.append(abs(thetaGoalie - desiredHeading))
# if i == 0:
# vDiff = Kp*error[i]
# else:
# errorSum = 0
# for j in range(i):
# errorSum += error[j]
# vDiff = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.leftMotor,-vDiff,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.rightMotor,vDiff,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
# time.sleep(Tsample)
# thetaGoalie = self.getRobotPose(self.bot)[2]
# #print thetaGoalie
# i += 1
#
# xTolerance = 0.0001
# error = []
# i = 0
# Kp = 100
# Tint = 1
# Tsample = 0.01
#
# while ((xGoalie < (xSaving - xTolerance) or xGoalie > (xSaving + xTolerance))) and i <= 25:
# error.append(xGoalie - xSaving)
# if i == 0:
# vComm = Kp*error[i]
# else:
# errorSum = 0
# for j in range(i):
# errorSum += error[j]
# vComm = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.leftMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.rightMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
# time.sleep(Tsample)
# xGoalie = self.getRobotPose(self.bot)[0]
# #print xGoalie
# i += 1
while 1:
robotConf = self.getRobotConf(self.bot)
print('robotConf.x=%f' % robotConf[0])
print('robotConf.y=%f' % robotConf[1])
print('robotConf.theta=%f' % robotConf[2])
print('savingPosition.x=%f' % savingPosition[0])
print('savingPosition.y=%f' % savingPosition[1])
print('savingPosition.theta=%f' % savingPosition[2])
vRobot = v2Pos(robotConf, savingPosition)
print('vRobot.x=%f' % vRobot[0])
print('vRobot.y=%f' % vRobot[1])
self.setMotorVelocities(vRobot[0], vRobot[1])
#self.setDriveVelocities(0, 0)
time.sleep(3)
def getRobotConf(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
robot_handle, -1,
vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def getRobotPose(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
robot_handle, -1,
vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def setMotorVelocities(self, forward_vel, omega):
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
omega,
g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.leftMotor, ctrl_sig_left,
vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.rightMotor, ctrl_sig_right,
vrep.simx_opmode_oneshot_wait) # set right wheel velocity
def unittestMoveForward(self):
self.setMotorVelocities(forward_vel=1, omega=0)
def unittestTurnSideways(self, not_first_time):
x, y, theta = self.getRobotPose()
if not_first_time:
goal_theta = self.first_theta + np.pi / 2
print goal_theta
error_theta = ThetaRange.angleDiff(theta, goal_theta)
# control
omega = 10 * error_theta
print omega
self.setMotorVelocities(0, omega)
# visualization
def plotCurrentDesiredHeadings():
plotVector(ThetaRange.pol2cart(1, theta), 'k')
plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
self.idash.add(plotCurrentDesiredHeadings)
self.idash.plotframe()
else:
self.first_theta = theta
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
def run(self):
if self.clientID != -1:
_ = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
self.robotCode()
self.clean_exit()
class Lab2Program:
def __init__(self):
self.initialize_vrep_client()
self.initilize_vrep_api()
self.define_constants()
self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initialize_vrep_client(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000,
5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID != -1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initilize_vrep_api(self):
# initialize ePuck handles and variables
_, self.bodyelements = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_bodyElements', vrep.simx_opmode_oneshot_wait)
_, self.leftMotor = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_leftJoint', vrep.simx_opmode_oneshot_wait)
_, self.rightMotor = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_rightJoint', vrep.simx_opmode_oneshot_wait)
_, self.ePuck = vrep.simxGetObjectHandle(self.clientID, 'ePuck',
vrep.simx_opmode_oneshot_wait)
_, self.ePuckBase = vrep.simxGetObjectHandle(
self.clientID, 'ePuck_base', vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of ePuck
_, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.ePuck, -1,
vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.ePuck, -1, vrep.simx_opmode_streaming)
# initialize overhead cam
_, self.overheadCam = vrep.simxGetObjectHandle(
self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
_, self.resolution, self.image = vrep.simxGetVisionSensorImage(
self.clientID, self.overheadCam, 0, vrep.simx_opmode_oneshot_wait)
# initialize goal handle + odom
_, self.goalHandle = vrep.simxGetObjectHandle(
self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
_, self.goalPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# STOP Motor Velocities. Not sure if this is necessary
_ = vrep.simxSetJointTargetVelocity(self.clientID, self.leftMotor, 0,
vrep.simx_opmode_oneshot_wait)
_ = vrep.simxSetJointTargetVelocity(self.clientID, self.rightMotor, 0,
vrep.simx_opmode_oneshot_wait)
def define_constants(self):
self.map_side_length = 2.55
# FIMXE: hard coded goals
# self.GOALS = [(40+2,6), (40, 6+2), (40,21), (35, 19), (30,22), (29,10), (27,5), (20,8), (20,33), (20, 48), (5,55)]
self.GOALS = None
self.worldNorthTheta = None
self.maxVelocity = 2.0
self.history_length = 5
self.theta_history = RingBuffer(self.history_length)
self.e_theta_h = RingBuffer(self.history_length)
self.blurred_paths = None
self.path_skip = 8
def global_map_preprocess(self, resolution, image):
im = format_vrep_image(resolution, image) # original image
im = image_vert_flip(im)
resize_length = int(im.shape[0] / 2)
im = skimage.transform.resize(im, (resize_length, resize_length, 3))
return im
def global_map_process(self, im):
walls = im[:, :, 0] > 0.25
paths = walls < 0.15
if self.blurred_paths is None:
print "Computed"
blurred_map = skimage.filters.gaussian_filter(walls, sigma=2)
blurred_paths = blurred_map < 0.15
# np.save('binary_grid.npy', blurred_paths)
return paths, blurred_paths
else:
return paths, self.blurred_paths
def robot_pose_get(self):
_, xyz = vrep.simxGetObjectPosition(self.clientID, self.ePuck, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
self.ePuck, -1,
vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
self.theta_history.append(theta)
return (x, y, theta)
def lidar_scan_get(self, window_size=21):
""" gets lidar scan range values """
fr = (window_size - 1) / 2 # fire range
lidarValues = pseudoLidarSensor(self.paths,
self.pose[0],
self.pose[1],
fr,
original_four=False)
return lidarValues
def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
numLidarValues = len(lidarValues)
lidarAngles = [
np.pi / numLidarValues * index for index in range(numLidarValues)
]
repulsionVectors = [
np.array(
pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle))
for val, angle in zip(lidarValues, lidarAngles)
]
return repulsionVectors
def attraction_vector_compute(self, k_attract=100.0):
self.attractionVal, self.attractionAngle = cart2pol(
self.goal_pose_pixel[1] -
self.pose_pixel[1], # cols counts same to normal horz axes
self.pose_pixel[0] -
self.goal_pose_pixel[0] # rows counts opposite to normal vert axes
)
attractionVector = np.array(
pol2cart(k_attract * self.attractionVal, self.attractionAngle))
return attractionVector
def robot_code(self):
t = time.time()
self.curr_goal = 0
while (time.time() - t) < 200:
# global map
_, resolution, image = vrep.simxGetVisionSensorImage(
self.clientID, self.overheadCam, 0,
vrep.simx_opmode_oneshot_wait) # Get image
im = self.global_map_preprocess(resolution, image)
self.pose = self.robot_pose_get()
x, y, theta = self.pose
# theta = self.theta_history.weighted_average(scheme='last', mathfunc=lambda x: np.exp(x))
# initialize worldNorthTheta for the first time
if self.worldNorthTheta is None:
self.worldNorthTheta = self.pose[2]
_ = [
self.theta_history.append(self.pose[2])
for _ in range(self.history_length)
]
self.pose_pixel = np.array(
odom2pixelmap(x, y, self.map_side_length, im.shape[0]))
m, n = self.pose_pixel
self.paths, self.blurred_paths = self.global_map_process(im)
# calculate intermediate goals once
if self.GOALS is None:
raw_input("")
# acquire pixel location of goal
_, finishPose = vrep.simxGetObjectPosition(
self.clientID, self.goalHandle, -1,
vrep.simx_opmode_buffer)
self.finish_pixel = odom2pixelmap(finishPose[0], finishPose[1],
self.map_side_length,
im.shape[0])
contiguousPath = AStarBinaryGrid(
self.blurred_paths).calculate_path(self.pose_pixel,
self.finish_pixel)
self.GOALS = contiguousPath[::self.path_skip]
# SKIP THIS FIRST LOOP AND CONTINUE
continue
else:
self.goal_pose_pixel = np.array(self.GOALS[self.curr_goal])
goal_m, goal_n = self.goal_pose_pixel
lidarValues = self.lidar_scan_get(window_size=21)
#############################
# Potential Field Algorithm #
#############################
repulsionVectors = self.repulsion_vectors_compute(lidarValues,
k_repulse=10.0)
attractionVector = self.attraction_vector_compute(k_attract=100.0)
finalVector = np.sum(np.vstack(
(repulsionVectors, attractionVector)),
axis=0)
finalUnitVector = finalVector / np.linalg.norm(finalVector)
print "finalUnitVector: ", finalUnitVector
# TODO: do we use the unit vector (for direction) only, or the finalVector?
finalValue, finalAngle = cart2pol(finalUnitVector[0],
finalUnitVector[1])
error_theta = angle_diff(
mapTheta2worldTheta(finalAngle, self.worldNorthTheta), theta)
self.e_theta_h.append(error_theta)
k_angular_p = 2.0 * self.maxVelocity
k_angular_D = 0.25
k_angular_S = 1.0
k_angular_I = 2.5
omega = k_angular_p * (
error_theta + k_angular_D / k_angular_S *
(self.e_theta_h[-1] - self.e_theta_h[-2]) +
k_angular_S / k_angular_I * sum(self.e_theta_h))
print "Omega: ", round(omega, 1)
#############################
# StateController Algorithm #
#############################
# if desired heading is not directly in front
if np.abs(error_theta) > np.pi / 3:
# turn in place
forward_vel = self.maxVelocity
# omega *= 0.25
else:
# Direct yourself to the goal
goal_distance = np.linalg.norm(self.goal_pose_pixel -
self.pose_pixel)
print "distance_from_goal: ", goal_distance
if goal_distance <= 4:
# Achieved Goal!
forward_vel = self.maxVelocity * 0.25 # Slow down to prepare for the next one
self.curr_goal += 1
else:
forward_vel = self.maxVelocity
# control the motors
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
omega,
g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.leftMotor, ctrl_sig_left,
vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.rightMotor, ctrl_sig_right,
vrep.simx_opmode_oneshot_wait) # set right wheel velocity
self.idash.add(lambda: self.plot_maze(self.blurred_paths * 1.0, m,
n, goal_m, goal_n))
self.idash.add(lambda: self.plot_maze(im, m, n, goal_m, goal_n))
def plot_current_and_desired_heading():
self.plot_unit_quiver(finalUnitVector, 'r')
self.plot_unit_quiver(
pol2cart(1, worldTheta2mapTheta(theta,
self.worldNorthTheta)),
'k')
plt.title("Error Theta: %f" % error_theta)
self.idash.add(plot_current_and_desired_heading)
self.idash.add(self.plot_theta_history)
self.idash.plotframe()
if self.killer.kill_now:
self.clean_exit()
def plot_maze(self, im, m, n, goal_m, goal_n):
""" plots the maze, with robot pose and goal pose visualized """
if len(im.shape) == 3:
goal_pixel_values = np.array((1.0, 1.0, 1.0))
robot_pixel_values = np.array(
(255.0 / 255.0, 192 / 255.0, 203 / 255.0))
elif len(im.shape) == 2:
goal_pixel_values = 0.5
robot_pixel_values = 0.5
im[goal_m, goal_n] = goal_pixel_values
im[m, n] = robot_pixel_values
plt.imshow(im)
def plot_unit_quiver(self, vector, color):
X, Y = (0, 0)
U, V = (vector[0], vector[1])
plt.quiver(X,
Y,
U,
V,
angles='xy',
scale_units='xy',
scale=1,
color=color)
plt.xlim([-1.1, 1.1])
plt.ylim([-1.1, 1.1])
def plot_theta_history(self, expansion=5):
plt.plot([theta for theta in self.theta_history])
if len(self.theta_history) < expansion:
plt.xlim([0, expansion])
else:
plt.xlim([0, len(self.theta_history)])
ylim = np.pi + 0.5
plt.ylim([-ylim, ylim])
def plot_all_goals(self, im):
# display all goals
for goal_idx in range(len(self.GOALS)):
im[self.GOALS[goal_idx][0], self.GOALS[goal_idx][1]] = np.array(
(1.0, 1.0, 1.0))
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
sys.exit(0)
def run(self):
if self.clientID != -1:
_ = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
self.robot_code()
self.clean_exit()
image_template = np.zeros((28, 28))
map_image = np.tile(image_template, map_shape)
map_image_w = map_image.copy()
lattice_predictions = np.zeros(map_size)
for node_count in range(map_size):
m_row, n_col = idx1D_to_idx2D(node_count, map_shape)
closest_x_idx = np.argmin(cdist(w[:, node_count].reshape(1, -1), trX))
image_of_closest_x = trX[closest_x_idx].reshape((28, 28))
lattice_predictions[node_count] = trY[closest_x_idx] # prediction mapping
image_of_w = w[:, node_count].reshape((28, 28))
map_image[m_row * 28:(m_row + 1) * 28,
n_col * 28:(n_col + 1) * 28] = image_of_closest_x
map_image_w[m_row * 28:(m_row + 1) * 28,
n_col * 28:(n_col + 1) * 28] = image_of_w
dash.add(lambda: plt.imshow(map_image, cmap='gray'))
dash.add(lambda: plt.imshow(map_image_w, cmap='gray'))
# predictions
def predict(X, w, lattice_predictions):
n_examples = X.shape[0]
Ypred = np.zeros(n_examples)
for row in range(n_examples):
closest_w_idx = np.argmin(cdist(X[row].reshape(1, -1), w.T))
Ypred[row] = lattice_predictions[closest_w_idx]
return Ypred
trYpred = predict(trX, w, lattice_predictions)
teYpred = predict(teX, w, lattice_predictions)
class FinalProjectProgram():
def __init__(self, color, number):
# parameter init
self.bot_name = '%s%d' % (color, number)
self.bot_player2 = 'Blue2'
self.bot_nameStriker = 'Red1'
self.bot_nameGoalie = 'Red3'
self.bot_nameBall = 'Ball'
# run startup methods
self.initializeVrepClient()
self.initializeVrepApi()
self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initializeVrepClient(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart('127.0.0.1', 19999, True, True,
5000,
5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID != -1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor = vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.rightMotor = vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.bot = vrep.simxGetObjectHandle(self.clientID, self.bot_name,
vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.bot, -1,
vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
_, self.player2 = vrep.simxGetObjectHandle(
self.clientID, self.bot_player2, vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of bot
_, self.xyzPlayer2 = vrep.simxGetObjectPosition(
self.clientID, self.player2, -1, vrep.simx_opmode_streaming)
_, self.eulerAnglesPlayer2 = vrep.simxGetObjectOrientation(
self.clientID, self.player2, -1, vrep.simx_opmode_streaming)
# # initialize overhead cam
# _, self.overheadCam=vrep.simxGetObjectHandle(
# self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
# _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
# self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# # initialize goal handle + odom
# _, self.goalHandle=vrep.simxGetObjectHandle(
# self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
# _, self.goalPose = vrep.simxGetObjectPosition(
# self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# initialize bot handles and variables for Red1
_, self.leftMotorStriker = vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker = vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(self.clientID,
self.botStriker, -1,
vrep.simx_opmode_streaming)
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
_, self.leftMotorGoalie = vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.rightMotorGoalie = vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.botGoalie = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameGoalie, vrep.simx_opmode_oneshot_wait)
_, xyzGoalie = vrep.simxGetObjectPosition(self.clientID,
self.botGoalie, -1,
vrep.simx_opmode_streaming)
_, eulerAnglesGoalie = vrep.simxGetObjectOrientation(
self.clientID, self.botGoalie, -1, vrep.simx_opmode_streaming)
_, self.ball = vrep.simxGetObjectHandle(self.clientID,
self.bot_nameBall,
vrep.simx_opmode_oneshot_wait)
_, xyzBall = vrep.simxGetObjectPosition(self.clientID, self.ball, -1,
vrep.simx_opmode_streaming)
_, eulerAnglesBall = vrep.simxGetObjectOrientation(
self.clientID, self.ball, -1, vrep.simx_opmode_streaming)
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
# Shooting Test
vrep.simxSetObjectPosition(self.clientID, self.ball, -1, [0, -0.35],
vrep.simx_opmode_streaming)
target = self.aim()
vrep.simxSetObjectPosition(self.clientID, self.player2, -1, target,
vrep.simx_opmode_streaming)
position = []
position = self.kickingPose(target)
kickingPosition = [position[0], position[1], z]
vrep.simxSetObjectPosition(self.clientID, self.bot, -1,
kickingPosition, vrep.simx_opmode_streaming)
vrep.simxSetObjectOrientation(
self.clientID, self.bot, -1,
[self.eulerAngles[0], self.eulerAngles[1], position[2]],
vrep.simx_opmode_streaming)
time.sleep(2)
# Testing kicking Positions
# vrep.simxSetObjectPosition(
# self.clientID, self.ball,-1,[0, 0], vrep.simx_opmode_streaming)
#
# maximum = 36
# for i in range (0,maximum):
# print i
# radius = 0.3
# angle = 2*math.pi/maximum*i
# target = [radius*math.sin(angle), radius*math.cos(angle)]
#
# vrep.simxSetObjectPosition(
# self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
#
# position = []
# position = self.kickingPose(target)
# kickingPosition = [position[0], position[1], z]
# vrep.simxSetObjectPosition(
# self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
# vrep.simxSetObjectOrientation(
# self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
#
# time.sleep(1)
def getRobotConf(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
robot_handle, -1,
vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def getRobotPose(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
robot_handle, -1,
vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def setMotorVelocities(self, forward_vel, omega):
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
omega,
g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.leftMotor, ctrl_sig_left,
vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.rightMotor, ctrl_sig_right,
vrep.simx_opmode_oneshot_wait) # set right wheel velocity
def getBallPose(self):
_, xyz = vrep.simxGetObjectPosition(self.clientID, self.ball, -1,
vrep.simx_opmode_streaming)
x, y, z = xyz
return (x, y)
def aim(self, goaliePosition):
# assuming: robot is already in perfect position
# assuming: ball has velocity of zero
ballRadius = 0.05
leftGoalPost = [0.2, -0.75] # position of left goal post
rightGoalPost = [-0.2, 0.75] # position of right goal post
tolerance = 0.01
# if goalieOrientation <= .. :
# goalieHeading = # different variable name
# else:
# goalieHeading = # different variable name
#
# predicGoaliePos = goaliePosition + goalieHeading
gapRight = abs(goaliePosition[0] - rightGoalPost[0])
gapLeft = abs(goaliePosition[0] - leftGoalPost[0])
if gapRight >= gapLeft:
xAim = rightGoalPost[0] + ballRadius + tolerance
else:
xAim = leftGoalPost[0] - ballRadius - tolerance
return [xAim, goaliePosition[1], z]
def kickingPose(self, target):
# CHANGE TO ACTUAL POSITION IF NOT TESTING
ballPositionX, ballPositionY = (0, -0.35) #self.getBallPose()
distance2ball = 0.08
deltaX = target[0] - ballPositionX
deltaY = target[1] - ballPositionY
if deltaY != 0:
gamma = math.atan(deltaX / deltaY)
else:
gamma = math.pi / 2
# transform gamma into theta orientation
# CHECK FOR POINTS IN BETWEEN
if ballPositionX <= target[0]:
if ballPositionY <= target[1]:
print '1'
theta = -gamma
xOffset = -distance2ball * math.sin(gamma)
yOffset = -distance2ball * math.cos(gamma)
else:
# CHECK THIS ONE!
print '2'
theta = -gamma + math.pi
xOffset = distance2ball * math.sin(gamma)
yOffset = distance2ball * math.cos(gamma)
else:
if ballPositionY <= target[1]:
print '3'
theta = -gamma
xOffset = -distance2ball * math.sin(gamma)
yOffset = -distance2ball * math.cos(gamma)
print xOffset
print yOffset
else:
print '4'
print gamma
theta = -gamma + math.pi
xOffset = distance2ball * math.sin(gamma)
yOffset = distance2ball * math.cos(gamma)
xPosition = ballPositionX + xOffset
yPosition = ballPositionY + yOffset
pose = [xPosition, yPosition, theta]
return pose
def kick(self, speed):
self.setMotorVelocities(speed, speed)
time.sleep(0.5)
self.setMotorVelocities(0, 0)
def unittestMoveForward(self):
self.setMotorVelocities(forward_vel=1, omega=0)
def unittestTurnSideways(self, not_first_time):
x, y, theta = self.getRobotPose()
if not_first_time:
goal_theta = self.first_theta + np.pi / 2
print goal_theta
error_theta = ThetaRange.angleDiff(theta, goal_theta)
# control
omega = 10 * error_theta
print omega
self.setMotorVelocities(0, omega)
# visualization
def plotCurrentDesiredHeadings():
plotVector(ThetaRange.pol2cart(1, theta), 'k')
plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
self.idash.add(plotCurrentDesiredHeadings)
self.idash.plotframe()
else:
self.first_theta = theta
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
def run(self):
if self.clientID != -1:
_ = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
self.robotCode()
self.clean_exit()
class MyRobotRunner(base_robot.BaseRobotRunner):
def __init__(self, *args, **kwargs):
super(MyRobotRunner, self).__init__(*args, **kwargs)
self.idash = IDash(0.005)
def robotCode(self):
#waypoint = [(0.2, 0.4), (0, 0.4), (-0.2, 0.4),(-0.2, 0.2), (-0.2, 0),(0, 0), (0.2, 0),(0.2, -0.2), (0.2, -0.4),(0, -0.4), (-0.2, -0.4)]
waypoint = [(0, 0.4), (-0.2, 0.4), (-0.2, 0.2), (-0.2, 0), (0, 0),
(0.2, 0), (0.2, -0.2), (0.2, -0.4), (0, -0.4),
(-0.05, -0.4), (-0.1, -0.4), (-0.15, -0.4), (-0.2, -0.4),
(-0.2, -0.4)]
# waypoint = []
# r = 0.2
# increments = 5
# xCenter = 0
# yCenter = 0.2
# for i in range(increments-1):
# # circle center: (0, 0.2)
# x = r*math.sin(i*math.pi/increments) + xCenter
# y = r*math.cos(i*math.pi/increments) + yCenter
# waypoint.append((x,y))
# xCenter = 0
# yCenter = -0.2
# for i in range(increments):
# # circle center: (0, -0.2)
# x = -r*math.sin(i*math.pi/increments) + xCenter
# y = r*math.cos(i*math.pi/increments) + yCenter
# waypoint.append((x,y))
# for i in range(3):
# waypoint.append((-0.2/4*i,-0.4))
#k=0.0345 # ball model: d(t) = vmot*k*(1-exp(-t/T))
for i in range(len(waypoint)):
p0 = self.ballEngine.getBallPose()
curr_bot_pose = list(self.getRobotConf(self.bot))
curr_bot_pose[1] -= 0.05
self.path, status = passPath(curr_bot_pose,
p0,
waypoint[i],
vmax=10,
vr=7,
kq=0.0035)
print self.path
# dash = IDash(framerate=0.1)
# dash.add(lambda: plt.plot(-self.path[1,:], self.path[0,:], 'b-*') and
# plt.xlim([-0.7, 0.7]) and plt.ylim([-0.7, 0.7]))
# dash.plotframe()
print 'estimated time path'
# print calculatePathTime(self.path)
t = self.ballEngine.getSimTime()
t = self.ballEngine.getSimTime() # time in seconds
while (self.ballEngine.getSimTime() -
t) < 10: #End program after 30sec
# cc=1
# while cc:
def vizBots():
actx, acty = self.getRobotConf()[:2]
ballx, bally = self.ballEngine.getBallPose()[:2]
plt.hold('on')
plt.plot(-waypoint[i][1], waypoint[i][0], 'k*')
plt.plot(-bally, ballx, 'b.')
plt.plot(-acty, actx, 'g+')
plt.plot(-self.path[1, :], self.path[0, :], 'r.')
plt.xlim([-0.9, 0.9]) # y axis in the field
plt.ylim([-0.65, 0.65]) # x axis in the field
plt.xlabel('active path length: {}'.format(
self.path.shape[1]))
self.idash.add(vizBots)
robotConf = self.getRobotConf(self.bot)
cc = self.followPath(robotConf, rb=0.05)
self.ballEngine.update()
p1 = self.ballEngine.getBallPose()
p3 = self.ballEngine.getNextRestPos()
dist_from_start = np.sqrt((p1[0] - p0[0])**2 +
(p1[1] - p0[1])**2)
velocity_measure = np.sqrt((p1[0] - p3[0])**2 +
(p1[1] - p3[1])**2)
#self.idash.plotframe()
if dist_from_start > 0.01: # the ball has been touched
if velocity_measure < 0.003:
break # wait til velocity reaches zero
# robotConf = self.getRobotConf(self.bot)
# ballPos = self.ballEngine.getBallPose() # (x, y)
# vRobot = v2Pos(robotConf, ballPos)
self.setMotorVelocities(0, 0)
print 'real time path'
print(self.ballEngine.getSimTime() - t)
class FinalProjectProgram():
def __init__(self, color, number):
# parameter init
self.bot_name = '%s%d' % (color, number)
self.bot_player2 = 'Blue2'
self.bot_nameStriker = 'Red1'
self.bot_nameGoalie = 'Red3'
self.bot_nameBall = 'Ball'
# run startup methods
self.initializeVrepClient()
self.initializeVrepApi()
self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initializeVrepClient(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID!=-1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.bot=vrep.simxGetObjectHandle(
self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
_, self.player2=vrep.simxGetObjectHandle(
self.clientID, self.bot_player2, vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of bot
_, self.xyzPlayer2 = vrep.simxGetObjectPosition(
self.clientID, self.player2, -1, vrep.simx_opmode_streaming)
_, self.eulerAnglesPlayer2 = vrep.simxGetObjectOrientation(
self.clientID, self.player2, -1, vrep.simx_opmode_streaming)
# # initialize overhead cam
# _, self.overheadCam=vrep.simxGetObjectHandle(
# self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
# _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
# self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# # initialize goal handle + odom
# _, self.goalHandle=vrep.simxGetObjectHandle(
# self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
# _, self.goalPose = vrep.simxGetObjectPosition(
# self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# initialize bot handles and variables for Red1
_, self.leftMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
_, self.leftMotorGoalie =vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotorGoalie =vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.botGoalie = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameGoalie, vrep.simx_opmode_oneshot_wait)
_, xyzGoalie = vrep.simxGetObjectPosition(
self.clientID, self.botGoalie, -1, vrep.simx_opmode_streaming)
_, eulerAnglesGoalie = vrep.simxGetObjectOrientation(
self.clientID, self.botGoalie, -1, vrep.simx_opmode_streaming)
_, self.ball = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameBall, vrep.simx_opmode_oneshot_wait)
_, xyzBall = vrep.simxGetObjectPosition(
self.clientID, self.ball, -1, vrep.simx_opmode_streaming)
_, eulerAnglesBall = vrep.simxGetObjectOrientation(
self.clientID, self.ball, -1, vrep.simx_opmode_streaming)
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
# Shooting Test
vrep.simxSetObjectPosition(
self.clientID, self.ball,-1,[0, -0.35], vrep.simx_opmode_streaming)
target = self.aim()
vrep.simxSetObjectPosition(
self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
position = []
position = self.kickingPose(target)
kickingPosition = [position[0], position[1], z]
vrep.simxSetObjectPosition(
self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
vrep.simxSetObjectOrientation(
self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
time.sleep(2)
# Testing kicking Positions
# vrep.simxSetObjectPosition(
# self.clientID, self.ball,-1,[0, 0], vrep.simx_opmode_streaming)
#
# maximum = 36
# for i in range (0,maximum):
# print i
# radius = 0.3
# angle = 2*math.pi/maximum*i
# target = [radius*math.sin(angle), radius*math.cos(angle)]
#
# vrep.simxSetObjectPosition(
# self.clientID, self.player2, -1, target, vrep.simx_opmode_streaming)
#
# position = []
# position = self.kickingPose(target)
# kickingPosition = [position[0], position[1], z]
# vrep.simxSetObjectPosition(
# self.clientID, self.bot, -1, kickingPosition, vrep.simx_opmode_streaming)
# vrep.simxSetObjectOrientation(
# self.clientID, self.bot, -1, [self.eulerAngles[0], self.eulerAngles[1], position[2]], vrep.simx_opmode_streaming)
#
# time.sleep(1)
def getRobotConf(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def getRobotPose(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def setMotorVelocities(self, forward_vel, omega):
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
def getBallPose(self):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, self.ball, -1, vrep.simx_opmode_streaming)
x, y, z = xyz
return (x, y)
def aim(self, goaliePosition):
# assuming: robot is already in perfect position
# assuming: ball has velocity of zero
ballRadius = 0.05
leftGoalPost = [0.2, -0.75] # position of left goal post
rightGoalPost = [-0.2, 0.75]# position of right goal post
tolerance = 0.01
# if goalieOrientation <= .. :
# goalieHeading = # different variable name
# else:
# goalieHeading = # different variable name
#
# predicGoaliePos = goaliePosition + goalieHeading
gapRight = abs(goaliePosition[0] - rightGoalPost[0])
gapLeft = abs(goaliePosition[0] - leftGoalPost[0])
if gapRight >= gapLeft:
xAim = rightGoalPost[0] + ballRadius + tolerance
else:
xAim = leftGoalPost[0] - ballRadius - tolerance
return [xAim, goaliePosition[1], z]
def kickingPose(self, target):
# CHANGE TO ACTUAL POSITION IF NOT TESTING
ballPositionX, ballPositionY = (0,-0.35)#self.getBallPose()
distance2ball = 0.08
deltaX = target[0] - ballPositionX
deltaY = target[1] - ballPositionY
if deltaY != 0:
gamma = math.atan(deltaX/deltaY)
else:
gamma = math.pi/2
# transform gamma into theta orientation
# CHECK FOR POINTS IN BETWEEN
if ballPositionX <= target[0]:
if ballPositionY <= target[1]:
print '1'
theta = - gamma
xOffset = - distance2ball*math.sin(gamma)
yOffset = - distance2ball*math.cos(gamma)
else:
# CHECK THIS ONE!
print '2'
theta = - gamma + math.pi
xOffset = distance2ball*math.sin(gamma)
yOffset = distance2ball*math.cos(gamma)
else:
if ballPositionY <= target[1]:
print '3'
theta = - gamma
xOffset = - distance2ball*math.sin(gamma)
yOffset = - distance2ball*math.cos(gamma)
print xOffset
print yOffset
else:
print '4'
print gamma
theta = - gamma + math.pi
xOffset = distance2ball*math.sin(gamma)
yOffset = distance2ball*math.cos(gamma)
xPosition = ballPositionX + xOffset
yPosition = ballPositionY + yOffset
pose = [xPosition, yPosition, theta]
return pose
def kick(self, speed):
self.setMotorVelocities(speed, speed)
time.sleep(0.5)
self.setMotorVelocities(0, 0)
def unittestMoveForward(self):
self.setMotorVelocities(forward_vel=1, omega=0)
def unittestTurnSideways(self, not_first_time):
x, y, theta = self.getRobotPose()
if not_first_time:
goal_theta = self.first_theta + np.pi / 2
print goal_theta
error_theta = ThetaRange.angleDiff(theta, goal_theta)
# control
omega = 10 * error_theta
print omega
self.setMotorVelocities(0, omega)
# visualization
def plotCurrentDesiredHeadings():
plotVector(ThetaRange.pol2cart(1, theta), 'k')
plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
self.idash.add(plotCurrentDesiredHeadings)
self.idash.plotframe()
else:
self.first_theta = theta
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
self.robotCode()
self.clean_exit()
class Master(base_robot.MultiRobotCyclicExecutor):
def __init__(self, *args, **kwargs):
super(Master, self).__init__(*args, **kwargs)
self.color = None
self.idash = IDash(framerate=0.005)
def getClosestZone(self, pose):
""" Zones will be from downfield (0) to close to own goal (1) """
pose = np.array(pose[:2]) # x-y only
assert pose.size == 2 # pose should be 2-D
# 2 ZONES: 0 and 1
two_zone_boundary_y = 0.01
zone = pose[1] > two_zone_boundary_y
if self.color == 'Red':
zone = not zone
return zone
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
# robots have been added
self.color = self.bots[0].color
self.originalRoles = ['attacker', 'midfielder', 'goalie']
self.roles = ['attacker', 'midfielder', 'goalie']
goalie = self.bots[2]
activeidx = 0 # striker starts first
t0 = time.time()
while time.time() - t0 < 600:
self.ballEngine.update()
assert activeidx != 2 # now only have active 0,1
activebot = self.bots[activeidx]
offense = self.originalRoles[activeidx] == 'attacker'
if offense:
if self.roles[1] != 'dumb':
self.roles[1] = 'attacker'
else:
if self.roles[1] != 'dumb':
self.roles[1] = 'midfielder'
secondaryidx = not activeidx
ballx, bally = self.ballEngine.getBallPose()
robotX = np.zeros(2)
robotY = np.zeros(2)
robotX[0], robotY[0], _ = self.bots[0].getRobotConf()
robotX[1], robotY[1], _ = self.bots[1].getRobotConf()
distance2ball = 9999
for i in xrange(len(self.bots[:-1])):
dist = np.sqrt((ballx - robotX[i])**2 + (bally- robotY[i])**2)
if dist < distance2ball:
distance2ball = dist
closestRobot = i
ballPosX, ballPosY = self.ballEngine.getBallPose()
for idx in range(len(self.bots[:-1])):
if offense:
oppGoalieConf, oppGoalieIdx = self.findOppGoalieConf(return_index=True)
oppGoalieIdx = self.convertOppBotsIdx2BotsIdx(oppGoalieIdx)
if idx == closestRobot:
self.bots[idx].robotCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs([idx, oppGoalieIdx]), # dont ignore yourself, or the oppGoalie
goaliePosition = self.findOppGoalieConf())
else: # robot is not closest robot
if True: # be a bitch
passivePos = oppGoalieConf
else:
if ballPosX >= 0:
passivePos = [-0.2, ballPosY, 0]
else: # ballPosX < 0
passivePos = [0.2, ballPosY, 0]
self.bots[idx].secondaryCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs([idx]), # attack the goalie
passivePos=passivePos)
# defense
else:
# secondary defender (original attacker being passive)
if idx == secondaryidx:
if self.bots[idx].color == 'Red':
passivePos = [ballPosX, 0.2, 0]
else:
passivePos = [ballPosX, -0.2, 0]
self.bots[idx].secondaryCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs(secondaryidx),
passivePos=passivePos)
# primary defender (original defender being active)
else:
self.bots[idx].robotCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs(activeidx),
goaliePosition = self.findOppGoalieConf())
goalie.robotCode(
role=self.roles[2],
obstacleConfs=self.getObstacleConfs(activeidx),
goaliePosition = self.findOppGoalieConf())
def vizBots():
actx, acty = activebot.getRobotConf()[:2]
ballx, bally = self.ballEngine.getBallPose()
plt.hold('on')
try:
plt.plot(-self.activebot.passPos[1], self.activebot.passPos, 'ro')
except:
pass
plt.plot(-acty, actx, 'g+')
try:
plt.plot(-activebot.target[1], activebot.target[0], 'm*')
except:
pass
plt.plot(-oppGoalieConf[1], oppGoalieConf[0], 'yo')
plt.plot(-bally,ballx, 'ro')
plt.plot(-self.bots[secondaryidx].path[1,:], self.bots[secondaryidx].path[0,:], 'b.')
plt.plot(-activebot.path[1,:], activebot.path[0,:], 'g.')
plt.xlim([-0.8, 0.8]) # y axis in the field
plt.ylim([-0.7, 0.7]) # x axis in the field
plt.title('Red = RCV, Green = Active')
plt.xlabel('active path length: {}'.format(activebot.path.shape[1]))
self.idash.add(vizBots)
# if time.time() - activebot.time_started_2b_dumb > 2:
# self.roles = self.originalRoles[:]
# if activebot.path.shape[1] < 5:
# self.roles[activeidx] = 'dumb'
# activebot.time_started_2b_dumb = time.time()
p0 = activebot.p0
p1 = self.ballEngine.getBallPose()
p3 = self.ballEngine.getNextRestPos()
dist_from_start = np.sqrt((p1[0] - p0[0])**2 + (p1[1] - p0[1])**2)
velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
closest_zone = self.getClosestZone(p1)
if dist_from_start > 0.01: # the ball has been touched
# if velocity_measure < 0.003: # wait til velocity reaches zero
# if True:
# if velocity_measure < 1.0: # start kicking while ball is moving...
for bot in self.bots:
bot.executing = False
if closest_zone != activeidx: # success
# increment what is the new active zone
# self.setMotorVelocities(0, 0)
activeidx = not activeidx
class ZonePasserMasterCyclic(base_robot.MultiRobotCyclicExecutor):
"""After doing part A of Question 2, the ball will already be
placed in Zone 4; so let's transport it there now"""
def __init__(self, *args, **kwargs):
super(ZonePasserMasterCyclic, self).__init__(*args, **kwargs)
self.idash = IDash(0.005)
self.activezones = []
self.receivingzones = []
self.activeplayer = None
self.receivingplayer = None
self.zones = {( 1, 1): 1,
( 1, -1): 2,
(-1, 1): 3,
(-1, -1): 4}
self.zonesigns = [( 1, 1), ( 1, -1), (-1, 1), (-1, -1)]
self.zone_centers = np.array([[CENTER_X, CENTER_X, -CENTER_X, -CENTER_X],
[CENTER_Y, -CENTER_Y, CENTER_Y, -CENTER_Y]])
self.zone_corners = np.array([[CORNER_X, CORNER_X, -CORNER_X, -CORNER_X],
[CORNER_Y, -CORNER_Y, CORNER_Y, -CORNER_Y]])
# TODO: remove me since we're not allowed to set the ball pose
# start ball at zone 4 - 1 (0 indexed)
ball_start = self.zone_centers[:,3]
ball_start[1] -= 0.05 # move closer to players center, but further distance q from player
# self.ballEngine.setBallPose(ball_start)
self.ballEngine.update()
# FIXME: Hardcoded is good for drill, not good for game!
self.zone_pass_plan = [4, 2, 1, 3, 4, 2] # the locations between the 5 passes
self.activebot_idx_plan = [0, 1, 2, 0, 1, 2] # which robot should take the active zone during the zone pass plan?
def getClosestZone(self, pose):
""" get zone which the current pose is closest to. This pose could
be a ball, an opposing player, etc. """
pose = np.array(pose[:2]) # x-y only
assert pose.size == 2 # pose should be 2-D
dist_from_zones = cdist(np.expand_dims(pose, axis=0), self.zone_corners.T)
return np.argmin(dist_from_zones) + 1 # zones are 1-indexed
def getBotInZone(self, zone):
"""
Not using now
Returns the index of the bot which is inside the zone, otherwise None
if there is no bot inside this zone"""
bot_zones = np.zeros(len(self.bots))
for bot_idx, bot in enumerate(self.bots):
bot_zones[bot_idx] = self.getClosestZone(bot.getRobotConf(bot.bot))
# we use [0][0] since return of np.where looks like (array([2]),)
zone_as_array = np.where(bot_zones == zone)[0]
if zone_as_array.size == 0:
return None
else:
return zone_as_array[0] # the first, if there are multiple in that zone
def getNearestBotToZone(self, zone, bot_to_ignore=None):
""" Returns the index of the bot closest to the zone, even if the bot
may not be directly inside the zone """
bot_poses = np.vstack([bot.getRobotConf() for bot in self.bots])
bot_poses = bot_poses[:, :2]
assert bot_poses.shape == (len(self.bots), 2)
zone_pose = self.zone_corners[:, zone - 1].reshape(1, 2)
dist_from_bots = cdist(zone_pose, bot_poses, 'cityblock')
if bot_to_ignore is None:
return np.argmin(dist_from_bots)
else: # the closest bot might be active, in which case we should return next closest
sortedargs = np.argsort(dist_from_bots.flatten())
if sortedargs[0] == bot_to_ignore: # first closest is active, not valid to be receive
return sortedargs[1] # return second closest
else:
return sortedargs[0]
def planToMoveIntoReceivingPosition(self, idx, rcvzone=None, startSmoothPathConf=None, vel=15, r=0.01):
""" Move into the corner, facing center
Parameters
----------
idx: integer
index of the robot (in self.bots)
startSmoothPathConf: array-like, shape (3,) or None
the start configuration of the robot to calculate the smooth path.
Pass this if you are calculating a path from a future point.
If None, then use current robot configuration.
"""
if startSmoothPathConf is None:
startSmoothPathConf = self.bots[idx].getRobotConf()
if rcvzone is None:
rcvzone = self.zone_pass_plan[idx]
final_x = self.bots[idx].zone_corners[0, rcvzone - 1]
final_y = self.bots[idx].zone_corners[1, rcvzone - 1]
# final theta will be facing towards center field
_ , final_theta = ThetaRange.cart2pol(final_x, final_y)
final_theta = ThetaRange.normalize_angle(final_theta + np.pi) # flip it towards center
smooth_path, status = smoothPath(
startSmoothPathConf, # robotConf
[final_x, final_y, final_theta], # finalConf
r=r
)
v = vel*np.ones((1, np.size(smooth_path,1)))
smooth_path = np.concatenate((smooth_path, v), axis=0)
if self.bots[idx].path is None:
self.bots[idx].add_to_path(smooth_path)
elif self.bots[idx].path.shape[1] == 1:
self.bots[idx].add_to_path(smooth_path)
else:
self.bots[idx].path = smooth_path
def calculateReceivingDestination(self, xy1, xy2, k=0.2):
""" receiving BallPos should be between rcvbot and next passing zone
Parameters
----------
xy1: array-like, shape (2,)
x-y position of receiving bot
xy2: array-like, shape (2,)
x-y position of next receiving zone
k: float, between 0-1
multiplier to determine how much in between the rcv and nextrcv
"""
x1, y1 = xy1
x2, y2 = xy2
theta = np.arctan2(y1 - y2, x1 - x2)
hyp = np.sqrt((x1 - x2)**2 + (y1 - y2)**2)
rcvBallPos = [x1 - k*hyp*np.cos(theta), y1 - k*hyp*np.sin(theta)]
return rcvBallPos
def calculateShootingDestination(self):
# TODO: Intelligently use position of moving opponenet goalie
posToAim = [0, -1.0] # in the center of the goal
return posToAim
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
# set them up in the first 3 zones
for idx in range(len(self.bots)):
if self.zone_pass_plan[idx] == 2:
# if False:
# zone 2 has some robots blocking, so navigate to zone 2
# in a couple of steps
# TOOD: may want to do this less hardcoded, more like planned path with obstacle avoidance
# step 1. get to midfield passing the opposing team
x_coord_zone = 0.3
self.bots[idx].add_to_path([x_coord_zone, 0, 20])
startSmoothPathConf = [x_coord_zone, 0, -np.pi/2]
else:
startSmoothPathConf = self.bots[idx].getRobotConf(self.bots[idx].bot)
self.planToMoveIntoReceivingPosition(idx, startSmoothPathConf=startSmoothPathConf, vel=10)
self.bots[idx].add_delay(1*idx) # delay each by one second
# TODO: replace system time with vrep simxTime
t0 = time.time()
# get the bots into position
while time.time() - t0 < 15:
for bot in self.bots:
if time.time() - t0 >= bot.delay:
bot.robotCode()
# follow the zone passing plan
first_loop = True
activezone_idx = 0
shoot_flag = False
plan_length = len(self.zone_pass_plan)
executing = [False] * len(self.bots)
# TODO: maybe something like below with states for all bots?
bot_states = [STATE_READY_POS] * len(self.bots)
t1 = time.time()
while time.time() - t1 < 1000:
self.ballEngine.update()
activezone = self.zone_pass_plan[activezone_idx]
activebot_idx = self.activebot_idx_plan[activezone_idx]
activebot = self.bots[activebot_idx]
if plan_length > activezone_idx + 1:
rcvzone = self.zone_pass_plan[activezone_idx + 1]
rcvbot_idx = self.activebot_idx_plan[activezone_idx + 1]
rcvbot = self.bots[rcvbot_idx]
else:
rcvzone = None
rcvbot_idx = None
rcvbot = None
shoot_flag = True
if plan_length > activezone_idx + 2:
next_rcvzone = self.zone_pass_plan[activezone_idx + 2]
else:
next_rcvzone = None
# def vizZones():
# zones = np.zeros(4)
# zones[activezone-1] = 0.5
# if rcvzone:
# zones[rcvzone-1] = 1.0
# plt.imshow(zones.reshape(2,2), interpolation='nearest')
# plt.title('Red = RCV, Green = Active')
# self.idash.add(vizZones)
# -- STATE MACHINE UPDATE PARAMETERS
if shoot_flag:
bot_states[activebot_idx] = STATE_SHOOT
else:
bot_states[activebot_idx] = STATE_PASS
if rcvbot_idx:
bot_states[rcvbot_idx] = STATE_READY_POS
# -- STATE MACHINE EXECUTE
# _, proximitySensors = rcvbot.senseObstacles()
# if bot_states[rcvbot_idx] == STATE_READY_POS:
# rcvbot.receiveBall(proximitySensors)
# rcvp1 = np.array(rcvbot.getRobotConf()[:2])
# rcvp2 = self.zone_corners[:, rcvzone - 1]
# # not yet in position
# if cdist(rcvp1.reshape(1,2), rcvp2.reshape(1,2))[0] > 0.001: # radius buffer
# # FIXME: if receiving bot is moving at the same time as active bot, processing time increases by ~100ms
# if not executing[rcvbot_idx]:
# if next_rcvzone:
# xy2 = self.zone_centers[:,next_rcvzone-1]
# else:
# xy2 = [0, -0.75]
# self.planToMoveIntoReceivingPosition(rcvbot_idx, rcvzone, vel=10)
# rcvbot.prunePath()
# rcvbot.pause_before_kick = rcvbot.path.shape[1] > 2
# executing[rcvbot_idx] = True
# rcvbot.robotCode(rb=0.05, pause_before_kick=rcvbot.pause_before_kick)
# else:
# executing[rcvbot_idx] = False
if bot_states[activebot_idx] == STATE_PASS:
if not executing[activebot_idx]:
activeRobotConf = activebot.getRobotConf()
p0 = self.ballEngine.getBallPose()
# -- fancy calculation of finalBallPos, using info about where next
xy1 = self.zone_centers[:,rcvzone-1]
if next_rcvzone:
xy2 = self.zone_centers[:,next_rcvzone-1]
else:
xy2 = [0, -0.75]
finalBallPos = self.calculateReceivingDestination(xy1, xy2, k=0.15)
# -- simple calculation of finalBallPos
# mult_x, mult_y = self.zonesigns[rcvzone-1]
# finalBallPos = [ np.abs(p0[0])*mult_x , np.abs(p0[1])*mult_y ]
# slow mode (tested)
# activebot.path, status = passPath(activeRobotConf, p0, finalBallPos, vmax=10, vr=7, kq=0.0035, hold=True)
# fast mode (good luck!)
activebot.path, status = passPath(activeRobotConf, p0, finalBallPos, vmax=20, vr=15, kq=0.0010, hold=False)
# avoid all friendly and opposing robots
obstacleConf = [self.oppBots[i].getRobotConf() for i in range(len(self.oppBots))]
obstacleConf.extend([self.bots[i].getRobotConf() for i in range(len(self.bots)) if i != activebot_idx])
for i in xrange(len(obstacleConf)):
activebot.obstacleAwarePath(obstacleConf[i],0.05)
# avoid the wall boundaries
activebot.prunePath()
activebot.pause_before_kick = activebot.path.shape[1] > 2
p2 = self.ballEngine.getNextRestPos()
# FIXME: if the path produced is invalid, i.e some part of it is off the field and invalid
# prune that part of the path to make it valid
# FIXME: if path is not long enough
# backup to give more room. or bump the ball and get more room.
def vizBots():
actx, acty = activebot.getRobotConf()[:2]
rcvx, rcvy = rcvbot.getRobotConf()[:2]
plt.hold('on')
plt.plot(-acty, actx, 'g+')
plt.plot(-activebot.path[1,:], activebot.path[0,:], 'g.')
plt.plot(-rcvy, rcvx, 'r+')
plt.plot(-rcvbot.path[1,:], rcvbot.path[0,:], 'r.')
plt.plot(-p0[1], p0[0], 'bo')
#plt.plot(-rcvp1[1], rcvp1[0], 'mx')
#plt.plot(-rcvp2[1], rcvp2[0], 'kx')
plt.xlim([-0.75, 0.75]) # y axis in the field
plt.ylim([-0.5, 0.5]) # x axis in the field
plt.title('Red = RCV, Green = Active')
plt.xlabel('active path length: {}'.format(activebot.path.shape[1]))
self.idash.add(vizBots)
executing[activebot_idx] = True
# Edit the path based on the ball movement
# self.ballEngine.update()
# deltaxy = np.array(self.ballEngine.getVelocity())
# print "deltaxy: ", deltaxy
# for idx in range(activebot.path.shape[1]):
# gamma = 1 # no decay
# # gamma = 1 / idx # decay factor
# activebot.path[:2,-(idx+1)] += gamma*deltaxy*10
# Follow the path
activebot.robotCode(rb=0.05, pause_before_kick=activebot.pause_before_kick)
# activebot.robotCode(rb=0.05, pause_before_kick=False)
if bot_states[activebot_idx] == STATE_SHOOT:
if not executing[activebot_idx]:
activeRobotConf = activebot.getRobotConf(activebot.bot)
ballRestPos = self.ballEngine.getBallPose()
finalBallPos = self.calculateShootingDestination()
activebot.path, status = passPath(activeRobotConf, ballRestPos, finalBallPos, vmax=25, vr=15, kq=0.0010, hold=True)
activebot.prunePath()
obstacleConfs = self.getObstacleConfs([activebot_idx])
activebot.multiObstacleAwarePath(obstacleConfs, 0.05)
activebot.prunePath()
executing[activebot_idx] = True
activebot.robotCode()
def vizShooting():
actx, acty = activebot.getRobotConf()[:2]
plt.hold('on')
plt.plot(-acty, actx, 'g+')
plt.plot(-activebot.path[1,:], activebot.path[0,:], 'g.')
plt.xlim([-0.75, 0.75]) # y axis in the field
plt.ylim([-0.5, 0.5]) # x axis in the field
plt.title('SHOOT!!!!')
self.idash.add(vizShooting)
if first_loop:
pass
first_loop = False
else:
pass
print "Elapsed (MS): ", (time.time() - time_start) * 1e3
# Do measurements, whether pass was successful and state changes
time_start = time.time()
p1 = self.ballEngine.getBallPose()
p3 = self.ballEngine.getNextRestPos()
dist_from_start = np.sqrt((p1[0] - p0[0])**2 + (p1[1] - p0[1])**2)
# dist_from_goal = np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
# velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
vx, vy = self.ballEngine.getVelocity()
velocity_measure = np.sqrt(vx**2 + vy**2)
print velocity_measure
# self.idash.add(lambda: plt.plot(velocity_measure))
closest_zone = self.getClosestZone(p1)
if dist_from_start > 0.01: # the ball has been touched
if velocity_measure < 0.003: # wait til velocity reaches zero
# if dist_from_goal < 0.1: # wait til the ball has entered the predicted zone
executing = [False] * len(self.bots) # everyone has new roles, plan next pass
if closest_zone == rcvzone: # success
# increment what is the new active zone
activebot.setMotorVelocities(0,0)
activezone_idx += 1
self.idash.plotframe() # ~100 ms shaved by removing this line
# time.sleep(50*1e-3)
self.clean_exit()
class BaseRobotRunner(object):
__metaclass__ = abc.ABCMeta
def __init__(self, color, number, clientID=None, ip='127.0.0.1', port=19998): # ip='127.0.0.1', '172.29.34.63'
"""
color: i.e. 'Blue'
number: i.e. 1
clientID: Default None; if provided as parameter,
`vrep.simxStart` was called outside this class
"""
# parameter init
self.ip = ip
self.port=port
self.color = color
self.bot_name = '%s%d' % (color, number)
self.bot_nameStriker = 'Red1'
self.vm=self.v=0
self.tv=self.tvm=0
# run startup methods
if clientID is not None:
# Use existing clientID (multi robot case)
self.clientID = clientID
self.multirobots = True
print("ClientID obtained as parameter! (Probably doing Multirobots...)")
else:
# Make connection to VREP client (single robot case)
self.initializeVrepClient()
self.multirobots = False
self.initializeVrepApi()
#self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
self.ballEngine = BallEngine(self.clientID)
def exploreClassAttributes(self):
for variable_name, variable_value in self.__dict__.iteritems():
locals()[self.bot_name + "_" + variable_name] = variable_value
# delete so there are no duplicate variables in the variable explorer
del(variable_name)
del(variable_value)
return # Place Spyder Breakpoint on this Line!
def initializeVrepClient(self):
""" Initialization for Python to connect to VREP.
We obtain a clientID after connecting to VREP, which is saved to
`self.clientID`
This method is only called if we controlling one robot with the remote API
"""
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart(self.ip,self.port,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID!=-1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.bot=vrep.simxGetObjectHandle(
self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)
# initialize proximity sensors
self.proxSensors = prox_sens_initialize(self.clientID, self.bot_name)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming )
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming )
# FIXME: striker handles shouldn't be part of the default base class
# FIXME: should be better way to have information regarding all the bots (Central Control?) (Publishers/Subscribers...)?
# initialize bot handles and variables for Red1
_, self.leftMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming )
@abc.abstractmethod
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
return
def secondaryCode(self, role, obstacleConfs=None, passivePos=None):
"""inner while loop for when attacker is not active"""
# FIXME: role does nothing, but maybe there should be different
# modes of operation for passive roles
if passivePos is None:
passivePos = self.passivePos
if not self.executing:
self.status = 0 # for moving freely, without theta adjust
self.path, self.status = smoothPath(
self.getRobotConf(self.bot),
passivePos,
r=0.08)
vf = 10
v = vf*np.ones((1, np.size(self.path,1)))
self.path = np.concatenate((self.path, v), axis=0)
self.path[-1, -1]=vf
self.prunePath()
self.multiObstacleAwarePath(obstacleConfs, 0.04)
self.prunePath()
self.followPath(self.getRobotConf(self.bot), self.status, rb=0.05)
def getRobotConf(self, robot_handle=None):
if robot_handle is None:
robot_handle = self.bot
_, xyz = vrep.simxGetObjectPosition(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer )
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer )
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def senseObstacles(self):
"""
Returns
-------
objectDetected: array-like, shape 4
whether an obstacle was detected for the 4 sensors
objectDistances: array-like, shape 4
the distances detected
"""
out = prox_sens_read(self.clientID, self.proxSensors)
objectDetected = np.zeros(4)
objectDistances = 100*np.ones(4)
for i, sens in enumerate(out):
if sens['detectionState'] == 1:
objectDetected[i] = 1
x, y, z = sens['detectedPoint']
a = np.sqrt(x**2 + y**2)
distance = np.sqrt(a**2 + z**2)
objectDistances[i] = distance
return objectDetected, objectDistances
def setMotorVelocities(self, forward_vel, omega):
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
self.driveMotor(ctrl_sig_left, ctrl_sig_right)
def driveMotor(self, vl, vr):
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,vl,vrep.simx_opmode_streaming ) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,vr,vrep.simx_opmode_streaming ) # set right wheel velocity
def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
numLidarValues = len(lidarValues)
lidarAngles = [np.pi / numLidarValues * index for index in range(numLidarValues)]
repulsionVectors = [np.array(pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle)) for val, angle in zip(lidarValues, lidarAngles)]
return repulsionVectors
def followPath(self, robotConf, status=0, rb=0.05, dtheta=0.001, k=3.5):
""" radius of the buffer zone
"""
robotpos = np.array(robotConf)[0:2]
if status==2 and self.path.shape[1]==2 and np.linalg.norm(robotpos - self.path[0:2,0])<rb:
# print 'path'
# print self.path
theta = math.atan2(self.path[1,-1]-self.path[1,0], self.path[0,-1]-self.path[0,0])
finalConf = (self.path[0,0], self.path[1,0],theta)
# print 'finalConf'
# print finalConf
vRobot = v2orientation(robotConf, finalConf)
self.setMotorVelocities(vRobot[0], vRobot[1])
if vRobot[1]==0:
self.path = self.path[:, 1:]
return 2
while self.path.shape[1]>1 and np.linalg.norm(robotpos - self.path[0:2,0])<rb :
self.path = self.path[:, 1:] # remove first node
if self.path.shape[1]==1 and np.linalg.norm(robotpos - self.path[0:2,0])<rb :
self.setMotorVelocities(0,0)
return 0
else:
vRobot = v2Pos(robotConf, self.path[0:2,0], self.path[2,0], k)
self.setMotorVelocities(vRobot[0], vRobot[1])
return 1
def keepGoal(self,robotConf, y=0.7,vmax=30, goalLim=0.2): # 0.7 for left goal, -0.7 for right goal
""" the robot keep the goal by staying on a line and focusing on ball displacement """
tol=0.0001
self.ballEngine.update()
pm = self.ballEngine.posm2 # previous position
p = self.ballEngine.getBallPose() # position
if math.fabs(p[1]-pm[1])<tol:
finalPos=[0,y]
vRobot = v2PosB(robotConf, finalPos)
self.setMotorVelocities(vRobot[0], vRobot[1])
return 1
if y*(p[1]-pm[1])<0:
self.setMotorVelocities(0, 0)
return 0
a = (y-pm[1])/(p[1]-pm[1]) # intersection: (x,y)^t = pm+a(p-pm)
x = pm[0]+a*(p[0]-pm[0])
if (x>goalLim):
x = goalLim
if (x<-goalLim):
x = -goalLim
finalPos=[x,y]
vRobot = v2PosB(robotConf, finalPos, vmax)
self.setMotorVelocities(vRobot[0], vRobot[1])
return 0
def keepGoal2(self, robotConf, Gy=0.72, vmax=25, Ex=0.18, Ey=0.07): # 0.72 for left goal, -0.72 for right goal
""" the robot keep the goal by staying on an ellipse and focusing on ball position
configuration of the robot, princial axis of the ellipse, center of the goal """
# self.ballEngine.update()
bp = self.ballEngine.getBallPose() # ball position
a=math.atan2(bp[0],Gy-bp[1])
rp=(Ex*math.sin(a), Gy-Ey*math.cos(a)) # desired robot position
vRobot = v2PosB(robotConf, rp, vmax)
self.setMotorVelocities(vRobot[0], vRobot[1])
def keepGoalP(self, robotConf, Gy=0.72, vmax=40, Ex=0.18, Ey=0.07): # 0.72 for left goal, -0.72 for right goal
""" the robot keep the goal with a P controller by staying on an ellipse and focusing on ball position
configuration of the robot, princial axis of the ellipse, center of the goal """
# self.ballEngine.update()
bp = self.ballEngine.getBallPose() # ball position
a=math.atan2(bp[0],Gy-bp[1])
rp=(Ex*math.sin(a), Gy-Ey*math.cos(a)) # desired robot position
vRobot = v2PosP(robotConf, rp, vmax)
self.setMotorVelocities(vRobot[0], vRobot[1])
time.sleep(0.001)
def v2PosA(self, robotConf, finalPos, vmax=40, k=2, kp=200, amax=100):
"""v2pos with acceleration bounded """
x = finalPos[0] - robotConf[0]
y = finalPos[1] - robotConf[1]
norm = (x**2 + y**2)**.5 # euclidian norm
tv=self.ballEngine.getSimTime()
v=kp*norm
if v>vmax:
v=vmax
va=math.fabs(self.vm)+amax*(tv-self.tvm)
print 'va'
print va
if v>va:
v=va
self.tvm=tv
self.vm=v
print 'v'
print v
return v2PosB(robotConf, finalPos, v, k, rb=0.01)
def add_to_path(self, path_objective):
""" path objective is array-like, shape (3,-1) """
path_objective = np.asarray(path_objective).reshape(3, -1)
if self.path is not None:
self.path = np.column_stack((
self.path,
path_objective
))
else:
self.path = path_objective
def prunePath(self, xlim=0.455, ylim=0.705):
""" Removes elements in the path if they are not within the bounds.
Changes the self.path attribute according to this pruning.
Parameters
----------
xlim: number
the magnitude of the bounds of the field in the x direction
ylim: number
the magnitude of the bounds of the field in the y direction
"""
xGoal = 0.18
yGoal = 0.07
tol = 0.0125
tol2 = 0.025
Ex = 0.18 + tol2
Ey = 0.07 + tol2
Gy = 0.72
if self.color == 'Red':
Gy *= -1
for idx in range(self.path.shape[1]):
exceededX = np.abs(self.path[0,idx]) > xlim
exceededY = np.abs(self.path[1,idx]) > ylim
# within the boundary
if exceededX:
if self.path[0,idx] >= 0:
self.path[0,idx] = xlim - tol
else:
self.path[0,idx] = - (xlim - tol)
if exceededY:
if self.path[1,idx] >= 0:
self.path[1,idx] = ylim - tol
else:
self.path[1,idx] = - (ylim - tol)
insideEllipsis = (self.path[0,idx])**2/Ex**2 + (self.path[1,idx] - Gy)**2/Ey**2 <= 1
if insideEllipsis:
theta = math.atan2(-self.path[1,idx] + Gy, self.path[0,idx])
xNew = Ex*math.cos(theta)
yNew = -Ey*math.sin(theta) + Gy
self.path[0,idx] = xNew
self.path[1,idx] = yNew
def obstacleAwarePath(self, obstacleConf, rb = 0.025):
robotPosition = self.getRobotConf()
obstaclePath = interpolatePath(self.path, robotPosition)
index, distance = obstacleDetector(obstacleConf, obstaclePath, rb)
self.path = avoidObstacle(obstaclePath, obstacleConf, index, distance, rb)
def multiObstacleAwarePath(self, obstacleConfs, rb):
if obstacleConfs:
for conf in obstacleConfs:
self.obstacleAwarePath(conf, 0.07)
def receiveBall(self, proximitySensors, interceptVel = 17.5):
robotPosition = self.getRobotConf()
ballPosition = self.ballEngine.getBallPose()
pathNew = np.zeros((3,1))
pathNew[0,0] = ballPosition[0]
pathNew[1,0] = ballPosition[1]
pathNew[2,0] = interceptVel
self.path = pathNew
self.followPath(robotPosition)
proximity = min(proximitySensors)
if proximity < 0.001:
self.path = None
def unittestMoveForward(self):
self.setMotorVelocities(forward_vel=1, omega=0)
def unittestTurnSideways(self, not_first_time):
x, y, theta = self.getRobotPose()
if not_first_time:
goal_theta = self.first_theta + np.pi / 2
print goal_theta
error_theta = ThetaRange.angleDiff(theta, goal_theta)
# control
omega = 10 * error_theta
print omega
self.setMotorVelocities(0, omega)
# visualization
def plotCurrentDesiredHeadings():
plotVector(ThetaRange.pol2cart(1, theta), 'k')
plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
self.idash.add(plotCurrentDesiredHeadings)
self.idash.plotframe()
else:
self.first_theta = theta
def clean_exit(self):
vrep.simxFinish(self.clientID)
print 'Program ended'
def run(self):
if self.clientID!=-1:
if not self.multirobots:
# Start simulation if MultiRobotRunner not already starting it
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot)
self.robotCode()
if not self.multirobots:
# Stop VREP simulation cleanly if MultiRobotRunner not already stopping it
self.clean_exit()
def displayPath(self):
print('path=')
for i in range(0, self.path.shape[1]):
print('%f, %f' % (self.path[0, i], self.path[1, i]))
class Master(base_robot.MultiRobotCyclicExecutor):
def __init__(self, *args, **kwargs):
super(Master, self).__init__(*args, **kwargs)
self.color = None
self.idash = IDash(framerate=0.005)
def getClosestZone(self, pose):
""" Zones will be from downfield (0) to close to own goal (1) """
pose = np.array(pose[:2]) # x-y only
assert pose.size == 2 # pose should be 2-D
# 2 ZONES: 0 and 1
two_zone_boundary_y = 0.01
zone = pose[1] > two_zone_boundary_y
if self.color == 'Red':
zone = not zone
return zone
def run(self):
if self.clientID != -1:
_ = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
# robots have been added
self.color = self.bots[0].color
self.originalRoles = ['attacker', 'midfielder', 'goalie']
self.roles = ['attacker', 'midfielder', 'goalie']
goalie = self.bots[2]
activeidx = 0 # striker starts first
t0 = time.time()
while time.time() - t0 < 600:
self.ballEngine.update()
assert activeidx != 2 # now only have active 0,1
activebot = self.bots[activeidx]
offense = self.originalRoles[activeidx] == 'attacker'
if offense:
if self.roles[1] != 'dumb':
self.roles[1] = 'attacker'
else:
if self.roles[1] != 'dumb':
self.roles[1] = 'midfielder'
secondaryidx = not activeidx
ballx, bally = self.ballEngine.getBallPose()
robotX = np.zeros(2)
robotY = np.zeros(2)
robotX[0], robotY[0], _ = self.bots[0].getRobotConf()
robotX[1], robotY[1], _ = self.bots[1].getRobotConf()
distance2ball = 9999
for i in xrange(len(self.bots[:-1])):
dist = np.sqrt((ballx - robotX[i])**2 +
(bally - robotY[i])**2)
if dist < distance2ball:
distance2ball = dist
closestRobot = i
ballPosX, ballPosY = self.ballEngine.getBallPose()
for idx in range(len(self.bots[:-1])):
if offense:
oppGoalieConf, oppGoalieIdx = self.findOppGoalieConf(
return_index=True)
oppGoalieIdx = self.convertOppBotsIdx2BotsIdx(
oppGoalieIdx)
if idx == closestRobot:
self.bots[idx].robotCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs([
idx, oppGoalieIdx
]), # dont ignore yourself, or the oppGoalie
goaliePosition=self.findOppGoalieConf())
else: # robot is not closest robot
if True: # be a bitch
passivePos = oppGoalieConf
else:
if ballPosX >= 0:
passivePos = [-0.2, ballPosY, 0]
else: # ballPosX < 0
passivePos = [0.2, ballPosY, 0]
self.bots[idx].secondaryCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs(
[idx]), # attack the goalie
passivePos=passivePos)
# defense
else:
# secondary defender (original attacker being passive)
if idx == secondaryidx:
if self.bots[idx].color == 'Red':
passivePos = [ballPosX, 0.2, 0]
else:
passivePos = [ballPosX, -0.2, 0]
self.bots[idx].secondaryCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs(
secondaryidx),
passivePos=passivePos)
# primary defender (original defender being active)
else:
self.bots[idx].robotCode(
role=self.roles[idx],
obstacleConfs=self.getObstacleConfs(activeidx),
goaliePosition=self.findOppGoalieConf())
goalie.robotCode(
role=self.roles[2],
obstacleConfs=self.getObstacleConfs(activeidx),
goaliePosition=self.findOppGoalieConf())
def vizBots():
actx, acty = activebot.getRobotConf()[:2]
ballx, bally = self.ballEngine.getBallPose()
plt.hold('on')
try:
plt.plot(-self.activebot.passPos[1],
self.activebot.passPos, 'ro')
except:
pass
plt.plot(-acty, actx, 'g+')
try:
plt.plot(-activebot.target[1], activebot.target[0],
'm*')
except:
pass
plt.plot(-oppGoalieConf[1], oppGoalieConf[0], 'yo')
plt.plot(-bally, ballx, 'ro')
plt.plot(-self.bots[secondaryidx].path[1, :],
self.bots[secondaryidx].path[0, :], 'b.')
plt.plot(-activebot.path[1, :], activebot.path[0, :], 'g.')
plt.xlim([-0.8, 0.8]) # y axis in the field
plt.ylim([-0.7, 0.7]) # x axis in the field
plt.title('Red = RCV, Green = Active')
plt.xlabel('active path length: {}'.format(
activebot.path.shape[1]))
self.idash.add(vizBots)
# if time.time() - activebot.time_started_2b_dumb > 2:
# self.roles = self.originalRoles[:]
# if activebot.path.shape[1] < 5:
# self.roles[activeidx] = 'dumb'
# activebot.time_started_2b_dumb = time.time()
p0 = activebot.p0
p1 = self.ballEngine.getBallPose()
p3 = self.ballEngine.getNextRestPos()
dist_from_start = np.sqrt((p1[0] - p0[0])**2 +
(p1[1] - p0[1])**2)
velocity_measure = np.sqrt((p1[0] - p3[0])**2 +
(p1[1] - p3[1])**2)
closest_zone = self.getClosestZone(p1)
if dist_from_start > 0.01: # the ball has been touched
# if velocity_measure < 0.003: # wait til velocity reaches zero
# if True:
# if velocity_measure < 1.0: # start kicking while ball is moving...
for bot in self.bots:
bot.executing = False
if closest_zone != activeidx: # success
# increment what is the new active zone
# self.setMotorVelocities(0, 0)
activeidx = not activeidx
class ZonePasserMasterCyclic(base_robot.MultiRobotCyclicExecutor):
"""After doing part A of Question 2, the ball will already be
placed in Zone 4; so let's transport it there now"""
def __init__(self, *args, **kwargs):
super(ZonePasserMasterCyclic, self).__init__(*args, **kwargs)
self.idash = IDash(0.005)
self.activezones = []
self.receivingzones = []
self.activeplayer = None
self.receivingplayer = None
self.zones = {(1, 1): 1, (1, -1): 2, (-1, 1): 3, (-1, -1): 4}
self.zonesigns = [(1, 1), (1, -1), (-1, 1), (-1, -1)]
self.zone_centers = np.array(
[[CENTER_X, CENTER_X, -CENTER_X, -CENTER_X],
[CENTER_Y, -CENTER_Y, CENTER_Y, -CENTER_Y]])
self.zone_corners = np.array(
[[CORNER_X, CORNER_X, -CORNER_X, -CORNER_X],
[CORNER_Y, -CORNER_Y, CORNER_Y, -CORNER_Y]])
# TODO: remove me since we're not allowed to set the ball pose
# start ball at zone 4 - 1 (0 indexed)
ball_start = self.zone_centers[:, 3]
ball_start[
1] -= 0.05 # move closer to players center, but further distance q from player
# self.ballEngine.setBallPose(ball_start)
self.ballEngine.update()
# FIXME: Hardcoded is good for drill, not good for game!
self.zone_pass_plan = [4, 2, 1, 3, 4,
2] # the locations between the 5 passes
self.activebot_idx_plan = [
0, 1, 2, 0, 1, 2
] # which robot should take the active zone during the zone pass plan?
def getClosestZone(self, pose):
""" get zone which the current pose is closest to. This pose could
be a ball, an opposing player, etc. """
pose = np.array(pose[:2]) # x-y only
assert pose.size == 2 # pose should be 2-D
dist_from_zones = cdist(np.expand_dims(pose, axis=0),
self.zone_corners.T)
return np.argmin(dist_from_zones) + 1 # zones are 1-indexed
def getBotInZone(self, zone):
"""
Not using now
Returns the index of the bot which is inside the zone, otherwise None
if there is no bot inside this zone"""
bot_zones = np.zeros(len(self.bots))
for bot_idx, bot in enumerate(self.bots):
bot_zones[bot_idx] = self.getClosestZone(bot.getRobotConf(bot.bot))
# we use [0][0] since return of np.where looks like (array([2]),)
zone_as_array = np.where(bot_zones == zone)[0]
if zone_as_array.size == 0:
return None
else:
return zone_as_array[
0] # the first, if there are multiple in that zone
def getNearestBotToZone(self, zone, bot_to_ignore=None):
""" Returns the index of the bot closest to the zone, even if the bot
may not be directly inside the zone """
bot_poses = np.vstack([bot.getRobotConf() for bot in self.bots])
bot_poses = bot_poses[:, :2]
assert bot_poses.shape == (len(self.bots), 2)
zone_pose = self.zone_corners[:, zone - 1].reshape(1, 2)
dist_from_bots = cdist(zone_pose, bot_poses, 'cityblock')
if bot_to_ignore is None:
return np.argmin(dist_from_bots)
else: # the closest bot might be active, in which case we should return next closest
sortedargs = np.argsort(dist_from_bots.flatten())
if sortedargs[
0] == bot_to_ignore: # first closest is active, not valid to be receive
return sortedargs[1] # return second closest
else:
return sortedargs[0]
def planToMoveIntoReceivingPosition(self,
idx,
rcvzone=None,
startSmoothPathConf=None,
vel=15,
r=0.01):
""" Move into the corner, facing center
Parameters
----------
idx: integer
index of the robot (in self.bots)
startSmoothPathConf: array-like, shape (3,) or None
the start configuration of the robot to calculate the smooth path.
Pass this if you are calculating a path from a future point.
If None, then use current robot configuration.
"""
if startSmoothPathConf is None:
startSmoothPathConf = self.bots[idx].getRobotConf()
if rcvzone is None:
rcvzone = self.zone_pass_plan[idx]
final_x = self.bots[idx].zone_corners[0, rcvzone - 1]
final_y = self.bots[idx].zone_corners[1, rcvzone - 1]
# final theta will be facing towards center field
_, final_theta = ThetaRange.cart2pol(final_x, final_y)
final_theta = ThetaRange.normalize_angle(
final_theta + np.pi) # flip it towards center
smooth_path, status = smoothPath(
startSmoothPathConf, # robotConf
[final_x, final_y, final_theta], # finalConf
r=r)
v = vel * np.ones((1, np.size(smooth_path, 1)))
smooth_path = np.concatenate((smooth_path, v), axis=0)
if self.bots[idx].path is None:
self.bots[idx].add_to_path(smooth_path)
elif self.bots[idx].path.shape[1] == 1:
self.bots[idx].add_to_path(smooth_path)
else:
self.bots[idx].path = smooth_path
def calculateReceivingDestination(self, xy1, xy2, k=0.2):
""" receiving BallPos should be between rcvbot and next passing zone
Parameters
----------
xy1: array-like, shape (2,)
x-y position of receiving bot
xy2: array-like, shape (2,)
x-y position of next receiving zone
k: float, between 0-1
multiplier to determine how much in between the rcv and nextrcv
"""
x1, y1 = xy1
x2, y2 = xy2
theta = np.arctan2(y1 - y2, x1 - x2)
hyp = np.sqrt((x1 - x2)**2 + (y1 - y2)**2)
rcvBallPos = [
x1 - k * hyp * np.cos(theta), y1 - k * hyp * np.sin(theta)
]
return rcvBallPos
def calculateShootingDestination(self):
# TODO: Intelligently use position of moving opponenet goalie
posToAim = [0, -1.0] # in the center of the goal
return posToAim
def run(self):
if self.clientID != -1:
_ = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot_wait)
# set them up in the first 3 zones
for idx in range(len(self.bots)):
if self.zone_pass_plan[idx] == 2:
# if False:
# zone 2 has some robots blocking, so navigate to zone 2
# in a couple of steps
# TOOD: may want to do this less hardcoded, more like planned path with obstacle avoidance
# step 1. get to midfield passing the opposing team
x_coord_zone = 0.3
self.bots[idx].add_to_path([x_coord_zone, 0, 20])
startSmoothPathConf = [x_coord_zone, 0, -np.pi / 2]
else:
startSmoothPathConf = self.bots[idx].getRobotConf(
self.bots[idx].bot)
self.planToMoveIntoReceivingPosition(
idx, startSmoothPathConf=startSmoothPathConf, vel=10)
self.bots[idx].add_delay(1 * idx) # delay each by one second
# TODO: replace system time with vrep simxTime
t0 = time.time()
# get the bots into position
while time.time() - t0 < 15:
for bot in self.bots:
if time.time() - t0 >= bot.delay:
bot.robotCode()
# follow the zone passing plan
first_loop = True
activezone_idx = 0
shoot_flag = False
plan_length = len(self.zone_pass_plan)
executing = [False] * len(self.bots)
# TODO: maybe something like below with states for all bots?
bot_states = [STATE_READY_POS] * len(self.bots)
t1 = time.time()
while time.time() - t1 < 1000:
self.ballEngine.update()
activezone = self.zone_pass_plan[activezone_idx]
activebot_idx = self.activebot_idx_plan[activezone_idx]
activebot = self.bots[activebot_idx]
if plan_length > activezone_idx + 1:
rcvzone = self.zone_pass_plan[activezone_idx + 1]
rcvbot_idx = self.activebot_idx_plan[activezone_idx + 1]
rcvbot = self.bots[rcvbot_idx]
else:
rcvzone = None
rcvbot_idx = None
rcvbot = None
shoot_flag = True
if plan_length > activezone_idx + 2:
next_rcvzone = self.zone_pass_plan[activezone_idx + 2]
else:
next_rcvzone = None
# def vizZones():
# zones = np.zeros(4)
# zones[activezone-1] = 0.5
# if rcvzone:
# zones[rcvzone-1] = 1.0
# plt.imshow(zones.reshape(2,2), interpolation='nearest')
# plt.title('Red = RCV, Green = Active')
# self.idash.add(vizZones)
# -- STATE MACHINE UPDATE PARAMETERS
if shoot_flag:
bot_states[activebot_idx] = STATE_SHOOT
else:
bot_states[activebot_idx] = STATE_PASS
if rcvbot_idx:
bot_states[rcvbot_idx] = STATE_READY_POS
# -- STATE MACHINE EXECUTE
# _, proximitySensors = rcvbot.senseObstacles()
# if bot_states[rcvbot_idx] == STATE_READY_POS:
# rcvbot.receiveBall(proximitySensors)
# rcvp1 = np.array(rcvbot.getRobotConf()[:2])
# rcvp2 = self.zone_corners[:, rcvzone - 1]
# # not yet in position
# if cdist(rcvp1.reshape(1,2), rcvp2.reshape(1,2))[0] > 0.001: # radius buffer
# # FIXME: if receiving bot is moving at the same time as active bot, processing time increases by ~100ms
# if not executing[rcvbot_idx]:
# if next_rcvzone:
# xy2 = self.zone_centers[:,next_rcvzone-1]
# else:
# xy2 = [0, -0.75]
# self.planToMoveIntoReceivingPosition(rcvbot_idx, rcvzone, vel=10)
# rcvbot.prunePath()
# rcvbot.pause_before_kick = rcvbot.path.shape[1] > 2
# executing[rcvbot_idx] = True
# rcvbot.robotCode(rb=0.05, pause_before_kick=rcvbot.pause_before_kick)
# else:
# executing[rcvbot_idx] = False
if bot_states[activebot_idx] == STATE_PASS:
if not executing[activebot_idx]:
activeRobotConf = activebot.getRobotConf()
p0 = self.ballEngine.getBallPose()
# -- fancy calculation of finalBallPos, using info about where next
xy1 = self.zone_centers[:, rcvzone - 1]
if next_rcvzone:
xy2 = self.zone_centers[:, next_rcvzone - 1]
else:
xy2 = [0, -0.75]
finalBallPos = self.calculateReceivingDestination(
xy1, xy2, k=0.15)
# -- simple calculation of finalBallPos
# mult_x, mult_y = self.zonesigns[rcvzone-1]
# finalBallPos = [ np.abs(p0[0])*mult_x , np.abs(p0[1])*mult_y ]
# slow mode (tested)
# activebot.path, status = passPath(activeRobotConf, p0, finalBallPos, vmax=10, vr=7, kq=0.0035, hold=True)
# fast mode (good luck!)
activebot.path, status = passPath(activeRobotConf,
p0,
finalBallPos,
vmax=20,
vr=15,
kq=0.0010,
hold=False)
# avoid all friendly and opposing robots
obstacleConf = [
self.oppBots[i].getRobotConf()
for i in range(len(self.oppBots))
]
obstacleConf.extend([
self.bots[i].getRobotConf()
for i in range(len(self.bots))
if i != activebot_idx
])
for i in xrange(len(obstacleConf)):
activebot.obstacleAwarePath(obstacleConf[i], 0.05)
# avoid the wall boundaries
activebot.prunePath()
activebot.pause_before_kick = activebot.path.shape[
1] > 2
p2 = self.ballEngine.getNextRestPos()
# FIXME: if the path produced is invalid, i.e some part of it is off the field and invalid
# prune that part of the path to make it valid
# FIXME: if path is not long enough
# backup to give more room. or bump the ball and get more room.
def vizBots():
actx, acty = activebot.getRobotConf()[:2]
rcvx, rcvy = rcvbot.getRobotConf()[:2]
plt.hold('on')
plt.plot(-acty, actx, 'g+')
plt.plot(-activebot.path[1, :],
activebot.path[0, :], 'g.')
plt.plot(-rcvy, rcvx, 'r+')
plt.plot(-rcvbot.path[1, :], rcvbot.path[0, :],
'r.')
plt.plot(-p0[1], p0[0], 'bo')
#plt.plot(-rcvp1[1], rcvp1[0], 'mx')
#plt.plot(-rcvp2[1], rcvp2[0], 'kx')
plt.xlim([-0.75, 0.75]) # y axis in the field
plt.ylim([-0.5, 0.5]) # x axis in the field
plt.title('Red = RCV, Green = Active')
plt.xlabel('active path length: {}'.format(
activebot.path.shape[1]))
self.idash.add(vizBots)
executing[activebot_idx] = True
# Edit the path based on the ball movement
# self.ballEngine.update()
# deltaxy = np.array(self.ballEngine.getVelocity())
# print "deltaxy: ", deltaxy
# for idx in range(activebot.path.shape[1]):
# gamma = 1 # no decay
# # gamma = 1 / idx # decay factor
# activebot.path[:2,-(idx+1)] += gamma*deltaxy*10
# Follow the path
activebot.robotCode(
rb=0.05, pause_before_kick=activebot.pause_before_kick)
# activebot.robotCode(rb=0.05, pause_before_kick=False)
if bot_states[activebot_idx] == STATE_SHOOT:
if not executing[activebot_idx]:
activeRobotConf = activebot.getRobotConf(activebot.bot)
ballRestPos = self.ballEngine.getBallPose()
finalBallPos = self.calculateShootingDestination()
activebot.path, status = passPath(activeRobotConf,
ballRestPos,
finalBallPos,
vmax=25,
vr=15,
kq=0.0010,
hold=True)
activebot.prunePath()
obstacleConfs = self.getObstacleConfs([activebot_idx])
activebot.multiObstacleAwarePath(obstacleConfs, 0.05)
activebot.prunePath()
executing[activebot_idx] = True
activebot.robotCode()
def vizShooting():
actx, acty = activebot.getRobotConf()[:2]
plt.hold('on')
plt.plot(-acty, actx, 'g+')
plt.plot(-activebot.path[1, :], activebot.path[0, :],
'g.')
plt.xlim([-0.75, 0.75]) # y axis in the field
plt.ylim([-0.5, 0.5]) # x axis in the field
plt.title('SHOOT!!!!')
self.idash.add(vizShooting)
if first_loop:
pass
first_loop = False
else:
pass
print "Elapsed (MS): ", (time.time() - time_start) * 1e3
# Do measurements, whether pass was successful and state changes
time_start = time.time()
p1 = self.ballEngine.getBallPose()
p3 = self.ballEngine.getNextRestPos()
dist_from_start = np.sqrt((p1[0] - p0[0])**2 +
(p1[1] - p0[1])**2)
# dist_from_goal = np.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)
# velocity_measure = np.sqrt((p1[0] - p3[0])**2 + (p1[1] - p3[1])**2)
vx, vy = self.ballEngine.getVelocity()
velocity_measure = np.sqrt(vx**2 + vy**2)
print velocity_measure
# self.idash.add(lambda: plt.plot(velocity_measure))
closest_zone = self.getClosestZone(p1)
if dist_from_start > 0.01: # the ball has been touched
if velocity_measure < 0.003: # wait til velocity reaches zero
# if dist_from_goal < 0.1: # wait til the ball has entered the predicted zone
executing = [False] * len(
self.bots
) # everyone has new roles, plan next pass
if closest_zone == rcvzone: # success
# increment what is the new active zone
activebot.setMotorVelocities(0, 0)
activezone_idx += 1
self.idash.plotframe() # ~100 ms shaved by removing this line
# time.sleep(50*1e-3)
self.clean_exit()
class BaseRobotRunner(object):
__metaclass__ = abc.ABCMeta
def __init__(self,
color,
number,
clientID=None,
ip='127.0.0.1',
port=19998): # ip='127.0.0.1', '172.29.34.63'
"""
color: i.e. 'Blue'
number: i.e. 1
clientID: Default None; if provided as parameter,
`vrep.simxStart` was called outside this class
"""
# parameter init
self.ip = ip
self.port = port
self.color = color
self.bot_name = '%s%d' % (color, number)
self.bot_nameStriker = 'Red1'
self.vm = self.v = 0
self.tv = self.tvm = 0
# run startup methods
if clientID is not None:
# Use existing clientID (multi robot case)
self.clientID = clientID
self.multirobots = True
print(
"ClientID obtained as parameter! (Probably doing Multirobots...)"
)
else:
# Make connection to VREP client (single robot case)
self.initializeVrepClient()
self.multirobots = False
self.initializeVrepApi()
#self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
self.ballEngine = BallEngine(self.clientID)
def exploreClassAttributes(self):
for variable_name, variable_value in self.__dict__.iteritems():
locals()[self.bot_name + "_" + variable_name] = variable_value
# delete so there are no duplicate variables in the variable explorer
del (variable_name)
del (variable_value)
return # Place Spyder Breakpoint on this Line!
def initializeVrepClient(self):
""" Initialization for Python to connect to VREP.
We obtain a clientID after connecting to VREP, which is saved to
`self.clientID`
This method is only called if we controlling one robot with the remote API
"""
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID = vrep.simxStart(self.ip, self.port, True, True,
5000,
5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID != -1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor = vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.rightMotor = vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.bot = vrep.simxGetObjectHandle(self.clientID, self.bot_name,
vrep.simx_opmode_oneshot_wait)
# initialize proximity sensors
self.proxSensors = prox_sens_initialize(self.clientID, self.bot_name)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(self.clientID, self.bot, -1,
vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
# FIXME: striker handles shouldn't be part of the default base class
# FIXME: should be better way to have information regarding all the bots (Central Control?) (Publishers/Subscribers...)?
# initialize bot handles and variables for Red1
_, self.leftMotorStriker = vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker = vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name,
vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(self.clientID,
self.botStriker, -1,
vrep.simx_opmode_streaming)
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
@abc.abstractmethod
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
return
def secondaryCode(self, role, obstacleConfs=None, passivePos=None):
"""inner while loop for when attacker is not active"""
# FIXME: role does nothing, but maybe there should be different
# modes of operation for passive roles
if passivePos is None:
passivePos = self.passivePos
if not self.executing:
self.status = 0 # for moving freely, without theta adjust
self.path, self.status = smoothPath(self.getRobotConf(self.bot),
passivePos,
r=0.08)
vf = 10
v = vf * np.ones((1, np.size(self.path, 1)))
self.path = np.concatenate((self.path, v), axis=0)
self.path[-1, -1] = vf
self.prunePath()
self.multiObstacleAwarePath(obstacleConfs, 0.04)
self.prunePath()
self.followPath(self.getRobotConf(self.bot), self.status, rb=0.05)
def getRobotConf(self, robot_handle=None):
if robot_handle is None:
robot_handle = self.bot
_, xyz = vrep.simxGetObjectPosition(self.clientID, robot_handle, -1,
vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(self.clientID,
robot_handle, -1,
vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def senseObstacles(self):
"""
Returns
-------
objectDetected: array-like, shape 4
whether an obstacle was detected for the 4 sensors
objectDistances: array-like, shape 4
the distances detected
"""
out = prox_sens_read(self.clientID, self.proxSensors)
objectDetected = np.zeros(4)
objectDistances = 100 * np.ones(4)
for i, sens in enumerate(out):
if sens['detectionState'] == 1:
objectDetected[i] = 1
x, y, z = sens['detectedPoint']
a = np.sqrt(x**2 + y**2)
distance = np.sqrt(a**2 + z**2)
objectDistances[i] = distance
return objectDetected, objectDistances
def setMotorVelocities(self, forward_vel, omega):
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel,
omega,
g=1)
self.driveMotor(ctrl_sig_left, ctrl_sig_right)
def driveMotor(self, vl, vr):
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.leftMotor, vl,
vrep.simx_opmode_streaming) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.rightMotor, vr,
vrep.simx_opmode_streaming) # set right wheel velocity
def repulsion_vectors_compute(self, lidarValues, k_repulse=10.0):
numLidarValues = len(lidarValues)
lidarAngles = [
np.pi / numLidarValues * index for index in range(numLidarValues)
]
repulsionVectors = [
np.array(
pol2cart(force_repulsion(k_repulse, np.sqrt(val), 2), angle))
for val, angle in zip(lidarValues, lidarAngles)
]
return repulsionVectors
def followPath(self, robotConf, status=0, rb=0.05, dtheta=0.001, k=3.5):
""" radius of the buffer zone
"""
robotpos = np.array(robotConf)[0:2]
if status == 2 and self.path.shape[1] == 2 and np.linalg.norm(
robotpos - self.path[0:2, 0]) < rb:
# print 'path'
# print self.path
theta = math.atan2(self.path[1, -1] - self.path[1, 0],
self.path[0, -1] - self.path[0, 0])
finalConf = (self.path[0, 0], self.path[1, 0], theta)
# print 'finalConf'
# print finalConf
vRobot = v2orientation(robotConf, finalConf)
self.setMotorVelocities(vRobot[0], vRobot[1])
if vRobot[1] == 0:
self.path = self.path[:, 1:]
return 2
while self.path.shape[1] > 1 and np.linalg.norm(
robotpos - self.path[0:2, 0]) < rb:
self.path = self.path[:, 1:] # remove first node
if self.path.shape[1] == 1 and np.linalg.norm(robotpos -
self.path[0:2, 0]) < rb:
self.setMotorVelocities(0, 0)
return 0
else:
vRobot = v2Pos(robotConf, self.path[0:2, 0], self.path[2, 0], k)
self.setMotorVelocities(vRobot[0], vRobot[1])
return 1
def keepGoal(self,
robotConf,
y=0.7,
vmax=30,
goalLim=0.2): # 0.7 for left goal, -0.7 for right goal
""" the robot keep the goal by staying on a line and focusing on ball displacement """
tol = 0.0001
self.ballEngine.update()
pm = self.ballEngine.posm2 # previous position
p = self.ballEngine.getBallPose() # position
if math.fabs(p[1] - pm[1]) < tol:
finalPos = [0, y]
vRobot = v2PosB(robotConf, finalPos)
self.setMotorVelocities(vRobot[0], vRobot[1])
return 1
if y * (p[1] - pm[1]) < 0:
self.setMotorVelocities(0, 0)
return 0
a = (y - pm[1]) / (p[1] - pm[1]) # intersection: (x,y)^t = pm+a(p-pm)
x = pm[0] + a * (p[0] - pm[0])
if (x > goalLim):
x = goalLim
if (x < -goalLim):
x = -goalLim
finalPos = [x, y]
vRobot = v2PosB(robotConf, finalPos, vmax)
self.setMotorVelocities(vRobot[0], vRobot[1])
return 0
def keepGoal2(self,
robotConf,
Gy=0.72,
vmax=25,
Ex=0.18,
Ey=0.07): # 0.72 for left goal, -0.72 for right goal
""" the robot keep the goal by staying on an ellipse and focusing on ball position
configuration of the robot, princial axis of the ellipse, center of the goal """
# self.ballEngine.update()
bp = self.ballEngine.getBallPose() # ball position
a = math.atan2(bp[0], Gy - bp[1])
rp = (Ex * math.sin(a), Gy - Ey * math.cos(a)
) # desired robot position
vRobot = v2PosB(robotConf, rp, vmax)
self.setMotorVelocities(vRobot[0], vRobot[1])
def keepGoalP(self,
robotConf,
Gy=0.72,
vmax=40,
Ex=0.18,
Ey=0.07): # 0.72 for left goal, -0.72 for right goal
""" the robot keep the goal with a P controller by staying on an ellipse and focusing on ball position
configuration of the robot, princial axis of the ellipse, center of the goal """
# self.ballEngine.update()
bp = self.ballEngine.getBallPose() # ball position
a = math.atan2(bp[0], Gy - bp[1])
rp = (Ex * math.sin(a), Gy - Ey * math.cos(a)
) # desired robot position
vRobot = v2PosP(robotConf, rp, vmax)
self.setMotorVelocities(vRobot[0], vRobot[1])
time.sleep(0.001)
def v2PosA(self, robotConf, finalPos, vmax=40, k=2, kp=200, amax=100):
"""v2pos with acceleration bounded """
x = finalPos[0] - robotConf[0]
y = finalPos[1] - robotConf[1]
norm = (x**2 + y**2)**.5 # euclidian norm
tv = self.ballEngine.getSimTime()
v = kp * norm
if v > vmax:
v = vmax
va = math.fabs(self.vm) + amax * (tv - self.tvm)
print 'va'
print va
if v > va:
v = va
self.tvm = tv
self.vm = v
print 'v'
print v
return v2PosB(robotConf, finalPos, v, k, rb=0.01)
def add_to_path(self, path_objective):
""" path objective is array-like, shape (3,-1) """
path_objective = np.asarray(path_objective).reshape(3, -1)
if self.path is not None:
self.path = np.column_stack((self.path, path_objective))
else:
self.path = path_objective
def prunePath(self, xlim=0.455, ylim=0.705):
""" Removes elements in the path if they are not within the bounds.
Changes the self.path attribute according to this pruning.
Parameters
----------
xlim: number
the magnitude of the bounds of the field in the x direction
ylim: number
the magnitude of the bounds of the field in the y direction
"""
xGoal = 0.18
yGoal = 0.07
tol = 0.0125
tol2 = 0.025
Ex = 0.18 + tol2
Ey = 0.07 + tol2
Gy = 0.72
if self.color == 'Red':
Gy *= -1
for idx in range(self.path.shape[1]):
exceededX = np.abs(self.path[0, idx]) > xlim
exceededY = np.abs(self.path[1, idx]) > ylim
# within the boundary
if exceededX:
if self.path[0, idx] >= 0:
self.path[0, idx] = xlim - tol
else:
self.path[0, idx] = -(xlim - tol)
if exceededY:
if self.path[1, idx] >= 0:
self.path[1, idx] = ylim - tol
else:
self.path[1, idx] = -(ylim - tol)
insideEllipsis = (self.path[0, idx])**2 / Ex**2 + (
self.path[1, idx] - Gy)**2 / Ey**2 <= 1
if insideEllipsis:
theta = math.atan2(-self.path[1, idx] + Gy, self.path[0, idx])
xNew = Ex * math.cos(theta)
yNew = -Ey * math.sin(theta) + Gy
self.path[0, idx] = xNew
self.path[1, idx] = yNew
def obstacleAwarePath(self, obstacleConf, rb=0.025):
robotPosition = self.getRobotConf()
obstaclePath = interpolatePath(self.path, robotPosition)
index, distance = obstacleDetector(obstacleConf, obstaclePath, rb)
self.path = avoidObstacle(obstaclePath, obstacleConf, index, distance,
rb)
def multiObstacleAwarePath(self, obstacleConfs, rb):
if obstacleConfs:
for conf in obstacleConfs:
self.obstacleAwarePath(conf, 0.07)
def receiveBall(self, proximitySensors, interceptVel=17.5):
robotPosition = self.getRobotConf()
ballPosition = self.ballEngine.getBallPose()
pathNew = np.zeros((3, 1))
pathNew[0, 0] = ballPosition[0]
pathNew[1, 0] = ballPosition[1]
pathNew[2, 0] = interceptVel
self.path = pathNew
self.followPath(robotPosition)
proximity = min(proximitySensors)
if proximity < 0.001:
self.path = None
def unittestMoveForward(self):
self.setMotorVelocities(forward_vel=1, omega=0)
def unittestTurnSideways(self, not_first_time):
x, y, theta = self.getRobotPose()
if not_first_time:
goal_theta = self.first_theta + np.pi / 2
print goal_theta
error_theta = ThetaRange.angleDiff(theta, goal_theta)
# control
omega = 10 * error_theta
print omega
self.setMotorVelocities(0, omega)
# visualization
def plotCurrentDesiredHeadings():
plotVector(ThetaRange.pol2cart(1, theta), 'k')
plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
self.idash.add(plotCurrentDesiredHeadings)
self.idash.plotframe()
else:
self.first_theta = theta
def clean_exit(self):
vrep.simxFinish(self.clientID)
print 'Program ended'
def run(self):
if self.clientID != -1:
if not self.multirobots:
# Start simulation if MultiRobotRunner not already starting it
_ = vrep.simxStartSimulation(self.clientID,
vrep.simx_opmode_oneshot)
self.robotCode()
if not self.multirobots:
# Stop VREP simulation cleanly if MultiRobotRunner not already stopping it
self.clean_exit()
def displayPath(self):
print('path=')
for i in range(0, self.path.shape[1]):
print('%f, %f' % (self.path[0, i], self.path[1, i]))
class FinalProjectProgram():
def __init__(self, color, number):
# parameter init
self.bot_name = '%s%d' % (color, number)
self.bot_nameStriker = 'Red1'
# run startup methods
self.initializeVrepClient()
self.initializeVrepApi()
# self.killer = GracefulKiller()
self.idash = IDash(framerate=0.05)
def initializeVrepClient(self):
#Initialisation for Python to connect to VREP
print 'Python program started'
count = 0
num_tries = 10
while count < num_tries:
vrep.simxFinish(-1) # just in case, close all opened connections
self.clientID=vrep.simxStart('127.0.0.1',19999,True,True,5000,5) #Timeout=5000ms, Threadcycle=5ms
if self.clientID!=-1:
print 'Connected to V-REP'
break
else:
"Trying again in a few moments..."
time.sleep(3)
count += 1
if count >= num_tries:
print 'Failed connecting to V-REP'
vrep.simxFinish(self.clientID)
def initializeVrepApi(self):
# initialize bot handles and variables
_, self.leftMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotor=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.bot=vrep.simxGetObjectHandle(
self.clientID, self.bot_name, vrep.simx_opmode_oneshot_wait)
# proxSens = prox_sens_initialize(self.clientID)
# initialize odom of bot
_, self.xyz = vrep.simxGetObjectPosition(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
_, self.eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, self.bot, -1, vrep.simx_opmode_streaming)
# # initialize overhead cam
# _, self.overheadCam=vrep.simxGetObjectHandle(
# self.clientID, 'Global_Vision', vrep.simx_opmode_oneshot_wait)
# _, self.resolution, self.image = vrep.simxGetVisionSensorImage(
# self.clientID,self.overheadCam,0,vrep.simx_opmode_oneshot_wait)
# # initialize goal handle + odom
# _, self.goalHandle=vrep.simxGetObjectHandle(
# self.clientID, 'Goal_Position', vrep.simx_opmode_oneshot_wait)
# _, self.goalPose = vrep.simxGetObjectPosition(
# self.clientID, self.goalHandle, -1, vrep.simx_opmode_streaming)
# initialize bot handles and variables for Red1
_, self.leftMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_leftJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.rightMotorStriker=vrep.simxGetObjectHandle(
self.clientID, '%s_rightJoint' % self.bot_name, vrep.simx_opmode_oneshot_wait)
_, self.botStriker = vrep.simxGetObjectHandle(
self.clientID, self.bot_nameStriker, vrep.simx_opmode_oneshot_wait)
_, xyzStriker = vrep.simxGetObjectPosition(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
_, eulerAnglesStriker = vrep.simxGetObjectOrientation(
self.clientID, self.botStriker, -1, vrep.simx_opmode_streaming)
def robotCode(self):
""" OUR ROBOT CODE GOES HERE """
# count = 0
# while True:
# self.unittestTurnSideways(count)
# count += 1
xGoalie = self.getRobotPose(self.bot)[0]
yGoalie = self.getRobotPose(self.bot)[1]
thetaGoalie = self.getRobotPose(self.bot)[2]
thetaStriker = self.getRobotPose(self.botStriker)[2]
# ball pose
ballPose = (0, 0) # (x, y)
goalie2Ball = abs(yGoalie - ballPose[1]) # distance between ball and goalkeeper
goalieDistance = abs(goalie2Ball * math.tan(thetaStriker))
if thetaStriker < 0:
xSaving = xGoalie + goalieDistance
else:
xSaving = xGoalie - goalieDistance
savingPosition = [xSaving, yGoalie, z] # position in which the the goalie saves the ball
# desiredHeading = -math.pi/2
# tolerance = 5*math.pi/360
# error = []
# Kp = 2
# Tdiff = 0.01
# Tint = 0.1
# Tsample = 0.02
# i = 0
#
# while ((thetaGoalie < (desiredHeading - tolerance) or thetaGoalie > (desiredHeading + tolerance))) and i <= 20:
# error.append(abs(thetaGoalie - desiredHeading))
# if i == 0:
# vDiff = Kp*error[i]
# else:
# errorSum = 0
# for j in range(i):
# errorSum += error[j]
# vDiff = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.leftMotor,-vDiff,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.rightMotor,vDiff,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
# time.sleep(Tsample)
# thetaGoalie = self.getRobotPose(self.bot)[2]
# #print thetaGoalie
# i += 1
#
# xTolerance = 0.0001
# error = []
# i = 0
# Kp = 100
# Tint = 1
# Tsample = 0.01
#
# while ((xGoalie < (xSaving - xTolerance) or xGoalie > (xSaving + xTolerance))) and i <= 25:
# error.append(xGoalie - xSaving)
# if i == 0:
# vComm = Kp*error[i]
# else:
# errorSum = 0
# for j in range(i):
# errorSum += error[j]
# vComm = Kp*(error[i] + Tdiff/Tsample*(error[i] - error[i-1]) + Tsample*Tint*errorSum)
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.leftMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
# _ = vrep.simxSetJointTargetVelocity(
# self.clientID,self.rightMotor,-vComm,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
# time.sleep(Tsample)
# xGoalie = self.getRobotPose(self.bot)[0]
# #print xGoalie
# i += 1
while 1:
robotConf = self.getRobotConf(self.bot)
print('robotConf.x=%f' % robotConf[0])
print('robotConf.y=%f' % robotConf[1])
print('robotConf.theta=%f' % robotConf[2])
print('savingPosition.x=%f' % savingPosition[0])
print('savingPosition.y=%f' % savingPosition[1])
print('savingPosition.theta=%f' % savingPosition[2])
vRobot = v2Pos(robotConf, savingPosition)
print('vRobot.x=%f' % vRobot[0])
print('vRobot.y=%f' % vRobot[1])
self.setMotorVelocities(vRobot[0], vRobot[1])
#self.setDriveVelocities(0, 0)
time.sleep(3)
def getRobotConf(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def getRobotPose(self, robot_handle):
_, xyz = vrep.simxGetObjectPosition(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
_, eulerAngles = vrep.simxGetObjectOrientation(
self.clientID, robot_handle, -1, vrep.simx_opmode_buffer)
x, y, z = xyz
theta = eulerAngles[2]
return (x, y, theta)
def setMotorVelocities(self, forward_vel, omega):
ctrl_sig_left, ctrl_sig_right = vomega2bytecodes(forward_vel, omega, g=1)
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.leftMotor,ctrl_sig_left,vrep.simx_opmode_oneshot_wait) # set left wheel velocity
_ = vrep.simxSetJointTargetVelocity(
self.clientID,self.rightMotor,ctrl_sig_right,vrep.simx_opmode_oneshot_wait) # set right wheel velocity
def unittestMoveForward(self):
self.setMotorVelocities(forward_vel=1, omega=0)
def unittestTurnSideways(self, not_first_time):
x, y, theta = self.getRobotPose()
if not_first_time:
goal_theta = self.first_theta + np.pi / 2
print goal_theta
error_theta = ThetaRange.angleDiff(theta, goal_theta)
# control
omega = 10 * error_theta
print omega
self.setMotorVelocities(0, omega)
# visualization
def plotCurrentDesiredHeadings():
plotVector(ThetaRange.pol2cart(1, theta), 'k')
plotVector(ThetaRange.pol2cart(1, goal_theta), 'r')
self.idash.add(plotCurrentDesiredHeadings)
self.idash.plotframe()
else:
self.first_theta = theta
def clean_exit(self):
_ = vrep.simxStopSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
vrep.simxFinish(self.clientID)
print 'Program ended'
def run(self):
if self.clientID!=-1:
_ = vrep.simxStartSimulation(self.clientID,vrep.simx_opmode_oneshot_wait)
self.robotCode()
self.clean_exit()