def execute(self,userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds)
ELOG.log('SelectTrajectory', 'xyz_new', xyz_new)
ELOG.log('SelectTrajectory', 'xyz_new_ds', xyz_new_ds)
ELOG.log('SelectTrajectory', 'dists_new', dists_new)
if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage]
else: candidate_demo_names = demos.keys()
global last_selected_segment
print 'Last selected segment:', last_selected_segment
if args.human_select_demo:
best_name = None
while best_name not in demos:
print 'Select demo from', demos.keys()
best_name = raw_input('> ')
else:
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
ELOG.log('SelectTrajectory', 'costs_names', costs_names)
_, best_name = min(costs_names)
ELOG.log('SelectTrajectory', 'best_name', best_name)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
last_selected_segment = best_name
xyz_demo_ds = best_demo["cloud_xyz_ds"]
ELOG.log('SelectTrajectory', 'xyz_demo_ds', xyz_demo_ds)
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
ELOG.log('SelectTrajectory', 'f', self.f)
else:
self.f, info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, return_full=True)#, interactive=True)
ELOG.log('SelectTrajectory', 'f', self.f)
ELOG.log('SelectTrajectory', 'f_info', info)
if args.use_nr:
rospy.loginfo('Using nonrigidity costs')
from lfd import tps
import scipy.spatial.distance as ssd
pts_grip = []
for lr in "lr":
if best_demo["arms_used"] in ["b", lr]:
pts_grip.extend(best_demo["%s_gripper_tool_frame"%lr]["position"])
pts_grip = np.array(pts_grip)
dist_to_rope = ssd.cdist(pts_grip, xyz_demo_ds).min(axis=1)
pts_grip_near_rope = pts_grip[dist_to_rope < .04,:]
pts_rigid = voxel_downsample(pts_grip_near_rope, .01)
self.f.lin_ag, self.f.trans_g, self.f.w_ng, self.f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
# if yes_or_no('dump warped demo?'):
# import pickle
# fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl'
# with open(fname, 'w') as f:
# pickle.dump(warped_demo, f)
# print 'saved to', fname
ELOG.log('SelectTrajectory', 'warped_demo', warped_demo)
def make_traj(warped_demo, inds=None, xyz_offset=0, feas_check_only=False):
traj = {}
total_feas_inds = 0
total_inds = 0
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
pos = warped_demo["%s_gripper_tool_frame"%lr]["position"] + xyz_offset
ori = warped_demo["%s_gripper_tool_frame"%lr]["orientation"]
if inds is not None:
pos, ori = pos[inds], ori[inds]
if feas_check_only:
feas_inds = lfd_traj.compute_feas_inds(
pos,
ori,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True)
traj["%s_arm_feas_inds"%lr] = feas_inds
else:
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
pos,
ori,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True)
traj["%s_arm"%lr] = arm_traj
traj["%s_arm_feas_inds"%lr] = feas_inds
total_feas_inds += len(feas_inds)
total_inds += len(pos)
rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(pos))
return traj, total_feas_inds, total_inds
# Check if we need to move the base for reachability
base_offset = np.array([0, 0, 0])
if args.use_base:
# First figure out how much we need to move the base to maximize feasible points
OFFSET = 0.1
XYZ_OFFSETS = np.array([[0., 0., 0.], [-OFFSET, 0, 0], [OFFSET, 0, 0], [0, -OFFSET, 0], [0, OFFSET, 0]])
inds_to_check = lfd_traj.where_near_rope(best_demo, xyz_demo_ds, add_other_points=30)
need_to_move_base = False
best_feas_inds, best_xyz_offset = -1, None
for xyz_offset in XYZ_OFFSETS:
_, n_feas_inds, n_total_inds = make_traj(warped_demo, inds=inds_to_check, xyz_offset=xyz_offset, feas_check_only=True)
rospy.loginfo('Cloud offset %s has feas inds %d', str(xyz_offset), n_feas_inds)
if n_feas_inds > best_feas_inds:
best_feas_inds, best_xyz_offset = n_feas_inds, xyz_offset
if n_feas_inds >= 0.99*n_total_inds: break
if np.linalg.norm(best_xyz_offset) > 0.01:
need_to_move_base = True
base_offset = -best_xyz_offset
rospy.loginfo('Best base offset: %s, with %d feas inds', str(base_offset), best_feas_inds)
# Move the base
if need_to_move_base:
rospy.loginfo('Will move base.')
userdata.base_offset = base_offset
return 'move_base'
else:
rospy.loginfo('Will not move base.')
Globals.pr2.update_rave()
# calculate joint trajectory using IK
trajectory = make_traj(warped_demo)[0]
# fill in gripper/grab stuff
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if len(trajectory["%s_arm_feas_inds"%lr]) == 0: return "failure"
trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07
trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr]
trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm"%lr],
Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
ignore_inds=[1] # ignore the 0--1 discontinuity, which is usually just moving from rest to the traj starting pose
)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST,
ns='warped_finger_traj'
))
# plot original trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST,
ns='demo_finger_traj'
))
ELOG.log('SelectTrajectory', 'trajectory', trajectory)
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"
def execute(self,userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0,_ = best_demo["cloud_xyz_ds"]
pts1,_ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0, pts1, plotting = 4, reg_init=1,reg_final=args.reg_final,n_iter=40)
else:
if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage]
else: candidate_demo_names = demos.keys()
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
_, best_name = min(costs_names)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
xyz_demo_ds = best_demo["cloud_xyz_ds"]
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
else:
self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False)#, interactive=True)
np.savez('registration_data', xyz_demo_ds=xyz_demo_ds, xyz_new_ds=xyz_new_ds)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
if yes_or_no('dump warped demo?'):
import pickle
fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(warped_demo, f)
print 'saved to', fname
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame"%lr]["position"],
warped_demo["%s_gripper_tool_frame"%lr]["orientation"],
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True
)
if len(feas_inds) == 0: return "failure"
trajectory["%s_arm"%lr] = arm_traj
trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07
trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr]
trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm"%lr],
Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr
)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST,
ns='warped_finger_traj'
))
# plot original trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST,
ns='demo_finger_traj'
))
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"
def execute(self, userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(
xyz_new, xyz_new_ds, ds_inds)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0, _ = best_demo["cloud_xyz_ds"]
pts1, _ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0,
pts1,
plotting=4,
reg_init=1,
reg_final=args.reg_final,
n_iter=40)
else:
if args.count_steps:
candidate_demo_names = self.count2segnames[Globals.stage]
else:
candidate_demo_names = demos.keys()
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(
parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds,
dists_new)
for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
_, best_name = min(costs_names)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
xyz_demo_ds = best_demo["cloud_xyz_ds"]
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
else:
self.f = registration.tps_rpm(xyz_demo_ds,
xyz_new_ds,
plotting=20,
reg_init=1,
reg_final=.01,
n_iter=n_iter,
verbose=False) #, interactive=True)
np.savez('registration_data',
xyz_demo_ds=xyz_demo_ds,
xyz_new_ds=xyz_new_ds)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points,
mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
if yes_or_no('dump warped demo?'):
import pickle
fname = '/tmp/warped_demo_' + str(
np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(warped_demo, f)
print 'saved to', fname
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(
warped_demo["l_gripper_tool_frame"]["position"][:, 2],
Globals.table_height + .032, np.inf)
clipinplace(
warped_demo["r_gripper_tool_frame"]["position"][:, 2],
Globals.table_height + .032, np.inf)
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame" % lr]["position"],
warped_demo["%s_gripper_tool_frame" % lr]["orientation"],
Globals.pr2.robot.GetManipulator("%sarm" % leftright),
"%s_gripper_tool_frame" % lr,
check_collisions=True)
if len(feas_inds) == 0: return "failure"
trajectory["%s_arm" % lr] = arm_traj
trajectory["%s_grab" %
lr] = best_demo["%s_gripper_joint" % lr] < .07
trajectory["%s_gripper" %
lr] = warped_demo["%s_gripper_joint" % lr]
trajectory["%s_gripper" % lr][trajectory["%s_grab" % lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory[
"%s_arm" %
lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm" % lr], Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm" % leftright),
"%s_gripper_tool_frame" % lr)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm" % other_lr] = lfd_traj.fill_stationary(
trajectory["%s_arm" % other_lr], discont_times,
n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab" %
tmp_lr] = lfd_traj.fill_stationary(
trajectory["%s_grab" % tmp_lr],
discont_times, n_steps)
trajectory["%s_gripper" %
tmp_lr] = lfd_traj.fill_stationary(
trajectory["%s_gripper" % tmp_lr],
discont_times, n_steps)
trajectory["%s_gripper" %
tmp_lr][trajectory["%s_grab" % tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(
Globals.rviz.draw_curve(conversions.array_to_pose_array(
alternate(
warped_demo["%s_gripper_l_finger_tip_link" %
lr]["position"],
warped_demo["%s_gripper_r_finger_tip_link" %
lr]["position"]), "base_footprint"),
width=.001,
rgba=(1, 0, 1, .4),
type=Marker.LINE_LIST,
ns='warped_finger_traj'))
# plot original trajectory
Globals.handles.append(
Globals.rviz.draw_curve(conversions.array_to_pose_array(
alternate(
best_demo["%s_gripper_l_finger_tip_link" %
lr]["position"],
best_demo["%s_gripper_r_finger_tip_link" %
lr]["position"]), "base_footprint"),
width=.001,
rgba=(0, 1, 1, .4),
type=Marker.LINE_LIST,
ns='demo_finger_traj'))
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"