def lcm_mode(): import_lcm_python() lc = LCM() subscription = lc.subscribe('VALVE', valve_handler) print 'Subscribed to the VAVLE channel, waiting for command...' while True: lc.handle()
def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = ConveyorBeltManipReactiveQuery(geo_setup_file) self._node = None with open(traj_setup_file) as fp: self._traj_setup = json.load(fp)
def test_publish(self): """ Ensures that no warnings are issued using `lcm.publish()`. """ lcm = LCM("memq://") with warnings.catch_warnings(): warnings.simplefilter("error", DeprecationWarning) lcm.publish("TEST_CHANNEL", b"")
def fun(restart_iter): np.random.seed(restart_iter) kern = LCM(input_dim=self.problem.DP, num_outputs=data.NI, Q=Q) # print('I am here') return kern.train_kernel(X=data.X, Y=data.Y, computer=self.computer, kwargs=kwargs)
def publish_floor_change(floor_number): lcm = LCM() msg = floor_change_msg_t() msg.utime = int(time.time() * 1e6) msg.floor_no = floor_number print "Publishing change to floor %d" % floor_number lcm.publish("FLOOR_CHANGE", msg.encode())
def train_mpi(self, data : Data, i_am_manager : bool, restart_iters : Collection[int] = None, **kwargs): if (kwargs['model_latent'] is None): Q = data.NI else: Q = kwargs['model_latent'] if (kwargs['distributed_memory_parallelism'] and i_am_manager): mpi_comm = self.computer.spawn(__file__, nproc=kwargs['model_restart_processes'], nthreads=kwargs['model_restart_threads'], kwargs=kwargs) # XXX add args and kwargs kwargs_tmp = kwargs # print("kwargs_tmp",kwargs_tmp) if "mpi_comm" in kwargs_tmp: del kwargs_tmp["mpi_comm"] # mpi_comm is not picklable _ = mpi_comm.bcast((self, data, restart_iters, kwargs_tmp), root=mpi4py.MPI.ROOT) tmpdata = mpi_comm.gather(None, root=mpi4py.MPI.ROOT) mpi_comm.Disconnect() res=[] for p in range(int(kwargs['model_restart_processes'])): res = res + tmpdata[p] elif (kwargs['shared_memory_parallelism']): #YL: not tested #with concurrent.futures.ProcessPoolExecutor(max_workers = kwargs['search_multitask_threads']) as executor: with concurrent.futures.ThreadPoolExecutor(max_workers = kwargs['model_restart_threads']) as executor: def fun(restart_iter): if ('seed' in kwargs): seed = kwargs['seed'] * kwargs['model_restart_threads'] + restart_iter else: seed = restart_iter np.random.seed(seed) kern = LCM(input_dim = len(data.P[0][0]), num_outputs = data.NI, Q = Q) if (restart_iter == 0 and self.M is not None): kern.set_param_array(self.M.kern.get_param_array()) return kern.train_kernel(X = data.P, Y = data.O, computer = self.computer, kwargs = kwargs) res = list(executor.map(fun, restart_iters, timeout=None, chunksize=1)) else: def fun(restart_iter): np.random.seed(restart_iter) kern = LCM(input_dim = len(data.P[0][0]), num_outputs = data.NI, Q = Q) # print('I am here') return kern.train_kernel(X = data.P, Y = data.O, computer = self.computer, kwargs = kwargs) res = list(map(fun, restart_iters)) if (kwargs['distributed_memory_parallelism'] and i_am_manager == False): return res kern = LCM(input_dim = len(data.P[0][0]), num_outputs = data.NI, Q = Q) bestxopt = min(res, key = lambda x: x[1])[0] kern.set_param_array(bestxopt) # YL: why sigma is enough to compute the likelihood, see https://gpy.readthedocs.io/en/deploy/GPy.likelihoods.html likelihoods_list = [GPy.likelihoods.Gaussian(variance = kern.sigma[i], name = "Gaussian_noise_%s" %i) for i in range(data.NI)] self.M = GPy.models.GPCoregionalizedRegression(data.P, data.O, kern, likelihoods_list = likelihoods_list) return
def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = StationaryMultiWPManipReactiveQuery(geo_setup_file) self._node = None self._traj_perturbed = False with open(traj_setup_file) as fp: self._traj_setup = json.load(fp)
class JsonManipulatorTrajRunner: def __init__(self, filename): with open(filename) as fp: self.plan = json.load(fp) self._lcm = LCM() self._lcm.subscribe("EXECUTION_STATUS", self._kuka_plan_runner_status_handler) self.wsg_utime = 0 self.torque_mode = 0 self.cur_node_id = 0 self.plan_started = False self.node_completed = False def run_plan(self): if len(self.plan): print("Node", self.cur_node_id) self._publish_node_traj(self.plan[self.cur_node_id]) while True: self._lcm.handle() if self.node_completed: self.node_completed = False self.cur_node_id += 1 if self.cur_node_id >= len(self.plan): print("Plan Completed") break print("Node", self.cur_node_id) self._publish_node_traj(self.plan[self.cur_node_id]) def _publish_node_traj(self, node): msg = dict_to_lcmt_manipulator_traj(node) if USE_TORQUE: msg.dim_torques = 7 else: msg.dim_torques = 0 if msg.n_time_steps: self._lcm.publish("COMMITTED_ROBOT_PLAN", msg.encode()) print("Trajectory Published!") def _kuka_plan_runner_status_handler(self, channel, msg): print("Kuka Traj Completed") self.node_completed = True def run_plan_lcm_wrapper(self): self._lcm.subscribe("START_PLAN", self._start_plan_handler) while (not self.plan_started): self._lcm.handle() def _start_plan_handler(self, channel, msg): self.plan_started = True print("Starting plan") self.run_plan()
def fun(restart_iter): if ('seed' in kwargs): seed = kwargs['seed'] * kwargs['model_restart_threads'] + restart_iter else: seed = restart_iter np.random.seed(seed) kern = LCM(input_dim = len(data.P[0][0]), num_outputs = data.NI, Q = Q) if (restart_iter == 0 and self.M is not None): kern.set_param_array(self.M.kern.get_param_array()) return kern.train_kernel(X = data.P, Y = data.O, computer = self.computer, kwargs = kwargs)
def fun(restart_iter): # np.random.seed(restart_iter) np.random.seed() kern = LCM(input_dim=len(data.P[0][0]), num_outputs=data.NI, Q=Q) return kern.train_kernel(X=data.P, Y=data.O, computer=self.computer, kwargs=kwargs)
class ConveyorBeltManipReactiveMotionPlanRunner(BasicManipMotionPlanRunner): def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = ConveyorBeltManipReactiveQuery(geo_setup_file) self._node = None with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def update_object_state(self, object_state): self._query.update_object_state(object_state)
def __init__(self, filename): with open(filename) as fp: self.plan = json.load(fp) self._lcm = LCM() self._lcm.subscribe("EXECUTION_STATUS", self._kuka_plan_runner_status_handler) self.wsg_utime = 0 self.torque_mode = 0 self.cur_node_id = 0 self.plan_started = False self.node_completed = False
class ReactiveMultiWPStaionaryManipMotionPlanRunner(MultiWPStaionaryManipMotionPlanRunner): def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = StationaryMultiWPManipReactiveQuery(geo_setup_file) self._node = None self._traj_perturbed = False with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def update_object_state(self, object_state): self._query.update_object_state(object_state)
def __init__(self, lcm=None): if lcm is None: lcm = LCM() self.lcm = lcm self.client_id = CLIENT_ID_FACTORY.new_client_id() self.tree = LazyTree() self.queue = CommandQueue() self.publish_immediately = True self.lcm.subscribe(self._response_channel(), self._handle_response) self.handler_thread = None
def __init__(self, query_file, ddp_traj_file): with open(query_file) as fp: data = json.load(fp) self._queries = [] for d in data: try: self._queries.append(dict_to_lcmt_multi_wp_manip_query(d)) except: self._queries.append(None) with open(ddp_traj_file) as fp: self._ddp_traj = json.load(fp) self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._query_results_handler) self._cur_query = None self._completed = False self.trajs = [] self.q_idx = 0
def gen_model_from_hyperparameters(self, data: Data, hyperparameters: list, **kwargs): if (kwargs['RCI_mode'] is False): from lcm import LCM if (kwargs['model_latent'] is None): Q = data.NI else: Q = kwargs['model_latent'] kern = LCM(input_dim=len(data.P[0][0]), num_outputs=data.NI, Q=Q) #print ("received hyperparameters: " + str(hyperparameters)) kern.set_param_array(hyperparameters) likelihoods_list = [ GPy.likelihoods.Gaussian(variance=kern.sigma[i], name="Gaussian_noise_%s" % i) for i in range(data.NI) ] self.M = GPy.models.GPCoregionalizedRegression( data.P, data.O, kern, likelihoods_list=likelihoods_list) return
def run(): try: import sheriff_config cfg_fname = sys.argv[1] except IndexError: cfg = None else: cfg = sheriff_config.config_from_filename(cfg_fname) lc = LCM() def handle(*a): try: lc.handle() except Exception, e: import traceback traceback.print_exc() return True
#!/usr/bin/env python import time import sys import os from lcm import LCM from syslcm.host_status_t import host_status_t lcm = LCM() if len(sys.argv) > 1: hostname = sys.argv[1] else: hostname = os.popen('uname -n').readline().strip() # determine number of processors temp = os.popen('cat /proc/cpuinfo | grep processor | wc -l').readline() num_processors = int(temp[0]) # do we have the cpufrequtils package temp = os.popen('which cpufreq-info').readline() have_cpufrequtils = 1 if (len(temp) == 0): print "cpufrequtils package required for cpu frequency info" have_cpufrequtils = 0 while (True): msg = host_status_t() msg.utime = int(time.time() * 1e6) msg.id = hostname
class MultiWPStaionaryManipMotionPlanRunner(BasicManipMotionPlanRunner): def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = StationaryMultiWPManipQuery(geo_setup_file) self._node = None self._traj_perturbed = False with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def _generate_move_query(self, node, op_name): """ Generates Multi WP move query for traj op Args: node: tree search node op_name: (string) name of operation Returns: msg: (lcmt_multi_wp_manip_query) """ if node.parent_traj: q_prev = node.parent_traj["states"][-1] gripper_width_prev = node.parent_traj["gripper_width"][-1] else: q_prev = self._traj_setup["init_iiwa_pos"] gripper_width_prev = self._traj_setup["gripper_open_width"] if node.parent is None: print("q_prev:", q_prev) input("") print("Parent EE:", node.parent.final_ee) if (node.parent.final_ee is not None) and ( "init_offset_wp" in self._traj_setup[op_name]): # self._traj_setup[op_name]["init_offset_wp"] is not None): print("Adding lift waypoint") print("Lift:", self._traj_setup[op_name]["init_offset_wp"]) wp = (np.array(node.parent.final_ee) + np.array(self._traj_setup[op_name]["init_offset_wp"])).tolist() ee_desired_list = [wp] time_horizon_list = [self._traj_setup[op_name]["init_offset_time"]] constrain_orientation_list = [self._traj_setup[op_name]["init_offset_constrain_orientation"]] else: ee_desired_list = [] time_horizon_list = [] constrain_orientation_list = [] ee_desired_list.extend(self._query.get_desired_ee_pos(node)) node.final_ee = ee_desired_list[-1] time_horizon_list.extend(self._traj_setup[op_name]["time_horizon"]) node.time = node.parent.time + sum(time_horizon_list) constrain_orientation_list.extend(self._traj_setup[op_name]["constrain_orientation"]) print("Prev q:", q_prev) print("DESIRED EE:", ee_desired_list) msg = lcmt_multi_wp_manip_query() msg.name = node.action.name[1:-1].split(" ")[0] msg.dim_q = 7 msg.n_wp = len(ee_desired_list) msg.option = node.option ################################################################################ # Setup false bypass ik msg.bypass_ik = False msg.q_goal = [] for i in range(msg.n_wp): msg.q_goal.append([0 for j in range(msg.dim_q)]) ################################################################################ if node.option=="ddp": msg.time_step = self._traj_setup["ddp_time_step"] elif node.option=="admm": msg.time_step = self._traj_setup["admm_time_step"] msg.time_horizon = time_horizon_list msg.prev_gripper_width = gripper_width_prev msg.desired_ee = ee_desired_list msg.prev_q = q_prev msg.gripper_force = self._traj_setup["gripper_force"] msg.constrain_orientation = constrain_orientation_list ################################################################################ # set object states op = node.action.name[1:-1] if (node.parent.final_ee is not None and (op.split(" ")[0]=="move-to-free-table" or op.split(" ")[0]=="move-to-goal-table")): object_name = op.split(" ")[2] vel_obj = [0 for _ in range(6)] xyz_iiwa = [-0.2, 0, 0] msg.use_object = True q_obj_init = self._query.get_q_object_init(object_name, node.parent.time, xyz_iiwa) q_obj_list = [] for i in range(msg.n_wp): if i == 0: ee_init = node.parent.final_ee q_init = q_obj_init else: ee_init = msg.desired_ee[i-1] q_init = q_obj_list[i-1] ee_final = msg.desired_ee[i] q_obj_list.append(self._query.get_q_object_final(q_init, ee_init, ee_final)) msg.q_init_object = [] msg.q_init_object.extend(q_obj_init) msg.q_init_object.extend(vel_obj) msg.q_desired_object = [] for i in range(msg.n_wp): q_obj_final = [] q_obj_final.extend(q_obj_list[i]) q_obj_final.extend(vel_obj) msg.q_desired_object.append(q_obj_final) print("q_init_object", msg.q_init_object) print("q_desired_object", msg.q_desired_object) else: msg.use_object = False msg.q_init_object = [0 for _ in range(13)] msg.q_desired_object = [] for i in range(msg.n_wp): msg.q_desired_object.append([0 for _ in range(13)]) ################################################################################ return msg
def train_mpi(self, data: Data, i_am_manager: bool, restart_iters: Collection[int] = None, **kwargs): if (kwargs['RCI_mode'] is False): from lcm import LCM if (kwargs['model_latent'] is None): Q = data.NI else: Q = kwargs['model_latent'] if (kwargs['distributed_memory_parallelism'] and i_am_manager): mpi_comm = self.computer.spawn( __file__, nproc=kwargs['model_restart_processes'], nthreads=kwargs['model_restart_threads'], kwargs=kwargs) # XXX add args and kwargs kwargs_tmp = kwargs # print("kwargs_tmp",kwargs_tmp) if "mpi_comm" in kwargs_tmp: del kwargs_tmp["mpi_comm"] # mpi_comm is not picklable _ = mpi_comm.bcast((self, data, restart_iters, kwargs_tmp), root=mpi4py.MPI.ROOT) tmpdata = mpi_comm.gather(None, root=mpi4py.MPI.ROOT) mpi_comm.Disconnect() res = [] for p in range(int(kwargs['model_restart_processes'])): res = res + tmpdata[p] elif (kwargs['shared_memory_parallelism']): #YL: not tested #with concurrent.futures.ProcessPoolExecutor(max_workers = kwargs['search_multitask_threads']) as executor: with concurrent.futures.ThreadPoolExecutor( max_workers=kwargs['model_restart_threads']) as executor: def fun(restart_iter): # if ('seed' in kwargs): # seed = kwargs['seed'] * kwargs['model_restart_threads'] + restart_iter # else: # seed = restart_iter # np.random.seed(seed) ## np.random.seed() kern = LCM(input_dim=len(data.P[0][0]), num_outputs=data.NI, Q=Q) # if (restart_iter == 0 and self.M is not None): # kern.set_param_array(self.M.kern.get_param_array()) return kern.train_kernel(X=data.P, Y=data.O, computer=self.computer, kwargs=kwargs) res = list( executor.map(fun, restart_iters, timeout=None, chunksize=1)) else: def fun(restart_iter): # np.random.seed(restart_iter) np.random.seed() kern = LCM(input_dim=len(data.P[0][0]), num_outputs=data.NI, Q=Q) return kern.train_kernel(X=data.P, Y=data.O, computer=self.computer, kwargs=kwargs) res = list(map(fun, restart_iters)) if (kwargs['distributed_memory_parallelism'] and i_am_manager == False): return res kern = LCM(input_dim=len(data.P[0][0]), num_outputs=data.NI, Q=Q) best_result = min(res, key=lambda x: x[1]) bestxopt = best_result[0] neg_log_marginal_likelihood = best_result[1] gradients = best_result[2] iteration = best_result[3] kern.set_param_array(bestxopt) if (kwargs['verbose'] == True): # print('hyperparameters:', kern.get_param_array()) print('theta:', kern.theta) print('var:', kern.var) print('kappa:', kern.kappa) print('sigma:', kern.sigma) print('WS:', kern.WS) # YL: likelihoods needs to be provided, since K operator doesn't take into account sigma/jittering, but Kinv does. The GPCoregionalizedRegression intialization will call inference in GPy/interence/latent_function_inference/exact_gaussian_inference.py, and add to diagonals of the K operator with sigma+1e-8 likelihoods_list = [ GPy.likelihoods.Gaussian(variance=kern.sigma[i], name="Gaussian_noise_%s" % i) for i in range(data.NI) ] self.M = GPy.models.GPCoregionalizedRegression( data.P, data.O, kern, likelihoods_list=likelihoods_list) #print ("kernel: " + str(kern)) #print ("bestxopt:" + str(bestxopt)) #print ("neg_log_marginal_likelihood:" + str(neg_log_marginal_likelihood)) #print ("gradients: " + str(gradients)) #print ("iteration: " + str(iteration)) #for i in range(data.NI): # print ("i: " + str(i)) # print ("sigma: " + str(kern.sigma[i])) # print ("likelihoods_list: " + str(likelihoods_list[i].to_dict())) #print ("likelihoods_list_len: " + str(data.NI)) #print ("self.M: " + str(self.M)) return (bestxopt, neg_log_marginal_likelihood, gradients, iteration)
new_type = TaskType(period=task_instance[0], release=task_instance[1], execution=task_instance[2], deadline=task_instance[3], name=name) task_types.append(new_type) if new_type.name[:8] == "Sporadic": sporadic_template = new_type else: aperiodic_tasks.append(Task_Inputs(a_start=task_instance[1], a_end=int(task_instance[1]) + int(task_instance[2]), a_deadline=task_instance[3], name=name)) """Calculate Hyperperiod from task""" for task_type in task_types: hyperperiod.append(task_type.period) hyperperiod = LCM(hyperperiod) # Sort task types task_types = sorted(task_types, key=cmp_to_key(tasktype_cmp)) aperiodic_tasks = sorted(aperiodic_tasks, key=cmp_to_key(aperiodic_cmp)) """Create Task Instances""" is_first_sporadic_set = False for i in range(0, hyperperiod): for task_type in task_types: if (i - task_type.release) % task_type.period == 0 and i >= task_type.release: if task_type.name[:8] == "Sporadic": if is_first_sporadic_set == True: continue else:
def run(joint_index, save_file_base): lcm = LCM(robot_lcm_url) status_sub = LcmMessagePoll(lcm, status_channel, lcmt_panda_status) events = [] print(f"joint_index: {joint_index}") print("Waiting for first message...") lcm.handle() count = 0 dt_des = 0.005 print("Received! Starting") t_abs_start = time.time() t = 0.0 t_transient_start = 3.0 t_transient_end = 0.25 t_sweep = 10.0 T_sec_min = 1.0 T_sec_max = 5.0 v = None try: while True: lcm_handle_all(lcm) status = status_sub.get() if status is None: continue t_prev = t t = time.time() - t_abs_start s = min(t / t_transient_start, 1.0) dt = t - t_prev v = np.array(status.joint_velocity) assert v.shape == (ndof,), v.shape # T_sec = np.array([4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0]) / 4 s_chirp = min(t / t_sweep, 1.0) if s_chirp >= 1.0: raise KeyboardInterrupt T_sec = T_sec_min + (1 - s_chirp) * (T_sec_max - T_sec_min) print(T_sec) w = 2 * np.pi / T_sec v_max = np.zeros(ndof) v_max[joint_index] = 1.0 # v_max = np.array([0.5, 1.5, 0.5, 1.0, 1.0, 1.5, 1.5]) vd = np.zeros(ndof) vd[:] = s * v_max * np.sin(w * t) cmd = make_command(vd) lcm.publish(cmd_channel, cmd.encode()) events.append((status, cmd)) count += 1 t_next = t + dt_des while time.time() - t_abs_start < t_next: time.sleep(dt_des / 100) except KeyboardInterrupt: # Wind down. assert v is not None v0 = v vd = np.zeros(ndof) t_abs_start = time.time() t = 0.0 while t < t_transient_end: lcm_handle_all(lcm) status = status_sub.get() if status is None: continue t = time.time() - t_abs_start s = min(t / t_transient_end, 1.0) vd = v0 * (1 - s) cmd = make_command(vd) lcm.publish(cmd_channel, cmd.encode()) events.append((status, cmd)) t_next = t + dt_des while time.time() - t_abs_start < t_next: time.sleep(dt_des / 100) finally: # Save data. file = expanduser(f"~/data/panda/tracking/{save_file_base}.pkl") with open(file, "wb") as f: pickle.dump(events, f) print(f"Wrote: {file}")
sys.exit(1) if len(args) > 1: script_name = args[1] if observer: if cfg: print "Loading a config file is not allowed when starting in observer mode." sys.exit(1) if not use_gui: print "Refusing to start an observer without a gui -- that would be useless." sys.exit(1) if spawn_deputy: print "Lone ranger mode and observer mode are mutually exclusive." sys.exit(1) lc = LCM() def handle(*a): try: lc.handle() except Exception: traceback.print_exc() return True gobject.io_add_watch(lc, gobject.IO_IN, handle) if use_gui: gui = SheriffGtk(lc) if observer: gui.set_observer(True) if spawn_deputy:
from lcm import LCM import pandas as pd LCM.get_new_lens = profile(LCM.get_new_lens) LCM._inner = profile(LCM._inner) #columns = ['song_id','user_id', 'artist_id', 'provider_id', 'ip'] #df = pd.read_csv('listen-20131115.log', names=columns).head(10000) path = '/Users/remiadon/Downloads/retail.dat' df = pd.read_csv(path, header=None, squeeze=True, error_bad_lines=False, nrows=int(1e3)).map(str.split) lcm = LCM(min_supp=2, n_jobs=4, filter_fn=lambda e: e, return_tids=False) lcm.add(df) lcm.discover()
class BasicManipMotionPlanRunner(object): """ Runs Motion Plan for conveyor belt domain """ def __init__(self, geo_setup_file, traj_setup_file): self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._results_handler) self._query = ConveyorBeltManipQuery(geo_setup_file) self._node = None self._traj_perturbed = False with open(traj_setup_file) as fp: self._traj_setup = json.load(fp) def _results_handler(self, channel, msg): print("message received.") data = lcmt_manipulator_traj.decode(msg) print("Cost:", data.cost) if not data.n_time_steps: print("IK Infeasible") elif data.cost == float('inf') or math.isnan( data.cost) or data.cost == 0: if self._node.move_query.time_step / 2 >= self._traj_setup[ "min_time_step"]: self._node.move_query.time_step /= 2 self._lcm.publish("TREE_SEARCH_QUERY", self._node.move_query.encode()) print("Decreasing time step to", self._node.move_query.time_step) return # ans = input("Trajectory not found. Try Again? [yes/no]") # while not (ans == "yes" or ans == "no"): # ans = input("Please input yes or no...") # if ans =="yes": # self._lcm.publish("TREE_SEARCH_QUERY", self._node.move_query.encode()) # print("Decreasing time step to", # self._node.move_query.time_step) # return # elif ans == "no": # print("Traj Op terminated by user. Trajectory cannot be found.") elif not self._traj_perturbed: self._node.move_query.time_step = self._traj_setup[ "admm_time_step"] self._perturb_desired_ee(self._node, sigma=0.005) self._lcm.publish("TREE_SEARCH_QUERY", self._node.move_query.encode()) print("Perturbed desired ee, reset time step") return else: print("Minimum time step reached. Trajectory cannot be found.") self.results = lcmt_manipulator_traj_to_dict(data) self.completed = True def run(self, node): """ returns results of query for conveyor belt manipulation domain Args: node: (PddlTampNode) Returns: results: (Dict) containing trajectory info """ self._node = node op_name = node.action.name[1:-1].split(" ")[0] obj_name = node.action.name[1:-1].split(" ")[2] if node.parent is not None: node.processed_object = node.parent.processed_object.copy() node.discrete_cost = node.parent.discrete_cost if obj_name not in node.processed_object: node.processed_object.append(obj_name) if "base_object_cost" in self._traj_setup: obj_name = node.action.name[1:-1].split(" ")[2] alpha = self._traj_setup["base_object_cost"]["alpha"] base_obj_cost = self._traj_setup["base_object_cost"][obj_name] discrete_obj_cost = base_obj_cost * (alpha**len( node.processed_object)) node.discrete_cost += discrete_obj_cost print("Adding New discrete cost", obj_name, discrete_obj_cost) print("Total discrete cost", node.discrete_cost) self.completed = False if self._traj_setup[op_name]["traj_op"]: if node.move_query is None: node.move_query = self._generate_move_query(node, op_name) self._lcm.publish("TREE_SEARCH_QUERY", node.move_query.encode()) print("LCM Query Published...") else: if isinstance(self._traj_setup[op_name]["time_horizon"], list): node.time = node.parent.time + sum( self._traj_setup[op_name]["time_horizon"]) else: node.time = node.parent.time + self._traj_setup[op_name][ "time_horizon"] self.results = self._generate_traj(node, op_name) self.completed = True timeout = 5000 while (True): if self.completed: return self.results rfds, wfds, efds = select.select([self._lcm.fileno()], [], [], timeout) if rfds: self._lcm.handle() def _perturb_desired_ee(self, node, sigma=0.01): if node.move_query is None: print("Warning: no move query available") return if isinstance(node.move_query.desired_ee, list): for ee in node.move_query.desired_ee: for element in ee: element += random.gauss(0, sigma) else: for element in node.move_query.desired_ee: element += random.gauss(0, sigma) self._traj_perturbed = True def _generate_move_query(self, node, op_name): if node.parent_traj: q_prev = node.parent_traj["states"][-1] gripper_width_prev = node.parent_traj["gripper_width"][-1] else: q_prev = self._traj_setup["init_iiwa_pos"] gripper_width_prev = self._traj_setup["gripper_open_width"] if node.parent is None: print("q_prev:", q_prev) input("") ee_desired = self._query.get_desired_ee_pos(node) print("Prev q:", q_prev) print("DESIRED EE:", ee_desired) msg = lcmt_motion_plan_query() msg.name = node.action.name[1:-1].split(" ")[0] msg.dim_q = 7 if node.option == "ddp": msg.level = 1 msg.time_step = self._traj_setup["ddp_time_step"] elif node.option == "admm": msg.level = 2 msg.time_step = self._traj_setup["admm_time_step"] msg.wait_time = self._traj_setup[op_name]["wait_time"] msg.time_horizon = self._traj_setup[op_name]["time_horizon"] msg.prev_gripper_width = gripper_width_prev msg.desired_ee = ee_desired msg.prev_q = q_prev msg.gripper_force = self._traj_setup["gripper_force"] node.final_ee = ee_desired return msg def _generate_traj(self, node, op_name): if isinstance(self._traj_setup[op_name]["time_horizon"], list): time_horizon = sum(self._traj_setup[op_name]["time_horizon"]) else: time_horizon = self._traj_setup[op_name]["time_horizon"] node.final_ee = node.parent.final_ee time_step = self._traj_setup["manual_time_step"] if self._traj_setup[op_name]["gripper_open"] < 0: gripper_width = self._traj_setup["gripper_close_width"] elif self._traj_setup[op_name]["gripper_open"] > 0: gripper_width = self._traj_setup["gripper_open_width"] else: if node.parent_traj: gripper_width = node.parent_traj["gripper_width"][-1] else: gripper_width = self._traj_setup["init_gripper_width"] msg = dict() msg["n_time_steps"] = int(time_horizon / time_step) msg["dim_torques"] = 0 msg["dim_states"] = self._traj_setup["n_joints"] msg["cost"] = self._traj_setup[op_name]["cost"] cur_time = 0 msg["times_sec"] = [] msg["states"] = [] msg["torques"] = [] msg["gripper_width"] = [] msg["gripper_force"] = [] if node.parent_traj: q = node.parent_traj["states"][-1] else: q = self._traj_setup["init_iiwa_pos"] for _ in range(msg["n_time_steps"]): msg["states"].append(q) msg["torques"].append([]) msg["gripper_width"].append(gripper_width) msg["gripper_force"].append(self._traj_setup["gripper_force"]) msg["times_sec"].append(cur_time) cur_time += time_step return msg
class MotionQueryHandler: def __init__(self, query_file, ddp_traj_file): with open(query_file) as fp: data = json.load(fp) self._queries = [] for d in data: try: self._queries.append(dict_to_lcmt_multi_wp_manip_query(d)) except: self._queries.append(None) with open(ddp_traj_file) as fp: self._ddp_traj = json.load(fp) self._lcm = LCM() self._lcm.subscribe("TREE_SEARCH_QUERY_RESULTS", self._query_results_handler) self._cur_query = None self._completed = False self.trajs = [] self.q_idx = 0 def _query_results_handler(self, channel, msg): print("message received.") data = lcmt_manipulator_traj.decode(msg) print("Cost:", data.cost) if not data.n_time_steps: print("IK Infeasible") elif data.cost == float('inf') or math.isnan( data.cost) or data.cost <= 1: if self._cur_query.time_step / 2 >= MIN_TIME_STEP: self._cur_query.time_step /= 2 self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("Decreasing time step to", self._cur_query.time_step) return else: print("Minimum time step reached. Trajectory cannot be found.") self.trajs.append(self._ddp_traj[self.q_idx]) self._completed = True return self.trajs.append(lcmt_manipulator_traj_to_dict(data)) self._completed = True def Run(self): self.q_idx = 0 total_time = 0 for self.q_idx in range(len(self._queries)): self._cur_query = self._queries[self.q_idx] if self._cur_query is None: print( "No move query in this action. Keeping original trajectory" ) self.trajs.append(self._ddp_traj[self.q_idx]) continue if self._cur_query.time_step > MAX_TIME_STEP: self._cur_query.time_step = MAX_TIME_STEP print("Computing ADMM " + str(self.q_idx + 1) + "/" + str(len(self._queries))) print("Action: " + self._cur_query.name) self._completed = False start_time = time.time() self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("LCM Query Published...") while not self._completed: self._lcm.handle() end_time = time.time() total_time += end_time - start_time print("Computation time:", end_time - start_time) print("Total computation time:", total_time) self._save_trajs() def run_single_traj(self, q_idx): self._cur_query = self._queries[q_idx] print("Action: " + self._cur_query.name) if self._cur_query.time_step > MAX_TIME_STEP: self._cur_query.time_step = MAX_TIME_STEP if self._cur_query is None: print("No move query in this action. Keeping original trajectory") self.trajs.append(self._ddp_traj[q_idx]) else: print("Computing ADMM " + str(q_idx + 1) + "/" + str(len(self._queries))) self._completed = False start_time = time.time() self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("LCM Query Published...") while not self._completed: self._lcm.handle() self._save_trajs() def run_action_names(self, action_name): self.q_idx = 0 total_time = 0 for self.q_idx in range(len(self._queries)): if self._queries[self.q_idx] is not None and self._queries[ self.q_idx].name.startswith(action_name): self._cur_query = self._queries[self.q_idx] else: self._cur_query = None if self._cur_query is None: print( "No move query in this action. Keeping original trajectory" ) self.trajs.append(self._ddp_traj[self.q_idx]) continue if self._cur_query.time_step > MAX_TIME_STEP: self._cur_query.time_step = MAX_TIME_STEP print("Computing ADMM " + str(self.q_idx + 1) + "/" + str(len(self._queries))) print("Action: " + self._cur_query.name) self._completed = False start_time = time.time() self._lcm.publish("TREE_SEARCH_QUERY", self._cur_query.encode()) print("LCM Query Published...") while not self._completed: self._lcm.handle() end_time = time.time() total_time += end_time - start_time print("Computation time:", end_time - start_time) print("Total computation time:", total_time) self._save_trajs() def _save_trajs(self): print("ADMM completed, saving results") filename = "/home/zhigen/code/drake/manipulation_tamp/results/admm_traj" + datetime.now( ).strftime("%Y%m%dT%H%M%S") + ".json" with open(filename, 'w') as out: json.dump(self.trajs, out)
def handle_lcm(): lc = LCM() subscription = lc.subscribe('ARM', arm_handler) print 'Subscribed to the ARM channel, waiting for command...' while True: lc.handle()
import time from random import randint # import pydrake.systems.lcm as mut from lcm import LCM import numpy as np import matplotlib.pyplot as plt import matplotlib.animation as animation from numpy import fft fig, axes = plt.subplots(len(lcm_channels), 1) x = [[], [], [], []] y = [[], [], [], []] #y2 = [] loop_time = 0 lcm_ = LCM() def initPlots(): index = 0 lcmSub = lcm_.subscribe(lcm_channels[index], callback) lcmSub.set_queue_capacity(10) for ax in axes: ax.set_title(lcm_channels[index]) index += 1 def runPlots(i): global loop_time, x, y, lcm_channels, lcm_, currentValues index = 0 for ax in axes:
sys.exit(1) if len(args) > 1: script_name = args[1] if observer: if cfg: print "Loading a config file is not allowed when starting in observer mode." sys.exit(1) if not use_gui: print "Refusing to start an observer without a gui -- that would be useless." sys.exit(1) if spawn_deputy: print "Lone ranger mode and observer mode are mutually exclusive." sys.exit(1) lcm_obj = LCM() def handle(*a): try: lcm_obj.handle() except Exception: traceback.print_exc() return True gobject.io_add_watch(lcm_obj, gobject.IO_IN, handle) gobject.threads_init() if use_gui: gui = SheriffGtk(lcm_obj) if observer: gui.sheriff.set_observer(True)