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)
