def compute_sensori_effect(self, m_traj):
c = self.current_context
s = DynamicEnvironment.compute_sensori_effect(self, m_traj)
#print "s", s
s_o_end = s[[-4,-1]]
#print "s_o_end", s_o_end
res = list(s[:-6]) + list(np.array(s_o_end) - np.array(c))
#print res
s_ = res
#print "s_", s_
obj_end_pos_y = s_o_end[1]
tool1_moved = (abs(s_[-5] - s_[-3]) > 0.0001)
#tool2_moved = (abs(ms[-5] - ms[-3]) > 0.0001)
tool1_touched_obj = tool1_moved and (abs(s_[-3] - obj_end_pos_y) < 0.0001)
#tool2_touched_obj = tool2_moved and (abs(ms[-3] - obj_end_pos_y) < 0.0001)
obj_moved = abs(s_[-1]) > 0.0001
obj_moved_with_hand = obj_moved and (not tool1_touched_obj)# and (not tool2_touched_obj)
#print "obj_end_pos_y", obj_end_pos_y, "tool end y", s_[-3]
# if tool1_moved:
# print "tool moved"
# if tool1_touched_obj:
# print "object moved by tool"
# #raise
# elif obj_moved_with_hand:
# print "object moved by hand"
if tool1_touched_obj or (tool1_moved and not obj_moved_with_hand):
tool_traj = [st[2:4] for st in self.s_traj]
min_dist = min([np.linalg.norm(np.array(st) - np.array(s_o_end)) for st in tool_traj])
#print min_dist
else:
hand_traj = [sh[:2] for sh in self.s_traj]
min_dist = min([np.linalg.norm(np.array(sh) - np.array(s_o_end)) for sh in hand_traj])
#print list(s[:-6]), [min_dist], list(np.array(s_o_end) - np.array(c))
res = list(s[:-6]) + [min_dist] + list(np.array(s_o_end) - np.array(c))
#print "s env", res
self.env.lower_env.reset() # reset arm and tools but not object
self.env.top_env.move = 0 # tools have been reset so object must not follow them
return res
