result = m * scalar + n
return result
def get_reward_color(r):
if r < 0.0:
return get_scaled_color(r + 1.0, yellow_color, red_color)
else:
return get_scaled_color(r, green_color, yellow_color)
# remove robot from view
for link in openrave_manager.robot.GetLinks():
link.SetVisible(False)
robot_goal_pose = openrave_manager.get_target_pose([0.0] + goal_joints)
if show_goal_end_effector_pose:
goal_sphere = create_sphere('goal', 0.01, openrave_manager)
move_body(goal_sphere, [robot_goal_pose[0], 0.0, robot_goal_pose[1]], 0.0)
goal_sphere.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(
np.array([
0,
0,
1.0,
])) # blue
# for i in range(21):
# r = 1.0 - 0.1*i
# r_color = get_reward_color(r)
# print r, r_color
# goal_sphere.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(r_color)
# time.sleep(2)
def main():
config_path = os.path.join(os.getcwd(), 'config/config.yml')
with open(config_path, 'r') as yml_file:
config = yaml.load(yml_file)
print('------------ Config ------------')
print(yaml.dump(config))
model_dir = os.path.join(trajectories_dir, model_name)
potential_points_location = os.path.join(model_dir, 'potential_points.p')
potential_points = pickle.load(open(potential_points_location))
# search_key = os.path.join(model_dir, '{}_step_{}_{}.p'.format(global_step, message, path_id))
search_key = os.path.join(model_dir, global_step,
'{}_{}.p'.format(message, path_id))
trajectory_files = glob.glob(search_key)
trajectory_file = trajectory_files[0]
pose_goal, trajectory, workspace_id = pickle.load(open(trajectory_file))
trajectory = [[0.0] + list(j) for j in trajectory]
openrave_manager = OpenraveManager(0.001, potential_points)
# get the parameters
if scenario == 'vision':
workspace_params_path = os.path.abspath(
os.path.expanduser(
os.path.join('~/ModelBasedDDPG/scenario_params/vision/',
workspace_id)))
else:
workspace_params_path = os.path.abspath(
os.path.expanduser(
os.path.join('~/ModelBasedDDPG/scenario_params', scenario,
'params.pkl')))
if workspace_params_path is not None:
openrave_manager.set_params(workspace_params_path)
openrave_manager.get_initialized_viewer()
openrave_manager.env.GetViewer().SetSize(1200, 800)
# for link in openrave_manager.robot.GetLinks():
# link.SetVisible(False)
if display_start_goal_end_spheres:
start = trajectory[0]
end = trajectory[-1]
pose_start = openrave_manager.get_target_pose(start)
pose_start = (pose_start[0], 0.0, pose_start[1])
pose_goal = (pose_goal[0], 0.0, pose_goal[1])
pose_end = openrave_manager.get_target_pose(end)
pose_end = (pose_end[0], 0.0, pose_end[1])
start_sphere = create_sphere('start', trajectory_spheres_radius,
openrave_manager)
move_body(start_sphere, pose_start, 0.0)
goal_sphere = create_sphere('goal', goal_radius, openrave_manager)
move_body(goal_sphere, pose_goal, 0.0)
end_sphere = create_sphere('end', trajectory_spheres_radius,
openrave_manager)
start_sphere.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(
np.array([0, 0, 204]))
goal_sphere.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(
np.array([240, 100, 10]))
goal_sphere.GetLinks()[0].GetGeometries()[0].SetTransparency(0.3)
end_sphere.GetLinks()[0].GetGeometries()[0].SetDiffuseColor(
np.array([100, 204, 204]))
move_body(end_sphere, pose_end, 0.0)
print 'len(trajectory) ', len(trajectory)
for i in range(len(trajectory)):
print 'i ', i
openrave_manager.robot.SetDOFValues(trajectory[i])
time.sleep(1 / speed)
time.sleep(0.2)
if message == 'collision':
time.sleep(1.2)
class OpenraveRLInterface:
# class StepResult(enum.Enum):
# free_space = 1
# collision = 2
# close_to_goal = 3
def __init__(self, config):
self.action_step_size = config['openrave_rl']['action_step_size']
self.goal_sensitivity = config['openrave_rl']['goal_sensitivity']
self.keep_alive_penalty = config['openrave_rl']['keep_alive_penalty']
self.truncate_penalty = config['openrave_rl']['truncate_penalty']
self.openrave_manager = OpenraveManager(
config['openrave_rl']['segment_validity_step'], PotentialPoint.from_config(config))
self.current_joints = None
self.goal_joints = None
self.start_joints = None
self.traj = None
def is_below_goal_sensitivity(self, start_joints, goal_joints):
start_pose = self.openrave_manager.get_target_pose(start_joints)
goal_pose = self.openrave_manager.get_target_pose(goal_joints)
pose_distance = np.linalg.norm(np.array(start_pose) - np.array(goal_pose))
return pose_distance < self.goal_sensitivity
def start_specific(self, traj, verify_traj=True):
self.traj = traj
start_joints = traj[0]
goal_joints = traj[-1]
# assert path is legal
if verify_traj:
step_size = self.action_step_size + 0.00001
for i in range(len(traj)-1):
step_i_size = np.linalg.norm(np.array(traj[i]) - np.array(traj[i+1]))
assert step_i_size < step_size, 'step_i_size {}'.format(step_i_size)
steps_required_for_motion_plan = len(traj)
self.current_joints = np.array(start_joints)
self.start_joints = np.array(start_joints)
self.goal_joints = np.array(goal_joints)
return start_joints, goal_joints, steps_required_for_motion_plan
@staticmethod
def _is_valid_region(start_pose, goal_pose):
return start_pose[1] > 0.0 and goal_pose[1] > 0.0
def _is_challenging(self, start_pose, goal_pose):
workspace_params = self.openrave_manager.loaded_params
if workspace_params is None or workspace_params.number_of_obstacles == 0:
return True
# check if the distance from any obstacle is smaller that the start-goal-distance
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 step(self, joints_action):
# compute next joints
step = joints_action * self.action_step_size
next_joints_before_truncate = self.current_joints + step
next_joints = self.openrave_manager.truncate_joints(next_joints_before_truncate)
reward = 0.0
if self.truncate_penalty > 0.0:
reward -= self.truncate_penalty * np.linalg.norm(next_joints_before_truncate - next_joints) / \
self.action_step_size
# if segment not valid return collision result
if not self.openrave_manager.check_segment_validity(self.current_joints, next_joints):
return self._get_step_result(next_joints, -1.0 + reward, True, 2)
# if close enough to goal, return positive reward
if self.is_below_goal_sensitivity(next_joints, self.goal_joints):
return self._get_step_result(next_joints, 1.0 + reward, True, 3)
# else, just a normal step...
return self._get_step_result(next_joints, -self.keep_alive_penalty + reward, False, 1)
def _get_step_result(self, next_joints, reward, is_terminal, enum_res):
if is_terminal:
self.current_joints = None
else:
self.current_joints = next_joints
return list(next_joints), reward, is_terminal, enum_res
class OpenraveTrajectoryGenerator:
def __init__(self, config):
self.action_step_size = config["openrave_rl"]["action_step_size"]
self.goal_sensitivity = config["openrave_rl"]["goal_sensitivity"]
self.challenging_trajectories_only = config["openrave_planner"][
"challenging_trajectories_only"]
self.planner_iterations_start = config["openrave_planner"][
"planner_iterations_start"]
self.planner_iterations_increase = config["openrave_planner"][
"planner_iterations_increase"]
self.planner_iterations_decrease = config["openrave_planner"][
"planner_iterations_decrease"]
self.max_planner_iterations = self.planner_iterations_start
self.openrave_manager = OpenraveManager(
config["openrave_rl"]["segment_validity_step"],
PotentialPoint.from_config(config),
)
def is_below_goal_sensitivity(self, start_joints, goal_joints):
start_pose = self.openrave_manager.get_target_pose(start_joints)
goal_pose = self.openrave_manager.get_target_pose(goal_joints)
pose_distance = np.linalg.norm(
np.array(start_pose) - np.array(goal_pose))
return pose_distance < self.goal_sensitivity
def find_random_trajectory_single_try(self):
# select at random
start_joints = self.openrave_manager.get_random_joints({0: 0.0})
goal_joints = self.openrave_manager.get_random_joints({0: 0.0})
# if the start and goal are too close, re-sample
if self.is_below_goal_sensitivity(start_joints, goal_joints):
return None
start_pose = self.openrave_manager.get_target_pose(start_joints)
goal_pose = self.openrave_manager.get_target_pose(goal_joints)
# valid region:
if not self._is_valid_region(start_pose, goal_pose):
return None
# trajectories that must cross an obstacle
if self.challenging_trajectories_only and not self._is_challenging(
start_pose, goal_pose):
return None
traj = self.openrave_manager.plan(start_joints, goal_joints,
self.max_planner_iterations)
return traj
def find_random_trajectory(self):
# lower_size = 0.0 # when doing curriculum, this this is the lowest possible distance between start and goal
while True:
traj = self.find_random_trajectory_single_try()
if traj is None:
# if failed to plan, give more power
self.max_planner_iterations += self.planner_iterations_increase
elif (self.max_planner_iterations > self.planner_iterations_start +
self.planner_iterations_decrease):
# if plan was found, maybe we need less iterations
self.max_planner_iterations -= self.planner_iterations_decrease
return self.split_trajectory(traj, self.action_step_size)
@staticmethod
def _is_valid_region(start_pose, goal_pose):
return start_pose[1] > 0.0 and goal_pose[1] > 0.0
def _is_challenging(self, start_pose, goal_pose):
workspace_params = self.openrave_manager.loaded_params
if workspace_params is None or workspace_params.number_of_obstacles == 0:
return True
# check if the distance from any obstacle is smaller that the start-goal-distance
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
@staticmethod
def split_trajectory(trajectory, action_step_size):
res = [tuple(trajectory[0])]
for i in range(len(trajectory) - 1):
current_step = np.array(trajectory[i])
next_step = np.array(trajectory[i + 1])
difference = next_step - current_step
difference_norm = np.linalg.norm(difference)
if difference_norm < action_step_size:
# if smaller than allowed step just append the next step
res.append(tuple(trajectory[i + 1]))
continue
scaled_step = (action_step_size / difference_norm) * difference
steps = []
for alpha in range(
int(np.floor(difference_norm / action_step_size))):
processed_step = current_step + (1 + alpha) * scaled_step
steps.append(processed_step)
# we probably have a leftover section, append it to res
last_step_difference = np.linalg.norm(steps[-1] - next_step)
if last_step_difference > 0.0:
steps.append(next_step)
# append to path
res += [tuple(s) for s in steps]
return res