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