def goto_aruco(tag_id, offset_x, offset_y, offset_z):
tf_listener = tf.TransformListener()
tag_frame_name = 'tag_'+str(tag_id)
tf_listener.waitForTransform('/world', tag_frame_name, rospy.Time(), rospy.Duration(4.0))
(trans_tag, rot_tag) = tf_listener.lookupTransform('/world', tag_frame_name, rospy.Time(0))
motion_primitives.set_pose([trans_tag[0]+offset_x, trans_tag[1]+offset_y, trans_tag[2]+offset_z, -0.7071, -0.0, 0.7071, 0.0])
print trans_tag, rot_tag
sim = config['sim']
table_height_wrt_world = -0.02
# Set TCP speed
group.set_max_velocity_scaling_factor(tcp_speed)
pre_pick_pose = [-0.092, 0.592, 0.416, 0.7071, -0.0, -0.7071, 0.0]
pick_pose = [-0.092, 0.592, 0.282, 0.7071, -0.0, -0.7071, 0.0]
init_pose=[-0.344, 0.538, 0.458, -0.7071, -0.0, 0.7071, 0.0]
regrasp_pose=[-0.344, 0.538, 0.350, -0.7071, -0.0, 0.7071, 0.0]
Robotiq.goto(robotiq_client, pos=0.04, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
rospy.sleep(0.5)
motion_primitives.set_pose(pre_pick_pose)
rospy.sleep(0.5)
motion_primitives.set_pose(pick_pose)
rospy.sleep(1)
Robotiq.goto(robotiq_client, pos=object_thickness+0.002, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
rospy.sleep(1)
motion_primitives.set_pose(pre_pick_pose)
#Robotiq.goto(robotiq_client, pos=object_thickness+0.003, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
motion_primitives.set_pose(regrasp_pose)
#for i in range(11):
# rospy.sleep(1)
# print 10-i
dynamixel.set_length(115)
#Robotiq.goto(robotiq_client, pos=object_thickness+0.015, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
#rospy.sleep(0.5)
#motion_primitives.set_pose(init_pose)
#motion_primitives.set_pose(prepick_pose)
#motion_primitives.set_pose(pick_pose)
Robotiq.goto(robotiq_client,
pos=object_thickness + 0.006,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
rospy.sleep(0.5)
#motion_primitives.set_pose(prepick_pose)
#motion_primitives.set_pose(tilt_pose)
motion_primitives.set_pose(prior_pose)
rospy.sleep(10)
motion_primitives.set_pose(initial_pose)
# read position from real robot.
p = group.get_current_pose().pose
trans_tool0 = [p.position.x, p.position.y, p.position.z]
rot_tool0 = [
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w
]
T_wg = tf.TransformerROS().fromTranslationRotation(
trans_tool0, rot_tool0)
P_g_center = [
tcp2fingertip + object_length - delta_0, -object_thickness / 2, 0,
1
#Robotiq.goto(robotiq_client, pos=0.04, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
#rospy.sleep(0.5)
#motion_primitives.set_pose(pre_pick_pose)
#motion_primitives.set_pose(pick_pose)
Robotiq.goto(robotiq_client,
pos=object_thickness + 0.006,
speed=config['gripper_speed'],
force=config['gripper_force'],
block=False)
#rospy.sleep(0.5)
#motion_primitives.set_pose(pre_pick_pose)
motion_primitives.set_pose(regrasp_pose)
# read position from real robot.
p = group.get_current_pose().pose
trans_tool0 = [p.position.x, p.position.y, p.position.z]
rot_tool0 = [
p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w
]
T_wg = tf.TransformerROS().fromTranslationRotation(
trans_tool0, rot_tool0)
P_g_center = [tcp2fingertip + object_length - delta_0, 0, 0, 1]
P_w_center = np.matmul(T_wg, P_g_center)
center = [P_w_center[0], P_w_center[1], P_w_center[2]]
# Tilt
try:
tcp_speed = config['tcp_speed']
theta_0 = config['theta_0']
delta_0 = config['delta_0']
psi_regrasp = config['psi_regrasp']
theta_tilt = config['theta_tilt']
tuck_angle = config['tuck']
axis = config['axis']
object_thickness = config['object_thickness']
object_length = config['object_length']
tcp2fingertip = config['tcp2fingertip']
sim = config['sim']
table_height_wrt_world = -0.02
pose = [-0.3, 0.630, table_height_wrt_world+tcp2fingertip+object_length-delta_0, 0.7071, 0, -0.7071, 0]
motion_primitives.set_pose(pose)
# read position from real robot.
p = group.get_current_pose().pose
trans_tool0 = [p.position.x, p.position.y, p.position.z]
rot_tool0 = [p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w]
T_wg = tf.TransformerROS().fromTranslationRotation(trans_tool0, rot_tool0)
P_g_center = [tcp2fingertip+object_length-delta_0, 0, 0, 1]
P_w_center = np.matmul(T_wg, P_g_center)
# Set TCP speed
group.set_max_velocity_scaling_factor(tcp_speed)
# Set gripper position
Robotiq.goto(robotiq_client, pos=object_thickness+0.005, speed=config['gripper_speed'], force=config['gripper_force'], block=False)
def calibration(trans_mag, ori_mag):
p = group.get_current_pose().pose
pos = [p.position.x, p.position.y, p.position.z]
ori = [p.orientation.x, p.orientation.y, p.orientation.z, p.orientation.w]
T_wg = tf.TransformerROS().fromTranslationRotation(pos, ori)
pos = []
rot = []
pos.append(copy.deepcopy([0, trans_mag, 0]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(-ori_mag, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, trans_mag, 0]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, trans_mag, trans_mag]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(-ori_mag, ori_mag, 0,
'rzyx')))
pos.append(copy.deepcopy([0, trans_mag, trans_mag]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, 0, trans_mag]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(0, ori_mag, 0, 'rzyx')))
pos.append(copy.deepcopy([0, 0, trans_mag]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, -trans_mag, trans_mag]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(ori_mag, ori_mag, 0,
'rzyx')))
pos.append(copy.deepcopy([0, -trans_mag, trans_mag]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, -trans_mag, 0]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(ori_mag, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, -trans_mag, 0]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, -trans_mag, -trans_mag]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(ori_mag, -ori_mag, 0,
'rzyx')))
pos.append(copy.deepcopy([0, -trans_mag, -trans_mag]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, 0, -trans_mag]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(0, -ori_mag, 0, 'rzyx')))
pos.append(copy.deepcopy([0, 0, -trans_mag]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, trans_mag, -trans_mag]))
rot.append(
copy.deepcopy(
tf.transformations.quaternion_from_euler(-ori_mag, -ori_mag, 0,
'rzyx')))
pos.append(copy.deepcopy([0, trans_mag, -trans_mag]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, 0, 0]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
pos.append(copy.deepcopy([0, 0, 0]))
rot.append(
copy.deepcopy(tf.transformations.quaternion_from_euler(
0, 0, 0, 'rzyx')))
robot_pose_file = open(os.path.join(path, 'robot_cali.txt'), "a+")
robot_pose_file.write(str(len(pos)) + "\n")
robot_pose_file.close()
for i in range(len(pos)):
T_gg_target = tf.TransformerROS().fromTranslationRotation(
pos[i], rot[i])
T_wg_target = np.matmul(T_wg, T_gg_target)
trans_target = T_wg_target[:3, 3]
rot_target = tf.transformations.quaternion_from_matrix(
T_wg_target[:4][:4])
pose_target = np.concatenate((trans_target, rot_target), axis=None)
motion_primitives.set_pose(pose_target)
get_save_image(i)
get_write_pose(i)