def initialize(load_gazebo=True):
openrave_sim = DynamicSimulationRobotWorld()
robot_xml = XmlSimulationObject("robots/pr2-beta-static.zae",
dynamic=False)
openrave_sim.add_objects([robot_xml], consider_finger_collisions=True)
table = BoxSimulationObject('table', \
[0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0], \
[0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0], \
dynamic=False)
openrave_sim.add_objects([table], consider_finger_collisions=True)
#table_xml = TableSimulationObject("table", \
# [0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0],
# [0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0],
# dynamic=False)
#openrave_sim.add_objects([table_xml], consider_finger_collisions=True)
cup = XmlSimulationObject(os.path.join(MODELS_DIR, CUP_MESH),
dynamic=False)
openrave_sim.add_objects([cup], consider_finger_collisions=True)
v = trajoptpy.GetViewer(
openrave_sim.env) # TODO: not sure if this is necessary
# Initialize interface for passing controls to Gazebo. Make sure to pass
# in the OpenRave environment we just created, so PR2 isn't referring (and
# updating) a separate OpenRave environment.
if load_gazebo:
pr2 = PR2.PR2(env=openrave_sim.env)
time.sleep(0.2)
else:
pr2 = None
return openrave_sim, pr2, v, cup
def create_viewer(self, window_prop=None, camera_matrix=None):
trajoptpy.GetViewer(self.env) # creates viewer
if window_prop is None:
window_prop = settings.WINDOW_PROP
self.viewer.SetWindowProp(*window_prop)
if camera_matrix is None:
camera_matrix = settings.CAMERA_MATRIX
self.viewer.SetCameraManipulatorMatrix(np.asarray(camera_matrix))
def animate_traj(traj, robot, pause=True, restore=True):
"""make sure to set active DOFs beforehand"""
if restore: _saver = openravepy.RobotStateSaver(robot)
viewer = trajoptpy.GetViewer(robot.GetEnv())
for (i,dofs) in enumerate(traj):
print "step %i/%i"%(i+1,len(traj))
robot.SetActiveDOFValues(dofs)
if pause: viewer.Idle()
else: viewer.Step()
def animate_traj(traj, robot, pause=True, step_viewer=1, restore=True, callback=None, execute_step_cond=None):
"""make sure to set active DOFs beforehand"""
if restore: _saver = openravepy.RobotStateSaver(robot)
if step_viewer or pause: viewer = trajoptpy.GetViewer(robot.GetEnv())
for (i,dofs) in enumerate(traj):
sys.stdout.write("step %i/%i\r"%(i+1,len(traj)))
sys.stdout.flush()
if callback is not None: callback(i)
if execute_step_cond is not None and not execute_step_cond(i): continue
robot.SetActiveDOFValues(dofs)
if pause: viewer.Idle()
elif step_viewer!=0 and (i%step_viewer==0 or i==len(traj)-1): viewer.Step()
sys.stdout.write("\n")
def animate_floating_traj(sim_env,
lhmats,
rhmats,
pause=True,
step_viewer=True,
callback=None,
step=5):
assert len(lhmats) == len(
rhmats
), "I don't know how to animate trajectory with different lengths"
if step_viewer or pause: viewer = trajoptpy.GetViewer(sim_env.sim.env)
for i in xrange(len(lhmats)):
if callback is not None: callback(i)
sim_env.sim.grippers['r'].set_toolframe_transform(rhmats[i])
sim_env.sim.grippers['l'].set_toolframe_transform(lhmats[i])
if pause: viewer.Idle()
elif step_viewer and not i % step: viewer.Step()
def main():
import openravepy
import trajoptpy
env = openravepy.Environment()
viewer = trajoptpy.GetViewer(env)
env.LoadData(make_table_xml(translation=[0, 0, -.05], extents=[1, 1, .05]))
np.random.seed(0)
sim_params = trackingpy.SimulationParams()
sim_params.dt = .01
sim_params.solver_iters = 10
sim_params.gravity = np.array([0, 0, -1.])
sim_params.damping = 1
sim_params.stretching_stiffness = 1
sim_params.bending_stiffness = .7
cloth = Mesh('/home/jonathan/Desktop/simple_cloth_with_slit.obj', 100,
sim_params)
# cloth = Cloth(res_x=50, res_y=50, len_x=.5, len_y=1., init_center=np.array([0, 0, .5]), total_mass=100, sim_params=sim_params)
# anchors for testing
# for i in range(cloth.res_x):
# cloth.sys.add_anchor_constraint(i, cloth.init_pos[i])
# add above-table constraints
for i in range(cloth.num_nodes):
cloth.sys.add_plane_constraint(i, np.array([0, 0, 0]),
np.array([0, 0, 1]))
# from timeit import Timer
# print 'timing'
# num_timing_iters = 1000
# t = Timer(lambda: cloth.step())
# result = t.timeit(number=num_timing_iters)
# print 'iters/sec =', float(num_timing_iters)/result, 'total time =', result
# raw_input('done timing')
print 'cloth made'
while True:
cloth.step()
pos = cloth.get_node_positions()
handles = [env.plot3(pos, 5)]
handles.append(
env.drawlinelist(cloth.get_edge_positions().reshape((-1, 3)), 1,
(0, 1, 0)))
viewer.Idle()
def test_viewer_side_effects(self):
"""
Check if stepping the viewer has side effects in the simulation
"""
sim_state0 = self.sim.get_state()
self.sim.set_state(sim_state0)
self.sim.settle(max_steps=1000, step_viewer=0)
sim_state1 = self.sim.get_state()
# create viewer
viewer = trajoptpy.GetViewer(self.sim.env)
self.sim.set_state(sim_state0)
self.sim.settle(max_steps=1000, step_viewer=10)
sim_state2 = self.sim.get_state()
self.assertArrayDictEqual(sim_state1[1], sim_state2[1])
def setup_lfd_environment_sim(args):
actions = h5py.File(args.gen_tasks.actionfile, 'r')
init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(
actions, args.gen_tasks.fake_data_segment,
args.gen_tasks.fake_data_transform)
table_height = init_rope_xyz[:, 2].mean() - .02
sim_objs = []
sim_objs.append(
BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)],
[.85, .85, .1],
dynamic=False))
sim = DynamicSimulation()
world = sim
sim.add_objects(sim_objs)
if args.animation:
viewer = trajoptpy.GetViewer(sim.env)
if os.path.isfile(args.window_prop_file) and os.path.isfile(
args.camera_matrix_file):
print "loading window and camera properties"
window_prop = np.loadtxt(args.window_prop_file)
camera_matrix = np.loadtxt(args.camera_matrix_file)
try:
viewer.SetWindowProp(*window_prop)
viewer.SetCameraManipulatorMatrix(camera_matrix)
except:
print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
else:
print "move viewer to viewpoint that isn't stupid"
print "then hit 'p' to continue"
viewer.Idle()
print "saving window and camera properties"
try:
window_prop = viewer.GetWindowProp()
camera_matrix = viewer.GetCameraManipulatorMatrix()
np.savetxt(args.window_prop_file, window_prop, fmt='%d')
np.savetxt(args.camera_matrix_file, camera_matrix)
except:
print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
viewer.Step()
return sim
def main():
demofile = h5py.File(args.demos_h5file, 'r')
trajoptpy.SetInteractive(False)
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
Globals.robot.SetDOFValues(PR2_L_POSTURES["side"], Globals.robot.GetManipulator("leftarm").GetArmIndices())
Globals.robot.SetDOFValues(mirror_arm_joints(PR2_L_POSTURES["side"]), Globals.robot.GetManipulator("rightarm").GetArmIndices())
init_rope_xyz, _ = load_random_start_segment(demofile)
init_rope_xyz_robot = apply_tfm(init_rope_xyz[()], args.inittfmfile)
table_height = init_rope_xyz_robot[:,2].mean() - .03
table_xml = make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01])
Globals.env.LoadData(table_xml)
Globals.sim = ropesim.Simulation(Globals.env, Globals.robot)
Globals.viewer = trajoptpy.GetViewer(Globals.env)
Globals.viewer.Idle()
if not args.same_as_training:
for i in range(0, args.dataset_size):
print "State ", i, " of ", args.dataset_size
rope_nodes, demo_id = sample_rope_state(demofile)
save_example_action(rope_nodes, demo_id)
else:
start_keys = [k for k in demofile.keys()
if k.startswith('demo') and k.endswith('00')]
for demo_id in start_keys:
xyz_camera = demofile[demo_id]['cloud_xyz'][()]
xyz_robot = apply_tfm(xyz_camera, args.inittfmfile)
rope_nodes = rope_initialization.find_path_through_point_cloud(
xyz_robot, perturb_peak_dist = None,
num_perturb_points = 0)
replace_rope(rope_nodes)
Globals.sim.settle()
Globals.viewer.Step()
new_xyz_robot = Globals.sim.observe_cloud()
save_example_action(new_xyz_robot, demo_id)
time.sleep(1)
def animate_traj(traj, robot):
with robot:
viewer = trajoptpy.GetViewer(robot.GetEnv())
for i, traj_mats in enumerate(zip(traj, new_hmats)):
dofs = traj_mats[0]
mat = traj_mats[1]
print mat
robot.SetDOFValues(
dofs,
robot.GetManipulator(manip_name).GetArmIndices())
colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)]
for i in range(3):
point = mat[:3, 3]
axis = mat[:3, i] / 10
handles.append(
Globals.env.drawarrow(p1=point,
p2=point + axis,
linewidth=0.004,
color=colors[i]))
viewer.Idle()
def viewer(self):
if not self.__viewer_cache: #and trajoptpy.ViewerExists(self.env):
self.__viewer_cache = trajoptpy.GetViewer(self.env)
return self.__viewer_cache
def main():
parser = argparse.ArgumentParser()
parser.add_argument('file', type=str)
parser.add_argument('--real_robot', action='store_true')
parser.add_argument('--view', action='store_true')
args = parser.parse_args()
if args.view:
print 'Opening file %s' % args.file
f = h5py.File(args.file, 'r')
rgbs = f['rgb']
depths = f['depth']
T_w_k = np.array(f['T_w_k'])
Globals.env = openravepy.Environment()
viewer = trajoptpy.GetViewer(Globals.env)
num_frames = len(rgbs)
for i in range(0, num_frames, 10):
print 'Showing frame %d/%d' % (i + 1, num_frames)
rgb, depth = rgbs[i], depths[i]
# XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f)
# XYZ_w = XYZ_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:]
# handle = Globals.env.plot3(XYZ_w.reshape(-1,3), 2, rgb.reshape(-1,3)[:,::-1]/255.)
xyz = clouds.extract_color(rgb, depth, T_w_k, clouds.yellow_mask)
handles = []
handles.append(Globals.env.plot3(xyz, 2, (1, 0, 0)))
draw_ax(T_w_k, .1, Globals.env, handles)
viewer.Idle()
return
if args.real_robot:
import rospy
rospy.init_node("recorder", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
Globals.pr2.update_rave()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
else:
Globals.env = openravepy.Environment()
T_w_k = np.eye(4)
viewer = trajoptpy.GetViewer(Globals.env)
#env.LoadData(make_table_xml(translation=[0, 0, -.05], extents=[1, 1, .05]))
num_frames = 0
rgbs, depths = [], []
subprocess.call("killall XnSensorServer", shell=True)
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
try:
while True:
rgb, depth = grabber.getRGBD()
rgbs.append(rgb)
depths.append(depth)
cv2.imshow("rgb", rgb)
cv2.imshow("depth", cmap[np.fmin((depth * .064).astype('int'),
255)])
cv2.waitKey(30)
num_frames += 1
print 'Captured frame', num_frames
viewer.Step()
except KeyboardInterrupt:
print 'Keyboard interrupt'
print 'Writing to %s ...' % args.file
out = h5py.File(args.file, 'w')
out.create_dataset('rgb',
data=rgbs,
compression='gzip',
chunks=(1, 256, 256, 3))
out.create_dataset('depth',
data=depths,
compression='gzip',
chunks=(1, 256, 256))
out['T_w_k'] = T_w_k
out.close()
args = parser.parse_args()
import h5py, openravepy, trajoptpy
from rapprentice import animate_traj, ros2rave, clouds
from numpy import asarray
import numpy as np
hdf = h5py.File(args.h5file)
segnames = [args.seg] if args.seg else hdf.keys()
env = openravepy.Environment()
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
robot = env.GetRobots()[0]
viewer = trajoptpy.GetViewer(env)
for segname in segnames:
seg_info = hdf[segname]
from rapprentice import berkeley_pr2
r2r = ros2rave.RosToRave(robot, seg_info["joint_states"]["name"])
rave_traj = [
r2r.convert(row)
for row in asarray(seg_info["joint_states"]["position"])
]
robot.SetActiveDOFs(r2r.rave_inds)
robot.SetActiveDOFValues(rave_traj[0])
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.execution:
rospy.init_node("exec_task", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
subprocess.call("killall XnSensorServer", shell=True)
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
new_xyz = np.squeeze(demofile[args.fake_data_segment]["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(
args.fake_data_transform[3:6])
hmat[:3, 3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :]
else:
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
Globals.pr2.join_all()
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
################################
redprint("Finding closest demonstration")
if args.fake_data_segment:
seg_name = args.fake_data_segment
elif args.choice == "costs":
f, seg_name = choose_segment(demofile, new_xyz)
else:
seg_name = choose_segment(demofile, new_xyz)
seg_info = demofile[seg_name]
# redprint("using demo %s, description: %s"%(seg_name, seg_info["description"]))
################################
redprint("Generating end-effector trajectory")
if not args.choice == "costs":
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0, 1)))
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1, 1)))
f = registration.tps_rpm(old_xyz,
new_xyz,
plot_cb=tpsrpm_plot_cb,
plotting=1)
handles.extend(
plotting_openrave.draw_grid(Globals.env,
f.transform_points,
old_xyz.min(axis=0),
old_xyz.max(axis=0),
xres=.1,
yres=.1,
zres=.04))
link2eetraj = {}
for lr in 'lr':
link_name = "%s_gripper_tool_frame" % lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
if not args.choice == "costs":
handles.append(
Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2,
(1, 0, 0, 1)))
handles.append(
Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2,
(0, 1, 0, 1)))
# TODO plot
# plot_warping_and_trajectories(f, old_xyz, new_xyz, old_ee_traj, new_ee_traj)
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
print colorize.colorize("mini segments:",
"red"), miniseg_starts, miniseg_ends
for (i_miniseg,
(i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if args.execution == "real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i" %
(seg_name, i_miniseg))
bodypart2traj = {}
arms_used = ""
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj = asarray(seg_info[manip_name][i_start:i_end +
1])
if arm_moved(old_joint_traj):
ee_link_name = "%s_gripper_tool_frame" % lr
new_ee_traj = link2eetraj[ee_link_name]
if args.execution: Globals.pr2.update_rave()
new_joint_traj = plan_follow_traj(
Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name),
new_ee_traj[i_start:i_end + 1], old_joint_traj)
# (robot, manip_name, ee_link, new_hmats, old_traj):
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
arms_used += lr
################################
redprint(
"Executing joint trajectory for segment %s, part %i using arms '%s'"
% (seg_name, i_miniseg, arms_used))
for lr in 'lr':
set_gripper_maybesim(
lr,
binarize_gripper(seg_info["%s_gripper_joint" %
lr][i_start]))
#trajoptpy.GetViewer(Globals.env).Idle()
if len(bodypart2traj) > 0:
exec_traj_maybesim(bodypart2traj)
# TODO measure failure condtions
if not success:
break
redprint("Segment %s result: %s" % (seg_name, success))
if args.fake_data_segment: break
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.log:
LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"),
datetime.datetime.now().strftime("%Y%m%d-%H%M%S"))
os.mkdir(LOG_DIR)
LOG_COUNT = 0
if args.execution:
rospy.init_node("exec_task", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
fake_seg = demofile[args.fake_data_segment]
new_xyz = np.squeeze(fake_seg["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(
args.fake_data_transform[3:6])
hmat[:3, 3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :]
r2r = ros2rave.RosToRave(Globals.robot,
asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot,
asarray(fake_seg["joint_states"]["position"][0]))
else:
Globals.pr2.head.set_pan_tilt(0, 1.2)
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
Globals.pr2.join_all()
time.sleep(.5)
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
#grab_end(new_xyz)
if args.log:
LOG_COUNT += 1
import cv2
cv2.imwrite(osp.join(LOG_DIR, "rgb%i.png" % LOG_COUNT), rgb)
cv2.imwrite(osp.join(LOG_DIR, "depth%i.png" % LOG_COUNT), depth)
np.save(osp.join(LOG_DIR, "xyz%i.npy" % LOG_COUNT), new_xyz)
################################
redprint("Finding closest demonstration")
if args.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s" % (seg_name))
if "done" in seg_name:
redprint("DONE!")
break
if args.log:
with open(osp.join(LOG_DIR, "neighbor%i.txt" % LOG_COUNT),
"w") as fh:
fh.write(seg_name)
################################
redprint("Generating end-effector trajectory")
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0)))
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f, _ = registration.tps_rpm_bij(scaled_old_xyz,
scaled_new_xyz,
plot_cb=tpsrpm_plot_cb,
plotting=5 if args.animation else 0,
rot_reg=np.r_[1e-4, 1e-4, 1e-1],
n_iter=50,
reg_init=10,
reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
handles.extend(
plotting_openrave.draw_grid(Globals.env,
f.transform_points,
old_xyz.min(axis=0) - np.r_[0, 0, .1],
old_xyz.max(axis=0) + np.r_[0, 0, .1],
xres=.1,
yres=.1,
zres=.04))
link2eetraj = {}
for lr in 'lr':
link_name = "%s_gripper_tool_frame" % lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
handles.append(
Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2,
(1, 0, 0, 1)))
handles.append(
Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2,
(0, 1, 0, 1)))
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
print colorize.colorize("mini segments:",
"red"), miniseg_starts, miniseg_ends
for (i_miniseg,
(i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if args.execution == "real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i" %
(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
lr2oldtraj = {}
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj = asarray(seg_info[manip_name][i_start:i_end +
1])
#print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end
if arm_moved(old_joint_traj):
lr2oldtraj[lr] = old_joint_traj
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
_, timesteps_rs = unif_resample(old_total_traj,
JOINT_LENGTH_PER_STEP)
####
### Generate fullbody traj
bodypart2traj = {}
for (lr, old_joint_traj) in lr2oldtraj.items():
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj_rs = mu.interp2d(timesteps_rs,
np.arange(len(old_joint_traj)),
old_joint_traj)
ee_link_name = "%s_gripper_tool_frame" % lr
new_ee_traj = link2eetraj[ee_link_name][i_start:i_end + 1]
new_ee_traj_rs = resampling.interp_hmats(
timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
if args.execution: Globals.pr2.update_rave()
new_joint_traj = planning.plan_follow_traj(
Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,
old_joint_traj_rs)
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
################################
redprint(
"Executing joint trajectory for segment %s, part %i using arms '%s'"
% (seg_name, i_miniseg, bodypart2traj.keys()))
for lr in 'lr':
success &= set_gripper_maybesim(
lr,
binarize_gripper(seg_info["%s_gripper_joint" %
lr][i_start]))
# Doesn't actually check if grab occurred, unfortunately
if not success: break
if len(bodypart2traj) > 0:
success &= exec_traj_maybesim(bodypart2traj)
if not success: break
redprint("Segment %s result: %s" % (seg_name, success))
if args.fake_data_segment: break
import openravepy, sensorsimpy, trajoptpy
env = openravepy.Environment()
env.StopSimulation()
env.Load("robots/pr2-beta-static.zae")
viewer = trajoptpy.GetViewer(env)
viewer.Idle()
sensor = sensorsimpy.CreateFakeKinect(env)
sensor.SetPose([0, 0, 2], [0, 0, 1, 0])
sensor.SetIntrinsics(525)
sensor.Update()
d = sensor.GetDepthImage()
def setup_lfd_environment_sim(args):
actions = h5py.File(args.eval.actionfile, 'r')
init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment(
actions, args.eval.fake_data_segment, args.eval.fake_data_transform)
table_height = init_rope_xyz[:, 2].mean() - .02
sim_objs = []
sim_objs.append(
XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False))
sim_objs.append(
BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)],
[.85, .85, .1],
dynamic=False))
print 'Setting up lfd environment'
sim = DynamicRopeSimulationRobotWorld()
world = sim
sim.add_objects(sim_objs)
if args.eval.ground_truth:
lfd_env = GroundTruthRopeLfdEnvironment(
sim,
world,
upsample=args.eval.upsample,
upsample_rad=args.eval.upsample_rad,
downsample_size=args.eval.downsample_size)
else:
lfd_env = LfdEnvironment(sim,
world,
downsample_size=args.eval.downsample_size)
dof_inds = sim_util.dof_inds_from_name(sim.robot,
'+'.join(init_joint_names))
values, dof_inds = zip(
*[(value, dof_ind)
for value, dof_ind in zip(init_joint_values, dof_inds)
if dof_ind != -1])
# this also sets the torso (torso_lift_joint) to the height in the data
sim.robot.SetDOFValues(values, dof_inds)
sim_util.reset_arms_to_side(sim)
if args.animation:
viewer = trajoptpy.GetViewer(sim.env)
if os.path.isfile(args.window_prop_file) and os.path.isfile(
args.camera_matrix_file):
print "loading window and camera properties"
window_prop = np.loadtxt(args.window_prop_file)
camera_matrix = np.loadtxt(args.camera_matrix_file)
try:
viewer.SetWindowProp(*window_prop)
viewer.SetCameraManipulatorMatrix(camera_matrix)
except:
print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
else:
print "move viewer to viewpoint that isn't stupid"
print "then hit 'p' to continue"
viewer.Idle()
print "saving window and camera properties"
try:
window_prop = viewer.GetWindowProp()
camera_matrix = viewer.GetCameraManipulatorMatrix()
np.savetxt(args.window_prop_file, window_prop, fmt='%d')
np.savetxt(args.camera_matrix_file, camera_matrix)
except:
print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
viewer.Step()
if args.eval.dof_limits_factor != 1.0:
assert 0 < args.eval.dof_limits_factor and args.eval.dof_limits_factor <= 1.0
active_dof_indices = sim.robot.GetActiveDOFIndices()
active_dof_limits = sim.robot.GetActiveDOFLimits()
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
dof_inds = sim.robot.GetManipulator(manip_name).GetArmIndices()
limits = np.asarray(sim.robot.GetDOFLimits(dof_inds))
limits_mean = limits.mean(axis=0)
limits_width = np.diff(limits, axis=0)
new_limits = limits_mean + args.eval.dof_limits_factor * \
np.r_[-limits_width / 2.0, limits_width / 2.0]
for i, ind in enumerate(dof_inds):
active_dof_limits[0][active_dof_indices.tolist().index(
ind)] = new_limits[0, i]
active_dof_limits[1][active_dof_indices.tolist().index(
ind)] = new_limits[1, i]
sim.robot.SetDOFLimits(active_dof_limits[0], active_dof_limits[1])
return lfd_env, sim
def load_simulation(args, sim_env):
sim_env.env = openravepy.Environment()
sim_env.env.StopSimulation()
sim_env.env.Load("robots/pr2-beta-static.zae")
sim_env.robot = sim_env.env.GetRobots()[0]
actions = h5py.File(args.actionfile, 'r')
fakedatafile = h5py.File(args.fakedatafile, 'r')
GlobalVars.init_tfm = fakedatafile['init_tfm'][()]
init_rope_xyz, _ = sim_util.load_fake_data_segment(
sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform
) # this also sets the torso (torso_lift_joint) to the height in the data
# Set table height to correct height of first rope in holdout set
holdoutfile = h5py.File(args.holdoutfile, 'r')
first_holdout = holdoutfile[holdoutfile.keys()[0]]
init_rope_xyz = first_holdout['rope_nodes'][:]
if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r':
init_rope_xyz = init_rope_xyz.dot(
GlobalVars.init_tfm[:3, :3].T) + GlobalVars.init_tfm[:3,
3][None, :]
table_height = init_rope_xyz[:, 2].mean() - .02 # Before: .02
table_xml = sim_util.make_table_xml(translation=[1, 0, table_height],
extents=[.85, .55, .01])
sim_env.env.LoadData(table_xml)
if 'bookshelve' in args.obstacles:
sim_env.env.Load("data/bookshelves.env.xml")
if 'boxes' in args.obstacles:
sim_env.env.LoadData(
sim_util.make_box_xml("box0",
[.7, .43, table_height + (.01 + .12)],
[.12, .12, .12]))
sim_env.env.LoadData(
sim_util.make_box_xml(
"box1", [.74, .47, table_height + (.01 + .12 * 2 + .08)],
[.08, .08, .08]))
cc = trajoptpy.GetCollisionChecker(sim_env.env)
for gripper_link in [
link for link in sim_env.robot.GetLinks()
if 'gripper' in link.GetName()
]:
cc.ExcludeCollisionPair(gripper_link,
sim_env.env.GetKinBody('table').GetLinks()[0])
sim_util.reset_arms_to_side(sim_env)
if args.animation:
sim_env.viewer = trajoptpy.GetViewer(sim_env.env)
if args.animation > 1 and os.path.isfile(
args.window_prop_file) and os.path.isfile(
args.camera_matrix_file):
print "loading window and camera properties"
window_prop = np.loadtxt(args.window_prop_file)
camera_matrix = np.loadtxt(args.camera_matrix_file)
try:
sim_env.viewer.SetWindowProp(*window_prop)
sim_env.viewer.SetCameraManipulatorMatrix(camera_matrix)
except:
print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
else:
print "move viewer to viewpoint that isn't stupid"
print "then hit 'p' to continue"
sim_env.viewer.Idle()
print "saving window and camera properties"
try:
window_prop = sim_env.viewer.GetWindowProp()
camera_matrix = sim_env.viewer.GetCameraManipulatorMatrix()
np.savetxt(args.window_prop_file, window_prop, fmt='%d')
np.savetxt(args.camera_matrix_file, camera_matrix)
except:
print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
def compute_dt_code(ctl_pts, plotting=False):
"""Takes rope control points (Nx3 array), closes the loop, and computes the Dowker-Thistlethwaite code for the knot.
The z-value for the points are used for determining over/undercrossings.
Follows procedure outlined here: http://katlas.math.toronto.edu/wiki/DT_(Dowker-Thistlethwaite)_Codes
"""
# First, close the loop by introducing extra points under the table and toward the robot (by subtracting z and x values)
# first_pt, last_pt = ctl_pts[0], ctl_pts[-1]
# flipped = False
# if first_pt[1] > last_pt[1]:
# first_pt, last_pt = last_pt, first_pt
# flipped = True
# min_z = ctl_pts[:,2].min()
# extra_first_pt, extra_last_pt = first_pt + [-.1, -.1, min_z-1], last_pt + [-.1, .1, min_z-1]
# if flipped:
# extra_pts = [extra_first_pt, extra_first_pt + [-1, 0, 0], extra_last_pt + [-1, 0, 0], extra_last_pt, last_pt]
# else:
# extra_pts = [extra_last_pt, extra_last_pt + [-1, 0, 0], extra_first_pt + [-1, 0, 0], extra_first_pt, first_pt]
# ctl_pts = np.append(ctl_pts, extra_pts, axis=0)
if plotting:
import trajoptpy, openravepy
env = openravepy.Environment()
viewer = trajoptpy.GetViewer(env)
handles = []
handles.append(env.plot3(ctl_pts, 5, [0, 0, 1]))
viewer.Idle()
# Upsampling loop: upsample until every segment has at most one crossing
need_upsample_ind = None
upsample_iters = 0
max_upsample_iters = 10
while True:
counter = 1
crossings = defaultdict(list)
# Walk along rope: for each segment, compute intersections with all other segments
for i in range(len(ctl_pts) - 1):
curr_seg = ctl_pts[i:i + 2, :]
intersections, ts, us = intersect_segs(ctl_pts[:, :2],
curr_seg[:, :2])
if len(intersections) == 0:
continue
if len(intersections) != 1:
LOG.debug(
'warning: more than one intersection for segment %d, now upsampling',
i)
need_upsample_ind = i
break
# for each intersection, determine and record over/undercrossing
i_int = intersections[0]
if plotting:
handles.append(
env.drawlinestrip(ctl_pts[i_int:i_int + 2], 5, [1, 0, 0]))
int_point_rope = ctl_pts[i_int] + ts[i_int] * (ctl_pts[i_int + 1] -
ctl_pts[i_int])
int_point_curr_seg = curr_seg[0] + us[i_int] * (curr_seg[1] -
curr_seg[0])
#assert np.allclose(int_point_rope[:2], int_point_curr_seg[:2])
above = int_point_curr_seg[2] > int_point_rope[2]
crossings[tuple(sorted(
(i, i_int)))].append(-counter if counter %
2 == 0 and above else counter)
counter += 1
if plotting: viewer.Idle()
# upsample if necessary
if need_upsample_ind is not None and upsample_iters < max_upsample_iters:
spacing = np.linspace(0, 1, len(ctl_pts))
new_spacing = np.insert(
spacing, need_upsample_ind + 1,
(spacing[need_upsample_ind] + spacing[need_upsample_ind + 1]) /
2.)
ctl_pts = math_utils.interp2d(new_spacing, spacing, ctl_pts)
upsample_iters += 1
need_upsample = None
continue
break
# Extract relevant part of crossing data to produce DT code
out = []
for pair in crossings.itervalues():
assert len(pair) == 2
odd = [p for p in pair if p % 2 == 1][0]
even = [p for p in pair if p % 2 == 0][0]
out.append((odd, even))
out.sort()
dt_code = [-o[1] for o in out]
return dt_code
