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 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 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 __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, **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_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 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_plan_trajectory(self, **kwargs):
joint_group = utils.get_var_from_kwargs("group", **kwargs)
samples = utils.get_var_from_kwargs("samples", **kwargs)
duration = utils.get_var_from_kwargs("duration", **kwargs)
solver = utils.get_var_from_kwargs("solver",
optional=True,
default="SCS",
**kwargs)
solver_config = utils.get_var_from_kwargs("solver_config",
optional=True,
**kwargs)
# some default config for the SQP solver
if solver_config is not None:
if "decimals_to_round" in solver_config:
decimals_to_round = int(solver_config["decimals_to_round"])
else:
decimals_to_round = 5
current_state = utils.get_var_from_kwargs("current_state", **kwargs)
goal_state = utils.get_var_from_kwargs("goal_state", **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)
solver_class = utils.get_var_from_kwargs("solver_class",
optional=True,
default="new",
**kwargs)
verbose = utils.get_var_from_kwargs("verbose",
optional=True,
default=False,
**kwargs)
# combing all necessary infos to model and plan a trajectory
if "current_state" in kwargs and "goal_state" in kwargs:
if type(current_state) is dict and type(current_state) is dict:
states = {}
joints = {}
for joint_in_group in joint_group:
if joint_in_group in current_state and joint_in_group in goal_state and \
joint_in_group in self.model.joint_map:
if self.model.joint_map[joint_in_group].type != "fixed":
ignore_state = False
if joint_in_group in ignore_goal_states:
ignore_state = True
states[joint_in_group] = {
"start": current_state[joint_in_group],
"end": goal_state[joint_in_group]
}
joints[joint_in_group] = {
"states": states[joint_in_group],
"limit":
self.model.joint_map[joint_in_group].limit,
"ignore_state": ignore_state
}
elif type(current_state) is list and type(current_state) is list:
joints = []
assert len(current_state) == len(goal_state) == len(
joint_group)
for joint, c_state, n_state in itertools.izip(
joint_group, current_state, goal_state):
if joint in self.model.joint_map:
ignore_state = False
if ignore_goal_states is not None and len(
ignore_goal_states):
if joint in ignore_goal_states:
ignore_state = True
joints.append([
c_state, n_state,
self.model.joint_map[joint].limit, ignore_state
])
if len(joints):
# initializing the trajectory planner to model the problem
self.planner.init(
joints=joints,
samples=samples,
duration=duration,
joint_group=joint_group,
collision_safe_distance=collision_safe_distance,
collision_check_distance=collision_check_distance,
solver=solver,
solver_config=solver_config,
solver_class=solver_class,
decimals_to_round=decimals_to_round,
verbose=verbose)
def init_plan_trajectory(self, **kwargs):
joint_group = utils.get_var_from_kwargs(
"group", **kwargs
) #['lbr_iiwa_joint_1', 'lbr_iiwa_joint_2', 'lbr_iiwa_joint_3', 'lbr_iiwa_joint_4', 'lbr_iiwa_joint_5', 'lbr_iiwa_joint_6', 'lbr_iiwa_joint_7']
samples = utils.get_var_from_kwargs("samples", **kwargs)
duration = utils.get_var_from_kwargs("duration", **kwargs)
solver = utils.get_var_from_kwargs("solver",
optional=True,
default="SCS",
**kwargs)
solver_config = utils.get_var_from_kwargs("solver_config",
optional=True,
**kwargs)
# some default config for the SQP solver
if solver_config is not None:
if "decimals_to_round" in solver_config:
decimals_to_round = int(solver_config["decimals_to_round"])
else:
decimals_to_round = 5
current_state = utils.get_var_from_kwargs("current_state", **kwargs)
goal_state = utils.get_var_from_kwargs("goal_state", **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)
solver_class = utils.get_var_from_kwargs("solver_class",
optional=True,
default="new",
**kwargs)
verbose = utils.get_var_from_kwargs("verbose",
optional=True,
default=False,
**kwargs)
# combing all necessary infos to model and plan a trajectory
if "current_state" in kwargs and "goal_state" in kwargs:
if type(current_state) is dict and type(current_state) is dict:
states = {}
joints = {}
for joint_in_group in joint_group:
if joint_in_group in current_state and joint_in_group in goal_state and \
joint_in_group in self.model.joint_map:
if self.model.joint_map[joint_in_group].type != "fixed":
ignore_state = False
if joint_in_group in ignore_goal_states:
ignore_state = True
states[joint_in_group] = {
"start": current_state[joint_in_group],
"end": goal_state[joint_in_group]
}
joints[joint_in_group] = {
"states": states[joint_in_group],
"limit":
self.model.joint_map[joint_in_group].limit,
"ignore_state": ignore_state
}
# Shivendra: In case of Kuka Arm this elif gets executed
elif type(current_state) is list and type(current_state) is list:
joints = []
assert len(current_state) == len(goal_state) == len(
joint_group)
for joint, c_state, n_state in itertools.izip(
joint_group, current_state, goal_state):
if joint in self.model.joint_map:
ignore_state = False
if ignore_goal_states is not None and len(
ignore_goal_states):
if joint in ignore_goal_states:
ignore_state = True
joints.append([
c_state, n_state,
self.model.joint_map[joint].limit, ignore_state
]) # Shivendra: ignore_state is always False
if len(joints):
# initializing the trajectory planner to model the problem
# Shivendra: solver is 'SCS' (splitting conic solver), solver_config is None, solver_class is ['new', 'old']
# joints is a list of lists with each internal list corresponding to a single joint with
# [current joint angle, goal joint angle, joint limit object (from the URDF parser), False]
self.planner.init(
joints=joints,
samples=samples,
duration=duration,
joint_group=joint_group,
collision_safe_distance=collision_safe_distance,
collision_check_distance=collision_check_distance,
solver=solver,
solver_config=solver_config,
solver_class=solver_class,
decimals_to_round=decimals_to_round,
verbose=verbose)