def add_trajectory(self, plan, goal_entities):
print "Problem idx", self.problem_idx
self.set_seed(self.problem_idx)
problem_env, openrave_env = self.create_environment()
motion_planner = BaseMotionPlanner(problem_env, 'prm')
problem_env.set_motion_planner(motion_planner)
idx = 0
parent_state = None
parent_action = None
moved_objs = {p.discrete_parameters['object'] for p in plan}
moved_obj_regions = {(p.discrete_parameters['object'],
p.discrete_parameters['region'])
for p in plan}
n_times_objs_moved = {obj_name: 0 for obj_name in moved_objs}
n_times_obj_region_moved = {
(obj_name, region_name): 0
for obj_name, region_name in moved_obj_regions
}
self.paps_used = self.get_pap_used_in_plan(plan)
curr_paps_used = self.account_for_used_picks_and_places(
n_times_objs_moved, n_times_obj_region_moved)
state = self.compute_state(parent_state, parent_action, goal_entities,
problem_env, curr_paps_used, 0)
for action_idx, action in enumerate(plan):
action.execute()
# mark that a pick or place in the plan has been used
target_obj_name = action.discrete_parameters['object']
target_region_name = action.discrete_parameters['region']
n_times_objs_moved[target_obj_name] += 1
n_times_obj_region_moved[(target_obj_name,
target_region_name)] += 1
curr_paps_used = self.account_for_used_picks_and_places(
n_times_objs_moved, n_times_obj_region_moved)
parent_state = state
parent_action = action
state = self.compute_state(parent_state, parent_action,
goal_entities, problem_env,
curr_paps_used, action_idx)
# execute the pap action
if action == plan[-1]:
reward = 0
else:
reward = -1
print "The reward is ", reward
self.add_sar_tuples(parent_state, action, reward)
print "Executed", action.discrete_parameters
self.add_state_prime()
openrave_env.Destroy()
openravepy.RaveDestroy()
def add_trajectory(self, plan, goal_entities):
print "Problem idx", self.problem_idx
self.set_seed(self.problem_idx)
problem_env, openrave_env = self.create_environment()
motion_planner = BaseMotionPlanner(problem_env, 'prm')
problem_env.set_motion_planner(motion_planner)
idx = 0
parent_state = None
parent_action = None
self.plan_sanity_check(problem_env, plan)
paps_used = self.get_pap_used_in_plan(plan)
pick_used = paps_used[0]
place_used = paps_used[1]
reward_function = ShapedRewardFunction(problem_env,
['square_packing_box1'],
'home_region', 3 * 8)
# utils.viewer()
state = self.compute_state(parent_state, parent_action, goal_entities,
problem_env, paps_used, 0)
for action_idx, action in enumerate(plan):
if 'place' in action.type:
continue
target_obj = openrave_env.GetKinBody(
action.discrete_parameters['object'])
color_before = get_color(target_obj)
set_color(target_obj, [1, 1, 1])
pick_used, place_used = self.delete_moved_objects_from_pap_data(
pick_used, place_used, target_obj)
paps_used = [pick_used, place_used]
action.is_skeleton = False
pap_action = copy.deepcopy(action)
pap_action = pap_action.merge_operators(plan[action_idx + 1])
pap_action.is_skeleton = False
pap_action.execute()
# set_color(target_obj, color_before)
parent_state = state
parent_action = pap_action
state = self.compute_state(parent_state, parent_action,
goal_entities, problem_env, paps_used,
action_idx)
# execute the pap action
reward = reward_function(parent_state, state, parent_action,
action_idx)
print "The reward is ", reward
self.add_sar_tuples(parent_state, pap_action, reward)
print "Executed", action.discrete_parameters
self.add_state_prime()
openrave_env.Destroy()
openravepy.RaveDestroy()
def rave_env():
if IMPORT_OPENRAVE:
logger.warn("Starting openrave")
orpy.RaveInitialize(load_all_plugins=True)
logger.warn("Starting a new environment")
env = orpy.Environment()
yield env
logger.warn("Destroying a new environment")
env.Destroy()
logger.warn("Destroying Rave runtime")
orpy.RaveDestroy()
else:
yield None
def solve_pddlstream():
pddlstream_problem, namo = get_problem()
namo.env.SetViewer('qtcoin')
stime = time.time()
#solution = solve_incremental(pddlstream_problem, unit_costs=True, max_time=500)
solution = solve_focused(pddlstream_problem, unit_costs=True, max_time=500)
search_time = time.time()-stime
plan, cost, evaluations = solution
print "Search time", search_time
if solution[0] is None:
print "No Solution"
sys.exit(-1)
print_solution(solution)
process_plan(plan, namo)
namo.problem_config['env'].Destroy()
openravepy.RaveDestroy()
return plan, search_time
def test_pf_update(M=1000):
eih_sys, particles = setup_environment('fov_occluded_color', M=M, zero_seed=False)
arm = eih_sys.manip
kinect = eih_sys.kinect
"""
arm.set_pose(tfx.pose(arm.get_pose().position, tfx.tb_angles(0, 90, 0)))
p = arm.get_pose()
center = [p.position.x + .75, p.position.y, p.position.z]
particles = list()
for i in xrange(M):
pos = [0,0,0]
for i in xrange(len(center)):
pos[i] = random_within(center[i] - .025, center[i] + .025)
particle = tfx.point(pos)
particles.append(particle)
"""
arm.set_pose(tfx.pose([2.901, -1.712, 0.868],tfx.tb_angles(-143.0, 77.9, 172.1))) # FOR rarm ONLY
x_t = arm.get_joint_values()
particles_t = particles
u_t = np.zeros(x_t.shape[0])
try:
t = 0
while True:
x_tp1, particles_tp1 = eih_sys.update_state_and_particles(x_t, particles_t, u_t, True, False)
particles_t = particles_tp1
print('Iter: {0}'.format(t))
t += 1
arm.teleop()
x_t = arm.get_joint_values()
#utils.save_view(env, 'figures/eih_pf_{0}.png'.format(t))
except KeyboardInterrupt:
rave.RaveDestroy()
print('Exiting')
sys.exit()
IPython.embed()
def solve_pddlstream():
pddlstream_problem, convbelt = get_problem()
stime = time.time()
#solution = solve_incremental(pddlstream_problem, unit_costs=True, max_time=np.inf)
solution = solve_focused(pddlstream_problem, unit_costs=True, max_time=300)
search_time = time.time()-stime
plan, cost, evaluations = solution
print "Search time", search_time
"""
if solution[0] is None:
sys.exit(-1)
print_solution(solution)
process_plan(plan, convbelt)
import pdb;pdb.set_trace()
"""
convbelt.problem_config['env'].Destroy()
openravepy.RaveDestroy()
return plan, search_time
def run_pickled_demo(fname):
envfile, start_config, goal_config, basetrajs1, armtrajs1, basetrajs2, armtrajs2, assembly, assembly_pose, robots, part_responsibilities, robot_start_poses = load_pickled_demo(
fname)
youbotenv = youbotpy.YoubotEnv(sim=True,viewer=True,env_xml=envfile, \
youbot_names=robots)
env = youbotenv.env
youbots = youbotenv.youbots
for y in basetrajs1:
btraj = orpy.RaveCreateTrajectory(env, "")
btraj.deserialize(basetrajs1[y])
basetrajs1[y] = btraj
atraj = orpy.RaveCreateTrajectory(env, "")
atraj.deserialize(armtrajs1[y])
armtrajs1[y] = atraj
for y in basetrajs2:
btraj = orpy.RaveCreateTrajectory(env, "")
btraj.deserialize(basetrajs2[y])
basetrajs2[y] = btraj
atraj = orpy.RaveCreateTrajectory(env, "")
atraj.deserialize(armtrajs2[y])
armtrajs2[y] = atraj
for name in youbots:
youbots[name].SetTransform(robot_start_poses[name])
youbotenv.MoveGripper(name, 0.01, 0.01) # open grippers
raw_input('Hit Enter to run.')
motionplanner = MultiYoubotPlanner.MultiYoubotPlanner(env, youbots)
motionplanner.Execute(basetrajs1, armtrajs1)
for obj in assembly.object_names:
youbots[part_responsibilities[obj]].Grab(env.GetKinBody(obj))
motionplanner.Execute(basetrajs2, armtrajs2)
raw_input('Hit Enter to destroy environment.')
youbotenv.env.Destroy()
orpy.RaveDestroy()
def setup():
env = orpy.Environment()
yield env
env.Destroy()
orpy.RaveDestroy()
def test_greedy(M=1000):
random.seed(0)
brett = pr2_sim.PR2('../envs/pr2-test.env.xml')
env = brett.env
arm = brett.rarm
kinect = brett.r_kinect
arm.set_posture('mantis')
p = arm.get_pose()
arm.set_pose(tfx.pose(p.position + [.5, 0, 0], tfx.tb_angles(0, 90, 0)))
# .23 just on edge of range
# .25 out of range
p = kinect.get_pose()
center = [p.position.x + .3, p.position.y, p.position.z]
P = list()
for i in xrange(M):
pos = [0,0,0]
for i in xrange(len(center)):
pos[i] = random_within(center[i] - .025, center[i] + .025)
particle = tfx.point(pos)
P.append(particle)
handles = list()
for p in P:
handles.append(utils.plot_point(env, p.array,color=[1,0,0]))
eih_sys = EihSystem(env, arm, kinect, 'fov')
kinect.render_on()
time.sleep(1)
x0 = arm.get_joint_values()
U = [np.zeros(x0.shape)]
try:
t = 0
while True:
print('Iter: {0}'.format(t))
t += 1
arm.set_joint_values(x0)
grad = eih_sys.cost_grad(x0, U, P, step=1e-3, use_discrete=False)[0]
print('grad: {0}'.format(list(grad)))
u_grad = -(2*(np.pi/180.))*grad/np.linalg.norm(grad, 2)
x1 = x0 + u_grad
arm.set_joint_values(x0)
p0 = arm.get_pose()
arm.set_joint_values(x1)
p1 = arm.get_pose()
delta_pos = .05*(p1.position - p0.position)/(p1.position - p0.position).norm
clipped_quat = tft.quaternion_slerp(p0.tb_angles.to_quaternion(), p1.tb_angles.to_quaternion(), .5)
p1_clipped = tfx.pose(p0.position + delta_pos, clipped_quat)
arm.set_pose(p1_clipped)
x1_clipped = arm.get_joint_values()
u_t = x1_clipped - x0
arm.set_joint_values(x0)
x_tp1, P_tp1 = eih_sys.update_state_and_particles(x0, P, u_t, plot=True, add_noise=False)
P = P_tp1
x0 = x_tp1
#utils.save_view(env, 'figures/eih_pf_{0}.png'.format(t))
except KeyboardInterrupt:
rave.RaveDestroy()
print('Exiting')
sys.exit()
'-v',
"validate_result",
is_flag=True,
help="Validate DH parameters before returning")
@click.option("--viewer",
"enable_viewer",
is_flag=True,
help="Enable OpenRAVE viewer")
def cli(robot, base_link, ee_link, modified_dh, validate_result, enable_viewer,
env):
""" Compute DH parameters of the ROBOT kinematic chain defined by BASE_LINK and EE_LINK.
Example usage: `python openrave.py robots/pr2-beta-static.zae base_link \\ r_gripper_palm_link`
"""
if enable_viewer:
env.SetViewer("qtcoin")
dh = get_dh_table(robot, base_link, ee_link, modified_dh, validate_result)
click.echo(dh)
if enable_viewer:
input("Press any key to quit...")
if __name__ == '__main__':
try:
rave.RaveInitialize(load_all_plugins=True, level=rave.DebugLevel.Error)
cli()
finally:
rave.RaveDestroy()
)
local_results.append(
(record_n_transfers, record_times, min_regrasp_assignment))
total_times_local.append(time.time() - start_time)
print 'one local result recorded.'
else: # optimal solution
min_regrasp_assignment, min_regrasp_constraints, variables, asm_op_variables = naive_planner.Plan(
env, yik, op_pose, assembly_operations, all_robot_names,
time_budget_minutes)
if min_regrasp_assignment is None:
optimal_results.append((None, None, None))
total_times_optimal.append(time.time() - start_time)
print 'one optimal failure recorded.'
else:
record_n_transfers, record_times = naive_planner.GetLastRunRecords(
)
optimal_results.append(
(record_n_transfers, record_times, min_regrasp_assignment))
total_times_optimal.append(time.time() - start_time)
print 'one optimal result recorded.'
with open('experiment_results/results_' + experiment_name + '.txt', 'a') as f:
pickle.dump(local_results, f)
pickle.dump(total_times_local, f)
pickle.dump(optimal_results, f)
pickle.dump(total_times_optimal, f)
#IPython.embed()
orpy.RaveDestroy()
def test_image_rays():
env = rave.Environment()
env.Load('../envs/pr2-test.env.xml')
env.SetViewer('qtcoin')
env.GetViewer().SendCommand(
'SetFiguresInCamera 1') # also shows the figures in the image
time.sleep(1)
robot = env.GetRobots()[0]
cam = robot.GetAttachedSensor('head_cam').GetSensor()
type = rave.Sensor.Type.Camera
cam_geom = cam.GetSensorGeometry(type)
depth = robot.GetAttachedSensor('head_depth').GetSensor()
type = rave.Sensor.Type.Laser
depth_geom = depth.GetSensorGeometry(type)
#cam.Configure(rave.Sensor.ConfigureCommand.PowerOn)
#cam.Configure(rave.Sensor.ConfigureCommand.RenderDataOn)
#cam_pose = tfx.pose(cam.GetTransform())
#cam_pose.position.z += .32
cam_pose = tfx.pose([0, 0, 0.05], frame='wide_stereo_optical_frame')
cam_pose_world = tfx.pose(
utils.openraveTransformFromTo(robot, cam_pose.matrix, cam_pose.frame,
'world'))
img_plane_center = cam_pose + [0, 0, .01]
global handles
img_plane_world = tfx.pose(
utils.openraveTransformFromTo(robot, img_plane_center.matrix,
cam_pose.frame, 'world'))
#handles.append(utils.plot_point(env, img_plane_world.position.array, size=.0005))
height, width, _ = cam_geom.imagedata.shape
f = cam_geom.KK[0, 0]
F = .01 # real focal length in meters
W = F * (width / f)
H = F * (height / f)
width_offsets = np.linspace(-W / 2.0, W / 2.0, 64)
height_offsets = np.linspace(-H / 2.0, H / 2.0, 48)
directions = np.zeros((len(width_offsets) * len(height_offsets), 3))
index = 0
for w_offset in width_offsets:
for h_offset in height_offsets:
p = img_plane_center + [w_offset, h_offset, 0]
p_world = tfx.pose(
utils.openraveTransformFromTo(robot, p.matrix, p.frame,
'world'))
direction = (p_world.position.array -
cam_pose_world.position.array)
direction = 5 * direction / np.linalg.norm(direction)
directions[index, :] = direction
index += 1
#closest_collision(env, cam_pose_world.position.array, direction, plot=False)
#handles.append(utils.plot_point(env, p_world.position.array, size=.0001))
start_time = time.time()
closest_collisions(env,
cam_pose_world.position.array,
directions,
plot=False)
print('Total time: {0}'.format(time.time() - start_time))
IPython.embed()
rave.RaveDestroy()
def main():
parser = argparse.ArgumentParser(description='MCTS parameters')
parser.add_argument('-uct', type=float, default=0.0)
parser.add_argument('-w', type=float, default=5.0)
parser.add_argument('-epsilon', type=float, default=0.3)
parser.add_argument('-sampling_strategy', type=str, default='unif')
parser.add_argument('-problem_idx', type=int, default=-1)
parser.add_argument('-domain', type=str, default='minimum_displacement_removal')
parser.add_argument('-planner', type=str, default='mcts')
parser.add_argument('-v', action='store_true', default=False)
parser.add_argument('-debug', action='store_true', default=False)
parser.add_argument('-use_ucb', action='store_true', default=False)
parser.add_argument('-pw', action='store_true', default=False)
parser.add_argument('-mcts_iter', type=int, default=1500)
parser.add_argument('-max_time', type=float, default=np.inf)
parser.add_argument('-c1', type=float, default=1) # weight for measuring distances in SE(2)
parser.add_argument('-n_feasibility_checks', type=int, default=50)
parser.add_argument('-random_seed', type=int, default=-1)
parser.add_argument('-voo_sampling_mode', type=str, default='uniform')
parser.add_argument('-voo_counter_ratio', type=int, default=1)
parser.add_argument('-n_switch', type=int, default=10)
parser.add_argument('-add', type=str, default='')
parser.add_argument('-use_max_backup', action='store_true', default=False)
parser.add_argument('-pick_switch', action='store_true', default=False)
parser.add_argument('-n_actions_per_node', type=int, default=1)
parser.add_argument('-value_threshold', type=float, default=40.0)
args = parser.parse_args()
RaveSetDebugLevel(DebugLevel.Error)
if args.domain == 'convbelt':
args.mcts_iter = 3000
args.n_switch = 10
args.pick_switch = False
args.use_max_backup = True
args.n_feasibility_checks = 50
args.problem_idx = 3
args.n_actions_per_node = 3
if args.pw:
args.sampling_strategy = 'unif'
args.pw = True
args.use_ucb = True
else:
args.w = 5.0
if args.sampling_strategy == 'voo':
args.voo_sampling_mode = 'uniform'
elif args.sampling_strategy == 'randomized_doo':
pass
#args.epsilon = 1.0
if args.pw:
args.add = 'pw_reevaluates_infeasible'
else:
args.add = 'no_averaging'
elif args.domain == 'minimum_displacement_removal':
args.mcts_iter = 2000
args.n_switch = 10
args.pick_switch = True
args.use_max_backup = True
args.n_feasibility_checks = 50
args.problem_idx = 0
args.n_actions_per_node = 1
if args.pw:
args.sampling_strategy = 'unif'
args.pw = True
args.use_ucb = True
else:
args.w = 5.0
if args.sampling_strategy == 'voo':
args.voo_sampling_mode = 'uniform'
elif args.sampling_strategy == 'randomized_doo':
pass
#args.epsilon = 1.0
elif args.sampling_strategy == 'doo':
pass
#args.epsilon = 1.0
if args.pw:
args.add = 'pw_reevaluates_infeasible'
else:
args.add = 'no_averaging'
else:
if args.problem_idx == 0:
args.mcts_iter = 10000
args.n_switch = 5
elif args.problem_idx == 1:
args.mcts_iter = 10000
args.n_switch = 5
elif args.problem_idx == 2:
args.mcts_iter = 10000
args.n_switch = 3
else:
raise NotImplementedError
if args.pw:
args.sampling_strategy = 'unif'
args.pw = True
args.use_ucb = True
else:
args.w = 100
if args.domain == 'synthetic_rastrigin' and args.problem_idx == 1:
args.value_threshold = -50
args.voo_sampling_mode = 'centered_uniform'
args.use_max_backup = True
if args.pw:
assert args.w > 0 and args.w <= 1
else:
pass
if args.sampling_strategy != 'unif':
assert args.epsilon >= 0.0
if args.random_seed == -1:
args.random_seed = args.problem_idx
print "Problem number ", args.problem_idx
print "Random seed set: ", args.random_seed
set_random_seed(args.random_seed)
save_dir = make_save_dir(args)
print "Save dir is", save_dir
stat_file_names = os.listdir(save_dir)
rwds = np.array([pickle.load(open(save_dir+stat_file_name, 'r'))['search_time'][-1][-2] for stat_file_name in stat_file_names])
stat_file_name = save_dir+stat_file_names[np.argmax(rwds)]
if args.domain == 'minimum_displacement_removal':
problem_instantiator = MinimumConstraintRemovalInstantiator(args.problem_idx, args.domain)
environment = problem_instantiator.environment
elif args.domain == 'convbelt':
# todo make root switching in conveyor belt domain
problem_instantiator = ConveyorBeltInstantiator(args.problem_idx, args.domain, args.n_actions_per_node)
environment = problem_instantiator.environment
else:
if args.domain.find("rastrigin") != -1:
environment = RastriginSynthetic(args.problem_idx, args.value_threshold)
elif args.domain.find("griewank") != -1:
environment = GriewankSynthetic(args.problem_idx)
elif args.domain.find("shekel") != -1:
environment = ShekelSynthetic(args.problem_idx)
environment.env.SetViewer('qtcoin')
viewer = environment.env.GetViewer()
cam_transform = np.array(
[[0.9998789, 0.00486718, -0.01478187, -1.67376077],
[0.01244485, -0.82038901, 0.57167036, -10.30184746],
[-0.00934446, -0.57178508, -0.82035023, 11.19177628],
[0., 0., 0., 1.]]
)
viewer.SetCamera(cam_transform)
plan = pickle.load(open(stat_file_name, 'r'))['plan']
places = plan[1::2]
objs = [environment.env.GetKinBody(obj) for place in places for obj in place.discrete_parameters['objects']]
places = places[:-1]
place_motions = [l for place in places for l in place.low_level_motion if place.low_level_motion is not None]
n_objs_placed = len(place_motions)
objs = objs[0:n_objs_placed]
import pdb;pdb.set_trace()
play_plan(objs, place_motions, environment)
environment.init_saver.Restore()
if args.domain != 'synthetic':
environment.env.Destroy()
openravepy.RaveDestroy()
def __del__(self):
orpy.RaveDestroy()
def rave_env():
env = orpy.Environment()
yield env
env.Destroy()
orpy.RaveDestroy()
class Wam7Testcase(unittest.TestCase):
def wam7_test(self):
env = openravepy.Environment()
try:
robot = env.ReadRobotXMLFile('robots/barrettwam.robot.xml')
env.Add(robot)
robot.SetActiveDOFs(robot.GetManipulator('arm').GetArmIndices())
robot.SetActiveDOFValues(q_start)
planner = prpy_lemur.LEMURPlanner(
roadmap=prpy_lemur.roadmaps.Halton(num=1000, radius=2.0))
traj = planner.PlanToConfiguration(robot, q_goal, max_batches=1)
self.assertEqual(6, traj.GetNumWaypoints())
for iwp in range(6):
wp = traj.GetWaypoint(iwp)
self.assertEqual(7, len(wp))
for a, b in zip(wp, traj_expected[iwp]):
self.assertAlmostEqual(a, b, delta=1.0e-5)
finally:
env.Destroy()
if __name__ == '__main__':
openravepy.RaveInitialize(True, level=openravepy.DebugLevel.Info)
unittest.main()
openravepy.RaveDestroy()
def tearDown(self):
self.env.Destroy()
openravepy.RaveDestroy()