def sim_full_traj_maybesim(sim_env, full_traj, animate=False, interactive=False):
def sim_callback(i):
sim_env.sim.step()
animate_speed = 10 if animate else 0
traj, dof_inds = full_traj
# make the trajectory slow enough for the simulation
traj = ropesim.retime_traj(sim_env.robot, dof_inds, traj)
# in simulation mode, we must make sure to gradually move to the new starting position
sim_env.robot.SetActiveDOFs(dof_inds)
curr_vals = sim_env.robot.GetActiveDOFValues()
transition_traj = np.r_[[curr_vals], [traj[0]]]
unwrap_in_place(transition_traj)
transition_traj = ropesim.retime_traj(sim_env.robot, dof_inds, transition_traj, max_cart_vel=.05)
animate_traj.animate_traj(transition_traj, sim_env.robot, restore=False, pause=interactive,
callback=sim_callback, step_viewer=animate_speed)
traj[0] = transition_traj[-1]
unwrap_in_place(traj)
animate_traj.animate_traj(traj, sim_env.robot, restore=False, pause=interactive,
callback=sim_callback, step_viewer=animate_speed)
if sim_env.viewer:
sim_env.viewer.Step()
return True
def exec_traj_sim(bodypart2traj, animate):
def sim_callback(i):
Globals.sim.step()
dof_inds = []
trajs = []
for (part_name, traj) in bodypart2traj.items():
manip_name = {"larm": "leftarm", "rarm": "rightarm"}[part_name]
dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())
trajs.append(traj)
full_traj = np.concatenate(trajs, axis=1)
Globals.robot.SetActiveDOFs(dof_inds)
# make the trajectory slow enough for the simulation
#orig full_traj = ropesim.retime_traj(Globals.robot, dof_inds, full_traj)
full_traj = ropesim.retime_traj(Globals.robot, dof_inds, full_traj, max_cart_vel=.01)
# in simulation mode, we must make sure to gradually move to the new starting position
curr_vals = Globals.robot.GetActiveDOFValues()
transition_traj = np.r_[[curr_vals], [full_traj[0]]]
unwrap_in_place(transition_traj)
transition_traj = ropesim.retime_traj(Globals.robot, dof_inds, transition_traj, max_cart_vel=.05)
#transition_traj = ropesim.retime_traj(Globals.robot, dof_inds, transition_traj, max_cart_vel=.005)
animate_traj.animate_traj(transition_traj, Globals.robot, restore=False, pause=False,
callback=sim_callback, step_viewer=animate)
full_traj[0] = transition_traj[-1]
unwrap_in_place(full_traj)
animate_traj.animate_traj(full_traj, Globals.robot, restore=False, pause=False,
callback=sim_callback, step_viewer=animate)
return True
def exec_traj_maybesim(bodypart2traj):
if args.animation:
dof_inds = []
trajs = []
for (part_name, traj) in bodypart2traj.items():
manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name]
dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())
trajs.append(traj)
full_traj = np.concatenate(trajs, axis=1)
Globals.robot.SetActiveDOFs(dof_inds)
animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True)
if args.execution:
pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj)
def exec_traj_maybesim(bodypart2traj):
if args.animation:
dof_inds = []
trajs = []
for (part_name, traj) in bodypart2traj.items():
manip_name = {"larm": "leftarm", "rarm": "rightarm"}[part_name]
dof_inds.extend(
Globals.robot.GetManipulator(manip_name).GetArmIndices())
trajs.append(traj)
full_traj = np.concatenate(trajs, axis=1)
Globals.robot.SetActiveDOFs(dof_inds)
animate_traj.animate_traj(full_traj,
Globals.robot,
restore=False,
pause=True)
if args.execution:
pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj)
def sim_full_traj_maybesim(sim_env,
full_traj,
animate=False,
interactive=False):
def sim_callback(i):
sim_env.sim.step()
animate_speed = 10 if animate else 0
traj, dof_inds = full_traj
# make the trajectory slow enough for the simulation
traj = ropesim.retime_traj(sim_env.robot, dof_inds, traj)
# in simulation mode, we must make sure to gradually move to the new starting position
sim_env.robot.SetActiveDOFs(dof_inds)
curr_vals = sim_env.robot.GetActiveDOFValues()
transition_traj = np.r_[[curr_vals], [traj[0]]]
unwrap_in_place(transition_traj)
transition_traj = ropesim.retime_traj(sim_env.robot,
dof_inds,
transition_traj,
max_cart_vel=.05)
animate_traj.animate_traj(transition_traj,
sim_env.robot,
restore=False,
pause=interactive,
callback=sim_callback,
step_viewer=animate_speed)
traj[0] = transition_traj[-1]
unwrap_in_place(traj)
animate_traj.animate_traj(traj,
sim_env.robot,
restore=False,
pause=interactive,
callback=sim_callback,
step_viewer=animate_speed)
if sim_env.viewer:
sim_env.viewer.Step()
return True
def exec_traj_maybesim(bodypart2traj, speed_factor=0.5):
if args.animation:
"""
dof_inds = []
trajs = []
for (part_name, traj) in bodypart2traj.items():
manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name]
dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())
trajs.append(traj)
full_traj = np.concatenate(trajs, axis=1)
Globals.robot.SetActiveDOFs(dof_inds)
animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True)
"""
name2part = {"lgrip":Globals.pr2.lgrip,
"rgrip":Globals.pr2.rgrip,
"larm":Globals.pr2.larm,
"rarm":Globals.pr2.rarm,
"base":Globals.pr2.base}
dof_inds = []
trajs = []
vel_limits = []
for (part_name, traj) in bodypart2traj.items():
manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name]
vel_limits.extend(name2part[part_name].vel_limits)
dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())
if traj.ndim == 1: traj = traj.reshape(-1,1)
trajs.append(traj)
trajectories = np.concatenate(trajs, 1)
print trajectories.shape
times = retiming.retime_with_vel_limits(trajectories, np.array(vel_limits))
times_up = np.linspace(0, times[-1], int(np.ceil(times[-1]/.1)))
full_traj = mu.interp2d(times_up, times, trajectories)
print full_traj.shape
Globals.robot.SetActiveDOFs(dof_inds)
animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True)
#return True
if args.execution:
pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj, speed_factor=speed_factor)
return True
rave_traj = [
r2r.convert(row)
for row in asarray(seg_info["joint_states"]["position"])
]
robot.SetActiveDOFs(r2r.rave_inds)
robot.SetActiveDOFValues(rave_traj[0])
handles = []
T_w_k = berkeley_pr2.get_kinect_transform(robot)
o = T_w_k[:3, 3]
x = T_w_k[:3, 0]
y = T_w_k[:3, 1]
z = T_w_k[:3, 2]
handles.append(env.drawarrow(o, o + .3 * x, .005, (1, 0, 0, 1)))
handles.append(env.drawarrow(o, o + .3 * y, .005, (0, 1, 0, 1)))
handles.append(env.drawarrow(o, o + .3 * z, .005, (0, 0, 1, 1)))
XYZ_k = clouds.depth_to_xyz(np.asarray(seg_info["depth"]), berkeley_pr2.f)
Twk = asarray(seg_info["T_w_k"])
XYZ_w = XYZ_k.dot(Twk[:3, :3].T) + Twk[:3, 3][None, None, :]
RGB = np.asarray(seg_info["rgb"])
handles.append(
env.plot3(XYZ_w.reshape(-1, 3), 2,
RGB.reshape(-1, 3)[:, ::-1] / 255.))
animate_traj.animate_traj(rave_traj, robot, pause=not args.nopause)
print "DONE"
trajoptpy.GetViewer(env).Idle()
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])
handles = []
T_w_k = berkeley_pr2.get_kinect_transform(robot)
o = T_w_k[:3,3]
x = T_w_k[:3,0]
y = T_w_k[:3,1]
z = T_w_k[:3,2]
handles.append(env.drawarrow(o, o+.3*x, .005,(1,0,0,1)))
handles.append(env.drawarrow(o, o+.3*y, .005,(0,1,0,1)))
handles.append(env.drawarrow(o, o+.3*z, .005,(0,0,1,1)))
cloud_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(env.plot3(cloud_xyz,5))
animate_traj.animate_traj(rave_traj, robot, pause = not args.nopause)
print "DONE"
trajoptpy.GetViewer(env).Idle()
