def simulate(self, command, max_steps):
if 'trajectory' in command:
draw.draw_trajectory(self.state.env, command.trajectory, reset=True)
for step in xrange(max_steps):
start = time.time()
if self.verbose:
print '\nstep: {}, time: {}'.format(step, self.get_sim_time())
self.state.update(step)
pipeline = [command]
for c in self.controllers:
pipeline.append(c.update(pipeline[-1], self.params.timestep))
self.state.apply(
self.aerodynamics.apply(
pipeline[-1].wrench,
self.params.timestep),
self.params.timestep)
finished = self.controllers[0].finished()
if finished or not self.state.valid():
if not self.state.valid():
utils.pv('self.state.load_factor')
utils.pv('self.state.position')
return False, step
break
if self.params.sleep:
time.sleep(max(self.params.timestep - time.time() + start, 0.0))
return True, step
def simulate(self, command, max_steps):
if 'trajectory' in command:
draw.draw_trajectory(self.state.env,
command.trajectory,
reset=True)
for step in xrange(max_steps):
start = time.time()
if self.verbose:
print '\nstep: {}, time: {}'.format(step, self.get_sim_time())
self.state.update(step)
pipeline = [command]
for c in self.controllers:
pipeline.append(c.update(pipeline[-1], self.params.timestep))
self.state.apply(
self.aerodynamics.apply(pipeline[-1].wrench,
self.params.timestep),
self.params.timestep)
finished = self.controllers[0].finished()
if finished or not self.state.valid():
if not self.state.valid():
utils.pv('self.state.load_factor')
utils.pv('self.state.position')
return False, step
break
if self.params.sleep:
time.sleep(max(self.params.timestep - time.time() + start,
0.0))
return True, step
def draw(self, color='b'):
"""
Draw the COM trajectory.
Parameters
----------
color : char or triplet, optional
Color letter or RGB values, default is 'b' for green.
Returns
-------
handle : openravepy.GraphHandle
OpenRAVE graphical handle. Must be stored in some variable,
otherwise the drawn object will vanish instantly.
"""
handles = draw_trajectory(self.P, color=color)
handles.extend(draw_trajectory(self.Z, color='m'))
com_last = self.p_last
dcm_last = self.p_last + self.pd_last / self.omega
cp_target = self.dcm_target + gravity / self.omega2
cp_last = dcm_last + gravity / self.omega2
for k in xrange(self.nb_steps):
p, z = self.P[k], self.Z[k]
handles.append(draw_line(z, p, color='c', linewidth=0.2))
handles.extend([
draw_point(self.dcm_target, color='b', pointsize=0.025),
draw_point(cp_target, color='b', pointsize=0.025),
draw_line(self.dcm_target, cp_target, color='b', linewidth=1),
draw_point(com_last, color='g', pointsize=0.025),
draw_point(cp_last, color='g', pointsize=0.025),
draw_line(com_last, cp_last, color='g', linewidth=1),
])
return handles
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)
def plan_with_inittraj(self, start, goal, inittraj=None):
if self.verbose:
draw.draw_trajectory(self.env, inittraj, colors=np.array((0.5, 0.5, 0.5)))
if self.params.n_steps > 2:
with self.robot:
self.robot.SetActiveDOFValues(start)
request = self.make_fullbody_request(goal, inittraj, self.params.n_steps)
prob = trajoptpy.ConstructProblem(json.dumps(request), self.env)
def constraint(dofs):
valid = True
if self.params.dof == 6:
valid &= abs(dofs[3]) < 0.1
valid &= abs(dofs[4]) < 0.1
with self.robot:
self.robot.SetActiveDOFValues(dofs)
valid &= not self.env.CheckCollision(self.robot)
return 0 if valid else 1
for t in range(1, self.params.n_steps):
prob.AddConstraint(
constraint, [(t, j) for j in range(self.params.dof)], "EQ", "constraint%i" % t)
result = trajoptpy.OptimizeProblem(prob)
traj = result.GetTraj()
prob.SetRobotActiveDOFs()
if traj is not None:
if self.verbose:
draw.draw_trajectory(self.env, traj)
if check_traj.traj_is_safe(traj, self.robot):
cost = sum(cost[1] for cost in result.GetCosts())
return traj, cost
elif self.params.n_steps <= 2:
return [start, goal], utils.dist(start, goal)
return None, None
def draw(self, color='b'):
"""
Draw positions of the interpolated trajectory.
Parameters
----------
color : char or triplet, optional
Color letter or RGB values, default is 'b' for green.
Returns
-------
handle : openravepy.GraphHandle
OpenRAVE graphical handle. Must be stored in some variable,
otherwise the drawn object will vanish instantly.
"""
points = [self.eval_pos(s) for s in linspace(0, 1, 10)]
return draw_trajectory(points, color=color)
def draw(self, nb_segments=15, color='b'):
"""
Draw the interpolated foot path.
Parameters
----------
nb_segments : scalar
Number of segments approximating the polynomial curve.
color : char or triplet, optional
Color letter or RGB values, default is 'b' for green.
Returns
-------
handle : openravepy.GraphHandle
OpenRAVE graphical handle. Must be stored in some variable,
otherwise the drawn object will vanish instantly.
"""
dx = 1. / nb_segments
points = [self.poly(x) for x in arange(0., 1. + dx, dx)]
return draw_trajectory(points, color=color)
occluded = track.occlusion_detection(fwd_flow,
(bck_flow_u, bck_flow_v))
if occluded:
# Kill if occluded (or if something went wrong)
curr_traj.live = False
else:
# if curr_traj.curr_position[0] - new_pos[0] > 1 or curr_traj.curr_position[1] - new_pos[1] > 1:
#
# If not occluded, update the point
# if new_pos[0] == 4.0 and new_pos[0] == 4.0:
# print("Trajectory at position ", curr_traj.curr_position, "moved to (", new_pos, ')')
# print("FRAME:",frame)
curr_traj.set_position(int(np.round(new_pos[0])),
int(np.round(new_pos[1])), frame + 1)
draw.draw_trajectory(frame_0, trajectories, 0, 1, 5)
print('Done')
# STEP 3) Construct affinity matrix
# STEP 4) Populate affinity values
# Step 5) Spectral Clustering
print(len(trajectories))
# D squared - spatial distance times distance of ddt
D_sq = np.zeros((len(trajectories), len(trajectories)))
# D ddt - maximum distance of ddt
D_ddt = np.zeros((len(trajectories), len(trajectories)))
def main():
frame_0 = image_functions.open_image(
'results/Marple13_eig/eig_marple13_20.jpg')
frame_0 = image_functions.output_intensity_mapping(frame_0)
frame_1 = image_functions.open_image(
'results/Marple13_eig/eig_marple13_21.jpg')
frame_1 = image_functions.output_intensity_mapping(frame_1)
trajectories = []
frames = [frame_0, frame_1]
frame_dimensions = frames[0].shape
flow_fore = np.zeros(frame_dimensions, dtype=np.float32)
flow_back = np.zeros(frame_dimensions, dtype=np.float32)
for frame in range(0, len(frames)):
# If the frame is the final frame in the sequence, optical flow cannot
# be calculated (as there is no frame (t+1)). In this case, we go throgh
# all trajectories and set them as 'dead.' Then we skip out of the loop.
if frame == len(frames) - 1:
for trajectory in trajectories:
trajectory.live = False
continue
# Calculate the forward and backwards flow between the current and next frame
flow_fore = cv.calcOpticalFlowFarneback(frames[frame],
frames[frame + 1], flow_fore,
0.5, 5, 5, 5, 5, 1.1,
cv.OPTFLOW_FARNEBACK_GAUSSIAN)
flow_back = cv.calcOpticalFlowFarneback(frames[frame + 1],
frames[frame], flow_back, 0.5,
5, 5, 5, 5, 1.1,
cv.OPTFLOW_FARNEBACK_GAUSSIAN)
flow_back_u, flow_back_v = cv.split(flow_back)
draw.draw_flow_arrows(frame_0, flow_fore, 20, 100)
draw.draw_flow_arrows(frame_1, flow_back, 20, 100)
# Check if new objects are in scene in the FORWARD FLOW
# At the authors' suggestion, down sample this step by a factor of [4,16]
# in order to keep trajectory numbers at a minimum.
for row in range(0, frame_dimensions[0], 4):
for col in range(0, frame_dimensions[1], 4):
# check each pixel for flow response
if abs(flow_fore[row][col][0]) > 1e-20 or abs(
flow_fore[row][col][1]) > 1e-20:
tracked = False
# check each trajectory to see if it is currently tracking that point
for trajectory in trajectories:
if trajectory.curr_position == (row, col):
tracked = True
break
# Create new trajectory if not currently tracked
if not tracked:
trajectories.append(track.Track(row, col, frame))
# For all trajectories, we track them from frame (t) to (t+1).
for curr_traj in trajectories:
curr_pos = curr_traj.get_curr_position()
# sample the forward flow vector at the point's current location
fwd_flow = flow_fore[curr_pos[0]][curr_pos[1]]
# New position is (x + u, y + v)
new_pos = (curr_pos[0] + fwd_flow[1], curr_pos[1] + fwd_flow[0])
# If the point goes out of frame, kill the trajectory
if image_functions.out_of_bounds(new_pos, frame_dimensions):
curr_traj.live = False
continue
# The new point is likely 'off the grid' so we sample the backwards flow by
# bilinear interpolation, as the u and v vectors are orthogonal, they can be
# processed independently
bck_flow_u = image_functions.bilinear_interpolation(
new_pos[0], new_pos[1], flow_back_u)
bck_flow_v = image_functions.bilinear_interpolation(
new_pos[0], new_pos[1], flow_back_v)
if bck_flow_u == np.nan or bck_flow_v == np.nan:
continue
# We check if the backwards and forwards flow vectors are inverses
occluded = track.occlusion_detection(fwd_flow,
(bck_flow_u, bck_flow_v))
if occluded:
# Kill if occluded (or if something went wrong)
curr_traj.live = False
else:
if curr_traj.curr_position[0] - new_pos[
0] > 1 or curr_traj.curr_position[1] - new_pos[1] > 1:
print("Trajectory at position ", curr_traj.curr_position,
"moved to (", new_pos, ')\n')
# If not occluded, update the point
curr_traj.set_position(new_pos[0], new_pos[1], frame)
draw.draw_trajectory(frame_0, trajectories, 0, 1, 5)
# STEP 3) Construct affinity matrix
# STEP 4) Populate affinity values
# Step 5) Spectral Clustering
return 0
