def remove_tight_rope_pull(sim_env, full_traj):
if sim_env.constraints['l'] and sim_env.constraints['r']:
ee_trajs = {}
for lr in 'lr':
ee_trajs[lr] = get_ee_traj(
sim_env,
lr,
full_traj,
ee_link_name_fmt="%s_gripper_l_finger_tip_link")
min_length = np.inf
hs = sim_env.sim.rope.GetHalfHeights()
rope_links = sim_env.sim.rope.GetKinBody().GetLinks()
for l_rope_link in sim_env.constraints_links['l']:
if l_rope_link not in rope_links:
continue
for r_rope_link in sim_env.constraints_links['r']:
if r_rope_link not in rope_links:
continue
i_cnt_l = rope_links.index(l_rope_link)
i_cnt_r = rope_links.index(r_rope_link)
if i_cnt_l > i_cnt_r:
i_cnt_l, i_cnt_r = i_cnt_r, i_cnt_l
min_length = min(
min_length, 2 * hs[i_cnt_l + 1:i_cnt_r].sum() +
hs[i_cnt_l] + hs[i_cnt_r])
valid_inds = np.apply_along_axis(
np.linalg.norm, 1, (ee_trajs['r'][:, :3, 3] -
ee_trajs['l'][:, :3, 3])) < min_length - 0.02
if not np.all(valid_inds):
full_traj = (full_traj[0][valid_inds, :], full_traj[1])
util.yellowprint(
"The grippers of the trajectory goes too far apart. Some steps of the trajectory are being removed."
)
return full_traj
def sim_full_traj_maybesim(sim_env, full_traj, animate=False, interactive=False, max_cart_vel_trans_traj=.05):
def sim_callback(i):
sim_env.step()
animate_speed = 20 if animate else 0
traj, dof_inds = full_traj
# clip finger joint angles to the binary gripper angles if necessary
for lr in 'lr':
joint_ind = sim_env.robot.GetJoint("%s_gripper_l_finger_joint"%lr).GetDOFIndex()
if joint_ind in dof_inds:
ind = dof_inds.index(joint_ind)
traj[:,ind] = np.minimum(traj[:,ind], get_binary_gripper_angle(True))
traj[:,ind] = np.maximum(traj[:,ind], get_binary_gripper_angle(False))
# 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, dof_inds=dof_inds)
transition_traj = ropesim.retime_traj(sim_env.robot, dof_inds, transition_traj, max_cart_vel=max_cart_vel_trans_traj)
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, dof_inds=dof_inds)
traj = ropesim.retime_traj(sim_env.robot, dof_inds, traj) # make the trajectory slow enough for the simulation
valid_inds = grippers_exceed_rope_length(sim_env, (traj, dof_inds), 0.05)
min_gripper_dist = [np.inf] # minimum distance between gripper when the rope capsules are too far apart
def is_rope_pulled_too_tight(i_step):
if valid_inds is None or valid_inds[i_step]: # gripper is not holding the rope or the grippers are not that far apart
return True
rope = sim_env.sim.rope
trans = rope.GetTranslations()
hhs = rope.GetHalfHeights()
rots = rope.GetRotations()
fwd_pts = (trans + hhs[:,None]*rots[:,:3,0])
bkwd_pts = (trans - hhs[:,None]*rots[:,:3,0])
pts_dists = np.apply_along_axis(np.linalg.norm, 1, fwd_pts[:-1] - bkwd_pts[1:])[:,None] # these should all be zero if the rope constraints are satisfied
if np.any(pts_dists > sim_env.sim.rope_params.radius):
if i_step == 0:
return True
ee_trajs = {}
for lr in 'lr':
ee_trajs[lr] = get_ee_traj(sim_env, lr, (traj[i_step-1:i_step+1], dof_inds), ee_link_name_fmt="%s_gripper_l_finger_tip_link")
min_gripper_dist[0] = min(min_gripper_dist[0], np.linalg.norm(ee_trajs['r'][0,:3,3] - ee_trajs['l'][0,:3,3]))
grippers_moved_closer = np.linalg.norm(ee_trajs['r'][1,:3,3] - ee_trajs['l'][1,:3,3]) < min_gripper_dist[0]
return grippers_moved_closer
return True
animate_traj.animate_traj(traj, sim_env.robot, restore=False, pause=interactive,
callback=sim_callback, step_viewer=animate_speed, execute_step_cond=is_rope_pulled_too_tight)
if min_gripper_dist[0] != np.inf:
util.yellowprint("Some steps of the trajectory were not executed because the gripper was pulling the rope too tight.")
if animate:
sim_env.viewer.Step()
return True
def remove_tight_rope_pull(sim_env, full_traj):
if sim_env.constraints['l'] and sim_env.constraints['r']:
ee_trajs = {}
for lr in 'lr':
ee_trajs[lr] = get_ee_traj(sim_env, lr, full_traj, ee_link_name_fmt="%s_gripper_l_finger_tip_link")
min_length = np.inf
hs = sim_env.sim.rope.GetHalfHeights()
rope_links = sim_env.sim.rope.GetKinBody().GetLinks()
for l_rope_link in sim_env.constraints_links['l']:
if l_rope_link not in rope_links:
continue
for r_rope_link in sim_env.constraints_links['r']:
if r_rope_link not in rope_links:
continue
i_cnt_l = rope_links.index(l_rope_link)
i_cnt_r = rope_links.index(r_rope_link)
if i_cnt_l > i_cnt_r:
i_cnt_l, i_cnt_r = i_cnt_r, i_cnt_l
min_length = min(min_length, 2*hs[i_cnt_l+1:i_cnt_r].sum() + hs[i_cnt_l] + hs[i_cnt_r])
valid_inds = np.apply_along_axis(np.linalg.norm, 1, (ee_trajs['r'][:,:3,3] - ee_trajs['l'][:,:3,3])) < min_length - 0.02
if not np.all(valid_inds):
full_traj = (full_traj[0][valid_inds,:], full_traj[1])
util.yellowprint("The grippers of the trajectory goes too far apart. Some steps of the trajectory are being removed.")
return full_traj
def sim_full_traj_maybesim(sim_env,
full_traj,
animate=False,
interactive=False,
max_cart_vel_trans_traj=.05):
def sim_callback(i):
sim_env.step()
animate_speed = 20 if animate else 0
traj, dof_inds = full_traj
# clip finger joint angles to the binary gripper angles if necessary
for lr in 'lr':
joint_ind = sim_env.robot.GetJoint("%s_gripper_l_finger_joint" %
lr).GetDOFIndex()
if joint_ind in dof_inds:
ind = dof_inds.index(joint_ind)
traj[:, ind] = np.minimum(traj[:, ind],
get_binary_gripper_angle(True))
traj[:, ind] = np.maximum(traj[:, ind],
get_binary_gripper_angle(False))
# 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, dof_inds=dof_inds)
transition_traj = retiming.retime_traj(
sim_env.robot,
dof_inds,
transition_traj,
max_cart_vel=max_cart_vel_trans_traj)
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, dof_inds=dof_inds)
traj = retiming.retime_traj(
sim_env.robot, dof_inds,
traj) # make the trajectory slow enough for the simulation
valid_inds = grippers_exceed_rope_length(sim_env, (traj, dof_inds), 0.05)
min_gripper_dist = [
np.inf
] # minimum distance between gripper when the rope capsules are too far apart
def is_rope_pulled_too_tight(i_step):
if valid_inds is None or valid_inds[
i_step]: # gripper is not holding the rope or the grippers are not that far apart
return True
rope = sim_env.sim.rope
trans = rope.GetTranslations()
hhs = rope.GetHalfHeights()
rots = rope.GetRotations()
fwd_pts = (trans + hhs[:, None] * rots[:, :3, 0])
bkwd_pts = (trans - hhs[:, None] * rots[:, :3, 0])
pts_dists = np.apply_along_axis(
np.linalg.norm, 1, fwd_pts[:-1] - bkwd_pts[1:]
)[:,
None] # these should all be zero if the rope constraints are satisfied
if np.any(pts_dists > sim_env.sim.rope_params.radius):
if i_step == 0:
return True
ee_trajs = {}
for lr in 'lr':
ee_trajs[lr] = get_ee_traj(
sim_env,
lr, (traj[i_step - 1:i_step + 1], dof_inds),
ee_link_name_fmt="%s_gripper_l_finger_tip_link")
min_gripper_dist[0] = min(
min_gripper_dist[0],
np.linalg.norm(ee_trajs['r'][0, :3, 3] -
ee_trajs['l'][0, :3, 3]))
grippers_moved_closer = np.linalg.norm(
ee_trajs['r'][1, :3, 3] -
ee_trajs['l'][1, :3, 3]) < min_gripper_dist[0]
return grippers_moved_closer
return True
animate_traj.animate_traj(traj,
sim_env.robot,
restore=False,
pause=interactive,
callback=sim_callback,
step_viewer=animate_speed,
execute_step_cond=is_rope_pulled_too_tight)
if min_gripper_dist[0] != np.inf:
util.yellowprint(
"Some steps of the trajectory were not executed because the gripper was pulling the rope too tight."
)
if animate:
sim_env.viewer.Step()
return True