def visualize_placing_box(bbox_info,place_pose,viz_pub):
if viz_pub!=None:
markers_msg = MarkerArray()
m=createCubeMarker2(offset=tuple(place_pose[0:3]), marker_id = 7, rgba=(0,0,1,1), orientation=tuple(place_pose[3:7]), scale=tuple(box_dim[0:3]), frame_id="/map", ns = 'object_bounding_box_for_collision')
m.frame_locked = True
markers_msg.markers.append(m)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
rospy.sleep(0.1)
def visualize_grasping_proposals(proposal_viz_array_pub,
proposals,
listener,
br,
is_selected=False):
n = proposals.shape[0]
if n > 0 and proposals[0] is None:
return
scale = (0.001, 0.02, 0.2)
markers_msg = MarkerArray()
#if not is_selected: # don't delete other candidate
# markers_msg.markers.append(createDeleteAllMarker('pick_proposals'))
proposals_score = proposals[0:min(n, 10), 11]
if len(proposals_score) > 0:
max_score = np.amax(proposals_score)
min_score = np.amin(proposals_score)
print max_score, min_score
for i in range(0, min(n, 10)):
pick_proposal = proposals[i, :]
objInput = pick_proposal
#~get grasp pose and gripper opening from vision
if len(objInput) == 12:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = helper.get_picking_params_from_12(
objInput)
graspPos = graspPos + hand_X * 0.02 * 1
grasp_score = objInput[-1]
elif len(objInput) == 7:
graspPos, hand_X, hand_Y, hand_Z, grasp_width = helper.get_picking_params_from_7(
objInput, 'dummy', listener, br)
graspPos = graspPos + hand_X * 0.02 * 1
grasp_score = 1
color = matplotlib.cm.seismic(
(1 - (grasp_score - min_score) / (max_score - min_score)))
if is_selected:
color = (0, 1, 0, 1)
scale_bar = (grasp_width, 0.003, 0.2)
#import ipdb; ipdb.set_trace()
rotmat = np.vstack((hand_X, hand_Y, hand_Z, np.zeros((1, 3)))).T
rotmat = np.vstack((rotmat, np.array([[0, 0, 0, 1]])))
quat = helper.quat_from_matrix(rotmat)
m1 = createCubeMarker2(rgba=color,
scale=scale,
offset=tuple(graspPos +
hand_X * grasp_width / 2),
orientation=tuple(quat),
marker_id=i * 3,
ns='pick_proposals')
m2 = createCubeMarker2(rgba=color,
scale=scale,
offset=tuple(graspPos -
hand_X * grasp_width / 2),
orientation=tuple(quat),
marker_id=i * 3 + 1,
ns='pick_proposals')
m3 = createCubeMarker2(rgba=color,
scale=scale_bar,
offset=tuple(graspPos),
orientation=tuple(quat),
marker_id=i * 3 + 2,
ns='pick_proposals')
markers_msg.markers.append(m1)
markers_msg.markers.append(m2)
markers_msg.markers.append(m3)
for i in range(0, 10):
proposal_viz_array_pub.publish(markers_msg)
def pose_transform_precise_placing(rel_pose,
BoxBody,
place_pose,
base_pose,
bin_pts,
finger_pts,
margin=0,
show_plot=False,
viz_pub=None):
margin = 0.0
print 'rel_pose', rel_pose
print 'place_pose', place_pose
#drop_pose is the first guess at the desired pose of the hand
drop_pose = transformBack(rel_pose, place_pose)
base_rot = tfm.quaternion_matrix(base_pose[3:7])
BoxBody_base_pose = []
for i in range(0, 8):
BoxBody_base_pose.append(np.dot(base_rot[0:3, 0:3], BoxBody[i]))
BoxOrigin_base_pose = np.dot(base_rot[0:3, 0:3], rel_pose[0:3])
BoxBody_base_pose = np.vstack(BoxBody_base_pose)
drop_rot = tfm.quaternion_matrix(drop_pose[3:7])
box_pose_at_base = transformTo(base_pose, rel_pose)
theta_mat = np.dot(
tfm.quaternion_matrix(place_pose[3:7]),
tfm.inverse_matrix(tfm.quaternion_matrix(box_pose_at_base[3:7])))
theta = np.imag(np.log(theta_mat[0, 0] + theta_mat[1, 0] * 1j))
if abs(theta - np.pi) <= .5 * np.pi:
theta = theta - np.pi
orient_mat_4x4 = [[np.cos(theta), -np.sin(theta), 0, 0],
[np.sin(theta), np.cos(theta), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]]
drop_quat = quat_from_matrix(
np.dot(orient_mat_4x4, tfm.quaternion_matrix(base_pose[3:7])))
bin_ptsXY = bin_pts[:, 0:2]
finger_ptsXY = np.concatenate(
(finger_pts[:, 0:2], BoxBody_base_pose[:, 0:2]), axis=0)
(shape_translation, dist_val_min, feasible_solution,
nearest_point) = collision_detection.collision_projection.projection_func(
bin_ptsXY, finger_ptsXY, np.array(place_pose[0:2], ndmin=2),
np.array(BoxOrigin_base_pose[0:2], ndmin=2), theta, show_plot, margin)
p0 = BoxBody[0]
px = BoxBody[4]
py = BoxBody[2]
pz = BoxBody[1]
box_dim = [
np.linalg.norm(p0 - px),
np.linalg.norm(p0 - py),
np.linalg.norm(p0 - pz)
]
#feasible_solution=False
if feasible_solution:
drop_pose = shape_translation.tolist() + [drop_pose[2]
] + drop_quat.tolist()
else:
drop_pose = [np.mean(bin_pts[:, 0]),
np.mean(bin_pts[:, 1])] + [drop_pose[2]
] + drop_quat.tolist()
actual_pose = transformTo(drop_pose, rel_pose)
actual_pose[2] = place_pose[2]
if False:
markers_msg = MarkerArray()
markers_msg.markers.append(
createAllMarker('object_bounding_box_for_collision2'))
m = createCubeMarker2(offset=tuple(place_pose[0:3]),
marker_id=4,
rgba=(1, 1, 1, 1),
orientation=tuple(place_pose[3:7]),
scale=tuple(box_dim),
frame_id="/map",
ns='object_bounding_box_for_collision2')
m.frame_locked = True
markers_msg.markers.append(m)
m = createCubeMarker2(offset=tuple(actual_pose[0:3]),
marker_id=5,
rgba=(0, 1, 0, 1),
orientation=tuple(actual_pose[3:7]),
scale=tuple(box_dim),
frame_id="/map",
ns='object_bounding_box_for_collision2')
m.frame_locked = True
markers_msg.markers.append(m)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
viz_pub.publish(markers_msg)
rospy.sleep(0.1)
return (drop_pose, actual_pose)
