def plot_avg_planning_time_vs_trust_region(self, results):
# results = list(self.db.find({"type": "kuka_only_random_trust_region"}))
# result = list(self.db.find({"type": "donbot_arm_only_random_trust_region"}))
# print len(result)
trust_region = DefaultOrderedDict(list)
plan_t = DefaultOrderedDict(list)
sol_t = DefaultOrderedDict(list)
col_t = DefaultOrderedDict(list)
prb_t = DefaultOrderedDict(list)
for res in results:
trust = res["solver_config"]["trust_region_size"]
trust_region[trust].append(trust)
plan_t[trust].append(res["planning_time"])
sol_t[trust].append(res["solving_time"])
col_t[trust].append(res["collision_check_time"])
prb_t[trust].append(res["prob_model_time"])
avg_planning_time = []
avg_solving_time = []
avg_collision_check_time = []
avg_prob_model_time = []
trust_region = OrderedDict(sorted(trust_region.items()))
for k in trust_region:
avg_planning_time.append(sum(plan_t[k]) / len(plan_t[k]))
avg_solving_time.append(sum(sol_t[k]) / len(sol_t[k]))
avg_collision_check_time.append(sum(col_t[k]) / len(col_t[k]))
avg_prob_model_time.append(sum(prb_t[k]) / len(prb_t[k]))
ys = [avg_planning_time, avg_solving_time, avg_collision_check_time, avg_prob_model_time]
xs = [trust_region.keys()] * len(ys)
labels = ["Planning time", "Solving time", "Collision check time", "Problem modelling time"]
plotter.multi_plot_best_fit_curve(xs, ys, labels, "Average time taken vs Trust region size", "Trust region size", "Average Time (s)",
deg=2)
def __init__(self, **kwargs):
use_gui = utils.get_var_from_kwargs("use_gui", optional=True, default=False, **kwargs)
verbose = utils.get_var_from_kwargs("verbose", optional=True, default=False, **kwargs)
log_file = utils.get_var_from_kwargs("log_file", optional=True, default=False, **kwargs)
use_real_time_simulation = utils.get_var_from_kwargs("use_real_time_simulation", optional=True,
default=False, **kwargs)
fixed_time_step = utils.get_var_from_kwargs("fixed_time_step", optional=True, default=0.01, **kwargs)
logger_name = utils.get_var_from_kwargs("logger_name", optional=True, default=__name__, **kwargs)
self.CYLINDER = sim.GEOM_CYLINDER
self.BOX = sim.GEOM_BOX
self.MESH = sim.GEOM_MESH
self.logger = logging.getLogger(logger_name + __name__)
utils.setup_logger(self.logger, logger_name, verbose, log_file)
if use_gui:
self.gui = sim.connect(sim.GUI_SERVER)
# self.gui = sim.connect(sim.GUI)
else:
self.gui = sim.connect(sim.DIRECT)
sim.setAdditionalSearchPath(pybullet_data.getDataPath())
self.joint_name_to_id = OrderedDict()
self.joint_id_to_name = OrderedDict()
self.start_state_for_traj_planning = OrderedDict()
self.end_state_for_traj_planning = OrderedDict()
self.scene_items = OrderedDict()
self.joint_name_to_info = OrderedDict()
self.joint_id_to_info = OrderedDict()
self.joint_name_to_jac_id = OrderedDict()
self.ignored_collisions = DefaultOrderedDict(bool)
self.link_pairs = DefaultOrderedDict(list)
self.robot_info = DefaultOrderedDict(list)
self.planning_group = []
self.planning_group_ids = []
self.joint_ids = []
self.planning_samples = 0
self.collision_safe_distance = 0.4
self.collision_check_distance = 0.2
self.collision_check_time = 0
self.robot = None
if use_real_time_simulation:
sim.setRealTimeSimulation(use_real_time_simulation)
else:
sim.setTimeStep(fixed_time_step)
self.collision_constraints = []
def __init__(self):
self.__trajectory = -1
self.__no_of_samples = -1
self.__duration = -1
self.__trajectory_by_joint_name = DefaultOrderedDict(list)
self.__initial = None
self.__trajectories = []
self.__final = None
self.__trajectory_group = None
def __init__(self, logger_name=__name__, verbose=False, log_file=False):
self.id = -1
self.model = None
self.planner = TrajectoryPlanner(logger_name, verbose, log_file)
self.logger = logging.getLogger(logger_name + __name__)
self.ignored_collisions = DefaultOrderedDict(bool)
self.srdf = None
self.group_states_map = OrderedDict()
self.joint_states_map = OrderedDict()
self.group_map = OrderedDict()
utils.setup_logger(self.logger, logger_name, verbose, log_file)
def setUpClass(cls):
with open("problem_1_joint.yaml", 'r') as config:
cls.problem = dict(yaml.load(config))
cls.planner = planner.TrajectoryPlanner()
cls.cvx_optimizer = ConvexOptimizer()
np.random.seed(0)
length = 1
cls.samples = np.random.random_integers(20, 30, size=(1, length)).flatten()
cls.durations = np.random.random_integers(5, 20, size=(1, length)).flatten()
joints_random = np.random.random_integers(7, 20, size=(1, length)).flatten()
cls.joints = []
for i in range(len(joints_random)):
joints = DefaultOrderedDict(lambda: DefaultOrderedDict(lambda: DefaultOrderedDict()))
for j in range(joints_random[i]):
joints[str(j)]["states"]["start"] = cls.random_float(-2, 2)
joints[str(j)]["states"]["end"] = cls.random_float(-2, 2)
joints[str(j)]["limit"]["lower"] = cls.random_float(-5, -2)
joints[str(j)]["limit"]["upper"] = cls.random_float(2, 5)
joints[str(j)]["limit"]["velocity"] = cls.random_float(10, 12)
cls.joints.append(joints)
def load_srdf(self):
srdf_file = home + "/catkin_ws/src/robot_descriptions/kuka_husky_description/moveit_config/config/kuka_husky.srdf"
stream = open(srdf_file, 'r')
srdf = SRDF.from_xml_string(stream.read())
ignored_collisions = DefaultOrderedDict(bool)
for collision in srdf.disable_collisionss:
ignored_collisions[collision.link1, collision.link2] = True
ignored_collisions[collision.link2, collision.link1] = True
# print ignored_collisions
self.planner.world.ignored_collisions = ignored_collisions
def plot_avg_planning_time_vs_samples(self, results):
# result = list(self.db.find({"solver_config.trust_region_size": 30}))
# print len(result)
sam_t = DefaultOrderedDict(list)
sol_t = DefaultOrderedDict(list)
col_t = DefaultOrderedDict(list)
prb_t = DefaultOrderedDict(list)
cost = DefaultOrderedDict(list)
for res in results:
sam = res["planning_request"]["samples"]
sam_t[sam].append(res["planning_time"])
sol_t[sam].append(res["solving_time"])
col_t[sam].append(res["collision_check_time"])
prb_t[sam].append(res["prob_model_time"])
a_cost = res["actual_reductions"]
imp = a_cost[0] - a_cost[-1]
imp /= a_cost[0]
imp *= 100
cost[sam].append(imp)
samples = []
avg_planning_time = []
avg_solving_time = []
avg_collision_check_time = []
avg_prob_model_time = []
avg_cost = []
sam_t = OrderedDict(sorted(sam_t.items()))
for k in sam_t:
# print k, len(sam_t[k])
samples.append(k)
avg_planning_time.append(sum(sam_t[k]) / len(sam_t[k]))
avg_solving_time.append(sum(sol_t[k]) / len(sol_t[k]))
avg_collision_check_time.append(sum(col_t[k]) / len(col_t[k]))
avg_prob_model_time.append(sum(prb_t[k]) / len(prb_t[k]))
avg_cost.append(sum(cost[k]) / len(cost[k]))
ys = [avg_solving_time, avg_collision_check_time, avg_prob_model_time]
xs = [samples] * len(ys)
labels = ["Solving time", "Collision check time", "problem modelling time"]
# plotter.multi_plot_best_fit_curve(xs, ys, labels, "Time vs Number of samples", "Number of samples", "Time (S)",
plotter.bar_chart(xs, ys, labels, "Time taken vs Number of samples", "Number of samples", "Average Time (s)")
def iterations_vs_trust_region(self, results):
trust_region = DefaultOrderedDict(list)
qp_iters = DefaultOrderedDict(list)
sqp_iters = DefaultOrderedDict(list)
for res in results:
trust = res["solver_config"]["trust_region_size"]
trust_region[trust].append(trust)
qp_iters[trust].append(res["num_qp_iterations"])
sqp_iters[trust].append(res["num_sqp_iterations"])
avg_qp_iters = []
avg_sqp_iters = []
trust_region = OrderedDict(sorted(trust_region.items()))
for k in trust_region:
avg_qp_iters.append(sum(qp_iters[k]) / len(qp_iters[k]))
avg_sqp_iters.append(sum(sqp_iters[k]) / len(sqp_iters[k]))
ys = [avg_qp_iters, avg_sqp_iters]
xs = [trust_region.keys()] * len(ys)
labels = ["QP iterations", "SQP iterations"]
plotter.multi_plot_best_fit_curve(xs, ys, labels, "Number of SQP and QP iterations vs Trust region", "Trust region", "Number of iterations",
deg=2)
def plot_avg_planning_time_vs_penalty_1_and_2(self, results):
penalty_norm = DefaultOrderedDict(list)
sol_t = DefaultOrderedDict(list)
col_t = DefaultOrderedDict(list)
prb_t = DefaultOrderedDict(list)
penalty_norm1 = DefaultOrderedDict(list)
sol_t1 = DefaultOrderedDict(list)
col_t1 = DefaultOrderedDict(list)
prb_t1 = DefaultOrderedDict(list)
for res in results:
p = res["solver_config"]["penalty_norm"]
if p == 1:
trust = res["solver_config"]["trust_region_size"]
penalty_norm[trust].append(res["planning_time"])
sol_t[trust].append(res["solving_time"])
col_t[trust].append(res["collision_check_time"])
prb_t[trust].append(res["prob_model_time"])
elif p == 2:
trust = res["solver_config"]["trust_region_size"]
penalty_norm1[trust].append(res["planning_time"])
sol_t1[trust].append(res["solving_time"])
col_t1[trust].append(res["collision_check_time"])
prb_t1[trust].append(res["prob_model_time"])
tr = []
avg_planning_time = []
avg_solving_time = []
avg_collision_check_time = []
avg_prob_model_time = []
penalty_norm = OrderedDict(sorted(penalty_norm.items()))
for k in penalty_norm:
# print k, len(penalty_norm[k])
tr.append(k)
avg_planning_time.append(sum(penalty_norm[k]) / len(penalty_norm[k]))
avg_solving_time.append(sum(sol_t[k]) / len(sol_t[k]))
avg_collision_check_time.append(sum(col_t[k]) / len(col_t[k]))
avg_prob_model_time.append(sum(prb_t[k]) / len(prb_t[k]))
tr1 = []
avg_planning_time1 = []
avg_solving_time1 = []
avg_collision_check_time1 = []
avg_prob_model_time1 = []
penalty_norm1 = OrderedDict(sorted(penalty_norm.items()))
for k in penalty_norm1:
print k, len(penalty_norm1[k])
tr1.append(k)
avg_planning_time1.append(sum(penalty_norm1[k]) / len(penalty_norm1[k]))
avg_solving_time1.append(sum(sol_t1[k]) / len(sol_t1[k]))
avg_collision_check_time1.append(sum(col_t1[k]) / len(col_t1[k]))
avg_prob_model_time1.append(sum(prb_t1[k]) / len(prb_t1[k]))
ys = [avg_planning_time, avg_solving_time, avg_collision_check_time, avg_prob_model_time,
avg_planning_time1, avg_solving_time1, avg_collision_check_time1, avg_prob_model_time1]
xs = [tr, tr1] * (len(ys) / 2)
labels = ["$l_1$ penalty: Planning time", "$l_1$ penalty: Solving time",
"$l_1$ penalty: Collision check time", "$l_1$ penalty: Problem modelling time",
"$l_2$ penalty 2: Planning time", "$l_2$ penalty: Solving time",
"$l_2$ penalty: Collision check time", "$l_2$ penalty: Problem modelling time"]
plotter.multi_plot_best_fit_curve(xs, ys, labels, "$l_1$ Penalty vs $l_2$ penalty", "Number of samples", "Average time (s)",
deg=2)