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