def pipelines(): from os import listdir from os.path import isfile, join, basename, splitext onlyfiles = [ join(PIPELINE_FOLDER, f) for f in listdir(PIPELINE_FOLDER) if isfile(join(PIPELINE_FOLDER, f)) ] pipelines = [] for f in onlyfiles: if not f.endswith(".ini"): continue conf = Configuration(f) pipeline_metadata = { "basename": splitext(basename(f))[0], "name": conf.get("name"), "description": conf.get("description"), "author": conf.get("author") } svg_file = f.replace(".ini", ".svg") if os.path.isfile(svg_file): with open(svg_file, "rb") as image_file: encoded_string = base64.b64encode( image_file.read()).decode('utf-8') pipeline_metadata[ "icon"] = "data:image/svg+xml;base64," + encoded_string pipelines.append(pipeline_metadata) response = make_response(json.dumps(pipelines)) response.headers['Content-Type'] = 'application/json' return response
def __init__(self, *args, **kwargs): super(PipelinesModel, self).__init__(*args, **kwargs) pipeline_folder = "resources/pipelines" self.pipelines = [] onlyfiles = [ join(pipeline_folder, f) for f in listdir(pipeline_folder) if isfile(join(pipeline_folder, f)) ] for f in onlyfiles: if not f.endswith(".ini"): continue pipeline_conf = Configuration(f) pipeline_metadata = { "filename": f, "basename": splitext(basename(f))[0], "name": pipeline_conf.get("name"), "description": pipeline_conf.get("description"), "author": pipeline_conf.get("author") } self.pipelines.append(pipeline_metadata)
class VideoPipeline: def __init__(self, global_conf, pipeline_file ): self.pipeline_conf = Configuration(pipeline_file) self.filters = [] self.pipeline = self.pipeline_conf.sections() self.source = ImageSource() self.source.configure(global_conf, "source", self.pipeline_conf) # the input format for the first filter element input_format = "image" for pipeline_section in self.pipeline: print(pipeline_section) # ignore the common section if pipeline_section == "common": continue # ignore the source section if pipeline_section == "source": continue instance = clazz.instance_by_name(pipeline_section) instance.configure(global_conf, pipeline_section, self.pipeline_conf) # try to load an image/icon for the give filter python_file = inspect.getfile(instance.__class__) svg_file = python_file.replace(".py", ".svg") if os.path.isfile(svg_file): with open(svg_file, "rb") as image_file: encoded_string = base64.b64encode(image_file.read()).decode('utf-8') instance.icon = "data:image/svg+xml;base64,"+encoded_string # check that the output format if the predecessor filter matches with the input if this # filter meta = instance.meta() if not meta["input"] == input_format: print("Filter '{}' is unable to process input format '{}'. Expected was '{}'".format(python_file, input_format, meta["input"] )) print("Wrong pipeline definition. Exit") exit_process() # the output if this filter is the input of the next filter input_format = meta["output"] self.filters.append(instance) def meta(self): meta_info = [] for instance in self.filters: menu = self.pipeline_conf.get_boolean("menu", instance.conf_section) meta =instance.meta() meta["menu"]= menu meta_info.append(meta) return { "name":self.pipeline_conf.get("name"), "description":self.pipeline_conf.get("description"), "author":self.pipeline_conf.get("author"), "filters":meta_info } def filter_count(self): return len(self.filters) def override_source_image(self, value): self.source.set_image(value) def get_source_image(self): return self.source.get_image() def set_parameter(self, index, name, value): self.filters[index].set_parameter(name, value) @perf_tracker() def gcode(self, contour_3d): return self.filters[len(self.filters)-1].gcode(contour_3d) def process(self): result = [] image = self.get_source_image() cnt = [] for instance in self.filters: start = time.process_time() try: print("Running filter: ", type(instance)) image, cnt = instance.process(image, cnt) end = time.process_time() print(instance.meta()["name"], end - start) print("Contour Count:", len(cnt)) cnt = ensure_3D_contour(cnt) if image is None: print("unable to read image from filter: "+instance.meta()["name"]) break if len(image.shape) != 3: print("Image must have 3 color channels. Filter '{}' must return RGB image for further processing".format(instance.conf_section)) result.append({"filter": instance.conf_section, "image":image, "contour": cnt }) print("------------------------") except Exception as exc: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] print(exc_type, fname, exc_tb.tb_lineno) print(type(instance), exc) return result def stop(self): for instance in self.filters: instance.stop()
class VideoPipeline(QObject): processed = Signal(object) def __init__(self, pipeline_file): QObject.__init__(self) self.pipeline_conf = Configuration(pipeline_file) self.filters = [] self.pipeline = self.pipeline_conf.sections() icon_file = pipeline_file.replace(".ini", ".png") if os.path.isfile(icon_file): self.icon_path = icon_file # the input format for the first filter element input_format = None for pipeline_section in self.pipeline: # ignore the common section if pipeline_section == "common": continue instance = clazz.instance_by_name(pipeline_section, pipeline_section, self.pipeline_conf) instance.index = len(self.filters) # check that the output format if the predecessor filter matches with the input if this # filter meta = instance.meta() if not input_format==None and not meta["input"] == input_format: print("Filter '{}' is unable to process input format '{}'. Expected was '{}'".format(python_file, input_format, meta["input"])) print("Wrong pipeline definition. Exit") exit_process() instance.param_changed.connect(self.process) # the output if this filter is the input of the next filter input_format = meta["output"] self.filters.append(instance) def meta(self): meta_info = [] for instance in self.filters: menu = self.pipeline_conf.get_boolean("menu", instance.conf_section) meta = instance.meta() meta["menu"] = menu meta_info.append(meta) return { "name": self.pipeline_conf.get("name"), "description": self.pipeline_conf.get("description"), "author": self.pipeline_conf.get("author"), "filters": meta_info } def filter(self, index): return self.filters[index] def filter_count(self): return len(self.filters) def gcode(self, contour_3d): return self.filters[len(self.filters) - 1].gcode(contour_3d) def process(self): result = [] image = None cnt = [] for instance in self.filters: try: image, cnt = instance.process(image, cnt) result.append({"filter": instance.conf_section, "image": image, "contour": cnt}) print("------------------------") except Exception as exc: exc_type, exc_obj, exc_tb = sys.exc_info() fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1] print(exc_type, fname, exc_tb.tb_lineno) print(type(instance), exc) self.processed.emit(result)
from flask import Flask, render_template, make_response, send_file, request from utils.webgui import FlaskUI # get the FlaskUI class from pipeline import VideoPipeline from grbl import GrblWriter from utils.configuration import Configuration configuration_dir = os.path.abspath( os.path.join(os.path.dirname(__file__), "..", "config", "configuration.ini")) conf = Configuration(configuration_dir) POOL_TIME = conf.get_int("image-read-ms") / 1000 # convert to Seconds PIPELINE_FOLDER = os.path.abspath( os.path.join(os.path.dirname(__file__), conf.get("pipelines"))) SERIAL_PORT = conf.get("serial-port") SERIAL_BAUD = conf.get_int("serial-baud") grbl = GrblWriter(SERIAL_PORT, SERIAL_BAUD) log = logging.getLogger('werkzeug') log.setLevel(logging.ERROR) app = Flask(__name__) app.config['SEND_FILE_MAX_AGE_DEFAULT'] = 0 ui = FlaskUI(app=app, port=8080) # global, thread base variables pipelineResults = None # the result of each pipeline run dataLock = threading.Lock(
class CookingEnv(VrepEnvBase): ''' 'port_num' determines which simulation this env is connected to 'suffix' determines which robot this env is connected to 'reset' is a class that should return a dict with new_joint_angles as an entry ''' def __init__(self, config={}, port_num=19997, suffix="", reset=None, seed=None, logger=None): super(CookingEnv, self).__init__(config, port_num) self.CookingEnv_config = Configuration(default_config) self.CookingEnv_config.update(config) self.CookingEnv_reset = reset self.object_handles = None self.logger = logger self.suffix = suffix if not suffix: self.suffix = self.CookingEnv_config.get('suffix') self.all_info = {} self.robot = self.CookingEnv_config.get('arm') #### if self.CookingEnv_config.get('arm') == 'baxter': rh = robot_handles['Baxter'] self.init_angles = [-2.69, -61.47, -3.35, -23.27, 89.80, -2.68] elif self.CookingEnv_config.get('arm') == 'jaco': rh = robot_handles['Jaco'] self.init_angles = [-2.69, -61.47, -3.35, -23.27, 89.80, -2.68] else: raise ValueError('arm not supported') #### world frame #### rc, self.world_frame_handle = vrep.simxGetObjectHandle(self.clientID, rh['world_frame_handle'], vrep.simx_opmode_blocking) #### joint handles #### self.joint_handles = [] for jh in rh['joint_handles']: rc, h = vrep.simxGetObjectHandle(self.clientID, jh, vrep.simx_opmode_blocking) self.joint_handles.append(h) #### gripper handles #### self.gripper_toggle_signal_name = rh['gripper_handles']['toggle_handle'] _, self.gripper_attachpoint_handle = vrep.simxGetObjectHandle(self.clientID, rh['gripper_handles']['attachpoint_handle'], vrep.simx_opmode_blocking) rc, self.gripper_prox_sensor_handle = vrep.simxGetObjectHandle(self.clientID, rh['gripper_handles']['prox_sensor_handle'], vrep.simx_opmode_blocking) # rc, ds, dp, dh, dn = vrep.simxReadProximitySensor(self.clientID, self.gripper_prox_sensor_handle, vrep.simx_opmode_streaming) # while rc != 0: # rc, ds, dp, dh, dn = vrep.simxReadProximitySensor(self.clientID, self.gripper_prox_sensor_handle, vrep.simx_opmode_buffer) self.gripper_state = 0 self.set_gripper_state(self.gripper_state) #### ee handle #### if self.CookingEnv_config.get('particle_test'): _, self.ee_handle = vrep.simxGetObjectHandle(self.clientID, rh['particle_handle'], vrep.simx_opmode_blocking) else: _, self.ee_handle = vrep.simxGetObjectHandle(self.clientID, rh['ee_handle'], vrep.simx_opmode_blocking) #### target handle #### if self.CookingEnv_config.get('particle_test'): _, self.target_handle = vrep.simxGetObjectHandle(self.clientID, rh['particle_target_handle'], vrep.simx_opmode_blocking) else: _, self.target_handle = vrep.simxGetObjectHandle(self.clientID, rh['ee_target_handle'], vrep.simx_opmode_blocking) #### goal handle #### _, self.goal_handle = vrep.simxGetObjectHandle(self.clientID, rh['goal_handle'], vrep.simx_opmode_blocking) #### object handles #### self.object_handles = {} for oh in object_handles: _, h = vrep.simxGetObjectHandle(self.clientID, oh['handle'], vrep.simx_opmode_blocking) self.object_handles[oh['name']] = h #### obstacle handles #### self.obstacle_handles = [] for obs_h in rh['obstacle_handles']: _, h = vrep.simxGetObjectHandle(self.clientID, obs_h['handle'], vrep.simx_opmode_blocking) self.obstacle_handles.append(dict(name=obs_h['name'], handle=h)) #### ee sample region #### _, self.ee_sample_region_handle = vrep.simxGetObjectHandle(self.clientID, rh['ee_sample_region_handle'], vrep.simx_opmode_blocking) #### ee motion region #### _, self.ee_motion_region_handle = vrep.simxGetObjectHandle(self.clientID, rh['ee_motion_region_handle'], vrep.simx_opmode_blocking) # def set_velocity_control_mode(self): # joint_handles = self.joint_handles[self.CookingEnv_config.get('arm')] # for joint_handle in joint_handles: # vrep.simxSetJointForce(self.clientID, joint_handle, 10000, vrep.simx_opmode_oneshot) # vrep.simxSetObjectIntParameter(self.clientID, joint_handle, 2001, 0, vrep.simx_opmode_oneshot) def get_switch_state(self): rc = 1 button_joint_frame_angle = np.zeros(3) while rc != 0: rc, button_joint_frame_angle = vrep.simxGetObjectOrientation(self.clientID, self.object_handles['toaster_button'], self.object_handles['grill'], vrep.simx_opmode_streaming) switch_state = button_joint_frame_angle[2] return switch_state def get_region_info(self, region='sample_region'): if region == 'sample_region': handle = self.ee_sample_region_handle elif region == 'motion_region': handle = self.ee_motion_region_handle else: raise ValueError('unsupported region') _, pos = vrep.simxGetObjectPosition(self.clientID, handle, self.world_frame_handle, vrep.simx_opmode_blocking) _, bb_min_x = vrep.simxGetObjectFloatParameter(self.clientID, handle, 15, vrep.simx_opmode_blocking) _, bb_min_y = vrep.simxGetObjectFloatParameter(self.clientID, handle, 16, vrep.simx_opmode_blocking) _, bb_min_z = vrep.simxGetObjectFloatParameter(self.clientID, handle, 17, vrep.simx_opmode_blocking) _, bb_max_x = vrep.simxGetObjectFloatParameter(self.clientID, handle, 18, vrep.simx_opmode_blocking) _, bb_max_y = vrep.simxGetObjectFloatParameter(self.clientID, handle, 19, vrep.simx_opmode_blocking) _, bb_max_z = vrep.simxGetObjectFloatParameter(self.clientID, handle, 20, vrep.simx_opmode_blocking) bb = { 'x': np.array([bb_min_x, bb_max_x]) + pos[0], 'y': np.array([bb_min_y, bb_max_y]) + pos[1], 'z': np.array([bb_min_z, bb_max_z]) + pos[2], } return bb def set_position_control_mode(self): for jh in self.joint_handles: vrep.simxSetObjectIntParameter(self.clientID, jh, 2001, 1, vrep.simx_opmode_oneshot) self.synchronous_trigger() def robot_open_gripper(self): vrep.simxSetIntegerSignal(self.clientID, self.gripper_toggle_signal_name, 0, vrep.simx_opmode_oneshot) self.synchronous_trigger() def robot_close_gripper(self): # check which dynamically non-static and respondable object is in-between the fingers. Then attach the object to the gripper # for h in self.object_handles: # if h['handle'] == -1: # break # _, static = vrep.simxGetObjectIntParameter(self.clientID, h['handle'], vrep.sim_shapeintparam_static, vrep.simx_opmode_blocking) # _, respondable = vrep.simxGetObjectIntParameter(self.clientID, h['handle'], vrep.sim_shapeintparam_respondable, vrep.simx_opmode_blocking) # if not static and respondable and ds == 1: # break # close gripper vrep.simxSetIntegerSignal(self.clientID, self.gripper_toggle_signal_name, 1, vrep.simx_opmode_oneshot) self.synchronous_trigger() def move_to(self, pt): self.set_target_pose(pt) def set_quaternion(self, quat, handle): assert quat.shape == (4,) vrep.simxSetObjectQuaternion(self.clientID, handle, self.world_frame_handle, quat, vrep.simx_opmode_oneshot) self.synchronous_trigger() def set_euler_angles(self, rpy, handle): assert rpy.shape == (3,) vrep.simxSetObjectOrientation(self.clientID, handle, self.world_frame_handle, rpy, vrep.simx_opmode_oneshot) self.synchronous_trigger() def get_velocity(self, handle): rc = 1 while rc != 0: rc, linear_vel, angular_vel = vrep.simxGetObjectVelocity(self.clientID, handle, vrep.simx_opmode_streaming) return np.array(linear_vel), np.array(angular_vel) def get_pose(self, handle): rct = 1 target_pos = np.zeros(3) while rct != 0 or np.linalg.norm(target_pos) < 0.001: rct, target_pos = vrep.simxGetObjectPosition(self.clientID, handle, self.world_frame_handle, vrep.simx_opmode_streaming) rcq, target_quat = vrep.simxGetObjectQuaternion(self.clientID, handle, self.world_frame_handle, vrep.simx_opmode_streaming) return np.array(target_pos), np.array(target_quat) def set_pose(self, pt, handle): assert pt.shape == (7,) pos = pt[:3] quat = pt[3:] vrep.simxSetObjectPosition(self.clientID, handle, self.world_frame_handle, pos, vrep.simx_opmode_oneshot) vrep.simxSetObjectQuaternion(self.clientID, handle, self.world_frame_handle, quat, vrep.simx_opmode_oneshot) self.synchronous_trigger() #################### # Common Interface # #################### def set_goal_pose(self, pt): self.set_pose(pt, self.goal_handle) def get_goal_pose(self): return self.get_pose(self.goal_handle) def set_target_pose(self, pt): self.set_pose(pt, self.target_handle) if self.CookingEnv_config.get('particle_test'): self.set_pose(pt, self.ee_handle) def get_target_pose(self): return self.get_pose(self.target_handle) def get_target_velocity(self): return self.get_velocity(self.target_handle) def set_ee_pose(self, pt): self.set_pose(pt, self.ee_handle) def get_ee_pose(self): return self.get_pose(self.ee_handle) def get_ee_velocity(self): return self.get_velocity(self.ee_handle) def set_gripper_state(self, gripper_state): if gripper_state < 0.5: vrep.simxSetIntegerSignal(self.clientID, self.robot+'_open_gripper', 0, vrep.simx_opmode_oneshot) self.gripper_state = 0 else: vrep.simxSetIntegerSignal(self.clientID, self.robot+'_open_gripper', 1, vrep.simx_opmode_oneshot) self.gripper_state = 1 for _ in range(20): self.synchronous_trigger() def get_gripper_state(self): return self.gripper_state def get_obstacle_info(self): obs_info = [] for obs in self.obstacle_handles: rc = 1 while rc != 0: rc, pos = vrep.simxGetObjectPosition(self.clientID, obs['handle'], self.world_frame_handle, vrep.simx_opmode_streaming) _, bb_min_x = vrep.simxGetObjectFloatParameter(self.clientID, obs['handle'], 15, vrep.simx_opmode_streaming) _, bb_min_y = vrep.simxGetObjectFloatParameter(self.clientID, obs['handle'], 16, vrep.simx_opmode_streaming) _, bb_min_z = vrep.simxGetObjectFloatParameter(self.clientID, obs['handle'], 17, vrep.simx_opmode_streaming) _, bb_max_x = vrep.simxGetObjectFloatParameter(self.clientID, obs['handle'], 18, vrep.simx_opmode_streaming) _, bb_max_y = vrep.simxGetObjectFloatParameter(self.clientID, obs['handle'], 19, vrep.simx_opmode_streaming) _, bb_max_z = vrep.simxGetObjectFloatParameter(self.clientID, obs['handle'], 20, vrep.simx_opmode_streaming) scale = [bb_max_x - bb_min_x, bb_max_y - bb_min_y, bb_max_z - bb_min_z] obs_info.append({'name': obs['name'], 'position': pos, 'scale': scale}) return obs_info def reset(self): #### clear signals #### vrep.simxClearIntegerSignal(self.clientID, "", vrep.simx_opmode_oneshot) vrep.simxClearFloatSignal(self.clientID, "", vrep.simx_opmode_oneshot) vrep.simxClearStringSignal(self.clientID, "", vrep.simx_opmode_oneshot) #### reset robot and environment #### self.set_position_control_mode() if self.CookingEnv_reset is not None: new_joint_angles = self.CookingEnv_reset.reset(self.all_info)['new_joint_angles'] else: new_joint_angles = self.init_angles for _ in range(5): for i in range(6): return_code = vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[i], new_joint_angles[i], vrep.simx_opmode_oneshot) self.synchronous_trigger() # if self.CookingEnv_config.get('control_mode') == "velocity": # print("vel control mode enable") # self.set_velocity_control_mode() # return self.get_state() def synchronous_trigger(self): return_code, iteration1 = vrep.simxGetIntegerSignal(self.clientID, 'iteration', vrep.simx_opmode_buffer) if return_code != vrep.simx_return_ok: iteration1 = -1 vrep.simxSynchronousTrigger(self.clientID) # vrep.simxGetPingTime(self.clientID) iteration2 = iteration1 while iteration2 == iteration1: # wait until the iteration counter has changed return_code, iteration2 = vrep.simxGetIntegerSignal(self.clientID, 'iteration', vrep.simx_opmode_buffer) if return_code != vrep.simx_return_ok: iteration2 = -1 def get_joint_angles(self): #### retrive joint angles #### joint_positions = [] for joint_handle in self.joint_handles: rc = 1 while rc != 0: rc, joint_position = vrep.simxGetJointPosition(self.clientID, joint_handle, vrep.simx_opmode_streaming) joint_positions.append(joint_position) return np.array(joint_positions) def get_object_pose(self): #### retrive object pose #### if self.object_handles is not None: object_pose = {} for obj_name, obj_handle in viewitems(self.object_handles): rc = 1 while rc != 0: rc, object_position = vrep.simxGetObjectPosition(self.clientID, obj_handle, self.world_frame_handle, vrep.simx_opmode_streaming) rc, object_quat = vrep.simxGetObjectQuaternion(self.clientID, obj_handle, self.world_frame_handle, vrep.simx_opmode_streaming) object_pose[obj_name] = np.array(object_position + object_quat) return object_pose else: return None def step(self, actions, axis=0): if actions.ndim == 2: actions = actions[0,:] # clip actions to limits if self.CookingEnv_config.get('action_space') is not None: clipped_actions = np.clip(np.array(actions), self.action_space['lower_bound'], self.action_space['upper_bound']) else: clipped_actions = actions for i in range(len(self.joint_handles[self.CookingEnv_config.get('arm')])): if self.CookingEnv_config.get('control_mode') == "velocity": return_code = vrep.simxSetJointTargetVelocity(self.clientID, self.joint_handles[self.CookingEnv_config.get('arm')][i], clipped_actions[i], vrep.simx_opmode_oneshot) elif self.CookingEnv_config.get('control_mode') == "position": return_code = vrep.simxSetJointTargetPosition(self.clientID, self.joint_handles[self.CookingEnv_config.get('arm')][i], clipped_actions[i], vrep.simx_opmode_oneshot) elif self.CookingEnv_config.get('control_mode') is None: pass else: raise ValueError("control mode not supported") self.synchronous_trigger() self.update_all_info() def stop(self): vrep.simxStopSimulation(self.clientID, vrep.simx_opmode_oneshot) def pause(self): vrep.simxPauseSimulation(self.clientID, vrep.simx_opmode_oneshot) def close(self): pass
class FsaAugmentedEnv(object): def __init__(self, env_params={}, seed=0, base_env=None, suffix="", reset=None, logger=None, port_num=None): self.FsaAugmentedEnv_config = Configuration(default_config) self.FsaAugmentedEnv_config.update(env_params) #### construct base env #### self.base_env = base_env if self.base_env is None and self.FsaAugmentedEnv_config.get( ['base_env', 'type']) is not None: # construct base self.base_env = self.FsaAugmentedEnv_config.get([ 'base_env', 'type' ])(self.FsaAugmentedEnv_config.get(['base_env', 'config']), port_num=port_num, suffix=suffix, reset=reset, seed=seed, logger=logger) if self.base_env is not None: if not os.path.isdir( self.FsaAugmentedEnv_config.get('fsa_save_dir')): os.makedirs(self.FsaAugmentedEnv_config.get('fsa_save_dir')) self.base_env.set_seed(seed) self.logger = logger # construct fsa reward self.spec = self.FsaAugmentedEnv_config.get('spec') self.predicate_robustness = self.spec['predicate_robustness'] self.fsa = Fsa() self.fsa.from_formula( self.FsaAugmentedEnv_config.get(['spec', 'predicate_form'])) self.fsa.add_trap_state() self.fsa.visualize( draw='pydot', save_path=self.FsaAugmentedEnv_config.get('fsa_save_dir'), dot_file_name=self.FsaAugmentedEnv_config.get('dot_file_name'), svg_file_name=self.FsaAugmentedEnv_config.get('svg_file_name')) self.fsa_reward = FsaReward(self.fsa, self.FsaAugmentedEnv_config.get( ['spec', 'predicate_robustness']), logger=self.logger) self.qs = [ v for k, v in viewitems(self.fsa_reward.aut_states_dict) if v != 1 ] self.q = None # this is the numerical representation of the automata state (we use Q to represent the string version) # seed the environment self.seed(seed) self.all_info = {} self.FsaAugmentedEnv_reset = reset self.state = None #### hack #### # self.load_switchon_policy() self.condimentapplied = -10 from tl_utils.tl_config import TLConfig from utils.utils import get_object_goal_pose self.tl_conf = TLConfig( config={ 'robot': self.FsaAugmentedEnv_config.get('robot'), 'mode': 'sim' }) self.OBJECT_RELATIVE_POSE = self.tl_conf.OBJECT_RELATIVE_POSE self.get_object_goal_pose = get_object_goal_pose def get_state(self, **kwargs): self.update_all_info() return np.concatenate([np.array([self.q]), self.base_env.get_state()]) def reset(self, random_q=False, **kwargs): self.base_env_state = self.base_env.reset() if random_q: self.q = int(np.random.choice(self.qs, 1)) else: self.q = 0 self.Q = self.fsa_reward.get_node_name_from_value(self.q) self.fsa_done = False self.fsa_r = None self.Q_next = None self.curr_edge = None self.Dq = None self.state = self.get_state() return self.state def get_reward(self, state=None, action=None, next_state=None, **kwargs): # r = self.base_env.get_reward(state[1:], action, next_state[1:]) + self.fsa_r r = np.minimum( self.base_env.get_reward(state[1:], action, next_state[1:]), self.fsa_r) return r def is_done(self, state=None, action=None, next_state=None, **kwargs): base_env_done = False if state is not None: base_env_done = self.base_env.is_done(state[1:], action) if base_env_done: print("base env done") return any([base_env_done, self.fsa_done]) def get_info(self): return self.all_info def update_all_info(self, info={}): self.base_env.update_all_info( info={'condimentapplied': self.condimentapplied}) self.all_info.update(self.base_env.get_info()) self.all_info.update({ "Q": self.Q, "Q_next": self.Q_next, 'curr_edge': self.curr_edge, 'Dq': self.Dq }) self.all_info.update(info) def set_node_goal(self, best_node_guard): # print(best_node_guard) ee_goal = None gripper_action = None other_action = None if best_node_guard is not None: best_node_guard_pred_list = best_node_guard.strip().split("&") for node_guard_pred in best_node_guard_pred_list: node_guard_pred = node_guard_pred.strip() if node_guard_pred == 'opengripper': gripper_action = 'opengripper' if node_guard_pred == 'closegripper': gripper_action = 'closegripper' if 'moveto' in node_guard_pred and node_guard_pred[ 0] != "~" and node_guard_pred[0] != "!": ee_goal = node_guard_pred if node_guard_pred == 'flipswitchon': other_action = "flipswitchon" if node_guard_pred == 'flipswitchoff': other_action = "flipswitchoff" if node_guard_pred == 'applycondiment': other_action = 'applycondiment' if ee_goal is not None: object_name = ee_goal.split('_')[1] if len(ee_goal.split('_')) == 3: object_rel_pose_name = ee_goal.split('_')[2] else: object_rel_pose_name = object_name if object_name == 'world': pt = self.OBJECT_RELATIVE_POSE[object_rel_pose_name] else: pt = self.get_object_goal_pose( self.all_info['obj_poses'][object_name], self.OBJECT_RELATIVE_POSE[object_rel_pose_name]) #### tune offset if object_rel_pose_name == 'condimentpre': pt += np.array([0, 0, -0.04, 0, 0, 0, 0]) if object_rel_pose_name == 'condimentpost': pt += np.array([0, 0, -0.04, 0, 0, 0, 0]) if object_rel_pose_name in ['bunplate']: pt[2] += 0.04 if object_rel_pose_name == 'grill': pt[2] += 0.02 self.base_env.set_goal_pose(pt) if gripper_action is not None: if gripper_action == 'opengripper': if self.base_env.get_gripper_state() != 0: self.base_env.set_gripper_state(0) if gripper_action == 'closegripper': if self.base_env.get_gripper_state() != 1: self.base_env.set_gripper_state(1) if other_action is not None: if other_action == 'flipswitchon' or other_action == 'flipswitchoff': #### close gripper if self.base_env.get_gripper_state() != 1: self.base_env.set_gripper_state(1.) if other_action == 'flipswitchon': object_rel_pose = np.array( self.OBJECT_RELATIVE_POSE['switchon']) object_rel_pose += np.array([0, 0.03, -0.02, 0, 0, 0, 0]) #### tune offset if self.all_info['switchon'] > 0: pt_rise = self.all_info['curr_pose'] + np.array( [0.0, 0, 0.08, 0, 0, 0, 0]) for i in range(10): self.base_env.set_goal_pose(pt_rise) self.base_env.step(action=np.zeros( self.base_env.action_space['shape'][0])) elif other_action == 'flipswitchoff': object_rel_pose = np.array( self.OBJECT_RELATIVE_POSE['switchoff']) object_rel_pose += np.array([0, 0.03, -0.025, 0, 0, 0, 0]) else: raise ValueError('action not supported') pt = self.get_object_goal_pose( self.all_info['obj_poses']['grill'], object_rel_pose) self.base_env.set_goal_pose(pt) # curr_pos, curr_quat = self.base_env.get_ee_pose() # curr_linear_vel, curr_angular_vel = self.base_env.get_ee_velocity() # curr_angular_vel = curr_angular_vel * np.pi / 180 # curr_pose = np.concatenate([curr_pos, curr_quat]) # curr_vel = np.concatenate([curr_linear_vel, curr_angular_vel]) # # ddy, dy, y = self.skill_flipswitchon(pt, curr_pose, curr_vel, self.all_info['obs_info']) # self.base_env.set_target_pose(y) elif other_action == 'applycondiment': for i in range(20): vel_scale = 2. * np.sin(0.3 * i) self.base_env.pub_ee_frame_velocity(direction='z', vel_scale=vel_scale, duration_sec=0.1) self.condimentapplied = 10 self.update_all_info() else: raise ValueError('other_action not supported') def step_fsa(self, mdp_state, action, next_mdp_state): Q = self.fsa_reward.get_node_name_from_value(self.q) Q_next, fsa_r, curr_edge, fsa_done, DQ_nontrap, DQ_trap, best_node_guard, trap_node_guard = self.fsa_reward.step( Q, mdp_state, action, next_mdp_state) self.q = self.fsa_reward.get_node_value_from_name(Q_next) self.fsa_done = fsa_done self.fsa_r = fsa_r self.set_node_goal(best_node_guard) def step_base_env(self, actions, **kwargs): self.base_env.step(actions) def step(self, actions): # TODO: need to modify this for concurrent mode self.step_base_env(actions) state = self.get_state() self.step_fsa(self.state[1:], actions, state[1:]) self.state = state def seed(self, seed=0, **kwargs): np.random.seed(seed) def close(self, **kwargs): self.base_env.close() @property def state_space(self): return { "type": "float", "shape": (self.base_env.state_space['shape'][0] + 1, ), "upper_bound": [len(self.fsa_reward.aut_states_dict.keys())] + self.base_env.state_space['upper_bound'], "lower_bound": [0] + self.base_env.state_space['lower_bound'] } @property def action_space(self): return self.base_env.action_space def save(self, save_dir): pass def restore(self, restore_dir): ''' restores the environment ''' pass def teleop(self, cmd): pass ######## # Misc # ######## def load_switchon_policy(self): from utils.utils import load_policy_and_preprocessor ## flip switch open experiment_root_dir = os.path.join(os.environ['LEARNING_PATH'], 'learning', 'experiments') experiment_name = 'switchon' hyperparam_dir = 'seed0' itr = 100 learned_skill_config = { "state_space": { 'type': 'float', 'shape': (3, ), "upper_bound": [], 'lower_bound': [] }, "action_space": { 'type': 'float', 'shape': (3, ), "upper_bound": 70 * np.ones(3), 'lower_bound': -70 * np.ones(3) }, "training_config_restore_path": os.path.join(experiment_root_dir, experiment_name, 'config', hyperparam_dir, 'config.pkl'), "policy_restore_path": os.path.join(experiment_root_dir, experiment_name, 'transitions', hyperparam_dir, 'itr_' + str(itr)), "state_preprocessor_restore_path": os.path.join(experiment_root_dir, experiment_name, 'info', hyperparam_dir, 'state_preprocessor_params.pkl') } self.switchon_policy, self.switchon_state_preprocessor = load_policy_and_preprocessor( learned_skill_config) self.learned_skills_dict = { 'flipswitchon': { 'policy': self.switchon_policy, 'state_preprocessor': self.switchon_state_preprocessor } }