show_goal_end_effector_pose = True
# load configuration
config_path = os.path.join(os.getcwd(), 'config/config.yml')
with open(config_path, 'r') as yml_file:
config = yaml.load(yml_file)
# load the workspace
openrave_manager = OpenraveManager(
config['openrave_rl']['segment_validity_step'],
PotentialPoint.from_config(config))
params_file = os.path.abspath(
os.path.expanduser(
os.path.join('~/ModelBasedDDPG/scenario_params', scenario,
'params.pkl')))
openrave_manager.load_params(WorkspaceParams.load_from_file(params_file))
openrave_manager.robot.SetDOFValues([0.0] + goal_joints, [0, 1, 2, 3, 4])
openrave_manager.get_initialized_viewer()
red_color = np.array([1.0, 0.0, 0.0])
yellow_color = np.array([1.0, 1.0, 0.0])
green_color = np.array([0.0, 1.0, 0.0])
def create_sphere(id, radius, openrave_manager):
body = RaveCreateKinBody(openrave_manager.env, '')
body.SetName('sphere{}'.format(id))
body.InitFromSpheres(np.array([[0.0] * 3 + [radius]]), True)
openrave_manager.env.Add(body, True)
return body
class WorkerQueue(multiprocessing.Process):
def __init__(
self,
test_trajectories,
attempts_to_find_single_trajectory,
trajectories_required_to_pass,
planner_iterations,
workspace_generation_queue,
worker_specific_queue,
results_queue,
):
multiprocessing.Process.__init__(self)
# parameters
self.test_trajectories = test_trajectories
self.attempts_to_find_single_trajectory = attempts_to_find_single_trajectory
self.trajectories_required_to_pass = trajectories_required_to_pass
self.planner_iterations = planner_iterations
# queues
self.workspace_generation_queue = workspace_generation_queue
self.worker_specific_queue = worker_specific_queue
self.results_queue = results_queue
# members
self.generator = WorkspaceGenerator(obstacle_count_probabilities={
2: 0.05,
3: 0.5,
4: 0.4,
5: 0.05
})
config_path = os.path.join(os.getcwd(), "config/config.yml")
with open(config_path, "r") as yml_file:
self.config = yaml.load(yml_file)
self.openrave_manager = None
def _is_below_goal_sensitivity(self, start_pose, goal_pose):
pose_distance = np.linalg.norm(
np.array(start_pose) - np.array(goal_pose))
return pose_distance < self.config["openrave_rl"]["goal_sensitivity"]
@staticmethod
def _is_challenging(start_pose, goal_pose, workspace_params):
start = np.array(start_pose)
goal = np.array(goal_pose)
start_goal_distance = np.linalg.norm(start - goal)
for i in range(workspace_params.number_of_obstacles):
obstacle = np.array([
workspace_params.centers_position_x[i],
workspace_params.centers_position_z[i],
])
start_obstacle_distance = np.linalg.norm(start - obstacle)
goal_obstacle_distance = np.linalg.norm(goal - obstacle)
if (start_obstacle_distance < start_goal_distance
and goal_obstacle_distance < start_goal_distance):
return True
# all tests failed
return False
def _try_plan(self, workspace_params, openrave_manager):
for i in range(self.attempts_to_find_single_trajectory):
# select at random
start_joints = openrave_manager.get_random_joints({0: 0.0})
goal_joints = openrave_manager.get_random_joints({0: 0.0})
start_pose = openrave_manager.get_target_pose(start_joints)
goal_pose = openrave_manager.get_target_pose(goal_joints)
# if the start and goal are too close, re-sample
if self._is_below_goal_sensitivity(start_joints, goal_joints):
continue
# valid region:
if not start_pose[1] < 0.0 or goal_pose[1] < 0.0:
continue
# trajectories that must cross an obstacle
if self._is_challenging(start_pose, goal_pose, workspace_params):
continue
traj = openrave_manager.plan(start_joints, goal_joints,
self.planner_iterations)
return traj is not None
return None
def _generate_single_workspace(self, workspace_id):
while True:
a = datetime.datetime.now()
workspace_params = self.generator.generate_workspace()
self.openrave_manager = OpenraveManager(
self.config["openrave_rl"]["segment_validity_step"],
PotentialPoint.from_config(self.config),
)
self.openrave_manager.loaded_params_path = None
self.openrave_manager.load_params(workspace_params, "")
successful_trajectories_count = 0
i = 0
for i in range(self.test_trajectories):
# see if there is hope
trajectories_left = self.test_trajectories - i
if (trajectories_left + successful_trajectories_count <
self.trajectories_required_to_pass):
print("no hope to get the required ratio")
break
# try a trajectory
successful_trajectories_count += (self._try_plan(
workspace_params, self.openrave_manager) is not None)
# if successful update the status
if successful_trajectories_count >= self.trajectories_required_to_pass:
print("workspace found")
save_path = os.path.join(
output_dir, "{}_workspace.pkl".format(workspace_id))
workspace_params.save(save_path)
return
b = datetime.datetime.now()
print("trajectories tried {}".format(i))
print("success count {}".format(successful_trajectories_count))
print("time since start {}".format(b - a))
print("")
def run(self):
while True:
try:
# wait 1 second for a workspace generation request
workspace_id = self.workspace_generation_queue.get(block=True,
timeout=1)
self._generate_single_workspace(workspace_id)
self.results_queue.put(workspace_id)
self.workspace_generation_queue.task_done()
except Queue.Empty:
pass
try:
# need to terminate
worker_specific_task = self.worker_specific_queue.get(
block=True, timeout=0.001)
self.worker_specific_queue.task_done()
break
except Queue.Empty:
pass