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()
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 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}")
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)
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
def handle_lcm():
lc = LCM()
subscription = lc.subscribe('ARM', arm_handler)
print 'Subscribed to the ARM channel, waiting for command...'
while True:
lc.handle()
