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
def __init__(self, move_steps=50, max_params=None, perturbation=None, noise=0, gui=False): def motor_perturbation(m): if perturbation == "BrokenMotor": m[2] = 0 return m elif perturbation == "ShiftedMotor": m[0] = m[0] - 0.3 if m[0] < -1.: m[0] = -1. return m else: return m gripArm_cfg = dict( m_mins=[-1, -1, -1, -1], # joints pos + gripper state m_maxs=[1, 1, 1, 1], s_mins=[ -1, -1, -1, -1, -1 ], # hand pos + hand angle + gripper_change + gripper state s_maxs=[1, 1, 1, 1, 1], lengths=[0.5, 0.3, 0.2], angle_shift=0.5, rest_state=[0., 0., 0., 0.]) stick1_cfg = dict( m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], # Tool pos s_maxs=[2, 2], length=0.5, type="magnetic", handle_tol=0.03, handle_noise=0.1 if noise == 1 else 0., rest_state=[-0.75, 0.25, 0.75], perturbation=perturbation) stick2_cfg = dict(m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], s_maxs=[2, 2], length=0.5, type="scratch", handle_tol=0.03, handle_noise=0.1 if noise == 1 else 0., rest_state=[0.75, 0.25, 0.25]) sticks_cfg = dict( s_mins=[-2, -2, -2, -2], s_maxs=[2, 2, 2, 2], envs_cls=[Stick, Stick], envs_cfg=[stick1_cfg, stick2_cfg], combined_s=lambda s: s # from s: Tool1 end pos + Tool2 end pos ) arm_stick_cfg = dict( m_mins=list([-1.] * 4), # 3DOF + gripper m_maxs=list([1.] * 4), s_mins=list([-2.] * 7), s_maxs=list([2.] * 7), top_env_cls=CombinedEnvironment, lower_env_cls=GripArmEnvironment, top_env_cfg=sticks_cfg, lower_env_cfg=gripArm_cfg, fun_m_lower=lambda m: motor_perturbation(m[0:4]), fun_s_lower=lambda m, s: s + s, # (hand pos + hand angle + gripper_change + gripper state) * 2 tools fun_s_top=lambda m, s_lower, s: s_lower[0:2] + [s_lower[4]] + s ) # from s: Tool1 end pos + Tool2 end pos from m: hand_pos + gripper state mag_object_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-2., -2.], # new pos s_maxs=[2., 2.], object_tol1=0.03, rest_state=[-0.3, 1.1]) mag_object2_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_tol1=0., rest_state=[-0.5, 1.5]) mag_object3_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_tol1=0., rest_state=[-0.3, 1.5]) scratch_object_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_tol2=0.03, rest_state=[0.3, 1.1]) scratch_object2_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_tol2=0., rest_state=[0.3, 1.5]) scratch_object3_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_tol2=0., rest_state=[0.5, 1.5]) cat_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], species="cat", noise=0.01, rest_state=[-0.1, 1.1]) dog_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], species="dog", noise=0.01, rest_state=[0.1, 1.1]) static1_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_shape="rectangle", rest_state=[-0.7, 1.1]) static2_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_shape="rectangle", rest_state=[-0.5, 1.1]) static3_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_shape="rectangle", rest_state=[0.5, 1.1]) static4_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1.5, -1.5], # new pos s_maxs=[1.5, 1.5], object_shape="rectangle", rest_state=[0.7, 1.1]) objects_cfg = dict(s_mins=list([-1.5] * 24), s_maxs=list([1.5] * 24), envs_cls=[ MagneticObject, MagneticObject, MagneticObject, ScratchObject, ScratchObject, ScratchObject, Animal, Animal, StaticObject, StaticObject, StaticObject, StaticObject ], envs_cfg=[ mag_object_cfg, mag_object2_cfg, mag_object3_cfg, scratch_object_cfg, scratch_object2_cfg, scratch_object3_cfg, cat_cfg, dog_cfg, static1_cfg, static2_cfg, static3_cfg, static4_cfg ], combined_s=lambda s: s) def sensory_noise(s): return np.random.random(len(s)) * 0.1 + np.array(s) arm_sticks_objects_cfg = dict( m_mins=arm_stick_cfg['m_mins'], m_maxs=arm_stick_cfg['m_maxs'], s_mins=list([-2.] * 31), s_maxs=list( [2.] * 31 ), # (hand pos + gripper state + tool1 end pos + tool2 end pos + last objects pos top_env_cls=CombinedEnvironment, lower_env_cls=HierarchicallyCombinedEnvironment, top_env_cfg=objects_cfg, lower_env_cfg=arm_stick_cfg, fun_m_lower=lambda m: m, fun_s_lower=lambda m, s: s[3:] + s[3:] + s[3:] + s[3:] + s[3:] + s[ 3:] + s[3:] + s[3:] + s[3:] + s[3:] + s[3:] + s[3:], fun_s_top=lambda m, s_lower, s: sensory_noise(s_lower + s) if noise == 2 else s_lower + s) # box1_cfg = dict( # m_mins = [-2.]*12, # m_maxs = [2.]*12, # s_mins = [0.], # s_maxs = [1.], # box_m_mins = [-1.5, -0.1], # box_m_maxs = [-1.3, 0.1] # ) # # box2_cfg = dict( # m_mins = [-2.]*12, # m_maxs = [2.]*12, # s_mins = [0.], # s_maxs = [1.], # box_m_mins = [-1.2, 1.], # box_m_maxs = [-1., 1.2] # ) # # # boxes_cfg = dict( # s_mins = list([0., 0., 0., 0.]), # s_maxs = list([1., 2., 1., 2.]), # envs_cls = [Box, Box], # envs_cfg = [box1_cfg, box2_cfg], # combined_s = lambda s:s # ) # # static_env_cfg = dict(m_mins=arm_stick_cfg['m_mins'], # m_maxs=arm_stick_cfg['m_maxs'], # s_mins=list([-2.] * 19 + [0., 0., 0., 0.]), # s_maxs=list([2.] * 19 + [1., 2., 1., 2.]), # top_env_cls=CombinedEnvironment, # lower_env_cls=HierarchicallyCombinedEnvironment, # top_env_cfg=boxes_cfg, # lower_env_cfg=arm_sticks_objects_cfg, # fun_m_lower= lambda m:m, # fun_s_lower=lambda m,s:s[7:19]+s[7:19], # fun_s_top=lambda m,s_lower,s: s_lower + s) # denv_cfg = dict(env_cfg=arm_sticks_objects_cfg, env_cls=HierarchicallyCombinedEnvironment, m_mins=[-1.] * 4 * 3, m_maxs=[1.] * 4 * 3, s_mins=[-1.5] * 31 * 3, s_maxs=[1.5] * 31 * 3, n_bfs=2, n_motor_traj_points=3, n_sensori_traj_points=3, move_steps=move_steps, n_dynamic_motor_dims=4, n_dynamic_sensori_dims=31, max_params=max_params, motor_traj_type="DMP", sensori_traj_type="samples", optim_initial_position=False, optim_end_position=True, default_motor_initial_position=[0.] * 4, default_motor_end_position=[0.] * 4, default_sensori_initial_position=[ 0., 1., 0., 0., -0.85, 0.35, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0. ], default_sensori_end_position=[ 0., 1., 0., 0., -0.85, 0.35, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0. ], gui=gui) # DEFAULT S # s_t1 [-1.10355339 -1.10355339 -1.10355339 0.60355339 0.60355339 0.60355339] # s_t2 [ 1.10355339 1.10355339 1.10355339 0.60355339 0.60355339 0.60355339] # s_o1 [-0.3 -0.3 -0.3 1.1 1.1 1.1] # s_o2 [ 0.3 0.3 0.3 1.1 1.1 1.1] # s_o3 [-0.11926431 0.00277633 0.01955739 1.1217168 1.34855082 1.35216384] # s_o4 [ 0.13369242 0.16894663 0.15680744 1.08700156 1.0268538 0.9980367 ] # s_o5 [-0.5 -0.5 -0.5 1.1 1.1 1.1] # s_o6 [ 0.5 0.5 0.5 1.1 1.1 1.1] DynamicEnvironment.__init__(self, **denv_cfg)
def __init__(self, move_steps=50, max_params=None, noise=0, gui=False): arm_cfg = dict(m_mins=[-1, -1, -1], # joints pos m_maxs=[1, 1, 1], s_mins=[-1, -1, -1], # hand pos + hand angle s_maxs=[1, 1, 1], lengths=[0.5, 0.3, 0.2], angle_shift=0.5, rest_state=[0., 0., 0.]) stick1_cfg = dict(m_mins=[-1, -1, -1], m_maxs=[1, 1, 1], s_mins=[-2, -2], # Tool pos s_maxs=[2, 2], length=0.3, type="1", handle_tol=0.1, handle_noise=0.1 if noise == 1 else 0., rest_state=[-0.75, 0.25, 0.75]) # stick2_cfg = dict(m_mins=[-1, -1, -1, -1, -1], # m_maxs=[1, 1, 1, 1, 1], # s_mins=[-2, -2], # s_maxs=[2, 2], # length=0.2, # type="2", # handle_tol=0.1, # handle_noise=0.1 if noise == 1 else 0., # rest_state=[0.75, 0.25, 0.25]) # sticks_cfg = dict( # s_mins = [-2, -2, -2, -2], # s_maxs = [2, 2, 2, 2], # envs_cls = [Stick, Stick], # envs_cfg = [stick1_cfg, stick2_cfg], # combined_s = lambda s:s # from s: Tool1 end pos + Tool2 end pos # ) arm_stick_cfg = dict(m_mins=list([-1.] * 3), # 3DOF + gripper m_maxs=list([1.] * 3), s_mins=list([-2.] * 4), s_maxs=list([2.] * 4), top_env_cls=Stick, lower_env_cls=ArmEnvironment, top_env_cfg=stick1_cfg, lower_env_cfg=arm_cfg, fun_m_lower= lambda m:m, fun_s_lower=lambda m,s:s, # (hand pos + hand angle) * 2 tools fun_s_top=lambda m,s_lower,s:s_lower[0:2] + s) # from s: Tool1 end pos from m: hand_pos object_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-2., -2.], # new pos s_maxs = [2., 2.], object_tol_hand = 0.2, object_tol_tool = 0.1, bounds = np.array([[-0.5, -0.5], [0.5, 0.5]])) def sensory_noise(s): return np.random.random(len(s)) * 0.1 + np.array(s) arm_sticks_object_cfg = dict( m_mins=arm_stick_cfg['m_mins'], m_maxs=arm_stick_cfg['m_maxs'], s_mins=list([-2.] * 6), s_maxs=list([2.] * 6), # (hand pos + tool1 end pos + last objects pos top_env_cls=Object, lower_env_cls=HierarchicallyCombinedEnvironment, top_env_cfg=object_cfg, lower_env_cfg=arm_stick_cfg, fun_m_lower= lambda m:m, fun_s_lower=lambda m,s:s, fun_s_top=lambda m,s_lower,s: sensory_noise(s_lower + s) if noise == 2 else s_lower + s) denv_cfg = dict(env_cfg=arm_sticks_object_cfg, env_cls=HierarchicallyCombinedEnvironment, m_mins=[-1.] * 3 * 3, m_maxs=[1.] * 3 * 3, s_mins=[-1.5] * 6 * 3, s_maxs=[1.5] * 6 * 3, n_bfs = 2, n_motor_traj_points=3, n_sensori_traj_points=3, move_steps=move_steps, n_dynamic_motor_dims=3, n_dynamic_sensori_dims=6, max_params=max_params, motor_traj_type="DMP", sensori_traj_type="samples", optim_initial_position=False, optim_end_position=True, default_motor_initial_position=[0.]*3, default_motor_end_position=[0.]*3, default_sensori_initial_position=[0., 1., -0.85, 0.35, 0., 0.], default_sensori_end_position=[0., 1., -0.85, 0.35, 0., 0.], gui=gui) DynamicEnvironment.__init__(self, **denv_cfg)
def __init__(self, move_steps=50, max_params=None, perturbation=None, noise=0, gui=False): def motor_perturbation(m): if perturbation == "BrokenMotor": m[2] = 0 return m elif perturbation == "ShiftedMotor": m[0] = m[0] - 0.3 if m[0] < -1.: m[0] = -1. return m else: return m gripArm_cfg = dict(m_mins=[-1, -1, -1, -1], # joints pos + gripper state m_maxs=[1, 1, 1, 1], s_mins=[-1, -1, -1, -1, -1], # hand pos + hand angle + gripper_change + gripper state s_maxs=[1, 1, 1, 1, 1], lengths=[0.5, 0.3, 0.2], angle_shift=0.5, rest_state=[0., 0., 0., 0.]) stick1_cfg = dict(m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], # Tool pos s_maxs=[2, 2], length=0.5, type="magnetic", handle_tol=0.03, handle_noise=0.1 if noise == 1 else 0., rest_state=[-0.75, 0.25, 0.75], perturbation=perturbation) stick2_cfg = dict(m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], s_maxs=[2, 2], length=0.5, type="scratch", handle_tol=0.03, handle_noise=0.1 if noise == 1 else 0., rest_state=[0.75, 0.25, 0.25]) sticks_cfg = dict( s_mins = [-2, -2, -2, -2], s_maxs = [2, 2, 2, 2], envs_cls = [Stick, Stick], envs_cfg = [stick1_cfg, stick2_cfg], combined_s = lambda s:s # from s: Tool1 end pos + Tool2 end pos ) arm_stick_cfg = dict(m_mins=list([-1.] * 4), # 3DOF + gripper m_maxs=list([1.] * 4), s_mins=list([-2.] * 7), s_maxs=list([2.] * 7), top_env_cls=CombinedEnvironment, lower_env_cls=GripArmEnvironment, top_env_cfg=sticks_cfg, lower_env_cfg=gripArm_cfg, fun_m_lower= lambda m:motor_perturbation(m[0:4]), fun_s_lower=lambda m,s:s+s, # (hand pos + hand angle + gripper_change + gripper state) * 2 tools fun_s_top=lambda m,s_lower,s:s_lower[0:2] + [s_lower[4]] + s) # from s: Tool1 end pos + Tool2 end pos from m: hand_pos + gripper state mag_object_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-2., -2.], # new pos s_maxs = [2., 2.], object_tol1 = 0.03, rest_state = [-0.3, 1.1]) mag_object2_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_tol1 = 0., rest_state = [-0.5, 1.5]) mag_object3_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_tol1 = 0., rest_state = [-0.3, 1.5]) scratch_object_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_tol2 = 0.03, rest_state = [0.3, 1.1]) scratch_object2_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_tol2 = 0., rest_state = [0.3, 1.5]) scratch_object3_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_tol2 = 0., rest_state = [0.5, 1.5]) cat_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], species= "cat", noise = 0.01, rest_state = [-0.1, 1.1]) dog_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], species= "dog", noise = 0.01, rest_state = [0.1, 1.1]) static1_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_shape = "rectangle", rest_state = [-0.7, 1.1]) static2_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_shape = "rectangle", rest_state = [-0.5, 1.1]) static3_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_shape = "rectangle", rest_state = [0.5, 1.1]) static4_cfg = dict(m_mins = list([-1.] * 4), m_maxs = list([1.] * 4), s_mins = [-1.5, -1.5], # new pos s_maxs = [1.5, 1.5], object_shape = "rectangle", rest_state = [0.7, 1.1]) objects_cfg = dict( s_mins = list([-1.5]*24), s_maxs = list([1.5]*24), envs_cls = [MagneticObject, MagneticObject, MagneticObject, ScratchObject, ScratchObject, ScratchObject, Animal, Animal, StaticObject, StaticObject, StaticObject, StaticObject], envs_cfg = [mag_object_cfg, mag_object2_cfg, mag_object3_cfg, scratch_object_cfg, scratch_object2_cfg, scratch_object3_cfg, cat_cfg, dog_cfg, static1_cfg, static2_cfg, static3_cfg, static4_cfg], combined_s = lambda s:s ) def sensory_noise(s): return np.random.random(len(s)) * 0.1 + np.array(s) arm_sticks_objects_cfg = dict( m_mins=arm_stick_cfg['m_mins'], m_maxs=arm_stick_cfg['m_maxs'], s_mins=list([-2.] * 31), s_maxs=list([2.] * 31), # (hand pos + gripper state + tool1 end pos + tool2 end pos + last objects pos top_env_cls=CombinedEnvironment, lower_env_cls=HierarchicallyCombinedEnvironment, top_env_cfg=objects_cfg, lower_env_cfg=arm_stick_cfg, fun_m_lower= lambda m:m, fun_s_lower=lambda m,s:s[3:]+s[3:]+s[3:]+s[3:]+s[3:]+s[3:]+s[3:]+s[3:]+s[3:]+s[3:]+s[3:]+s[3:], fun_s_top=lambda m,s_lower,s: sensory_noise(s_lower + s) if noise == 2 else s_lower + s) # box1_cfg = dict( # m_mins = [-2.]*12, # m_maxs = [2.]*12, # s_mins = [0.], # s_maxs = [1.], # box_m_mins = [-1.5, -0.1], # box_m_maxs = [-1.3, 0.1] # ) # # box2_cfg = dict( # m_mins = [-2.]*12, # m_maxs = [2.]*12, # s_mins = [0.], # s_maxs = [1.], # box_m_mins = [-1.2, 1.], # box_m_maxs = [-1., 1.2] # ) # # # boxes_cfg = dict( # s_mins = list([0., 0., 0., 0.]), # s_maxs = list([1., 2., 1., 2.]), # envs_cls = [Box, Box], # envs_cfg = [box1_cfg, box2_cfg], # combined_s = lambda s:s # ) # # static_env_cfg = dict(m_mins=arm_stick_cfg['m_mins'], # m_maxs=arm_stick_cfg['m_maxs'], # s_mins=list([-2.] * 19 + [0., 0., 0., 0.]), # s_maxs=list([2.] * 19 + [1., 2., 1., 2.]), # top_env_cls=CombinedEnvironment, # lower_env_cls=HierarchicallyCombinedEnvironment, # top_env_cfg=boxes_cfg, # lower_env_cfg=arm_sticks_objects_cfg, # fun_m_lower= lambda m:m, # fun_s_lower=lambda m,s:s[7:19]+s[7:19], # fun_s_top=lambda m,s_lower,s: s_lower + s) # denv_cfg = dict(env_cfg=arm_sticks_objects_cfg, env_cls=HierarchicallyCombinedEnvironment, m_mins=[-1.] * 4 * 3, m_maxs=[1.] * 4 * 3, s_mins=[-1.5] * 31 * 3, s_maxs=[1.5] * 31 * 3, n_bfs = 2, n_motor_traj_points=3, n_sensori_traj_points=3, move_steps=move_steps, n_dynamic_motor_dims=4, n_dynamic_sensori_dims=31, max_params=max_params, motor_traj_type="DMP", sensori_traj_type="samples", optim_initial_position=False, optim_end_position=True, default_motor_initial_position=[0.]*4, default_motor_end_position=[0.]*4, default_sensori_initial_position=[0., 1., 0., 0., -0.85, 0.35, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], default_sensori_end_position=[0., 1., 0., 0., -0.85, 0.35, 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.], gui=gui) # DEFAULT S # s_t1 [-1.10355339 -1.10355339 -1.10355339 0.60355339 0.60355339 0.60355339] # s_t2 [ 1.10355339 1.10355339 1.10355339 0.60355339 0.60355339 0.60355339] # s_o1 [-0.3 -0.3 -0.3 1.1 1.1 1.1] # s_o2 [ 0.3 0.3 0.3 1.1 1.1 1.1] # s_o3 [-0.11926431 0.00277633 0.01955739 1.1217168 1.34855082 1.35216384] # s_o4 [ 0.13369242 0.16894663 0.15680744 1.08700156 1.0268538 0.9980367 ] # s_o5 [-0.5 -0.5 -0.5 1.1 1.1 1.1] # s_o6 [ 0.5 0.5 0.5 1.1 1.1 1.1] DynamicEnvironment.__init__(self, **denv_cfg)
def __init__(self, move_steps=50, max_params=None, perturbation=None, gui=False): def motor_perturbation(m): if perturbation == "BrokenMotor": m[2] = 0 return m elif perturbation == "ShiftedMotor": m[0] = m[0] - 0.3 if m[0] < -1.: m[0] = -1. return m else: return m gripArm_cfg = dict( m_mins=[-1, -1, -1, -1], # joints pos + gripper state m_maxs=[1, 1, 1, 1], s_mins=[ -1, -1, -1, -1, -1 ], # hand pos + hand angle + gripper_change + gripper state s_maxs=[1, 1, 1, 1, 1], lengths=[0.5, 0.3, 0.2], angle_shift=0.5, rest_state=[0., 0., 0., 0.]) stick1_cfg = dict( m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], # Tool pos s_maxs=[2, 2], length=0.6, handle_tol=0.2, handle_noise=0., rest_state=[-0.75, 0.25, 0.75], perturbation=perturbation) stick2_cfg = dict(m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], s_maxs=[2, 2], length=0.3, handle_tol=0.2, handle_noise=0., rest_state=[0.75, 0.25, 0.25]) sticks_cfg = dict( s_mins=[-2, -2, -2, -2], s_maxs=[2, 2, 2, 2], envs_cls=[Stick, Stick], envs_cfg=[stick1_cfg, stick2_cfg], combined_s=lambda s: s # from s: Tool1 end pos + Tool2 end pos ) arm_stick_cfg = dict( m_mins=list([-1.] * 4), # 3DOF + gripper m_maxs=list([1.] * 4), s_mins=list([-2.] * 7), s_maxs=list([2.] * 7), top_env_cls=CombinedEnvironment, lower_env_cls=GripArmEnvironment, top_env_cfg=sticks_cfg, lower_env_cfg=gripArm_cfg, fun_m_lower=lambda m: [motor_perturbation(m_t[0:4]) for m_t in m], fun_s_lower=lambda m, s: s + s, # (hand pos + hand angle + gripper_change + gripper state) * 2 tools fun_s_top=lambda m, s_lower, s: [s_l[0:2] + [s_l[4]] + s_ for s_l, s_ in zip(s_lower, s)] ) # from s: Tool1 end pos + Tool2 end pos from m: hand_pos + gripper state object_cfg = dict( m_mins=list([-1.] * 4), m_maxs=list([1.] * 4), s_mins=[-1., -1.], # new pos s_maxs=[1., 1.], object_shape="circle", object_tol1=0.1, object_tol2=0.1, rest_state=[0., 1.2]) arm_sticks_object_cfg = dict( m_mins=arm_stick_cfg['m_mins'], m_maxs=arm_stick_cfg['m_maxs'], s_mins=list([-2.] * (7 * move_steps + 2)), s_maxs=list( [2.] * (7 * move_steps + 2) ), # (hand pos + gripper state + tool1 end pos + tool2 end pos) traj + last object pos = 7 x samples + 2 top_env_cls=SceneObject, lower_env_cls=HierarchicallyCombinedEnvironment, top_env_cfg=object_cfg, lower_env_cfg=arm_stick_cfg, fun_m_lower=lambda m: m, fun_s_lower=lambda m, s: s[3:], fun_s_top=lambda m, s_lower, s: [si[:7] for si in s_lower] + s) box1_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[-1.5, -0.1], box_m_maxs=[-1.3, 0.1]) box2_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[-1.2, 1.], box_m_maxs=[-1., 1.2]) box3_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[-0.1, 1.3], box_m_maxs=[0.1, 1.5]) box4_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[1., 1.], box_m_maxs=[1.2, 1.2]) box5_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[1.3, -0.1], box_m_maxs=[1.5, 0.1]) box6_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[-1., -0.1], box_m_maxs=[-0.8, 0.1]) box7_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[-0.7, 0.5], box_m_maxs=[-0.5, 0.7]) box8_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[-0.1, 0.8], box_m_maxs=[0.1, 1.]) box9_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[0.5, 0.5], box_m_maxs=[0.7, 0.7]) box10_cfg = dict(m_mins=[-1., -1.], m_maxs=[1., 1.], s_mins=[0.], s_maxs=[1.], box_m_mins=[0.8, -0.1], box_m_maxs=[1., 0.1]) def combined_boxes(s): if s[0]: b = 1. elif s[2]: b = 2. elif s[4]: b = 3. elif s[6]: b = 4. elif s[8]: b = 5. elif s[10]: b = 6. elif s[12]: b = 7. elif s[14]: b = 8. elif s[16]: b = 9. elif s[18]: b = 10. else: b = 0. d = min([ 0.3, s[1], s[3], s[5], s[7], s[9], s[11], s[13], s[15], s[17], s[19] ]) return [b, d] boxes_cfg = dict( s_mins=list([0., 0.]), s_maxs=list([10., 2.]), envs_cls=[Box, Box, Box, Box, Box, Box, Box, Box, Box, Box], envs_cfg=[ box1_cfg, box2_cfg, box3_cfg, box4_cfg, box5_cfg, box6_cfg, box7_cfg, box8_cfg, box9_cfg, box10_cfg ], combined_s=combined_boxes) static_env_cfg = dict( m_mins=arm_stick_cfg['m_mins'], m_maxs=arm_stick_cfg['m_maxs'], s_mins=list([-2.] * (7 * move_steps + 2) + [0., 0.]), # 7x + 2 obj + 2 boxes s_maxs=list([2.] * (7 * move_steps + 2)) + [10., 2.], top_env_cls=CombinedEnvironment, lower_env_cls=HierarchicallyCombinedEnvironment, top_env_cfg=boxes_cfg, lower_env_cfg=arm_sticks_object_cfg, fun_m_lower=lambda m: m, fun_s_lower=lambda m, s: s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:], fun_s_top=lambda m, s_lower, s: s_lower + s) denv_cfg = dict(env_cfg=static_env_cfg, env_cls=HierarchicallyCombinedEnvironment, m_mins=[-1.] * 4 * 3, m_maxs=[1.] * 4 * 3, s_mins=[-1.] * (3 * 3) + [-1.5] * 3 + [0.] * 3 + [-1.5] * 3 + [0.] * 3 + [-2., -2.] + [0., 0.], s_maxs=[1.] * (3 * 3) + [1.5, 1.5] * 3 + [1.5, 1.5] * 3 + [2., 2.] + [10., 0.3], n_bfs=2, n_motor_traj_points=3, n_sensori_traj_points=3, move_steps=move_steps, n_dynamic_motor_dims=4, n_dynamic_sensori_dims=7, max_params=max_params, motor_traj_type="DMP", sensori_traj_type="samples", optim_initial_position=False, optim_end_position=True, default_motor_initial_position=[0.] * 4, default_motor_end_position=[0.] * 4, default_sensori_initial_position=[ 0., 1., 0., 0., -0.85, 0.35, 1.2, 0.7 ], default_sensori_end_position=[ 0., 1., 0., 0., -0.85, 0.35, 1.2, 0.7 ], gui=gui) DynamicEnvironment.__init__(self, **denv_cfg)
def __init__(self, move_steps=50, max_params=None, perturbation=None, gui=False): def motor_perturbation(m): if perturbation == "BrokenMotor": m[2] = 0 return m elif perturbation == "ShiftedMotor": m[0] = m[0] - 0.3 if m[0] < -1.0: m[0] = -1.0 return m else: return m gripArm_cfg = dict( m_mins=[-1, -1, -1, -1], # joints pos + gripper state m_maxs=[1, 1, 1, 1], s_mins=[-1, -1, -1, -1, -1], # hand pos + hand angle + gripper_change + gripper state s_maxs=[1, 1, 1, 1, 1], lengths=[0.5, 0.3, 0.2], angle_shift=0.5, rest_state=[0.0, 0.0, 0.0, 0.0], ) stick1_cfg = dict( m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], # Tool pos s_maxs=[2, 2], length=0.6, handle_tol=0.2, handle_noise=0.0, rest_state=[-0.75, 0.25, 0.75], perturbation=perturbation, ) stick2_cfg = dict( m_mins=[-1, -1, -1, -1, -1], m_maxs=[1, 1, 1, 1, 1], s_mins=[-2, -2], s_maxs=[2, 2], length=0.3, handle_tol=0.2, handle_noise=0.0, rest_state=[0.75, 0.25, 0.25], ) sticks_cfg = dict( s_mins=[-2, -2, -2, -2], s_maxs=[2, 2, 2, 2], envs_cls=[Stick, Stick], envs_cfg=[stick1_cfg, stick2_cfg], combined_s=lambda s: s, # from s: Tool1 end pos + Tool2 end pos ) arm_stick_cfg = dict( m_mins=list([-1.0] * 4), # 3DOF + gripper m_maxs=list([1.0] * 4), s_mins=list([-2.0] * 7), s_maxs=list([2.0] * 7), top_env_cls=CombinedEnvironment, lower_env_cls=GripArmEnvironment, top_env_cfg=sticks_cfg, lower_env_cfg=gripArm_cfg, fun_m_lower=lambda m: [motor_perturbation(m_t[0:4]) for m_t in m], fun_s_lower=lambda m, s: s + s, # (hand pos + hand angle + gripper_change + gripper state) * 2 tools fun_s_top=lambda m, s_lower, s: [s_l[0:2] + [s_l[4]] + s_ for s_l, s_ in zip(s_lower, s)], ) # from s: Tool1 end pos + Tool2 end pos from m: hand_pos + gripper state object_cfg = dict( m_mins=list([-1.0] * 4), m_maxs=list([1.0] * 4), s_mins=[-1.0, -1.0], # new pos s_maxs=[1.0, 1.0], object_shape="circle", object_tol1=0.1, object_tol2=0.1, rest_state=[0.0, 1.2], ) arm_sticks_object_cfg = dict( m_mins=arm_stick_cfg["m_mins"], m_maxs=arm_stick_cfg["m_maxs"], s_mins=list([-2.0] * (7 * move_steps + 2)), s_maxs=list( [2.0] * (7 * move_steps + 2) ), # (hand pos + gripper state + tool1 end pos + tool2 end pos) traj + last object pos = 7 x samples + 2 top_env_cls=SceneObject, lower_env_cls=HierarchicallyCombinedEnvironment, top_env_cfg=object_cfg, lower_env_cfg=arm_stick_cfg, fun_m_lower=lambda m: m, fun_s_lower=lambda m, s: s[3:], fun_s_top=lambda m, s_lower, s: [si[:7] for si in s_lower] + s, ) box1_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[-1.5, -0.1], box_m_maxs=[-1.3, 0.1], ) box2_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[-1.2, 1.0], box_m_maxs=[-1.0, 1.2], ) box3_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[-0.1, 1.3], box_m_maxs=[0.1, 1.5], ) box4_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[1.0, 1.0], box_m_maxs=[1.2, 1.2], ) box5_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[1.3, -0.1], box_m_maxs=[1.5, 0.1], ) box6_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[-1.0, -0.1], box_m_maxs=[-0.8, 0.1], ) box7_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[-0.7, 0.5], box_m_maxs=[-0.5, 0.7], ) box8_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[-0.1, 0.8], box_m_maxs=[0.1, 1.0], ) box9_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[0.5, 0.5], box_m_maxs=[0.7, 0.7], ) box10_cfg = dict( m_mins=[-1.0, -1.0], m_maxs=[1.0, 1.0], s_mins=[0.0], s_maxs=[1.0], box_m_mins=[0.8, -0.1], box_m_maxs=[1.0, 0.1], ) def combined_boxes(s): if s[0]: b = 1.0 elif s[2]: b = 2.0 elif s[4]: b = 3.0 elif s[6]: b = 4.0 elif s[8]: b = 5.0 elif s[10]: b = 6.0 elif s[12]: b = 7.0 elif s[14]: b = 8.0 elif s[16]: b = 9.0 elif s[18]: b = 10.0 else: b = 0.0 d = min([0.3, s[1], s[3], s[5], s[7], s[9], s[11], s[13], s[15], s[17], s[19]]) return [b, d] boxes_cfg = dict( s_mins=list([0.0, 0.0]), s_maxs=list([10.0, 2.0]), envs_cls=[Box, Box, Box, Box, Box, Box, Box, Box, Box, Box], envs_cfg=[ box1_cfg, box2_cfg, box3_cfg, box4_cfg, box5_cfg, box6_cfg, box7_cfg, box8_cfg, box9_cfg, box10_cfg, ], combined_s=combined_boxes, ) static_env_cfg = dict( m_mins=arm_stick_cfg["m_mins"], m_maxs=arm_stick_cfg["m_maxs"], s_mins=list([-2.0] * (7 * move_steps + 2) + [0.0, 0.0]), # 7x + 2 obj + 2 boxes s_maxs=list([2.0] * (7 * move_steps + 2)) + [10.0, 2.0], top_env_cls=CombinedEnvironment, lower_env_cls=HierarchicallyCombinedEnvironment, top_env_cfg=boxes_cfg, lower_env_cfg=arm_sticks_object_cfg, fun_m_lower=lambda m: m, fun_s_lower=lambda m, s: s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:] + s[-2:], fun_s_top=lambda m, s_lower, s: s_lower + s, ) denv_cfg = dict( env_cfg=static_env_cfg, env_cls=HierarchicallyCombinedEnvironment, m_mins=[-1.0] * 4 * 3, m_maxs=[1.0] * 4 * 3, s_mins=[-1.0] * (3 * 3) + [-1.5] * 3 + [0.0] * 3 + [-1.5] * 3 + [0.0] * 3 + [-2.0, -2.0] + [0.0, 0.0], s_maxs=[1.0] * (3 * 3) + [1.5, 1.5] * 3 + [1.5, 1.5] * 3 + [2.0, 2.0] + [10.0, 0.3], n_bfs=2, n_motor_traj_points=3, n_sensori_traj_points=3, move_steps=move_steps, n_dynamic_motor_dims=4, n_dynamic_sensori_dims=7, max_params=max_params, motor_traj_type="DMP", sensori_traj_type="samples", optim_initial_position=False, optim_end_position=True, default_motor_initial_position=[0.0] * 4, default_motor_end_position=[0.0] * 4, default_sensori_initial_position=[0.0, 1.0, 0.0, 0.0, -0.85, 0.35, 1.2, 0.7], default_sensori_end_position=[0.0, 1.0, 0.0, 0.0, -0.85, 0.35, 1.2, 0.7], gui=gui, ) DynamicEnvironment.__init__(self, **denv_cfg)