goal_rot = pose[3:7]
while (1):
# Check key press
cmd = update_keys()
# Update pose goal
goal_pos = goal_pos + cmd
#manipulator.set_frame_pose_goal(endEffectorIndex,goal_pos,goal_rot)
manipulator.set_frame_position_goal(endEffectorIndex, goal_pos)
# Check gripper flag and update gripper goal
if grip == 0:
manipulator.open_gripper()
elif grip == 1:
manipulator.close_gripper()
# Update manipulator state
manipulator.update()
# Check for contact and vibrate
c = manipulator.check_contact()
if c:
print("vibrate")
#vibrate()
#vibrate()
time.sleep(0.01)
p.disconnect()
