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()
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)
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 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 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
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()
