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 '''