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 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