def direction_to_target(mc, pos):
aPos = mc.getAgentPos()
aPos = [aPos[0], aPos[1] + 1.66, aPos[2], aPos[3], aPos[4]]
[pitch, yaw] = mc.dirToPos(aPos, pos)
pitch = normAngle(pitch - degree2rad(aPos[3]))
yaw = normAngle(yaw - degree2rad(aPos[4]))
dist = math.sqrt((aPos[0] - pos[0]) * (aPos[0] - pos[0]) +
(aPos[2] - pos[2]) * (aPos[2] - pos[2]))
return pitch, yaw, dist
def act(self):
result = []
pos = self.rob.waitNotNoneObserve('getAgentPos')
current_yaw = normAngle(degree2rad(pos[YAW]))
if self.start_yaw is None:
self.start_yaw = current_yaw
if abs(normAngle(current_yaw - self.start_yaw)) > 1:
result.append('turn 0')
return 'success', result
result.append('turn 0.02')
return 'running', result
def lookDir(rob, pitch, yaw):
logging.info("\tinside lookDir")
for t in range(3000):
sleep(0.02) # wait for action
aPos = rob.waitNotNoneObserve('getAgentPos')
dPitch = normAngle(pitch - degree2rad(aPos[3]))
dYaw = normAngle(yaw - degree2rad(aPos[4]))
if abs(dPitch) < 0.02 and abs(dYaw) < 0.02: break
rob.sendCommand("turn " + str(dYaw * 0.4))
rob.sendCommand("pitch " + str(dPitch * 0.4))
rob.sendCommand("turn 0")
rob.sendCommand("pitch 0")
def lookAt(rob, pos):
logging.info("\tinside lookAt")
for t in range(3000):
sleep(0.02)
aPos = rob.waitNotNoneObserve('getAgentPos')
[pitch, yaw] = rob.dirToAgentPos(pos)
pitch = normAngle(pitch - degree2rad(aPos[3]))
yaw = normAngle(yaw - degree2rad(aPos[4]))
if abs(pitch) < 0.02 and abs(yaw) < 0.02: break
rob.sendCommand("turn " + str(yaw * 0.4))
rob.sendCommand("pitch " + str(pitch * 0.4))
rob.sendCommand("turn 0")
rob.sendCommand("pitch 0")
return math.sqrt((aPos[0] - pos[0]) * (aPos[0] - pos[0]) +
(aPos[2] - pos[2]) * (aPos[2] - pos[2]))
def act(self):
assert not self._finished
if self.start_yaw is None:
self.start_yaw = self.getAngle()
status, action = self.root()
if status == 'failure':
self._finished = True
if status == 'success':
# we can either succed by finding a mountain
# or it just rotate finished
last_node = self.root.getLastNode()
if isinstance(last_node, Rotate):
diff = abs(normAngle(self.start_yaw - self.getAngle()))
if diff > 1.7 * math.pi:
# we're done
self._finished = True
else:
self.root = self._create_tree()
elif isinstance(last_node, CheckVisible):
print('found mountain')
print(last_node.found_item)
self._finished = True
else:
print('unexpected {0}'.format(last_node))
return action
def lookAt(mc, pos):
print('look at')
for t in range(3000):
sleep(0.02)
mc.observeProc()
aPos = mc.getAgentPos()
if aPos is None:
continue
[pitch, yaw] = mc.dirToPos([aPos[0], aPos[1] + 1.66, aPos[2]], pos)
pitch = normAngle(pitch - degree2rad(aPos[3]))
yaw = normAngle(yaw - degree2rad(aPos[4]))
if abs(pitch) < 0.02 and abs(yaw) < 0.02: break
mc.sendCommand("turn " + str(yaw * 0.4))
mc.sendCommand("pitch " + str(pitch * 0.4))
mc.sendCommand("turn 0")
mc.sendCommand("pitch 0")
return math.sqrt((aPos[0] - pos[0]) * (aPos[0] - pos[0]) +
(aPos[2] - pos[2]) * (aPos[2] - pos[2]))
def compute_turn(self, pos):
if self.target_yaw is None:
return 0
current_yaw = degree2rad(pos[YAW])
diff = - normAngle(current_yaw - self.target_yaw)
if abs(diff) < 0.05:
diff = 0
yaw = diff / 5
return yaw
def compute_pitch(self, pos):
if self.target_pitch is None:
return 0
current_pitch = degree2rad(pos[PITCH])
# proportional control
# should work good enough, no need for PID
diff = - normAngle(current_pitch - self.target_pitch)
if abs(diff) < 0.05:
diff = 0
pitch = diff / 5
return pitch
def lookAt(self, pitch_new, yaw_new):
mc = self.mc
print('look at')
for t in range(2000):
time.sleep(0.02)
mc.observeProc()
aPos = mc.getAgentPos()
if aPos is None:
continue
current_pitch = toRadAndNorm(aPos[3])
current_yaw = toRadAndNorm(aPos[4])
pitch = normAngle(normAngle(pitch_new) - current_pitch)
yaw = normAngle(normAngle(yaw_new) - current_yaw)
if abs(pitch) < 0.02 and abs(yaw) < 0.02: break
yaw = yaw * 0.5
while abs(yaw) > 1:
yaw *= 0.8
pitch = pitch * 0.5
while abs(pitch) > 1:
pitch *= 0.8
mc.sendCommand("turn " + str(yaw))
mc.sendCommand("pitch " + str(pitch))
mc.sendCommand("turn 0")
mc.sendCommand("pitch 0")
def getAngle(self):
pos = self.rob.waitNotNoneObserve('getAgentPos')
return normAngle(degree2rad(pos[YAW]))
def run_episode(self):
"""
Collect data for visual system
1) apply random rotation
2) collect 3d points in regular grid covering an image
3) map point to 2d images
"""
mc = self.mc
import pdb
pdb.set_trace()
#self._random_turn()
K = torch.as_tensor([
1.6740810033016248e+02, 0., 160., 0., 1.6740810033016248e+02, 120.,
0., 0., 1.
]).reshape((3, 3))
K_inv = torch.inverse(K).numpy()
time.sleep(1)
state = self.collect_state()
image = state['image']
_, height, width = image.shape
center_x = round(width / 2)
center_y = round(height / 2)
episode_data = dict()
episode_data['origin'] = state
pitch, yaw = state['pitch_yaw']
# unit vector
unit_pixel = [center_x, center_y, 1]
# go from top left to bottom right
X_pixel = [1, 1, 1]
# x, y, depth
# from pixels to centimeters
X_cm = K_inv @ X_pixel
unit_cm = K_inv @ unit_pixel
# yaw
vec_yaw = X_cm.copy()
vec_yaw[1] = unit_cm[1]
angle_yaw = common.vectors_angle(vec_yaw, unit_cm)
# pitch
vec_pitch = X_cm.copy()
vec_pitch[0] = unit_cm[0]
angle_pitch = common.vectors_angle(vec_pitch, unit_cm)
angleMaxW = abs(angle_yaw)
angleMaxH = abs(angle_pitch)
w_steps = 35
h_steps = 20
w_range = numpy.arange(yaw, yaw + angleMaxW * 2,
angleMaxW * 2 / w_steps) - angleMaxW
h_range = numpy.arange(pitch, pitch + angleMaxH * 2,
angleMaxH * 2 / h_steps) - angleMaxH
for i in range(w_steps):
for j in range(h_steps):
yaw_new = normAngle(w_range[i])
pitch_new = normAngle(h_range[j])
self.lookAt(pitch_new, yaw_new)
time.sleep(0.1)
state = self.collect_state()
episode_data[(i, j)] = state
pickle.dump(episode_data, open('episode7.pkl', 'wb'))
return episode_data