def adjust_approx_sim(self, real_traj, real_bounces, real_contacted_objs, action=None):
es_traj, es_bounces, es_contacted_objs = self.scenario.find_man_traj_bounce()
es_num_of_bounces = len(es_contacted_objs)
real_num_of_bounces = len(real_contacted_objs)
for i in xrange(es_num_of_bounces):
for j in xrange(real_num_of_bounces):
if i == j and es_contacted_objs[i] == real_contacted_objs[j] and es_contacted_objs[i] != 0:
es_bounce_pt_inx = es_bounces[i]
real_bounce_pt_inx = real_bounces[j]
#if len(es_traj) - 1 < es_bounce_pt_inx + 2 or real_bounce_pt_inx + 2 > len(real_traj) - 1:
# break
es_bounces_pts = [es_traj[es_bounce_pt_inx - 1], es_traj[es_bounce_pt_inx],
es_traj[es_bounce_pt_inx + 1]]
real_bounces_pts = [real_traj[real_bounce_pt_inx - 1], real_traj[real_bounce_pt_inx],
real_traj[real_bounce_pt_inx + 1]]
angle_difference, translation = Adjust.find_spatial_difference(es_bounces_pts, real_bounces_pts)
# step = sigma_angle
step = 0.001
if angle_difference > 0:
neg_flag = False
else:
neg_flag = True
obj = self.objs_map[es_contacted_objs[i]]
#obj['position'] = (obj['position'][0] + translation[0], obj['position'][1] + translation[1])
# last_obj_angle = obj['angle']
current_obj_angle = obj['angle']
last_obj_angle = obj['angle']
last_angle_difference = angle_difference
min_angle_difference = 100
min_obj_angle = obj['angle']
count = 400
original_angle = obj['angle']
reversed = False
if abs(angle_difference) <= 0.001:
print ' !!!!!! obj angle', min_obj_angle, ' angfle difference ', angle_difference
while (abs(angle_difference) > 0.001) and count >= 0:
count -= 1
# update
#print 'angle differ: ', angle_difference, ' obj angle: ', obj['angle']
#print "translation: ", translation
#print 'current angle difference: ', angle_difference, " last angle difference", last_angle_difference, " ", obj['angle']
# add a map record the last angle difference recorded for each adjusted object
'''
if not reversed:
obj['angle'] = last_obj_angle + step
last_obj_angle = obj['angle']
last_angle_difference = angle_difference
else:
obj['angle'] = last_obj_angle - step
last_obj_angle = obj['angle']
last_angle_difference = angle_difference
'''
'''
if angle_difference < 0:
obj['angle'] = last_obj_angle + step
last_obj_angle = obj['angle']
last_angle_difference = angle_difference
else:
obj['angle'] = last_obj_angle - step
last_obj_angle = obj['angle']
last_angle_difference = angle_difference
'''
'''
print 'angle difference: ', angle_difference, " current: ", current_obj_angle, " last: ", last_obj_angle, " neg flag: ", neg_flag
if angle_difference < 0:
if not neg_flag:
obj['angle'] = (last_obj_angle + current_obj_angle)/2
last_obj_angle = current_obj_angle
current_obj_angle = obj['angle']
neg_flag = True
else:
obj['angle'] += step
last_obj_angle = current_obj_angle
current_obj_angle = obj['angle']
else:
if neg_flag:
obj['angle'] = (last_obj_angle + current_obj_angle) / 2
last_obj_angle = current_obj_angle
current_obj_angle = obj['angle']
neg_flag = False
else:
obj['angle'] -= step
last_obj_angle = current_obj_angle
current_obj_angle = obj['angle']
'''
self.new_scenario()
self.scenario.apply_impulse_and_run(action)
_es_traj, _es_bounces, _es_contacted_objs = self.scenario.find_man_traj_bounce()
_es_bounces_pts = [_es_traj[es_bounce_pt_inx - 1], _es_traj[es_bounce_pt_inx],
_es_traj[es_bounce_pt_inx + 1]]
angle_difference, translation = Adjust.find_spatial_difference(_es_bounces_pts,
real_bounces_pts)
if abs(angle_difference) < min_angle_difference:
min_angle_difference = abs(angle_difference)
min_obj_angle = obj['angle']
if count < 200:
obj['angle'] += step
elif count == 200:
obj['angle'] = original_angle + step
else:
obj['angle'] -= step
'''
if abs(angle_difference) < min_angle_difference:
min_angle_difference = abs(angle_difference)
min_obj_angle = obj['angle']
#step *= 2
else:
reversed = not reversed
step /= 2
'''
#if abs(translation[0]) < sigma_pos and abs(translation[1]) < sigma_pos:
# obj['position'] = (obj['position'][0] + translation[0], obj['position'][1] + translation[1])
if min_angle_difference < self.obj_min_angle_difference[obj['id']]:
obj['angle'] = min_obj_angle
self.obj_min_angle_difference[obj['id']] = min_angle_difference
print 'adjusted angle: ', min_obj_angle, 'min_angle_difference: ', min_angle_difference, ' final angle: ', obj['angle'], ' final difference: ', self.obj_min_angle_difference[obj['id']]
break
else:
continue
return
x, y = real_traj[inx]
# print x, y
bounce_x_list.append(x)
bounce_y_list.append(y)
bounce_inx = 0
bounce_pt_inx = es_bounces[bounce_inx]
traj_x_list = [real_traj[bounce_pt_inx - 1][0], real_traj[bounce_pt_inx + 2][0]]
traj_y_list = [real_traj[bounce_pt_inx - 1][1], real_traj[bounce_pt_inx + 2][1]]
# analyze the first bouncing point
estimated_bounces_pts = [es_traj[bounce_pt_inx - 1], es_traj[bounce_pt_inx], es_traj[bounce_pt_inx + 2]]
print estimated_bounces_pts
real_bounces_pts = [real_traj[bounce_pt_inx - 1], real_traj[bounce_pt_inx], real_traj[bounce_pt_inx + 2]]
print real_bounces_pts
angle_difference, translation = Adjust.find_spatial_difference(estimated_bounces_pts, real_bounces_pts)
file_name = 's2p2'
scenario_width, scenario_height, immobile_objs, mobile_objs, manipulatable_obj, target_obj = loadScenario(
'./scenarios/' + file_name + '.json')
'''
for obj in immobile_objs:
if obj['id'] == es_contact_obj_id[bounce_inx]:
#print obj
obj['angle'] = obj['angle'] + angle_difference
obj['position'] = (obj['position'][0] + translation[0], obj['position'][1] + translation[1])
print "after adjsuted: ", obj
break
'''