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 __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):
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,
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 __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)