# Regrasp
print_transition("REGRASP START")
regrasp.regrasp(np.multiply(axis, -1), int(psi_regrasp), tcp_speed)
rospy.sleep(2)
print_transition("REGRASP FINISHED")
# Tilt
print_transition("TILT START")
tilt.tilt(center, axis, int(theta_tilt), tcp_speed)
print_transition("TILT FINISHED")
rospy.sleep(2)
# Place
print_transition("TUCK START")
tuck.rotate_tuck(np.multiply(axis, -1), int(tuck_angle), 0.035, tcp_speed)
print_transition("TUCK FINISHED")
rospy.sleep(1)
print_transition("RELEASING CONTACT")
motion_primitives.set_pose_relative([0,0,0.05])
#rospy.spin()
except rospy.ROSInterruptException: pass
'''
# Robot parameters
tcp_speed: 0.05
# Gripper parameters
motion_primitives.set_pose(init_pose)
p = group.get_current_pose().pose
'''
#TEST GRIPPER LENGTH
pos_initial = [p.position.x, p.position.y, p.position.z]
ori_initial = [p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w]
T_we = tf.TransformListener().fromTranslationRotation(pos_initial, ori_initial)
tcp2fingertip = config['tcp2fingertip']
contact_A_e = [tcp2fingertip, -object_thickness/2, 0, 1]
contact_A_w = np.matmul(T_we, contact_A_e)
visualization.visualizer(contact_A_w[:3], "s", 0.005, 2) #DEBUG
#TEST GRIPPER LENGTH
'''
center = [
p.position.x, p.position.y,
p.position.z - tcp2fingertip - object_length + delta_0
]
visualization.visualizer(center, "s", 0.01, 0)
rospy.sleep(0.5)
tilt.tilt(center, axis, int(90 - theta_0), tcp_speed)
regrasp.regrasp(np.multiply(axis, -1), int(psi_regrasp), tcp_speed)
tilt.tilt(center, axis, int(theta_tilt), tcp_speed)
tuck.rotate_tuck(np.multiply(axis, -1), int(theta_0 - theta_tilt),
0.03, tcp_speed)
rospy.spin()
except rospy.ROSInterruptException:
pass
