def execute_trajectory(self, full_traj, step_viewer=1, interactive=False,
max_cart_vel_trans_traj=.05, sim_callback=None):
# TODO: incorporate other parts of sim_full_traj_maybesim
if sim_callback is None:
sim_callback = lambda i: self.step()
traj, dof_inds = full_traj
# # clip finger joint angles to the binary gripper angles if necessary
# for lr in 'lr':
# joint_ind = self.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
self.robot.SetActiveDOFs(dof_inds)
curr_vals = self.robot.GetActiveDOFValues()
transition_traj = np.r_[[curr_vals], [traj[0]]]
sim_util.unwrap_in_place(transition_traj, dof_inds=dof_inds)
transition_traj = ropesim.retime_traj(self.robot, dof_inds, transition_traj,
max_cart_vel=max_cart_vel_trans_traj)
animate_traj.animate_traj(transition_traj, self.robot, restore=False, pause=interactive,
callback=sim_callback, step_viewer=step_viewer if self.viewer else 0)
traj[0] = transition_traj[-1]
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
traj = ropesim.retime_traj(self.robot, dof_inds, traj) # make the trajectory slow enough for the simulation
animate_traj.animate_traj(traj, self.robot, restore=False, pause=interactive,
callback=sim_callback, step_viewer=step_viewer if self.viewer else 0)
if self.viewer and step_viewer:
self.viewer.Step()
return True
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 execute_trajectory(self,
full_traj,
step_viewer=1,
interactive=False,
max_cart_vel_trans_traj=.05,
max_cart_vel=.02,
sim_callback=None):
# TODO: incorporate other parts of sim_full_traj_maybesim
if sim_callback is None:
sim_callback = lambda i: self.step()
traj, dof_inds = full_traj
# # clip finger joint angles to the binary gripper angles if necessary
# for lr in 'lr':
# joint_ind = self.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
self.robot.SetActiveDOFs(dof_inds)
curr_vals = self.robot.GetActiveDOFValues()
transition_traj = np.r_[[curr_vals], [traj[0]]]
sim_util.unwrap_in_place(transition_traj, dof_inds=dof_inds)
transition_traj = retiming.retime_traj(
self.robot,
dof_inds,
transition_traj,
max_cart_vel=max_cart_vel_trans_traj)
animate_traj.animate_traj(
transition_traj,
self.robot,
restore=False,
pause=interactive,
callback=sim_callback,
step_viewer=step_viewer if self.viewer else 0)
traj[0] = transition_traj[-1]
sim_util.unwrap_in_place(traj, dof_inds=dof_inds)
traj = retiming.retime_traj(
self.robot, dof_inds, traj, max_cart_vel=max_cart_vel
) # make the trajectory slow enough for the simulation
animate_traj.animate_traj(
traj,
self.robot,
restore=False,
pause=interactive,
callback=sim_callback,
step_viewer=step_viewer if self.viewer else 0)
if self.viewer and step_viewer:
self.viewer.Step()
return True
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
