def export_html(self, filename):
""" Exports HTML for debugging only. """
Utils.set_to_print_web()
s = DuMark.get_h1('List of publications (temporary)')
i = 0
for k in self._keys:
d = self._data[k]
if not d.is_visible():
continue
d.debug_pdf()
i += 1
s += '[%d] ' % i
s += d.get_authors().get_markdown()
s += '. '
s += d.get_title_markdown()
s += ' '
s += d.get_lowres_markdown()
s += ' '
s += d.get_book()
s += '. '
s += d.get_year()
s += '.'
s += Utils.get_separator()
s += DEBUG_HTML_SCRIPT
s = htmlmin.minify(markdown(s))
with open(filename, 'w', encoding='utf8') as f:
f.write(s)
def _export_people_names(self, filename):
Utils.set_to_print_file()
s = ''
for k in self._keys:
s += self._data[k].get_name()
s += Utils.get_line_break()
Utils.write_file(filename, s)
def get_apa(self):
"""Returns APA style citation.
Reference:
TODO: https://owl.purdue.edu/owl/research_and_citation/apa_style/apa_formatting_and_style_guide/reference_list_author_authors.html
"""
b = self._data
TAB, LB = Utils.get_tab(), Utils.get_line_break()
s = self.get_authors().get_apa()
s += ' (%s).' % b['year']
s += LB
s += b['title'] + '.'
s += LB
s += 'In %s, ' % self.get_book()
# Appends volume and number for journal articles.
if b['volume']:
s += b['volume']
if b['number']:
s += '(%s)' % b['number']
s += ', '
# Appends pages.
if 'b_pages' in b:
s += b['b_pages'].replace('--', '-')
elif 'b_numpages' in b:
s += 'pp. ' + b['b_pages']
s += '.'
return s
def export_bib_ids(self, filename):
Utils.set_to_print_file()
s = ''
for k in self._keys:
s += self._data[k].get_bib_id()
s += Utils.get_line_break()
with open(filename, 'w', encoding='utf8') as f:
f.write(s)
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 __init__(self, **kwargs):
self.robot_config = None
self.default_config = None
self.config = None
self.sqp_yaml = None
self.sqp_config = None
self.robot_default_config_params = None
self.elapsed_time = 0
main_logger_name = utils.get_var_from_kwargs(
"logger_name",
optional=True,
default="Trajectory_Optimization_Planner.",
**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)
robot_config = utils.get_var_from_kwargs("robot_config",
optional=True,
**kwargs)
self.load_configs(robot_config)
self.save_problem = utils.get_var_from_kwargs("save_problem",
optional=True,
**kwargs)
if self.save_problem is not None:
db_name = utils.get_var_from_kwargs(
"db_name",
optional=True,
default="Trajectory_planner_results",
**kwargs)
self.db_driver = MongoDriver(db_name)
else:
self.db_driver = None
self.save_problem = None
self.if_plot_traj = utils.get_var_from_kwargs("plot_trajectory",
optional=True,
default=False,
**kwargs)
self.robot = Robot(main_logger_name, verbose, log_file)
self.world = SimulationWorld(**kwargs)
self.logger = logging.getLogger(main_logger_name)
utils.setup_logger(self.logger, main_logger_name, verbose, log_file)
self.world.toggle_rendering(0)
self.load_robot_from_config()
self.world.toggle_rendering(1)
def export_cites_html(self, filename):
Utils.set_to_print_web()
with open(filename, 'r', encoding='utf8') as f:
lines = f.readlines()
for k in self._keys:
paper = self._data[k]
d = paper._data
html = ''
html += paper.fill_template(lines)
Dir.ensure_dir(os.path.join(Dir.builds, Dir.cites))
output_filename = os.path.join(Dir.builds, Dir.cites,
'%s.html' % d['bib'])
with open(output_filename, 'w', encoding='utf8') as f:
f.write(html)
def get_url(url, title, description=None):
if url:
url = Utils.filter_url(url)
return DuMark.LINK3 % (title, url, description
) if description else DuMark.LINK2 % (title, url)
else:
return ''
def export_file_names(self, filename):
s = ''
for k in self._keys:
s += self._data[k].get_filename()
s += Utils.get_line_break()
with open(filename, 'w', encoding='utf8') as f:
f.write(s)
def analyse_inputs(self):
# replacing lower limit none constraints with very lower value
if self.lbG is not None:
self.lbG = np.array([utils.replace_none(lb, float(self.solver_config["replace_none_with"]), negate=True)
for lb in self.lbG])
# replacing upper limit none constraints with very lower value
if self.ubG is not None:
self.ubG = np.array([utils.replace_none(ub, float(self.solver_config["replace_none_with"]))
for ub in self.ubG])
if self.G is not None and self.lbG is None and self.ubG is not None:
self.lbG = -self.ubG
if self.G is not None and self.ubG is None and self.lbG is not None:
self.G = np.vstack([-self.G, -self.G])
self.lbG = np.hstack([-self.ubG, -self.ubG])
self.ubG = np.hstack([self.ubG, -self.ubG])
def get_planning_group_and_corresponding_state(self, group_state,
**kwargs):
group = []
joint_states = []
group_name = utils.get_var_from_kwargs("group", **kwargs)
if group_name is not None:
if type(group_name) is str:
if kwargs["group"] in self.robot_config["joints_groups"]:
group = self.robot_config["joints_groups"][kwargs["group"]]
if not len(group):
group = self.robot.get_planning_group_from_srdf(group_name)
if group_state in kwargs and len(group):
joint_states = kwargs[group_state]
if type(joint_states
) is str and joint_states in self.robot_config[
"joint_configurations"]:
joint_states = self.robot_config["joint_configurations"][
joint_states]
# if not len(joint_states):
else:
group, joint_states = self.robot.get_group_state_from_srdf(
joint_states)
if type(joint_states) is dict or type(
joint_states) is OrderedDict:
joint_states = joint_states.values()
return group, joint_states
def unbias_ngrams(self, mtx_csr):
# iterate through rows ( docs)
for i in range(0, len(mtx_csr.indptr) - 1):
start_idx_ptr = mtx_csr.indptr[i]
end_idx_ptr = mtx_csr.indptr[i + 1]
# iterate through columns with non-zero entries
for j in range(start_idx_ptr + 1, end_idx_ptr):
col_idx = mtx_csr.indices[j]
big_ngram = self.feature_names[col_idx]
big_ngram_terms = big_ngram.split()
if len(big_ngram_terms) > 1:
col_idx1 = mtx_csr.indices[j - 1]
small_ngram = self.feature_names[col_idx1]
chopped_ngram = ' '.join(big_ngram_terms[1:])
if small_ngram == chopped_ngram:
mtx_csr.data[j - 1] = 0
if big_ngram > small_ngram:
start, end = 0, col_idx - 1
else:
start, end = col_idx + 1, len(
self.feature_names) - 1
term_idx, found = ut.bisearch_csr(
self.feature_names, chopped_ngram, start, end)
if found:
mtx_csr.data[term_idx] = 0
return mtx_csr
def __init__(self,
main_logger_name=__name__,
verbose=False,
log_file=False):
self.sqp = OrderedDict()
self.P = []
self.G = []
self.A = []
self.q = []
self.lb = []
self.ub = []
self.lbG = []
self.ubG = []
self.b = []
self.initial_guess = []
self.solver_status = []
self.joint_names = []
self.problem = OrderedDict()
self.max_penalty = -1
self.delta_max = -1
self.joints = -1
self.num_of_joints = -1
self.solver = -1
self.duration = -1
self.no_of_samples = -1
self.max_no_of_Iteration = -1
self.max_no_of_Iteration = -1
self.decimals_to_round = 5
self.collision_check_distance = 0.1
self.collision_safe_distance = 0.05
self.solver_class = 0
self.prob_model_time = 0
self.solver_config = None
self.trajectory = Trajectory.Trajectory()
self.problem_model = model.ProblemModelling()
self.sqp_solver = None
self.current_planning_joint_group = None
self.callback_function_to_get_collision_infos = None
self.sqp_solver = SQPsolver.SQPsolver(main_logger_name, verbose,
log_file)
self.sqp_solver_old = SQPsolver_old.SQPsolver(main_logger_name,
verbose, log_file)
self.logger = logging.getLogger(main_logger_name + __name__)
utils.setup_logger(self.logger, main_logger_name, verbose, log_file)
def __init__(self,
main_logger_name=__name__,
verbose=False,
log_file=False):
self.P = []
self.G = []
self.A = []
self.q = []
self.lb = []
self.ub = []
self.lbG = []
self.ubG = []
self.b = []
self.D = None
self.lbD = None
self.ubD = None
self.initial_guess = []
self.status = "-1"
self.norm_ = 1
self.rho_k = 0
self.is_converged = False
self.is_initialized = False
self.solver_config = OrderedDict()
self.solver = []
self.penalty_norm = 1
self.trust_region_norm = np.inf
self.num_qp_iterations = 0
self.num_sqp_iterations = 0
self.solving_time = 0
self.predicted_reductions = []
self.actual_reductions = []
self.model_costs = []
self.actual_costs = []
self.actual_reduction_improve = 0
self.predicted_reduction_improve = 0
self.actual_cost_improve = 0
self.model_cost_improve = 0
self.logger = logging.getLogger(main_logger_name + __name__)
utils.setup_logger(self.logger, main_logger_name, verbose, log_file)
def load_robot_from_config(self):
urdf_file = utils.get_var_from_kwargs("urdf",
optional=True,
**self.robot_config)
srdf_file = utils.get_var_from_kwargs("srdf",
optional=True,
**self.robot_config)
if urdf_file is not None:
pos = self.robot_config[
"position"] if "position" in self.robot_config else [0, 0, 0]
orn = self.robot_config[
"orientation"] if "orientation" in self.robot_config else [
0, 0, 0
]
self.load_robot(urdf_file, position=pos, orientation=orn)
if srdf_file is not None:
self.load_robot_srdf(srdf_file)
def formulate_collision_infos(self,
robot,
trajectory,
group,
distance=0.2):
initial_signed_distance = []
current_normal_T_times_jacobian = []
next_normal_T_times_jacobian = []
start_state = self.get_current_states_for_given_joints(robot.id, group)
time_step_count = 0
for previous_time_step_of_trajectory, current_time_step_of_trajectory, \
next_time_step_of_trajectory in utils.iterate_with_previous_and_next(trajectory):
time_step_count += 1
if next_time_step_of_trajectory is not None:
next_robot_state, next_link_states \
= self.get_joint_and_link_states_at(robot.id, next_time_step_of_trajectory, group)
current_robot_state, current_link_states \
= self.get_joint_and_link_states_at(robot.id, current_time_step_of_trajectory, group)
current_robot_state = current_robot_state[0]
next_robot_state = next_robot_state[0]
zero_vec = [0.0] * len(current_robot_state)
for link_index, current_link_state, next_link_state in itertools.izip(
self.planning_group_ids, current_link_states,
next_link_states):
# Shivendra: ? Don't know how current_normal_T_times_jacobian, next_normal_T_times_jacobian are actually filled
self.get_collision_infos_for_constraints(
robot, link_index, current_link_state, next_link_state,
distance, current_robot_state, next_robot_state,
zero_vec, trajectory, group, time_step_count,
initial_signed_distance,
current_normal_T_times_jacobian,
next_normal_T_times_jacobian)
self.get_robot_self_collision_infos(
robot.id, link_index, current_link_state,
next_link_state, distance, current_robot_state,
next_robot_state, zero_vec, trajectory, group,
time_step_count, initial_signed_distance,
current_normal_T_times_jacobian,
next_normal_T_times_jacobian)
if len(initial_signed_distance) > 0:
initial_signed_distance = np.vstack(initial_signed_distance)
if len(current_normal_T_times_jacobian) > 0:
current_normal_T_times_jacobian = np.vstack(
current_normal_T_times_jacobian)
if len(next_normal_T_times_jacobian) > 0:
next_normal_T_times_jacobian = np.vstack(
next_normal_T_times_jacobian)
self.reset_joint_states(robot.id, start_state, group)
return initial_signed_distance, current_normal_T_times_jacobian, next_normal_T_times_jacobian
def export_html(self, filename):
Utils.set_to_print_web()
s = ''
for k in self._keys:
d = self._data[k]
s += d.get_name_google_markdown()
if d.get_original_name():
s += ' (%s)' % d.get_original_name()
s += ': '
s += Utils.get_line_break()
s += d.get_best_face_markdown()
s += Utils.get_line_break()
s += ' '.join([
d.get_website_markdown(),
d.get_facebook_markdown(),
d.get_twitter_markdown(),
d.get_vimeo_markdown(),
d.get_linkedin_markdown(),
d.get_instagram_markdown(),
d.get_youtube_markdown(),
d.get_github_markdown(),
d.get_researchgate_markdown(),
d.get_scholar_markdown()
])
s += Utils.get_separator()
s += DEBUG_HTML_SCRIPT
s = htmlmin.minify(markdown(s))
Utils.write_file(filename, s)
def init(self, **kwargs):
self.D = None
self.lbD = None
self.ubD = None
self.P = utils.get_var_from_kwargs("P", **kwargs)
self.q = utils.get_var_from_kwargs("q", **kwargs)
self.G = utils.get_var_from_kwargs("G", optional=False, **kwargs)
self.lbG = utils.get_var_from_kwargs("lbG", optional=True, **kwargs)
self.ubG = utils.get_var_from_kwargs("ubG", optional=True, **kwargs)
self.A = utils.get_var_from_kwargs("A", optional=False, **kwargs)
self.b = utils.get_var_from_kwargs("b", optional=False, **kwargs)
self.initial_guess = utils.get_var_from_kwargs("initial_guess",
optional=True,
default=np.zeros(
(self.P.shape[0],
1)).flatten(),
**kwargs)
solver_config = utils.get_var_from_kwargs("solver_config",
optional=True,
**kwargs)
solver = utils.get_var_from_kwargs("solver", optional=True, **kwargs)
if solver_config is not None:
self.solver_config = solver_config
else:
file_path_prefix = os.path.join(os.path.dirname(__file__),
'../../config/')
sqp_config_file = file_path_prefix + 'sqp_config.yaml'
sqp_yaml = yaml.ConfigParser(sqp_config_file)
self.solver_config = sqp_yaml.get_by_key("sqp")
self.penalty_norm = self.solver_config["penalty_norm"]
self.trust_region_norm = self.solver_config["trust_region_norm"]
self.analyse_inputs()
if solver is not None:
self.solver = solver
else:
self.solver = self.solver_config["solver"][0]
def get_bibtex(self):
""" Returns BibTeX of this paper.
References:
http://www.bibtex.org/Format
https://verbosus.com/bibtex-style-examples.html
"""
b = self._data
TAB, LB = Utils.get_tab(), Utils.get_line_break()
s = '@%s{%s,%s' % (b['b_type'], b['bib'], LB)
for label in Paper.get_bib_items():
if label in b and b[label]:
key = label
if label[:2] == 'b_':
key = label[2:]
s += '%s%s = {%s},%s' % (TAB, key, b[label], LB)
s += '}%s' % LB
return s
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 get_vancouver(self):
"""Returns Vancouver style citation."""
b = self._data
TAB, LB = Utils.get_tab(), Utils.get_line_break()
s = self.get_authors().get_mla()
s += LB
s += '"%s".' % b['title']
s += LB
s += self.get_book() + ', '
if b['volume']:
s += b['volume']
if b['number']:
s += '(%s)' % b['number']
s += ', '
if 'b_pages' in b:
s += b['b_pages'].replace('--', '-')
elif 'b_numpages' in b:
s += 'pp. %s' % b['b_pages']
s += '. %s.' % b['year']
return s
def get_collision_infos_for_constraints(
self, robot, link_index, current_link_state, next_link_state,
distance, current_robot_state, next_robot_state, zero_vec,
trajectory, group, time_step_count, initial_signed_distance_,
current_normal_T_times_jacobian_, next_normal_T_times_jacobian_):
for constraint in self.collision_constraints:
if robot.id != constraint:
cast_closest_points = self.get_convex_sweep_closest_points_dummy(
robot.id,
# self.collision_constraints,
constraint,
link_index,
current_link_state,
next_link_state,
distance)
for closest_point in cast_closest_points:
closest_pt_on_A_at_t = closest_point.position_on_a
closest_pt_on_A_at_t_plus_1 = closest_point.position_on_a1
normal_ = np.vstack(
closest_point.contact_normal_on_b).reshape(3, 1)
normal_ = utils.normalize_vector(normal_)
closest_point._replace(contact_normal_on_b=normal_)
dist = closest_point.contact_distance
fraction = closest_point.contact_fraction
if closest_point.link_index_a == link_index and dist < 0:
current_state_jacobian_matrix = self.formulate_jacobian_matrix(
robot.id, link_index, current_robot_state,
current_link_state, closest_pt_on_A_at_t,
trajectory, group, time_step_count, zero_vec)
next_state_jacobian_matrix = self.formulate_jacobian_matrix(
robot.id, link_index, next_robot_state,
next_link_state, closest_pt_on_A_at_t_plus_1,
trajectory, group, time_step_count + 1, zero_vec)
initial_signed_distance_.append(dist)
current_normal_T_times_jacobian_.append(
np.matmul(fraction * normal_.T,
current_state_jacobian_matrix))
next_normal_T_times_jacobian_.append(
np.matmul((1 - fraction) * normal_.T,
next_state_jacobian_matrix))
def get_robot_self_collision_infos(
self, robot_id, link_index, current_link_state, next_link_state,
distance, current_robot_state, next_robot_state, zero_vec,
trajectory, group, time_step_count, initial_signed_distance,
current_normal_T_times_jacobian, next_normal_T_times_jacobian):
cast_closest_points = self.get_convex_sweep_closest_points_dummy(
robot_id, robot_id, link_index, current_link_state,
next_link_state, distance)
for cp in cast_closest_points:
if cp.link_index_a > -1 and cp.link_index_b > -1:
link_a = self.joint_id_to_info[cp.link_index_a].link_name
link_b = self.joint_id_to_info[cp.link_index_b].link_name
if cp.link_index_a == link_index and cp.link_index_a != cp.link_index_b and not self.ignored_collisions[
link_a, link_b]:
closest_pt_on_A_at_t = cp.position_on_a
closest_pt_on_A_at_t_plus_1 = cp.position_on_a1
normal_ = 1 * np.vstack(cp.contact_normal_on_b).reshape(
3, 1)
normal_ = utils.normalize_vector(normal_)
cp._replace(contact_normal_on_b=normal_)
dist = cp.contact_distance
fraction = cp.contact_fraction
if dist < 0:
# self.print_contact_points(cp, time_step_count, link_index)
current_state_jacobian_matrix = self.formulate_jacobian_matrix(
robot_id, link_index, current_robot_state,
current_link_state, closest_pt_on_A_at_t,
trajectory, group, time_step_count, zero_vec)
next_state_jacobian_matrix = self.formulate_jacobian_matrix(
robot_id, link_index, next_robot_state,
next_link_state, closest_pt_on_A_at_t_plus_1,
trajectory, group, time_step_count + 1, zero_vec)
initial_signed_distance.append(dist)
current_normal_T_times_jacobian.append(
np.matmul(fraction * normal_.T,
current_state_jacobian_matrix))
next_normal_T_times_jacobian.append(
np.matmul((1 - fraction) * normal_.T,
next_state_jacobian_matrix))
def get_trajectory(self, **kwargs):
group = []
group_name = utils.get_var_from_kwargs("group", **kwargs)
if group_name is not None:
if type(group_name) is list:
group = group_name
if type(group_name) is str and group_name in self.robot_config[
"joints_groups"]:
group = self.robot_config["joints_groups"][group_name]
if not len(group):
group = self.robot.get_planning_group_from_srdf(group_name)
start_state = utils.get_var_from_kwargs("start_state",
optional=True,
**kwargs)
if start_state is not None and len(group):
if type(start_state) is dict or type(start_state) is OrderedDict:
start_state = start_state.values()
if not type(start_state) is list:
_, start_state = self.get_planning_group_and_corresponding_state(
"start_state", **kwargs)
self.reset_robot_to(start_state, group, key="start_state")
status, is_collision_free, trajectory = "start state in collision", False, -1
is_start_state_in_collision = self.world.is_given_state_in_collision(
self.robot.id, start_state, group)
if is_start_state_in_collision:
print "is_start_state_in_collision", is_start_state_in_collision
status = "start state in collision"
return status, is_collision_free, trajectory
elif len(group):
start_state = self.world.get_current_states_for_given_joints(
self.robot.id, group)
goal_state = utils.get_var_from_kwargs("goal_state", **kwargs)
if goal_state is not None and len(group):
if type(goal_state) is dict or type(goal_state) is OrderedDict:
goal_state = goal_state.values()
if not type(goal_state) is list:
_, goal_state = self.get_planning_group_and_corresponding_state(
"goal_state", **kwargs)
status, is_collision_free, trajectory = "goal state in collision", False, -1
is_goal_in_collision = self.world.is_given_state_in_collision(
self.robot.id, goal_state, group)
if is_goal_in_collision:
print "is_goal_in_collision", is_goal_in_collision
status = "goal state in collision"
return status, is_collision_free, trajectory
samples = utils.get_var_from_kwargs("samples",
optional=True,
default=20,
**kwargs)
duration = utils.get_var_from_kwargs("duration",
optional=True,
default=10,
**kwargs)
collision_safe_distance = utils.get_var_from_kwargs(
"collision_safe_distance", optional=True, default=0.05, **kwargs)
collision_check_distance = utils.get_var_from_kwargs(
"collision_check_distance", optional=True, default=0.1, **kwargs)
ignore_goal_states = utils.get_var_from_kwargs("ignore_goal_states",
optional=True,
**kwargs)
self.robot.init_plan_trajectory(
group=group,
current_state=start_state,
goal_state=goal_state,
samples=samples,
duration=duration,
collision_safe_distance=collision_safe_distance,
collision_check_distance=collision_check_distance,
solver_class=self.sqp_config["solver_class"],
ignore_goal_states=ignore_goal_states)
self.world.toggle_rendering_while_planning(False)
_, planning_time, _ = self.robot.calulate_trajecotory(
self.callback_function_from_solver)
trajectory = self.robot.planner.get_trajectory(
) # Shivendra: returns self.trajectory from Planner object (not the TrajectoryOptimizationPlanner)
is_collision_free = self.world.is_trajectory_collision_free(
self.robot.id,
self.robot.get_trajectory().final, group, 0.02)
self.world.toggle_rendering_while_planning(True)
self.elapsed_time = self.robot.planner.sqp_solver.solving_time + \
self.world.collision_check_time + self.robot.planner.prob_model_time
status = "Optimal Trajectory has been found in " + str(
self.elapsed_time) + " secs"
self.logger.info(status)
self.log_infos()
if self.if_plot_traj:
self.robot.planner.trajectory.plot_trajectories()
if self.save_problem:
self.save_to_db(samples, duration, start_state, goal_state, group,
collision_safe_distance, collision_check_distance,
is_collision_free)
return status, is_collision_free, trajectory
def get_short_links_html(self): """Get a list of short links.""" return Utils.remove_p(markdown(self.get_short_links_markdown()))
def get_teaser_html(self): return Utils.remove_p(markdown(self.get_teaser_markdown()))
def get_doi_html(self): return Utils.remove_p(markdown(self.get_doi_markdown()))
def init(self, **kwargs):
self.__clear_all_data()
self.joints = utils.get_var_from_kwargs("joints", **kwargs)
if self.joints is not None:
self.num_of_joints = len(self.joints)
self.solver = utils.get_var_from_kwargs("solver",
optional=True,
**kwargs)
self.duration = utils.get_var_from_kwargs("duration",
optional=True,
default=10,
**kwargs)
self.no_of_samples = utils.get_var_from_kwargs("samples",
optional=True,
default=20,
**kwargs)
self.max_no_of_Iteration = utils.get_var_from_kwargs("max_iteration",
optional=True,
default=20,
**kwargs)
self.decimals_to_round = utils.get_var_from_kwargs("decimals_to_round",
optional=True,
default=5,
**kwargs)
self.collision_safe_distance = utils.get_var_from_kwargs(
"collision_safe_distance", optional=True, default=0.1, **kwargs)
self.collision_check_distance = utils.get_var_from_kwargs(
"collision_check_distance", optional=True, default=0.15, **kwargs)
self.current_planning_joint_group = utils.get_var_from_kwargs(
"joint_group", **kwargs)
self.solver_class = utils.get_var_from_kwargs("solver_class",
optional=True,
default="new",
**kwargs)
if self.solver_class is not None:
# considering 1st element of the solver class list from sqp config file
self.solver_class = self.solver_class[0]
if self.solver_class.lower() == "old":
self.sqp_solver = self.sqp_solver_old
self.solver_config = utils.get_var_from_kwargs("solver_config",
optional=True,
**kwargs)
start = time.time()
# creating model of the trajectory planning problem
self.problem_model.init(self.joints, self.no_of_samples, self.duration,
self.decimals_to_round,
self.collision_safe_distance,
self.collision_check_distance)
end = time.time()
self.prob_model_time += end - start
# initializing SQP solver
self.sqp_solver.init(P=self.problem_model.cost_matrix_P,
q=self.problem_model.cost_matrix_q,
G=self.problem_model.robot_constraints_matrix,
lbG=self.problem_model.constraints_lower_limits,
ubG=self.problem_model.constraints_upper_limits,
A=self.problem_model.start_and_goal_matrix,
b=self.problem_model.start_and_goal_limits,
initial_guess=self.problem_model.initial_guess,
solver_config=self.solver_config)
self.trajectory.init(
np.array(
(np.split(self.problem_model.initial_guess,
self.no_of_samples))), self.problem_model.samples,
self.problem_model.duration, self.current_planning_joint_group)
def __init__(self, logger_name=__name__, config_file=None, verbose=False, file_log=False, planner=None, width=900, height=400):
QtGui.QMainWindow.__init__(self)
self.setGeometry(200, 100, width, height)
# self.sim_world = SimulationWorld.SimulationWorld(self.robot_config["urdf"])
self.planner = planner
if config_file is None:
dirname = os.path.dirname(__file__)
file_path_prefix = os.path.join(dirname, '../../config/')
# file_path_prefix = '../../config/'
config_file = file_path_prefix + 'default_config.yaml'
self.default_config = yaml.ConfigParser(config_file)
self.config = self.default_config.get_by_key("config")
self.sqp_config_file = file_path_prefix + self.config["solver"]
self.sqp_yaml = yaml.ConfigParser(self.sqp_config_file)
self.sqp_config = self.sqp_yaml.get_by_key("sqp")
robot_config_file = file_path_prefix + self.config["robot"]["config"]
self.robot_default_config_params = self.config["robot"]["default_paramaters"]
robot_yaml = yaml.ConfigParser(robot_config_file)
self.robot_config = robot_yaml.get_by_key("robot")
self.sqp_labels = {}
self.sqp_spin_box = {}
self.sqp_combo_box = {}
self.sqp_config_group_box = QtGui.QGroupBox('SQP solver Parameters')
self.sqp_config_form = QtGui.QFormLayout()
self.sqp_config_scroll = QtGui.QScrollArea()
self.robot_config_params = {}
self.robot_config_labels = {}
self.robot_config_spin_box = {}
self.robot_config_combo_box = {}
self.robot_config_params_spin_box = {}
self.robot_config_group_box = QtGui.QGroupBox('Robot Actions')
self.simulation_action_group_box = QtGui.QGroupBox('Simulation')
self.robot_config_hbox = QtGui.QVBoxLayout()
self.robot_config_vbox_layout = QtGui.QVBoxLayout()
self.robot_config_form = QtGui.QFormLayout()
self.robot_action_buttons = {}
self.robot_action_button_group = QtGui.QButtonGroup(self)
self.robot_action_buttons["execute"] = QtGui.QPushButton('Execute')
self.robot_action_buttons["plan"] = QtGui.QPushButton('Plan')
self.robot_action_buttons["random_pose"] = QtGui.QPushButton('Random Pose')
self.robot_action_buttons["plan_and_execute"] = QtGui.QPushButton('Plan and Execute')
self.robot_action_button_hbox = QtGui.QHBoxLayout()
self.robot_config_scroll = QtGui.QScrollArea()
self.selected_robot_combo_value = {}
self.selected_robot_spin_value = {}
self.statusBar = QtGui.QStatusBar()
self.main_widget = QtGui.QWidget(self)
self.main_layout = QtGui.QVBoxLayout(self.main_widget)
self.main_hbox_layout = QtGui.QHBoxLayout()
self.last_status = "Last Status: "
self.init_ui(25)
self.can_execute_trajectory = False
self.logger = logging.getLogger(logger_name + __name__)
utils.setup_logger(self.logger, logger_name, verbose, file_log)
def get_title_html(self): return Utils.remove_p(markdown(self.get_title_markdown()))
