def init_environment(self): self.environment = Environment() self.environment.setupEnvironment(self.environment_file) self.environment.loadRobotFromURDF(self.robot_file) self.robot = self.environment.getRobot() self.robot_dof = self.robot.getDOF() goal_area = v_double() self.environment.getGoalArea(goal_area) self.goal_position = [goal_area[i] for i in xrange(0, 3)] self.goal_radius = goal_area[3] self.environment.showViewer() return True
class Play: def __init__(self, algorithm, dir, numb, play_failed): config_file = glob.glob(os.path.join(dir + "/config_*")) robot_filename = "" environment_filename = "" with open(config_file[0], "r") as f: for line in f.readlines(): if "robot_file:" in line: robot_filename = line.split(":")[1].strip() elif "environment_file:" in line: environment_filename = line.split(":")[1].strip() robot_files = glob.glob( os.path.join(os.path.join(dir + "/model/", robot_filename))) print robot_files environment_files = glob.glob( os.path.join( os.path.join(dir + "/environment/", environment_filename))) if len(robot_files) > 1: print "Error: Multiple robot files found" return False if len(environment_files) > 1: print "Error: Multiple environment files found" return False self.robot_file = robot_files[0] self.environment_file = environment_files[0] if algorithm == None: print "No algorithm provided. Use the -a flag" return if dir == None: print "No directory provided. Use the -d flag" return elif not os.path.isdir(dir): print "Provided directory doesn't exist" return if numb == None: numb = 0 if not self.init_environment(): return print "PLAY" self.play_runs(dir, algorithm, numb, play_failed) def is_terminal(self, state): ja = v_double() ja[:] = [state[j] for j in xrange(len(state) / 2)] ee_position_arr = v_double() self.robot.getEndEffectorPosition(ja, ee_position_arr) ee_position = np.array( [ee_position_arr[j] for j in xrange(len(ee_position_arr))]) norm = np.linalg.norm(ee_position - self.goal_position) if norm - 0.01 <= self.goal_radius: return True return False def play_runs(self, dir, algorithm, numb, play_failed): files = glob.glob(os.path.join(os.path.join(dir, "*.log"))) print files log_files = [] for file in files: file_str = file.split("/")[-1] if algorithm in file_str: log_files.append(file) with open(sorted(log_files)[numb]) as f: states = [] all_particles = [] particles = [] nominal_states = [] col = [] terminal = False for line in f: if "S: " in line: line_arr = line.rstrip("\n ").split(" ") state = np.array( [float(line_arr[i]) for i in xrange(1, len(line_arr))]) states.append(state) elif "S_NOMINAL: " in line: line_arr = line.rstrip("\n ").split(" ") state = np.array( [float(line_arr[i]) for i in xrange(1, len(line_arr))]) nominal_states.append(state) elif "Terminal:" in line: term_line = line.rstrip("\n ").split(": ")[1] if term_line == 'true': terminal = True elif "collided: " in line or "Collision detected" in line: if not "estimate" in line: if "collided: " in line: if line.rstrip("\n ").split(": ")[1] == "true": col.append(True) else: col.append(False) elif "Collision detected" in line: if line.rstrip("\n").split(": ")[2] == "True": col.append(True) else: col.append(False) elif "PARTICLES BEGIN" in line: particles = [] elif "PARTICLES END" in line: all_particles.append(particles) elif "p: " in line: line_arr = line.rstrip("\n").split(": ")[1].split(" ") particle = np.array( [float(line_arr[i]) for i in xrange(len(line_arr))]) particles.append(particle) elif "Final State:" in line: line_arr = line.rstrip("\n ").split(": ")[1].split(" ") state = np.array( [float(line_arr[i]) for i in xrange(len(line_arr))]) states.append(state) terminal = self.is_terminal(state) elif ("RUN #" in line or "Run #" in line or "#####" in line) and len(states) != 0: if play_failed: if terminal == False: self.show_nominal_path(nominal_states) self.play_states(states, col, all_particles) else: self.show_nominal_path(nominal_states) self.play_states(states, col, all_particles) states = [] nominal_states = [] col = [] all_particles = [] def show_nominal_path(self, path): """ Shows the nominal path in the viewer """ self.environment.removePermanentParticles() particle_joint_values = v2_double() particle_joint_colors = v2_double() pjvs = [] for p in path: particle = v_double() particle[:] = [p[i] for i in xrange(len(p) / 2)] pjvs.append(particle) color = v_double() color[:] = [0.0, 0.0, 0.0, 0.7] particle_joint_colors.append(color) particle_joint_values[:] = pjvs self.environment.plotPermanentParticles(particle_joint_values, particle_joint_colors) def play_states(self, states, col, particles): for i in xrange(len(states)): cjvals = v_double() cjvels = v_double() print "states[i] " + str(states[i]) cjvals_arr = [states[i][j] for j in xrange(len(states[i]) / 2)] cjvels_arr = [ states[i][j] for j in xrange(len(states[i]) / 2, len(states[i])) ] cjvals[:] = cjvals_arr cjvels[:] = cjvels_arr particle_joint_values = v2_double() particle_joint_colors = v2_double() if i > 1 and len(particles) > 0: for p in particles[i - 1]: particle = v_double() particle_color = v_double() particle[:] = [p[k] for k in xrange(len(p) / 2)] particle_color[:] = [0.5, 0.5, 0.5, 0.5] particle_joint_values.append(particle) particle_joint_colors.append(particle_color) print "updating robot values" self.environment.updateRobotValues(cjvals, cjvels, particle_joint_values, particle_joint_colors) print "updated robot values" '''for o in self.obstacles: self.robot.setObstacleColor(o.getName(), o.getStandardDiffuseColor(), o.getStandardAmbientColor()) ''' self.environment.setKinBodiesDefaultColor() try: if col[i] == True: diffuse_col = v_double() ambient_col = v_double() diffuse_col[:] = [0.5, 0.0, 0.0, 0.0] ambient_col[:] = [0.8, 0.0, 0.0, 0.0] for o in self.obstacles: self.robot.setObstacleColor(o.getName(), diffuse_col, ambient_col) except: pass time.sleep(0.3) def is_in_collision(self, previous_state, state): """ Is the given end effector position in collision with an obstacle? """ collidable_obstacles = [ o for o in self.obstacles if not o.isTraversable() ] joint_angles_goal = v_double() joint_angles_goal[:] = [state[i] for i in xrange(self.robot_dof)] print previous_state print state collision_objects_goal = self.robot.createRobotCollisionObjects( joint_angles_goal) if len(previous_state) > 0: """ Perform continuous collision checking if previous state is provided """ joint_angles_start = v_double() joint_angles_start[:] = [ previous_state[i] for i in xrange(self.robot_dof) ] collision_objects_start = self.robot.createRobotCollisionObjects( joint_angles_start) for obstacle in collidable_obstacles: for i in xrange(len(collision_objects_start)): if obstacle.inCollisionContinuous([ collision_objects_start[i], collision_objects_goal[i] ]): print "return true" return True, obstacle return False, None else: for obstacle in collidable_obstacles: if obstacle.inCollisionDiscrete(collision_objects_goal): return True, None return False, None def init_environment(self): self.environment = Environment() self.environment.setupEnvironment(self.environment_file) self.environment.loadRobotFromURDF(self.robot_file) self.robot = self.environment.getRobot() self.robot_dof = self.robot.getDOF() goal_area = v_double() self.environment.getGoalArea(goal_area) self.goal_position = [goal_area[i] for i in xrange(0, 3)] self.goal_radius = goal_area[3] self.environment.showViewer() return True
env.updateRobotValues(joint_angles_v, joint_angles_v, particle_joint_values, particle_joint_values) time.sleep(1) env.getSensorManager().activateSensor(sensor_name) time.sleep(1) env.drawBoxes() time.sleep(1) env.getSensorManager().deactivateSensor(sensor_name) time.sleep(1.0) sensor_file = "sensor_BaseFlashLidar3D.xml" sensor_name = "FlashLidar3D" env = Environment() env.setupEnvironment("env_3dof.xml") sensors = v_string() sensors[:] = [sensor_file] env.loadSensors(sensors) env.showViewer() env.getSensorManager() env.loadRobotFromURDF("model/block_model.urdf") #dof_values = v_double() #dof_values[:] = [1.5778636567,-3.28698057487e-06,4.93129297073e-06,-0.028272851672] #env.setRobotDOFValues(dof_values) #time.sleep(100) env.getRobot().setGravityConstant(9.81) #env.transformSensorToSensorLink(sensor_name)
class Play: def __init__(self, algorithm, dir, numb, play_failed): config_file = glob.glob(os.path.join(dir + "/config_*")) robot_filename = "" environment_filename = "" with open(config_file[0], "r") as f: for line in f.readlines(): if "robot_file:" in line: robot_filename = line.split(":")[1].strip() elif "environment_file:" in line: environment_filename = line.split(":")[1].strip() robot_files = glob.glob(os.path.join(os.path.join(dir + "/model/", robot_filename))) print robot_files environment_files = glob.glob(os.path.join(os.path.join(dir + "/environment/", environment_filename))) if len(robot_files) > 1: print "Error: Multiple robot files found" return False if len(environment_files) > 1: print "Error: Multiple environment files found" return False self.robot_file = robot_files[0] self.environment_file = environment_files[0] if algorithm == None: print "No algorithm provided. Use the -a flag" return if dir == None: print "No directory provided. Use the -d flag" return elif not os.path.isdir(dir): print "Provided directory doesn't exist" return if numb == None: numb = 0 if not self.init_environment(): return print "PLAY" self.play_runs(dir, algorithm, numb, play_failed) def is_terminal(self, state): ja = v_double() ja[:] = [state[j] for j in xrange(len(state) / 2)] ee_position_arr = v_double() self.robot.getEndEffectorPosition(ja, ee_position_arr) ee_position = np.array([ee_position_arr[j] for j in xrange(len(ee_position_arr))]) norm = np.linalg.norm(ee_position - self.goal_position) if norm - 0.01 <= self.goal_radius: return True return False def play_runs(self, dir, algorithm, numb, play_failed): files = glob.glob(os.path.join(os.path.join(dir, "*.log"))) print files log_files = [] for file in files: file_str = file.split("/")[-1] if algorithm in file_str: log_files.append(file) with open(sorted(log_files)[numb]) as f: states = [] all_particles = [] particles = [] nominal_states = [] col = [] terminal = False for line in f: if "S: " in line: line_arr = line.rstrip("\n ").split(" ") state = np.array([float(line_arr[i]) for i in xrange(1, len(line_arr))]) states.append(state) elif "S_NOMINAL: " in line: line_arr = line.rstrip("\n ").split(" ") state = np.array([float(line_arr[i]) for i in xrange(1, len(line_arr))]) nominal_states.append(state) elif "Terminal:" in line: term_line = line.rstrip("\n ").split(": ")[1] if term_line == 'true': terminal = True elif "collided: " in line or "Collision detected" in line: if not "estimate" in line: if "collided: " in line: if line.rstrip("\n ").split(": ")[1] == "true": col.append(True) else: col.append(False) elif "Collision detected" in line: if line.rstrip("\n").split(": ")[2] == "True": col.append(True) else: col.append(False) elif "PARTICLES BEGIN" in line: particles = [] elif "PARTICLES END" in line: all_particles.append(particles) elif "p: " in line: line_arr = line.rstrip("\n").split(": ")[1].split(" ") particle = np.array([float(line_arr[i]) for i in xrange(len(line_arr))]) particles.append(particle) elif "Final State:" in line: line_arr = line.rstrip("\n ").split(": ")[1].split(" ") state = np.array([float(line_arr[i]) for i in xrange(len(line_arr))]) states.append(state) terminal = self.is_terminal(state) elif ("RUN #" in line or "Run #" in line or "#####" in line) and len(states) != 0: if play_failed: if terminal == False: self.show_nominal_path(nominal_states) self.play_states(states, col, all_particles) else: self.show_nominal_path(nominal_states) self.play_states(states, col, all_particles) states = [] nominal_states = [] col = [] all_particles = [] def show_nominal_path(self, path): """ Shows the nominal path in the viewer """ self.environment.removePermanentParticles() particle_joint_values = v2_double() particle_joint_colors = v2_double() pjvs = [] for p in path: particle = v_double() particle[:] = [p[i] for i in xrange(len(p) / 2)] pjvs.append(particle) color = v_double() color[:] = [0.0, 0.0, 0.0, 0.7] particle_joint_colors.append(color) particle_joint_values[:] = pjvs self.environment.plotPermanentParticles(particle_joint_values, particle_joint_colors) def play_states(self, states, col, particles): for i in xrange(len(states)): cjvals = v_double() cjvels = v_double() print "states[i] " + str(states[i]) cjvals_arr = [states[i][j] for j in xrange(len(states[i]) / 2)] cjvels_arr = [states[i][j] for j in xrange(len(states[i]) / 2, len(states[i]))] cjvals[:] = cjvals_arr cjvels[:] = cjvels_arr particle_joint_values = v2_double() particle_joint_colors = v2_double() if i > 1 and len(particles) > 0: for p in particles[i-1]: particle = v_double() particle_color = v_double() particle[:] = [p[k] for k in xrange(len(p) / 2)] particle_color[:] = [0.5, 0.5, 0.5, 0.5] particle_joint_values.append(particle) particle_joint_colors.append(particle_color) print "updating robot values" self.environment.updateRobotValues(cjvals, cjvels, particle_joint_values, particle_joint_colors) print "updated robot values" '''for o in self.obstacles: self.robot.setObstacleColor(o.getName(), o.getStandardDiffuseColor(), o.getStandardAmbientColor()) ''' self.environment.setKinBodiesDefaultColor() try: if col[i] == True: diffuse_col = v_double() ambient_col = v_double() diffuse_col[:] = [0.5, 0.0, 0.0, 0.0] ambient_col[:] = [0.8, 0.0, 0.0, 0.0] for o in self.obstacles: self.robot.setObstacleColor(o.getName(), diffuse_col, ambient_col) except: pass time.sleep(0.3) def is_in_collision(self, previous_state, state): """ Is the given end effector position in collision with an obstacle? """ collidable_obstacles = [o for o in self.obstacles if not o.isTraversable()] joint_angles_goal = v_double() joint_angles_goal[:] = [state[i] for i in xrange(self.robot_dof)] print previous_state print state collision_objects_goal = self.robot.createRobotCollisionObjects(joint_angles_goal) if len(previous_state) > 0: """ Perform continuous collision checking if previous state is provided """ joint_angles_start = v_double() joint_angles_start[:] = [previous_state[i] for i in xrange(self.robot_dof)] collision_objects_start = self.robot.createRobotCollisionObjects(joint_angles_start) for obstacle in collidable_obstacles: for i in xrange(len(collision_objects_start)): if obstacle.inCollisionContinuous([collision_objects_start[i], collision_objects_goal[i]]): print "return true" return True, obstacle return False, None else: for obstacle in collidable_obstacles: if obstacle.inCollisionDiscrete(collision_objects_goal): return True, None return False, None def init_environment(self): self.environment = Environment() self.environment.setupEnvironment(self.environment_file) self.environment.loadRobotFromURDF(self.robot_file) self.robot = self.environment.getRobot() self.robot_dof = self.robot.getDOF() goal_area = v_double() self.environment.getGoalArea(goal_area) self.goal_position = [goal_area[i] for i in xrange(0, 3)] self.goal_radius = goal_area[3] self.environment.showViewer() return True