async def cozmo_program(robot: cozmo.robot.Robot):
visible = 0
c**t = 0
while True:
robotPose = Frame2D.fromPose(robot.pose)
#print("Robot pose: " + str(robotPose))
cubeIDs = (cozmo.objects.LightCube1Id,cozmo.objects.LightCube2Id,cozmo.objects.LightCube3Id)
c**t += 1
for cubeID in cubeIDs:
cube = robot.world.get_light_cube(cubeID)
if cube.is_visible:
visible += 1
if c**t % 100 == 0:
c**t = 0
print("cube visibility visibility ------> ",(visible / 100 ) * 100," %")
visible = 0
for cubeID in cubeIDs:
cube = robot.world.get_light_cube(cubeID)
if cube.is_visible:
print("Visible: " + cube.descriptive_name + " (id=" + str(cube.object_id) + ")")
cubePose = Frame2D.fromPose(cube.pose)
print(" pose: " + str(cubePose))
print(" relative pose (2D): " + str(robotPose.inverse().mult(cubePose)))
await asyncio.sleep(0.05)
def target_pose_to_velocity_spline(relativeTarget: Frame2D):
# TODO
velocity=0
angular=0
gin = 10
global didhut
x = relativeTarget.mat[0, 2]
y = relativeTarget.mat[1, 2]
s = np.sqrt(((x * x) + (y * y)))
l = np.sqrt(((x * x) + (y * y)))
t = relativeTarget.mat[1, 2]
hy = relativeTarget.mat[1,0]
k = 2 * (3 * t - s * hy) / (s * s)
#print("Distance from target -> ", l)
angular = l * k
velocity = l
if s > 30 and not(didhut):
# print("test1")
return [velocity / gin, angular / gin]
else:
didhut = True
if didhut:
dAngular = relativeTarget.angle()
# print("test2")
return [0, dAngular / gin]
return [velocity / gin , angular / gin]
def target_pose_to_velocity_linear(relativeTarget: Frame2D):
# TODO
velocity=0
angular=0
global didhut
x = relativeTarget.mat[0,2]
y = relativeTarget.mat[1,2]
d = np.sqrt(((x * x) + (y * y)))
theta = math.atan2(y,x)
print("\n")
print("theta -------> ", theta)
print("Distance from target -> ", d)
if d > 50 and abs(theta) > 0.1 and not(didhut):
print("test1")
print("didhut = -------> ", didhut)
return [velocity, theta]
if d > 50 and not(didhut):
print("test2")
return [0.2 * d, theta]
else:
didhut = True
if didhut:
dAngular = relativeTarget.angle()
print("test3")
print("dAngular - > ", dAngular)
return [0, dAngular]
return [velocity, angular]
async def cozmo_program(robot: cozmo.robot.Robot):
while True:
robotPose = Frame2D.fromPose(robot.pose)
print("Robot pose: " + str(robotPose))
cubeIDs = (cozmo.objects.LightCube1Id, cozmo.objects.LightCube2Id,
cozmo.objects.LightCube3Id)
for cubeID in cubeIDs:
cube = robot.world.get_light_cube(cubeID)
if cube is not None and cube.is_visible:
print("Visible: " + cube.descriptive_name + " (id=" +
str(cube.object_id) + ")")
cubePose = Frame2D.fromPose(cube.pose)
print(" pose: " + str(cubePose))
print(" relative pose (2D): " +
str(robotPose.inverse().mult(cubePose)))
print()
await asyncio.sleep(1)
def cozmo_cliff_sensor_model(robotPose: Frame2D, m: CozmoMap, cliffDetected):
def is_in_map(m: CozmoMap, x, y):
if x < m.grid.minX() or m.grid.maxX() < x:
return False
if y < m.grid.minY() or m.grid.maxY() < y:
return False
return True
sensorPose = robotPose.mult(Frame2D.fromXYA(20, 0, 0))
if not is_in_map(m, robotPose.x(), robotPose.y()):
return 0
if not is_in_map(m, sensorPose.x(), sensorPose.y()):
return 0
c = Coord2D(sensorPose.x(), sensorPose.y())
if m.grid.isOccupied(c) == cliffDetected:
return 0.99
else:
return 0.01
def loadU08520Map():
# based on a 60cm x 80cm layout
sizeX = 32
# sizeY = 42
sizeY = 44
grid = OccupancyGrid(Coord2D(-10, -10), 20.0, sizeX, sizeY)
grid.setOccupied(Coord2DGrid(0, 0), Coord2DGrid(sizeX, sizeY))
grid.setFree(Coord2DGrid(1, 1), Coord2DGrid(sizeX - 1, sizeY - 1))
grid.setOccupied(Coord2DGrid(16, 21), Coord2DGrid(sizeX, 23))
landmarks = {
cozmo.objects.LightCube1Id: Frame2D.fromXYA(540, 40, 0),
cozmo.objects.LightCube2Id: Frame2D.fromXYA(40, 440, 0),
cozmo.objects.LightCube3Id: Frame2D.fromXYA(540, 500, 0)
}
targets = [Frame2D.fromXYA(400, 760, 0)]
return CozmoMap(grid, landmarks, targets)
def pos(): global particles global globalPos global difrens aveX = 0.0 aveY = 0.0 aveQ = [] for i in range(0, numParticles): aveX += particles[i].x() aveY += particles[i].y() aveQ.append(particles[i].angle()) aveX = aveX / numParticles aveY = aveY / numParticles return Frame2D.fromXYA(aveX, aveY, mean_angle(aveQ)) print(globalPos)
def track_speed_to_pose_change(left, right, time):
# TODO
lDis = left * time
rDis = right * time
theta = (rDis - lDis) / wheelDistance
x = rDis
y = 0
if (abs(theta) > 0.001):
r = (rDis + lDis) / (2 * theta)
x = np.sin(theta) * r
y = (np.cos(theta) - 1) * (-1 * r)
else:
x = (rDis + lDis) / 2
y = 0
return Frame2D.fromXYA(x,y,theta)
def cozmo_drive_to_target(robot: cozmo.robot.Robot):
global currentPose
didhut = False
while True:
relativeTarget = Frame2D() # TODO determine current position of target relative to robot
relativeTarget = currentPose.inverse().mult(targetPose)
print("relativeTarget"+str(relativeTarget))
velocity = target_pose_to_velocity_linear(relativeTarget)
print("velocity"+str(velocity))
trackSpeed = velocity_to_track_speed(velocity[0],velocity[1])
print("trackSpeedCommand"+str(trackSpeed))
lspeed = robot.left_wheel_speed.speed_mmps
rspeed = robot.right_wheel_speed.speed_mmps
print("trackSpeed"+str([lspeed, rspeed]))
delta = track_speed_to_pose_change(lspeed, rspeed,interval)
currentPose = delta.mult(currentPose)
print("currentPose"+str(currentPose))
print()
robot.drive_wheel_motors(l_wheel_speed=trackSpeed[0],r_wheel_speed=trackSpeed[1])
time.sleep(interval)
def cozmo_program(robot: cozmo.robot.Robot):
global posiblePos
timeInterval = 0.1
threading.Thread(target=runMCLLoop, args=(robot,)).start()
threading.Thread(target=runPlotLoop, args=(robot,)).start()
robot.enable_stop_on_cliff(True)
# main loop
# TODO insert driving and navigation behavior HERE
# t = 0
# targetPose=Frame2D.fromXYA(400,700,0.5*3.1416) # this is the target position
# relativeTarget = targetPose
differentTarget = []
targetPose = Frame2D.fromXYA(180, 320, 0.5 * 3.1416)
differentTarget.append(targetPose)
targetPose = Frame2D.fromXYA(400, 700, 0.5 * 3.1416)
differentTarget.append(targetPose)
# explore, move
action = "move1"
timer = 0
while True:
if robot.is_cliff_detected:
robot.drive_wheel_motors(l_wheel_speed=-25, r_wheel_speed=-25)
time.sleep(5)
robot.drive_wheel_motors(l_wheel_speed=15, r_wheel_speed=-15)
time.sleep(10.5)
if (action == "move1"):
#robot.turn_in_place(degrees(360), speed=degrees(13)).wait_for_completed()
robot.drive_wheel_motors(l_wheel_speed= 15, r_wheel_speed=-15)
time.sleep(15.6)
print("yeszzzz")
action = "move"
elif (action == "move"):
if (timer < 300):
timer += 1
print(timer ,"timer")
else:
timer = 0
action = "move1"
# spline approach
currentPose = pos()
relativeTarget = currentPose.inverse().mult(targetPose)
#print("relativeTarget" + str(relativeTarget))
velocity = target_pose_to_velocity_spline(
relativeTarget) # target pose to velocity is the method for spline driving
#print("velocity" + str(velocity))
trackSpeed = velocity_to_track_speed(velocity[0], velocity[1])
#print("trackSpeedCommand" + str(trackSpeed))
robot.drive_wheel_motors(l_wheel_speed=trackSpeed[0], r_wheel_speed=trackSpeed[1])
#print("currentPose" + str(currentPose))
print()
else:
print("error")
exit(-1)
print(action)
print()
time.sleep(timeInterval)
def runMCLLoop(robot: cozmo.robot.Robot):
global particles
particleWeights = np.zeros([numParticles])
cubeIDs = [cozmo.objects.LightCube1Id, cozmo.objects.LightCube2Id, cozmo.objects.LightCube3Id]
# main loop
timeInterval = 0.1
t = 0
while True:
t0 = time.time()
# read cube sensors
robotPose = Frame2D.fromPose(robot.pose)
cubeVisibility = {}
cubeRelativeFrames = {}
numVisibleCubes = 0
for cubeID in cubeIDs:
cube = robot.world.get_light_cube(cubeID)
relativePose = Frame2D()
visible = False
if cube is not None and cube.is_visible:
cubePose = Frame2D.fromPose(cube.pose)
relativePose = robotPose.inverse().mult(cubePose)
visible = True
numVisibleCubes = numVisibleCubes + 1
cubeVisibility[cubeID] = visible
cubeRelativeFrames[cubeID] = relativePose
# read cliff sensor
cliffDetected = robot.is_cliff_detected
# read track speeds
lspeed = robot.left_wheel_speed.speed_mmps
rspeed = robot.right_wheel_speed.speed_mmps
# read global variable
currentParticles = particles
poseChange = track_speed_to_pose_change(lspeed, rspeed, timeInterval)
poseChangeXYA = poseChange.toXYA()
# MCL step 1: prediction (shift particle through motion model)
# For each particle
for i in range(0, numParticles):
# add gaussian error to x,y,a
poseChangeXYAnoise = np.add(poseChangeXYA, xyaNoise.sample())
# integrate change
poseChangeNoise = Frame2D.fromXYA(poseChangeXYAnoise)
currentParticles[i] = currentParticles[i].mult(poseChangeNoise)
# TODO Instead: shift particles along deterministic motion model, then add perturbation with xyaNoise (see above)
# See run-odom-vis.py under "Run simulations"
visible = False
# MCL step 2: weighting (weigh particles with sensor model
for i in range(0, numParticles):
# TODO this is all wrong (again) ...
p = cozmo_cliff_sensor_model(currentParticles[i], m, cliffDetected)
invFrame = Frame2D.fromXYA(currentParticles[i].x(), currentParticles[i].y(),
currentParticles[i].angle()).inverse()
for cubeID in cubeIDs:
relativeTruePose = invFrame.mult(m.landmarks[cubeID])
p = p * cube_sensor_model(relativeTruePose, cubeVisibility[cubeID], cubeRelativeFrames[cubeID])
if cubeVisibility[cubeID]:
visible = cubeVisibility[cubeID]
particleWeights[i] = p
# TODO instead, assign the product of all individual sensor models as weight (including cozmo_cliff_sensor_model!)
# See run-sensor-model.py under "compute position beliefs"
if visible:
# MCL step 3: resampling (proportional to weights)
# TODO not completely wrong, but not yet solving the problem
newParticles = resampleIndependent(currentParticles, particleWeights, numParticles - 15 , xyaResampleNoise)
# TODO Draw a number of "fresh" samples from all over the map and add them in order
# to recover form mistakes (use sampleFromPrior from mcl_tools.py)
ampleFromPrior = sampleFromPrior(mapPrior, 15)
# TODO Keep the overall number of samples at numParticles
# TODO Compare the independent re-sampling with "resampleLowVar" from mcl_tools.py
particl = resampleLowVar(particles,particleWeights,numParticles - 15,xyaResampleNoise)
# TODO Find reasonable amplitues for the resampling noise xyaResampleNoise (see above)
# TODO Can you dynamically determine a reasonable number of "fresh" samples.
# For instance: under which circumstances could it be better to insert no fresh samples at all?
# write global variable
particles = particl + ampleFromPrior
else:
particles = resampleLowVar(particles, particleWeights, numParticles,xyaResampleNoise)
print("t = " + str(t))
t = t + 1
t1 = time.time()
timeTaken = t1 - t0
if timeTaken < timeInterval:
time.sleep(timeInterval - timeTaken)
else:
print("Warning: loop iteration tool more than " + str(timeInterval) + " seconds (t=" + str(timeTaken) + ")")
import time # this data structure represents the map m = loadU08520Map() # this probability distribution represents a uniform distribution over the entire map in any orientation mapPrior = Uniform( np.array([m.grid.minX(), m.grid.minY(), 0]), np.array([m.grid.maxX(), m.grid.maxY(), 2 * math.pi])) HANDOUT = False # TODO Major parameter to choose: number of particles numParticles = 150 globalPos = Frame2D() difrens = 999999999999 # The main data structure: array for particles, each represnted as Frame2D particles = sampleFromPrior(mapPrior, numParticles) # noise injected in re-sampling process to avoid multiple exact duplications of a particle # TODO Choose sensible re-sampling variation xyaResampleVar = np.diag([10, 10, 10 * math.pi / 35000]) # note here: instead of creating new gaussian random numbers every time, which is /very/ expensive, # precompute a large table of them an recycle. GaussianTable does that internally xyaResampleNoise = GaussianTable(np.zeros([3]), xyaResampleVar, 10000) # Motor error model xyaNoiseVar = np.diag([cozmoOdomNoiseX, cozmoOdomNoiseY, cozmoOdomNoiseTheta])
speeds = np.load("test4.npy")
trackSpeeds = speeds
# Setup Noise model
zero3 = np.zeros([3])
zero33 = np.zeros([3, 3])
xyaNoiseVar = np.diag([cozmoOdomNoiseX, cozmoOdomNoiseY, cozmoOdomNoiseTheta])
xyaNoise = Gaussian(zero3, xyaNoiseVar)
numParticles = 20
# 3D array for positions of particles. Index 1: Particle index. Index 2: timestep. Index 3: x,y,a
poseVecs = np.zeros([numParticles, len(trackSpeeds) + 1, 3])
# 2D array for frames of particles. Index 1: Particle index. Index 2: timestep.
poseFrames = []
for run in range(0, numParticles):
poseFrames.append([Frame2D()])
# Run simulations
for t in range(0, len(trackSpeeds)):
# Deterministic model
poseChange = track_speed_to_pose_change(trackSpeeds[t, 0],
trackSpeeds[t, 1], 0.1)
poseChangeXYA = poseChange.toXYA()
# Individual probabilistic sampling per particle
for run in range(0, numParticles):
# add gaussian error to x,y,a
poseChangeXYAnoise = np.add(poseChangeXYA, xyaNoise.sample(1))
# integrate change
poseChangeNoise = Frame2D.fromXYA(poseChangeXYAnoise)
pose = poseFrames[run][t].mult(poseChangeNoise)
poseXYA = pose.toXYA()
import cozmo
from cozmo.util import degrees, Pose, distance_mm, speed_mmps
import numpy as np
from frame2d import Frame2D
from cozmo_interface import wheelDistance, target_pose_to_velocity_linear, velocity_to_track_speed, \
track_speed_to_pose_change ,cool
import time
currentPose = Frame2D()
interval = 0.1
arr = []
def recod(acon, robot: cozmo.robot.Robot):
global currentPose
while not acon.is_completed:
lspeed = robot.left_wheel_speed.speed_mmps
rspeed = robot.right_wheel_speed.speed_mmps
delta = track_speed_to_pose_change(lspeed, rspeed, interval)
currentPose = currentPose.mult(delta)
arr.append([rspeed, lspeed])
print("trackSpeed" + str([lspeed, rspeed]))
time.sleep(interval)
def sampleFromPrior(mapPrior, numParticles):
particles = []
for i in range(0, numParticles):
xya = mapPrior.sample()
particles.append(Frame2D.fromXYA(xya))
return particles
def resample(particle, xyaDistro=None):
# re-sample
if xyaDistro is None:
return particle
else:
return particle.mult(Frame2D.fromXYA(xyaDistro.sample()))
async def cozmo_program(robot: cozmo.robot.Robot):
# load map and create plot
m = loadU08520Map()
plt.ion()
plt.show()
fig = plt.figure(figsize=(8, 8))
ax = fig.add_subplot(1, 1, 1, aspect=1)
ax.set_xlim(m.grid.minX(), m.grid.maxX())
ax.set_ylim(m.grid.minY(), m.grid.maxY())
plotMap(ax, m)
# setup belief grid data structures
grid = m.grid
minX = grid.minX()
maxX = grid.maxX()
minY = grid.minY()
maxY = grid.maxY()
tick = grid.gridStepSize
numX = grid.gridSizeX
numY = grid.gridSizeY
gridXs = []
gridYs = []
gridCs = []
for xIndex in range(0, numX):
for yIndex in range(0, numY):
gridXs.append(minX + 0.5 * tick + tick * xIndex)
gridYs.append(minY + 0.5 * tick + tick * yIndex)
gridCs.append((1, 1, 1))
pop = plt.scatter(gridXs, gridYs, c=gridCs)
# TODO try me out: choose which robot angles to compute probabilities for
gridAs = [0] # just facing one direction
#gridAs = np.linspace(0, 2 * math.pi, 11) # facing different possible directions
# TODO try me out: choose which cubes are considered
cubeIDs = [cozmo.objects.LightCube3Id]
#cubeIDs = [cozmo.objects.LightCube1Id, cozmo.objects.LightCube2Id, cozmo.objects.LightCube3Id]
# precompute inverse coordinate frames for all x/y/a grid positions
index = 0
positionInverseFrames = [
] # 3D array of Frame2D objects (inverse transformation of every belief position x/y/a on grid)
for xIndex in range(0, numX):
yaArray = []
x = minX + 0.5 * tick + tick * xIndex
for yIndex in range(0, numY):
aArray = []
y = minY + 0.5 * tick + tick * yIndex
for a in gridAs:
aArray.append(Frame2D.fromXYA(x, y, a).inverse())
yaArray.append(aArray)
positionInverseFrames.append(yaArray)
# main loop
while True:
# read sensors
robotPose = Frame2D.fromPose(robot.pose)
cubeVisibility = {}
cubeRelativeFrames = {}
for cubeID in cubeIDs:
cube = robot.world.get_light_cube(cubeID)
print(cube)
relativePose = Frame2D()
visible = False
if cube is not None and cube.is_visible:
print("Visible: " + cube.descriptive_name + " (id=" +
str(cube.object_id) + ")")
cubePose = Frame2D.fromPose(cube.pose)
print(" pose: " + str(cubePose))
relativePose = robotPose.inverse().mult(cubePose)
print(" relative pose (2D): " + str(relativePose))
visible = True
cubeVisibility[cubeID] = visible
cubeRelativeFrames[cubeID] = relativePose
# compute position beliefs over grid (and store future visualization colors in gridCs)
index = 0
for xIndex in range(0, numX):
# x = minX+0.5*tick+tick*xIndex
for yIndex in range(0, numY):
# y = minY+0.5*tick+tick*yIndex
maxP = 0
for aIndex in range(len(gridAs)):
invFrame = positionInverseFrames[xIndex][yIndex][
aIndex] # precomputes inverse frames
p = 1. # empty product of probabilities (initial value) is 1.0
for cubeID in cubeIDs:
# compute pose of cube relative to robot
relativeTruePose = invFrame.mult(m.landmarks[cubeID])
# overall probability is the product of individual probabilities (assumed conditionally independent)
p = p * cube_sensor_model(relativeTruePose,
cubeVisibility[cubeID],
cubeRelativeFrames[cubeID])
# maximum probability over different angles is the one visualized in the end
if maxP < p:
maxP = p
gridCs[index] = (1 - maxP, 1 - maxP, 1 - maxP)
index = index + 1
# update position belief plot
pop.set_facecolor(gridCs)
plt.draw()
plt.pause(0.001)
await asyncio.sleep(1)
'''
import cozmo
from cozmo.util import degrees, Pose, distance_mm, speed_mmps
import numpy as np
from frame2d import Frame2D
from cozmo_interface import wheelDistance, target_pose_to_velocity_linear,velocity_to_track_speed,track_speed_to_pose_change ,didhut
import time
currentPose=Frame2D()
# TODO allow the target to be chosen as console parameter
targetPose=Frame2D.fromXYA(250,150,3)
interval = 0.1
def cozmo_drive_to_target(robot: cozmo.robot.Robot):
global currentPose
didhut = False
while True:
relativeTarget = Frame2D() # TODO determine current position of target relative to robot
relativeTarget = currentPose.inverse().mult(targetPose)
print("relativeTarget"+str(relativeTarget))
velocity = target_pose_to_velocity_linear(relativeTarget)