def transfer(self, reg, demo, plotting=False):
handles = []
if plotting:
demo_cloud = demo.scene_state.cloud
test_cloud = reg.test_scene_state.cloud
demo_color = demo.scene_state.color
test_color = reg.test_scene_state.color
handles.append(self.sim.env.plot3(demo_cloud[:,:3], 2, demo_color if demo_color is not None else (1,0,0)))
handles.append(self.sim.env.plot3(test_cloud[:,:3], 2, test_color if test_color is not None else (0,0,1)))
self.sim.viewer.Step()
active_lr = ""
for lr in 'lr':
if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved(demo.aug_traj.lr2arm_traj[lr]):
active_lr += lr
_, timesteps_rs = sim_util.unif_resample(np.c_[(1./settings.JOINT_LENGTH_PER_STEP) * np.concatenate([demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1),
(1./settings.FINGER_CLOSE_RATE) * np.concatenate([demo.aug_traj.lr2finger_traj[lr] for lr in active_lr], axis=1)],
1.)
demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs)
if self.init_trajectory_transferer:
warm_init_traj = self.init_trajectory_transferer.transfer(reg, demo, plotting=plotting)
manip_name = ""
ee_link_names = []
transformed_ee_trajs_rs = []
init_traj = np.zeros((len(timesteps_rs),0))
for lr in active_lr:
arm_name = {"l":"leftarm", "r":"rightarm"}[lr]
ee_link_name = "%s_gripper_tool_frame"%lr
if manip_name:
manip_name += "+"
manip_name += arm_name
ee_link_names.append(ee_link_name)
if self.init_trajectory_transferer:
init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr]]
else:
init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr]]
transformed_ee_traj_rs = reg.f.transform_hmats(demo_aug_traj_rs.lr2ee_traj[lr])
transformed_ee_trajs_rs.append(transformed_ee_traj_rs)
if plotting:
handles.append(self.sim.env.drawlinestrip(demo.aug_traj.lr2ee_traj[lr][:,:3,3], 2, (1,0,0)))
handles.append(self.sim.env.drawlinestrip(demo_aug_traj_rs.lr2ee_traj[lr][:,:3,3], 2, (1,1,0)))
handles.append(self.sim.env.drawlinestrip(transformed_ee_traj_rs[:,:3,3], 2, (0,1,0)))
self.sim.viewer.Step()
if not self.init_trajectory_transferer:
# modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do)
dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name)
joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint"%lr)
init_traj[:,dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits([joint_ind])[0][0]
print "planning pose trajectory following"
test_traj, obj_value, pose_errs = planning.plan_follow_trajs(self.sim.robot, manip_name, ee_link_names, transformed_ee_trajs_rs, init_traj,
start_fixed=False,
use_collision_cost=self.use_collision_cost,
beta_pos=self.beta_pos, beta_rot=self.beta_rot)
# the finger trajectory is the same for the demo and the test trajectory
for lr in active_lr:
finger_name = "%s_gripper_l_finger_joint"%lr
manip_name += "+" + finger_name
test_traj = np.c_[test_traj, demo_aug_traj_rs.lr2finger_traj[lr]]
full_traj = (test_traj, sim_util.dof_inds_from_name(self.sim.robot, manip_name))
test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj(self.sim.robot, full_traj, lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj, lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj)
if plotting:
for lr in active_lr:
handles.append(self.sim.env.drawlinestrip(test_aug_traj.lr2ee_traj[lr][:,:3,3], 2, (0,0,1)))
self.sim.viewer.Step()
return test_aug_traj
def transfer(self, demo, test_scene_state, callback=None, plotting=False):
reg = self.registration_factory.register(demo,
test_scene_state,
callback=callback)
handles = []
if plotting:
demo_cloud = demo.scene_state.cloud
test_cloud = reg.test_scene_state.cloud
demo_color = demo.scene_state.color
test_color = reg.test_scene_state.color
handles.append(
self.sim.env.plot3(
demo_cloud[:, :3], 2,
demo_color if demo_color is not None else (1, 0, 0)))
handles.append(
self.sim.env.plot3(
test_cloud[:, :3], 2,
test_color if test_color is not None else (0, 0, 1)))
self.sim.viewer.Step()
active_lr = ""
for lr in 'lr':
if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved(
demo.aug_traj.lr2arm_traj[lr]):
active_lr += lr
_, timesteps_rs = sim_util.unif_resample(
np.c_[(1. / settings.JOINT_LENGTH_PER_STEP) * np.concatenate(
[demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1
), (1. / settings.FINGER_CLOSE_RATE) * np.concatenate(
[demo.aug_traj.lr2finger_traj[lr]
for lr in active_lr], axis=1)], 1.)
demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs)
if self.init_trajectory_transferer:
warm_init_traj = self.init_trajectory_transferer.transfer(
reg, demo, plotting=plotting)
manip_name = ""
flr2finger_link_names = []
flr2demo_finger_pts_trajs_rs = []
init_traj = np.zeros((len(timesteps_rs), 0))
for lr in active_lr:
arm_name = {"l": "leftarm", "r": "rightarm"}[lr]
finger_name = "%s_gripper_l_finger_joint" % lr
if manip_name:
manip_name += "+"
manip_name += arm_name + "+" + finger_name
if self.init_trajectory_transferer:
init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr],
warm_init_traj.lr2finger_traj[lr]]
else:
init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr],
demo_aug_traj_rs.lr2finger_traj[lr]]
if plotting:
handles.append(
self.sim.env.drawlinestrip(
demo.aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (1, 0, 0)))
handles.append(
self.sim.env.drawlinestrip(
demo_aug_traj_rs.lr2ee_traj[lr][:, :3, 3], 2,
(1, 1, 0)))
transformed_ee_traj_rs = reg.f.transform_hmats(
demo_aug_traj_rs.lr2ee_traj[lr])
handles.append(
self.sim.env.drawlinestrip(
transformed_ee_traj_rs[:, :3, 3], 2, (0, 1, 0)))
self.sim.viewer.Step()
flr2demo_finger_pts_traj_rs = sim_util.get_finger_pts_traj(
self.sim.robot, lr, (demo_aug_traj_rs.lr2ee_traj[lr],
demo_aug_traj_rs.lr2finger_traj[lr]))
flr2demo_finger_pts_trajs_rs.append(flr2demo_finger_pts_traj_rs)
flr2transformed_finger_pts_traj_rs = {}
flr2finger_link_name = {}
flr2finger_rel_pts = {}
for finger_lr in 'lr':
flr2transformed_finger_pts_traj_rs[
finger_lr] = reg.f.transform_points(
np.concatenate(flr2demo_finger_pts_traj_rs[finger_lr],
axis=0)).reshape((-1, 4, 3))
flr2finger_link_name[
finger_lr] = "%s_gripper_%s_finger_tip_link" % (lr,
finger_lr)
flr2finger_rel_pts[finger_lr] = sim_util.get_finger_rel_pts(
finger_lr)
flr2finger_link_names.append(flr2finger_link_name)
if plotting:
handles.extend(
sim_util.draw_finger_pts_traj(self.sim,
flr2demo_finger_pts_traj_rs,
(1, 1, 0)))
handles.extend(
sim_util.draw_finger_pts_traj(
self.sim, flr2transformed_finger_pts_traj_rs,
(0, 1, 0)))
self.sim.viewer.Step()
if not self.init_trajectory_transferer:
# modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do)
dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name)
joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint" %
lr)
init_traj[:,
dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits(
[joint_ind])[0][0]
print "planning joint TPS and finger points trajectory following"
test_traj, obj_value, tps_rel_pts_costs, tps_cost = planning.joint_fit_tps_follow_finger_pts_trajs(
self.sim.robot,
manip_name,
flr2finger_link_names,
flr2finger_rel_pts,
flr2demo_finger_pts_trajs_rs,
init_traj,
reg.f,
use_collision_cost=self.use_collision_cost,
start_fixed=False,
alpha=self.alpha,
beta_pos=self.beta_pos,
gamma=self.gamma)
full_traj = (test_traj,
sim_util.dof_inds_from_name(self.sim.robot, manip_name))
test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj(
self.sim.robot,
full_traj,
lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj,
lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj)
if plotting:
for lr in active_lr:
flr2new_transformed_finger_pts_traj_rs = {}
for finger_lr in 'lr':
flr2new_transformed_finger_pts_traj_rs[
finger_lr] = reg.f.transform_points(
np.concatenate(
flr2demo_finger_pts_traj_rs[finger_lr],
axis=0)).reshape((-1, 4, 3))
handles.extend(
sim_util.draw_finger_pts_traj(
self.sim, flr2new_transformed_finger_pts_traj_rs,
(0, 1, 1)))
handles.append(
self.sim.env.drawlinestrip(
test_aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (0, 0, 1)))
flr2test_finger_pts_traj = sim_util.get_finger_pts_traj(
self.sim.robot, lr, full_traj)
handles.extend(
sim_util.draw_finger_pts_traj(self.sim,
flr2test_finger_pts_traj,
(0, 0, 1)))
self.sim.viewer.Step()
return test_aug_traj
def transfer(self, demo, test_scene_state, callback=None, plotting=False):
reg = self.registration_factory.register(demo, test_scene_state, callback=callback)
handles = []
if plotting:
demo_cloud = demo.scene_state.cloud
test_cloud = reg.test_scene_state.cloud
demo_color = demo.scene_state.color
test_color = reg.test_scene_state.color
handles.append(self.sim.env.plot3(demo_cloud[:,:3], 2, demo_color if demo_color is not None else (1,0,0)))
handles.append(self.sim.env.plot3(test_cloud[:,:3], 2, test_color if test_color is not None else (0,0,1)))
self.sim.viewer.Step()
active_lr = ""
for lr in 'lr':
if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved(demo.aug_traj.lr2arm_traj[lr]):
active_lr += lr
_, timesteps_rs = sim_util.unif_resample(np.c_[(1./settings.JOINT_LENGTH_PER_STEP) * np.concatenate([demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1),
(1./settings.FINGER_CLOSE_RATE) * np.concatenate([demo.aug_traj.lr2finger_traj[lr] for lr in active_lr], axis=1)],
1.)
demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs)
if self.init_trajectory_transferer:
warm_init_traj = self.init_trajectory_transferer.transfer(reg, demo, plotting=plotting)
manip_name = ""
flr2finger_link_names = []
flr2demo_finger_pts_trajs_rs = []
init_traj = np.zeros((len(timesteps_rs),0))
for lr in active_lr:
arm_name = {"l":"leftarm", "r":"rightarm"}[lr]
finger_name = "%s_gripper_l_finger_joint"%lr
if manip_name:
manip_name += "+"
manip_name += arm_name + "+" + finger_name
if self.init_trajectory_transferer:
init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr], warm_init_traj.lr2finger_traj[lr]]
else:
init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr], demo_aug_traj_rs.lr2finger_traj[lr]]
if plotting:
handles.append(self.sim.env.drawlinestrip(demo.aug_traj.lr2ee_traj[lr][:,:3,3], 2, (1,0,0)))
handles.append(self.sim.env.drawlinestrip(demo_aug_traj_rs.lr2ee_traj[lr][:,:3,3], 2, (1,1,0)))
transformed_ee_traj_rs = reg.f.transform_hmats(demo_aug_traj_rs.lr2ee_traj[lr])
handles.append(self.sim.env.drawlinestrip(transformed_ee_traj_rs[:,:3,3], 2, (0,1,0)))
self.sim.viewer.Step()
flr2demo_finger_pts_traj_rs = sim_util.get_finger_pts_traj(self.sim.robot, lr, (demo_aug_traj_rs.lr2ee_traj[lr], demo_aug_traj_rs.lr2finger_traj[lr]))
flr2demo_finger_pts_trajs_rs.append(flr2demo_finger_pts_traj_rs)
flr2transformed_finger_pts_traj_rs = {}
flr2finger_link_name = {}
flr2finger_rel_pts = {}
for finger_lr in 'lr':
flr2transformed_finger_pts_traj_rs[finger_lr] = reg.f.transform_points(np.concatenate(flr2demo_finger_pts_traj_rs[finger_lr], axis=0)).reshape((-1,4,3))
flr2finger_link_name[finger_lr] = "%s_gripper_%s_finger_tip_link"%(lr,finger_lr)
flr2finger_rel_pts[finger_lr] = sim_util.get_finger_rel_pts(finger_lr)
flr2finger_link_names.append(flr2finger_link_name)
if plotting:
handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2demo_finger_pts_traj_rs, (1,1,0)))
handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2transformed_finger_pts_traj_rs, (0,1,0)))
self.sim.viewer.Step()
if not self.init_trajectory_transferer:
# modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do)
dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name)
joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint"%lr)
init_traj[:,dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits([joint_ind])[0][0]
print "planning joint TPS and finger points trajectory following"
test_traj, obj_value, tps_rel_pts_costs, tps_cost = planning.joint_fit_tps_follow_finger_pts_trajs(self.sim.robot, manip_name,
flr2finger_link_names, flr2finger_rel_pts,
flr2demo_finger_pts_trajs_rs, init_traj,
reg.f,
use_collision_cost=self.use_collision_cost,
start_fixed=False,
alpha=self.alpha, beta_pos=self.beta_pos, gamma=self.gamma)
full_traj = (test_traj, sim_util.dof_inds_from_name(self.sim.robot, manip_name))
test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj(self.sim.robot, full_traj, lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj, lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj)
if plotting:
for lr in active_lr:
flr2new_transformed_finger_pts_traj_rs = {}
for finger_lr in 'lr':
flr2new_transformed_finger_pts_traj_rs[finger_lr] = reg.f.transform_points(np.concatenate(flr2demo_finger_pts_traj_rs[finger_lr], axis=0)).reshape((-1,4,3))
handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2new_transformed_finger_pts_traj_rs, (0,1,1)))
handles.append(self.sim.env.drawlinestrip(test_aug_traj.lr2ee_traj[lr][:,:3,3], 2, (0,0,1)))
flr2test_finger_pts_traj = sim_util.get_finger_pts_traj(self.sim.robot, lr, full_traj)
handles.extend(sim_util.draw_finger_pts_traj(self.sim, flr2test_finger_pts_traj, (0,0,1)))
self.sim.viewer.Step()
return test_aug_traj
def transfer(self, reg, demo, plotting=False):
handles = []
if plotting:
demo_cloud = demo.scene_state.cloud
test_cloud = reg.test_scene_state.cloud
demo_color = demo.scene_state.color
test_color = reg.test_scene_state.color
handles.append(
self.sim.env.plot3(
demo_cloud[:, :3], 2,
demo_color if demo_color is not None else (1, 0, 0)))
handles.append(
self.sim.env.plot3(
test_cloud[:, :3], 2,
test_color if test_color is not None else (0, 0, 1)))
self.sim.viewer.Step()
active_lr = ""
for lr in 'lr':
if lr in demo.aug_traj.lr2arm_traj and sim_util.arm_moved(
demo.aug_traj.lr2arm_traj[lr]):
active_lr += lr
_, timesteps_rs = sim_util.unif_resample(
np.c_[(1. / settings.JOINT_LENGTH_PER_STEP) * np.concatenate(
[demo.aug_traj.lr2arm_traj[lr] for lr in active_lr], axis=1
), (1. / settings.FINGER_CLOSE_RATE) * np.concatenate(
[demo.aug_traj.lr2finger_traj[lr]
for lr in active_lr], axis=1)], 1.)
demo_aug_traj_rs = demo.aug_traj.get_resampled_traj(timesteps_rs)
if self.init_trajectory_transferer:
warm_init_traj = self.init_trajectory_transferer.transfer(
reg, demo, plotting=plotting)
manip_name = ""
ee_link_names = []
transformed_ee_trajs_rs = []
init_traj = np.zeros((len(timesteps_rs), 0))
for lr in active_lr:
arm_name = {"l": "leftarm", "r": "rightarm"}[lr]
ee_link_name = "%s_gripper_tool_frame" % lr
if manip_name:
manip_name += "+"
manip_name += arm_name
ee_link_names.append(ee_link_name)
if self.init_trajectory_transferer:
init_traj = np.c_[init_traj, warm_init_traj.lr2arm_traj[lr]]
else:
init_traj = np.c_[init_traj, demo_aug_traj_rs.lr2arm_traj[lr]]
transformed_ee_traj_rs = reg.f.transform_hmats(
demo_aug_traj_rs.lr2ee_traj[lr])
transformed_ee_trajs_rs.append(transformed_ee_traj_rs)
if plotting:
handles.append(
self.sim.env.drawlinestrip(
demo.aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (1, 0, 0)))
handles.append(
self.sim.env.drawlinestrip(
demo_aug_traj_rs.lr2ee_traj[lr][:, :3, 3], 2,
(1, 1, 0)))
handles.append(
self.sim.env.drawlinestrip(
transformed_ee_traj_rs[:, :3, 3], 2, (0, 1, 0)))
self.sim.viewer.Step()
if not self.init_trajectory_transferer:
# modify the shoulder joint angle of init_traj to be the limit (highest arm) because this usually gives a better local optima (but this might not be the right thing to do)
dof_inds = sim_util.dof_inds_from_name(self.sim.robot, manip_name)
joint_ind = self.sim.robot.GetJointIndex("%s_shoulder_lift_joint" %
lr)
init_traj[:,
dof_inds.index(joint_ind)] = self.sim.robot.GetDOFLimits(
[joint_ind])[0][0]
print "planning pose trajectory following"
test_traj, obj_value, pose_errs = planning.plan_follow_trajs(
self.sim.robot,
manip_name,
ee_link_names,
transformed_ee_trajs_rs,
init_traj,
start_fixed=False,
use_collision_cost=self.use_collision_cost,
beta_pos=self.beta_pos,
beta_rot=self.beta_rot)
# the finger trajectory is the same for the demo and the test trajectory
for lr in active_lr:
finger_name = "%s_gripper_l_finger_joint" % lr
manip_name += "+" + finger_name
test_traj = np.c_[test_traj, demo_aug_traj_rs.lr2finger_traj[lr]]
full_traj = (test_traj,
sim_util.dof_inds_from_name(self.sim.robot, manip_name))
test_aug_traj = demonstration.AugmentedTrajectory.create_from_full_traj(
self.sim.robot,
full_traj,
lr2open_finger_traj=demo_aug_traj_rs.lr2open_finger_traj,
lr2close_finger_traj=demo_aug_traj_rs.lr2close_finger_traj)
if plotting:
for lr in active_lr:
handles.append(
self.sim.env.drawlinestrip(
test_aug_traj.lr2ee_traj[lr][:, :3, 3], 2, (0, 0, 1)))
self.sim.viewer.Step()
return test_aug_traj
