def process(self):
if self.input_.trajectory:
while True:
self.output.pose = self.current_target()
if self.dist_to_target() > self.error:
break
self.traj_idx += 1
if self.traj_idx > len(self.input_.trajectory):
break
draw.draw_pose(self.state.env, self.output.pose)
def process(self):
if self.input_.trajectory:
while True:
self.output.pose = self.current_target()
if self.dist_to_target() > self.error:
break
self.traj_idx += 1
if self.traj_idx > len(self.input_.trajectory):
break
draw.draw_pose(self.state.env, self.output.pose)
def follow(self, traj):
if self.params.physics:
command = addict.Dict()
command.trajectory = []
for t in traj:
pose = addict.Dict(x=t[0], y=t[1], z=t[2], yaw=t[5])
command.trajectory.append(pose)
self.run(command)
else:
for (i, row) in enumerate(traj):
self.robot.SetActiveDOFValues(row)
draw.draw_pose(self.env, row)
time.sleep(self.params.timestep)
def follow(self, traj):
if self.params.physics:
command = addict.Dict()
command.trajectory = []
for t in traj:
pose = addict.Dict(x=t[0], y=t[1], z=t[2], yaw=t[5])
command.trajectory.append(pose)
self.run(command)
else:
for (i, row) in enumerate(traj):
self.robot.SetActiveDOFValues(row)
draw.draw_pose(self.env, row)
time.sleep(self.params.timestep)
def run(self):
while True:
start = self.robot.GetActiveDOFValues()
goal = self.collision_free(self.random_goal)
draw.draw_pose(self.env, goal, reset=True)
draw.draw_trajectory(self.env, None, reset=True)
with self.robot:
if self.verbose:
print 'planning to: {}'.format(goal)
traj, cost = self.planner.plan(start, goal)
if traj is not None:
time.sleep(1)
draw.draw_trajectory(self.env, traj, reset=True)
if self.verbose:
utils.pv('traj', 'cost')
self.execute_trajectory(traj)
time.sleep(1)
def run(self):
while True:
start = self.robot.GetActiveDOFValues()
goal = self.collision_free(self.random_goal)
draw.draw_pose(self.env, goal, reset=True)
draw.draw_trajectory(self.env, None, reset=True)
with self.robot:
if self.verbose:
print 'planning to: {}'.format(goal)
traj, cost = self.planner.plan(start, goal)
if traj is not None:
time.sleep(1)
draw.draw_trajectory(self.env, traj, reset=True)
if self.verbose:
utils.pv('traj', 'cost')
self.execute_trajectory(traj)
time.sleep(1)
print("Found", len(files), "files")
images = [cv2.imread(os.path.join(args.input, f)) for f in files]
data = []
for image in images:
# Create new datum
datum = op.Datum()
datum.cvInputData = image
datum.handRectangles = [
# Left/Right hands person 0
[
op.Rectangle(0., 0., 0., 0.),
op.Rectangle(0, 0, image.shape[0], image.shape[1]),
],
]
data.append(datum)
get_hand_detector().emplaceAndPop([datum])
for f, image, datum in zip(files, images, data):
pose = [[float(x) for x in r][:2]
for r in np.array(datum.handKeypoints[1][0], dtype=np.float16)]
print(pose)
save_loc = os.path.join(args.output, f)
if args.format == "json":
json.dump(pose, open(save_loc + ".json", "w"))
else:
new_image = draw_pose(image, pose)
cv2.imwrite(save_loc, new_image)