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)
