def get_resampled_traj(self, timesteps_rs):
lr2arm_traj_rs = None if self.lr2arm_traj is None else {}
lr2finger_traj_rs = None if self.lr2finger_traj is None else {}
for (lr2traj_rs, self_lr2traj) in [(lr2arm_traj_rs, self.lr2arm_traj), (lr2finger_traj_rs, self.lr2finger_traj)]:
if self_lr2traj is None:
continue
for lr in self_lr2traj.keys():
lr2traj_rs[lr] = mu.interp2d(timesteps_rs, np.arange(len(self_lr2traj[lr])), self_lr2traj[lr])
if self.lr2ee_traj is None:
lr2ee_traj_rs = None
else:
lr2ee_traj_rs = {}
for lr in self.lr2ee_traj.keys():
lr2ee_traj_rs[lr] = np.asarray(resampling.interp_hmats(timesteps_rs, np.arange(len(self.lr2ee_traj[lr])), self.lr2ee_traj[lr]))
lr2open_finger_traj_rs = None if self.lr2open_finger_traj is None else {}
lr2close_finger_traj_rs = None if self.lr2close_finger_traj is None else {}
for (lr2oc_finger_traj_rs, self_lr2oc_finger_traj) in [(lr2open_finger_traj_rs, self.lr2open_finger_traj), (lr2close_finger_traj_rs, self.lr2close_finger_traj)]:
if self_lr2oc_finger_traj is None:
continue
for lr in self_lr2oc_finger_traj.keys():
self_oc_finger_traj = self_lr2oc_finger_traj[lr]
self_oc_inds = np.where(self_oc_finger_traj)[0]
oc_inds_rs = np.abs(timesteps_rs[:,None] - self_oc_inds[None,:]).argmin(axis=0)
oc_finger_traj_rs = np.zeros(len(timesteps_rs), dtype=bool)
oc_finger_traj_rs[oc_inds_rs] = True
lr2oc_finger_traj_rs[lr] = oc_finger_traj_rs
return AugmentedTrajectory(lr2arm_traj=lr2arm_traj_rs, lr2finger_traj=lr2finger_traj_rs, lr2ee_traj=lr2ee_traj_rs, lr2open_finger_traj=lr2open_finger_traj_rs, lr2close_finger_traj=lr2close_finger_traj_rs)
def get_move_traj(t_start, t_end, R_start, R_end, start_fixed):
hmat_start = np.r_[np.c_[R_start, t_start], np.c_[0,0,0,1]]
hmat_end = np.r_[np.c_[R_end, t_end], np.c_[0,0,0,1]]
new_hmats = np.asarray(resampling.interp_hmats(np.arange(n_steps), np.r_[0, n_steps-1], [hmat_start, hmat_end]))
dof_vals = sim.robot.GetManipulator(manip_name).GetArmDOFValues()
old_traj = np.tile(dof_vals, (n_steps,1))
traj, _, _ = planning.plan_follow_traj(sim.robot, manip_name, ee_link, new_hmats, old_traj, start_fixed=start_fixed, beta_rot=10000.0)
return traj
def get_resampled_traj(self, timesteps_rs):
lr2arm_traj_rs = None if self.lr2arm_traj is None else {}
lr2finger_traj_rs = None if self.lr2finger_traj is None else {}
for (lr2traj_rs, self_lr2traj) in [(lr2arm_traj_rs, self.lr2arm_traj),
(lr2finger_traj_rs,
self.lr2finger_traj)]:
if self_lr2traj is None:
continue
for lr in self_lr2traj.keys():
lr2traj_rs[lr] = mu.interp2d(timesteps_rs,
np.arange(len(self_lr2traj[lr])),
self_lr2traj[lr])
if self.lr2ee_traj is None:
lr2ee_traj_rs = None
else:
lr2ee_traj_rs = {}
for lr in self.lr2ee_traj.keys():
lr2ee_traj_rs[lr] = np.asarray(
resampling.interp_hmats(
timesteps_rs, np.arange(len(self.lr2ee_traj[lr])),
self.lr2ee_traj[lr]))
lr2open_finger_traj_rs = None if self.lr2open_finger_traj is None else {}
lr2close_finger_traj_rs = None if self.lr2close_finger_traj is None else {}
for (lr2oc_finger_traj_rs, self_lr2oc_finger_traj) in [
(lr2open_finger_traj_rs, self.lr2open_finger_traj),
(lr2close_finger_traj_rs, self.lr2close_finger_traj)
]:
if self_lr2oc_finger_traj is None:
continue
for lr in self_lr2oc_finger_traj.keys():
self_oc_finger_traj = self_lr2oc_finger_traj[lr]
self_oc_inds = np.where(self_oc_finger_traj)[0]
oc_inds_rs = np.abs(timesteps_rs[:, None] -
self_oc_inds[None, :]).argmin(axis=0)
oc_finger_traj_rs = np.zeros(len(timesteps_rs), dtype=bool)
oc_finger_traj_rs[oc_inds_rs] = True
lr2oc_finger_traj_rs[lr] = oc_finger_traj_rs
return AugmentedTrajectory(
lr2arm_traj=lr2arm_traj_rs,
lr2finger_traj=lr2finger_traj_rs,
lr2ee_traj=lr2ee_traj_rs,
lr2open_finger_traj=lr2open_finger_traj_rs,
lr2close_finger_traj=lr2close_finger_traj_rs)
def get_move_traj(t_start, t_end, start_fixed):
hmat_start = np.r_[np.c_[R, t_start], np.c_[0, 0, 0, 1]]
hmat_end = np.r_[np.c_[R, t_end], np.c_[0, 0, 0, 1]]
new_hmats = np.asarray(
resampling.interp_hmats(np.arange(n_steps), np.r_[0, n_steps - 1],
[hmat_start, hmat_end]))
dof_vals = sim.robot.GetManipulator(manip_name).GetArmDOFValues()
old_traj = np.tile(dof_vals, (n_steps, 1))
traj, _, _ = planning.plan_follow_traj(sim.robot,
manip_name,
ee_link,
new_hmats,
old_traj,
start_fixed=start_fixed,
beta_rot=10000.0)
return traj
def plan_full_traj(robot, hmat_start, hmat_end, start_fixed, n_steps=10, \
lr='r', beta_rot=1000.0, grasp_cup=False, R=0.02, D=0.035, \
cup_xyz=None):
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
ee_link_name = "%s_gripper_tool_frame" % lr
ee_link = robot.GetLink(ee_link_name)
new_hmats = np.asarray(resampling.interp_hmats(np.arange(n_steps), \
np.r_[0, n_steps-1], [hmat_start, hmat_end]))
dof_vals = robot.GetDOFValues(
robot.GetManipulator(manip_name).GetArmIndices())
old_traj = np.tile(dof_vals, (n_steps, 1))
pose_weights = np.logspace(0, 1, num=n_steps, base=10) / 10
# pose_costs = (rotation squared err, position squared error)
traj, _, pose_costs = \
planning.plan_follow_traj(robot, manip_name, ee_link, new_hmats,
old_traj, start_fixed=start_fixed,
beta_rot=beta_rot, beta_pos=100000.0, \
pose_weights=pose_weights, gamma=1000, \
grasp_cup=grasp_cup, R=R, D=D, cup_xyz=cup_xyz)
dof_inds = sim_util.dof_inds_from_name(robot, manip_name)
return traj, dof_inds, pose_costs
