def planned_place(self, fixed_container=None):
fixed_container = [
1 - self.tote_ID
] #TODO_M: planner only accepts bins 1,2,3 and names them as 0,1,2
if self.PlacingPlanner.visionType == 'real': #Update HM
self.PlacingPlanner.update_real_height_map(fixed_container[0])
drop_pose = self.PlacingPlanner.place_object_local_best(
None, containers=fixed_container
) #TODO_M : change placing to return drop_pose
print('drop_pose', drop_pose)
#~frank hack: drop pose
drop_pose = get_params_yaml('bin' + str(fixed_container[0]) + '_pose')
# Place object using grasping
self.rel_pose, self.BoxBody = vision_transform_precise_placing_with_visualization(
self.bbox_info, viz_pub=self.viz_array_pub, listener=self.listener)
grasp(objInput=self.grasp_point,
listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=fixed_container[0],
flag=2,
withPause=self.withPause,
rel_pose=self.rel_pose,
BoxBody=self.BoxBody,
place_pose=drop_pose,
viz_pub=self.viz_array_pub,
is_drop=False,
recorder=self.gdr)
def planned_place(self):
# Place approx. at the center bin
binId = 0
drop_pose = get_params_yaml('bin{}_pose'.format(binId))
drop_pose[1] = drop_pose[1] + 0.03
# Place depending on the position of the COM of the contact patch
image_list = self.capture_images()
img0, aux, contact = crop_contact(self.back_img_list[0],
image_list[0],
gel_id=1,
is_zeros=True)
img1, aux, contact = crop_contact(self.back_img_list[1],
image_list[1],
gel_id=2,
is_zeros=True)
img0 = scipy.misc.imresize(img0, (224, 224, 3))
img1 = scipy.misc.imresize(img1, (224, 224, 3))
pos_pixel_img0 = get_center_of_mass(img0)
pos_pixel_img1 = get_center_of_mass(img1)
pos_pixel_avg = np.mean(np.array([pos_pixel_img0, pos_pixel_img1]),
axis=0)
# Compute actual motion for the robot and check if there is collision (x in pixel frame -> y in hand frame y in pixel frame -> -z in hand frame)
pos_world_avg = np.nan_to_num(
convert_image2world([225 / 2 - pos_pixel_avg[1],
0])) #np.array([y,-z])
print('Pixel positions: ', pos_pixel_avg, '. World positions: ',
pos_world_avg)
drop_pose[1] = drop_pose[1] + pos_world_avg[0]
print(drop_pose)
# Place object using graspingc
self.rel_pose, self.BoxBody = vision_transform_precise_placing_with_visualization(
self.bbox_info, viz_pub=self.viz_array_pub, listener=self.listener)
place(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=binId,
withPause=self.withPause,
rel_pose=self.rel_pose,
BoxBody=self.BoxBody,
place_pose=drop_pose,
viz_pub=self.viz_array_pub,
is_drop=False,
recorder=self.gdr)
def planned_place(self):
if len(self.get_objects()) == 1:
fixed_container = [self.tote_ID]
else:
fixed_container = [
1 - self.tote_ID
] #TODO_M: planner only accepts bins 1,2,3 and names them as 0,1,2
if self.PlacingPlanner.visionType == 'real': #Update HM
self.PlacingPlanner.update_real_height_map(fixed_container[0])
#Get obj dimensions:
try:
asking_for = '/obj/' + self.obj_ID
aux_obj = rospy.get_param(asking_for)
obj_dim = aux_obj['dimensions']
except:
print('Could not get the object dimensions')
obj_dim = [0.12, 0.12, 0.12]
drop_pose = self.PlacingPlanner.place_object_local_best(
obj_dim=obj_dim, containers=fixed_container
) #TODO_M : change placing to return drop_pose
print('drop_pose', drop_pose)
#~frank hack: drop pose
drop_pose = get_params_yaml('bin' + str(fixed_container[0]) + '_pose')
drop_pose[1] = drop_pose[1] + 0.03
# Place object using grasping
self.rel_pose, self.BoxBody = vision_transform_precise_placing_with_visualization(
self.bbox_info, viz_pub=self.viz_array_pub, listener=self.listener)
place(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=fixed_container[0],
withPause=self.withPause,
rel_pose=self.rel_pose,
BoxBody=self.BoxBody,
place_pose=drop_pose,
viz_pub=self.viz_array_pub,
is_drop=False,
recorder=self.gdr)
def update_pose_placed_obj(self, obj):
if type(obj) == int:
obj = self.objects[obj]
#Add to the HM
self.add_object_to_height_map(obj, int(obj.container - 1))
obj.pose[0:3] = [
obj.pos[0] - self.tote_length / 2.,
obj.pos[1] - self.tote_width / 2., obj.w_dim[2] / 2 + self.h_max
] #Put pos with respect to the center of the tote
center_bin = get_params_yaml(
'bin' + str(obj.container) + '_pose'
)[0:
3] #TODO_M: there is a relation between boxes names and identity that is not related to our 4,5,6
obj.pose[0:3] = [obj.pose[_] + center_bin[_] for _ in range(3)]
#convert objprint into quaternion:
orient_mat_3x3 = rotmatZ(0 * 180.0 /
np.pi) #rotmatZ(objprint*180.0/np.pi)
obj.pose[3:7] = mat2quat(orient_mat_3x3)
print('Added object:', obj.label, ' to HM considering obj.pos: ',
obj.pos, ', bin center: ', center_bin, 'and obj.pose: ',
obj.pose, ' in obj.container: ', obj.container)
return obj
def get_bin_corners(bin_id):
if bin_id == 0:
term = [0, 1, 2, 3]
elif bin_id == 1:
term = [5, 6, 7, 8]
elif bin_id == 2:
term = [10, 11, 12, 13]
elif bin_id == 3:
term = [15, 16, 17, 18]
elif bin_id == 4:
term = [0, 1, 2, 3]
elif bin_id == 5:
term = [5, 6, 7, 8]
elif bin_id == 6:
term = [10, 11, 12, 13]
elif bin_id == 7:
term = [15, 16, 17, 18]
elif bin_id == 8:
term = [20, 21, 22, 23]
points_warped_list = []
points_list = []
points = []
x_axis = np.array([1, 0])
y_axis = np.array([0, 1])
bin_pose = get_params_yaml('/bin' + str(bin_id) + '_pose')
bin_center = np.asarray(bin_pose[0:2])
b = [(1, 1), (1, -1), (-1, 1), (-1, -1)]
bin_length = rospy.get_param('/bin' + str(bin_id) + '/length')
bin_length_2 = bin_length / 2.
bin_width = rospy.get_param('/bin' + str(bin_id) + '/width')
bin_width_2 = bin_width / 2.
for (dx, dy) in b:
points_tmp = bin_center + dx * x_axis * bin_length_2 + dy * y_axis * bin_width_2
points_list.append(points_tmp.tolist())
return points_list
def calibrate_storage():
#################################################
#~ Generate Plans
gripperOri = []
gripperOri.append([-1., 0., 0., 0.])
gripperOri.append([-0.70710678118654757, -0.70710678118654757, 0., 0.])
gripperOri.append([0., -1., 0., 0.])
gripperOri.append([-0.70710678118654757, 0.70710678118654757, 0., 0.])
#~build total list plan
gripperOri_list = []
ori_list = [
0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
0, 0, 1, 2, 3, 0, 0, 0
]
for i in range(0, len(ori_list)):
gripperOri_list.append(gripperOri[ori_list[i]])
#~positions
pos = []
#~bin0
bin_pose = get_params_yaml('bin0_pose')
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.00 + .12, -0.46 + 0.19647, 0.41])
pos.append([0.75 + .12, bin_pose[1], 0.41])
pos.append([1. + .12, -0.697281360626 + 0.19647, 0.41])
pos.append([1.19 + .12, bin_pose[1], 0.41])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append(
[1.00278019905 + .12, -0.763054132462 + 0.19647, 0.447774887085 + .05])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin1
bin_pose = get_params_yaml('bin1_pose')
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.00 + .12, -0.0943171977997 + 0.19647, 0.41])
pos.append([0.75 + .12, bin_pose[1], 0.41])
pos.append([1. + .12, -0.330679833889 + 0.19647, 0.41])
pos.append([1.19 + .12, bin_pose[1], 0.41])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([
0.961158931255 + .12, -0.0221027284861 + 0.19647, 0.447779148817 + .05
])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin2
bin_pose = get_params_yaml('bin2_pose')
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.00 + .12, 0.307756274939 + 0.19647, 0.41])
pos.append([0.8 + .12, bin_pose[1], 0.41])
pos.append([1 + .12, 0.0618322640657 + 0.19647, 0.41])
pos.append([1.18 + .12, bin_pose[1], 0.41])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append(
[0.940047621727 + .12, 0.353745698929 + 0.19647, 0.447779148817 + .05])
pos.append([
bin_pose[0] + .12, bin_pose[1],
0.1 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
####################
## Generate Plans ##
####################
q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
tip_hand_transform = [0., 0., 0., 0., 0., 0.]
dir_list = [
None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
0, 1, 2, 3, None
]
point_counter = 0
#~generate plans
plans = []
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.5)
goToHome.goToARC()
#~ goToHome.prepGripperPicking()
for i in range(0, len(pos)):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
plans.append(plan)
q_initial = qf
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move(dir_list[i], q_initial, pos[i], gripperOri_list[i],
tip_hand_transform, listener, point_counter)
point_counter += 1
def calibrate_boxes():
#################################################
#~ Generate Plans
gripperOri = []
gripperOri.append([-1., 0., 0., 0.])
gripperOri.append([-0.715217411518, -0.715217411518, 0., 0.])
gripperOri.append([0., -1., 0., 0.])
gripperOri.append([-0.715217411518, 0.715217411518, 0., 0.])
#~build total list plan
gripperOri_list = []
ori_list = [
0, 0, 1, 2, 3, 3, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0,
0, 0, 1, 2, 3, 0, 0, 0, 0, 0, 1, 2, 3, 0, 0, 0
]
for i in range(0, len(ori_list)):
gripperOri_list.append(gripperOri[ori_list[i]])
#~positions
pos = []
#~bin4
bin_pose = get_params_yaml('bin4_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.15 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.1, -0.498443692923, 0.5512911677361 - .15])
pos.append([1.05, -0.599031984806, 0.5512911677361 - .15])
pos.append([1.14, -0.70, 0.5512911677361 - .15])
pos.append([1.2, -0.618239402771, 0.5512911677361 - .15])
pos.append([
bin_pose[0], bin_pose[1],
0.30 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([1.08824729919, -0.808329701424, 0.5512911677361 - 0.05])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin5
bin_pose = get_params_yaml('bin5_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.79457449913, -0.498443692923, 0.5512911677361 - .15])
pos.append([0.757445454597, -0.599031984806, 0.5512911677361 - .15])
pos.append([0.79457449913, -0.70, 0.5512911677361 - .15])
pos.append([0.889182715416, -0.618239402771, 0.5512911677361 - .15])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.79457449913, -0.788900852203, 0.5512911677361 - .05])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin6
bin_pose = get_params_yaml('bin6_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.09962657094, -0.55, 0.539945185184 + 0.0])
pos.append([0.0830265730619, -0.6, 0.539945185184 + 0.0])
pos.append([0.099, -0.68, 0.539945185184 + 0.0])
pos.append([0.15, -0.6, 0.539945185184 + 0.0])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.145160362124, -0.846666872501, 0.65])
pos.append([
bin_pose[0], bin_pose[1],
0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin7
bin_pose = get_params_yaml('bin7_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.4 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([-0.199928298593, -0.5, 0.539945185184 + 0.0])
pos.append([-0.272910028696, -0.6, 0.539945185184 + 0.0])
pos.append([-0.195086687803, -0.75, 0.539945185184 + 0.0])
pos.append([-0.100125610828, -0.6, 0.539945185184 + 0.0])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([-0.203513264656, -0.86, 0.65])
pos.append([
bin_pose[0], bin_pose[1],
0.3 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
#~bin8
bin_pose = get_params_yaml('bin8_pose')
pos.append([
bin_pose[0], bin_pose[1],
0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([-0.09, 0.68, 0.716840863228])
pos.append([-0.22, 0.62, 0.716840863228])
pos.append([-0.09, 0.5, 0.716840863228])
pos.append([0.1, 0.62, 0.716840863228])
pos.append([
bin_pose[0], bin_pose[1],
0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
pos.append([0.106256209314, 0.78, 0.8])
pos.append([
bin_pose[0], bin_pose[1],
0.6 + rospy.get_param('/gripper/spatula_tip_to_tcp_dist')
])
####################
## Generate Plans ##
####################
q_initial = [-0.0014, 0.2129, 0.3204, 0, 1.0374, -0.0014]
tip_hand_transform = [0., 0., 0., 0., 0., 0.]
dir_list = [
None, 0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None,
0, 1, 2, 3, None, 4, None, None, 0, 1, 2, 3, None, 4, None, None, 0, 1,
2, 3, None, 4, None
]
point_counter = 0
#~generate plans
plans = []
listener = tf.TransformListener()
br = tf.TransformBroadcaster()
rospy.sleep(0.5)
goToHome.goToARC()
goToHome.prepGripperPicking()
###############################
## Unwrap angles example use ##
###############################
#~1) function to unwarp an angle to the range [-pi,pi]
def unwrap(angle):
if angle > np.pi:
angle = np.mod(angle, 2 * np.pi)
if angle > np.pi:
angle = angle - 2 * np.pi
elif angle < -np.pi:
angle = np.mod(angle, -2 * np.pi)
if angle < -np.pi:
angle = angle + 2 * np.pi
return angle
for i in range(0, 16):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
#~2) unwrap angle of joint 6 only
plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
plans.append(plan)
#~3) set q_initial (for next plan) with the unwrapped angle joint state
q_initial = plan.q_traj[-1]
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move_boxes(dir_list[i], q_initial, pos[i],
gripperOri_list[i], tip_hand_transform, listener,
point_counter)
point_counter += 1
##################################
plans_col_free = []
plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
q_initial, plans_col_free, [0, 1, 0, 0], False, 6)
executePlanForward(plans_col_free, True)
for i in range(16, 32):
#~ for i in range(0,16):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
plans.append(plan)
q_initial = plan.q_traj[-1]
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move_boxes(dir_list[i], q_initial, pos[i],
gripperOri_list[i], tip_hand_transform, listener,
point_counter)
point_counter += 1
plans_col_free = []
plans_col_free, q_initial, plan_possible, motion = collision_free_plan(
q_initial, plans_col_free, [0, 1, 0, 0], False, 8)
executePlanForward(plans_col_free, True)
for i in range(32, 40):
#~ for i in range(16,24):
plan, qf, plan_possible = generatePlan(q_initial, pos[i],
gripperOri_list[i],
tip_hand_transform, 'fastest')
if plan_possible:
plan.q_traj[-1][5] = unwrap(plan.q_traj[-1][5])
plans.append(plan)
q_initial = plan.q_traj[-1]
else:
return '[IK] Error'
#~execute plan
executePlanForward(plans, True)
plans = []
if dir_list[i] is not None:
gentle_move_boxes(dir_list[i], q_initial, pos[i],
gripperOri_list[i], tip_hand_transform, listener,
point_counter)
point_counter += 1
def planned_place(self):
if len(self.get_objects()) == 1:
fixed_container = [self.tote_ID]
else:
fixed_container = [
1 - self.tote_ID
] #TODO_M: planner only accepts bins 1,2,3 and names them as 0,1,2
if self.PlacingPlanner.visionType == 'real': #Update HM
self.PlacingPlanner.update_real_height_map(fixed_container[0])
#Get obj dimensions:
try:
asking_for = '/obj/' + self.obj_ID
aux_obj = rospy.get_param(asking_for)
obj_dim = aux_obj['dimensions']
except:
print('Could not get the object dimensions')
obj_dim = [0.12, 0.12, 0.12]
drop_pose = self.PlacingPlanner.place_object_local_best(
obj_dim=obj_dim, containers=fixed_container
) #TODO_M : change placing to return drop_pose
print('drop_pose', drop_pose)
#~frank hack: drop pose
drop_pose = get_params_yaml('bin' + str(fixed_container[0]) + '_pose')
drop_pose[1] = drop_pose[1] + 0.03
## TODO add here a correction depending on the com of the contact patch of the object?
image_list = self.capture_images()
image_list[0] = crop_contact(self.background_images[0],
image_list[0],
gel_id=1,
is_zeros=use_COM)
image_list[1] = crop_contact(self.background_images[1],
image_list[1],
gel_id=2,
is_zeros=use_COM)
img0 = scipy.misc.imresize(image_list[0], (224, 224, 3))
img1 = scipy.misc.imresize(image_list[1], (224, 224, 3))
pos_pixel_img0 = get_center_of_mass(img0)
pos_pixel_img1 = get_center_of_mass(img1)
pos_pixel_avg = np.mean(np.array([pos_pixel_img0, pos_pixel_img1]),
axis=0) #TODO: check axis
#x in pixel frame -> y in hand frame
#y in pixel frame -> -z in hand frame
# Compute actual motion for the robot and check if there is collision
pos_world_avg = convert_image2world([225 / 2 - pos_pixel_avg[1],
0]) #np.array([y,-z])
delta_pos_avg = np.array([0, -pos_world_avg[0], 0])
print('Pixel positions: ', pos_pixel_avg, '. World positions: ',
pos_world_avg)
drop_pose[1] = drop_pose[1] + pos_world_avg[0]
# Place object using graspingc
self.rel_pose, self.BoxBody = vision_transform_precise_placing_with_visualization(
self.bbox_info, viz_pub=self.viz_array_pub, listener=self.listener)
place(listener=self.listener,
br=self.br,
isExecute=self.isExecute,
binId=fixed_container[0],
withPause=self.withPause,
rel_pose=self.rel_pose,
BoxBody=self.BoxBody,
place_pose=drop_pose,
viz_pub=self.viz_array_pub,
is_drop=False,
recorder=self.gdr)
def get_local_scores(self, obj_dim=[0.12,0.12,0.12], b=0):
self.best_local_score = self.INF*10
best_x = -1
best_y = -1
HeightMap = self.HeightMap[b]
VarHeightMap = self.VarHeightMap[b]
ExpHeightMap = self.ExpHeightMap[b]
if not self.available[b]: #If bin is not available, assign high scores
Scores = 5.*self.INF*np.ones([len(HeightMap), len(HeightMap[0])])
return Scores, [self.best_local_score, b, best_x, best_y, 1000]
Scores = np.zeros([len(HeightMap), len(HeightMap[0])])
max_h = 100
min_max_h = 100
for x in range(len(HeightMap)):
for y in range(len(HeightMap[0])):
#Check if in the cell (x,y) the object center would be outside the tote:
if x < self.limit_x or y< self.limit_y or x >= len(HeightMap)-self.limit_x or y >= len(HeightMap[0])-self.limit_y:
Scores[x][y] = 4*self.INF
continue
p = [x*self.disc, y*self.disc]
#Check if the object fits there
[[mx, Mx], [my, My]] = [[int(math.floor((p[_]-obj_dim[_]/2.)/self.disc)), int(math.ceil((p[_]+obj_dim[_]/2.)/self.disc))] for _ in range(2)]
if mx < self.limit_x or my < self.limit_y or Mx >= len(HeightMap)-self.limit_x or My >= len(HeightMap[0])-self.limit_y:
Scores[x][y] = 2*self.INF
continue
##################
## Height score ##
##################
Scores[x][y] += 0.01*x
Scores[x][y] += 0.01*y
if Scores[x][y] > self.best_local_score:
continue
# Get maximum and minum height in the place where the object will be placed
max_h = self.get_minmax_HM(False, [mx, my], [Mx, My], HeightMap)
min_h = self.get_minmax_HM(True, [mx, my], [Mx, My], HeightMap)
min_max_h = min(max_h, min_max_h)
if max_h + obj_dim[2] > self.h[b]:
Scores[x][y] = self.INF + max_h*self.INF/10.
else:
if max_h < 0.02: #TODO: this seems pretty wrong
Scores[x][y] += (self.h[b]-obj_dim[2])/self.disc #small in small big in big
else:
Scores[x][y] += self.h[b]/self.disc #Always prefer low options
if Scores[x][y] > self.best_local_score:
continue
##################
# Variance score #
##################
variance = VarHeightMap[Mx][My]
expvar = ExpHeightMap[Mx][My]
if my > 0:
variance -= VarHeightMap[Mx][my-1]
expvar -= ExpHeightMap[Mx][my-1]
if mx > 0:
variance -= VarHeightMap[mx-1][My]
expvar -= ExpHeightMap[mx-1][My]
if mx > 0 and my > 0:
variance += VarHeightMap[mx-1][my-1]
expvar += ExpHeightMap[mx-1][my-1]
N = float((Mx-mx+1)*(My-my+1))
var = max(0, variance/N-pow(expvar/N, self.pow_variance)) + pow(0.0001, self.pow_variance)
var = pow(var, 1./self.pow_variance)
Scores[x][y] += var/self.disc
Scores[x][y] += (max_h-min_h)/self.disc
if Scores[x][y] < self.best_local_score:
self.best_local_score = copy.deepcopy(Scores[x][y])
best_x = copy.deepcopy(x)
best_y = copy.deepcopy(y)
print('object dims', obj_dim)
if best_x == -1:
print('The object dimensions: ', obj_dim, ' are probably very large Placing in the center of bin:', b)
best_x = copy.deepcopy(floor(len(HeightMap)/2))
best_y = copy.deepcopy(floor(len(HeightMap[0])/2))
self.best_local_score = copy.deepcopy(self.INF*10)
print('best score: ',self.best_local_score, ' in bin: ', b, ' is at (x,y): ', best_x, best_y)
''' Compute true position'''
obj_pos = [best_x*self.disc, best_y*self.disc]
obj_pose = [obj_pos[0]-self.tote_length/2., obj_pos[1]-self.tote_width/2.,obj_dim[2]/2+self.h_max] #Put pos with respect to the center of the tote
center_bin = get_params_yaml('bin'+str(b)+'_pose')[0:3]
obj_pose[0:3] = [obj_pose[_] + center_bin[_] for _ in range(3)]
#convert obj.theta = 0 into quaternion:
orient_mat_3x3 = rotmatZ(0)
obj_pose[3:7] = mat2quat(orient_mat_3x3)
return Scores, [obj_pose, self.best_local_score, b, best_x, best_y, max_h]