def __init__(self, formula):
self.formula = formula
self.potential = rand_float(0, 7.5)
self.initial_conc = rand_float(0, 2)
self.inflow = rand_float(0, 1)
self.decay = rand_float(0, 1)
self.is_stimulus = False
self.is_control = False
self.is_output = False
self.is_food = False
self.conc = 0
self.delta = 0
def generate_Box_goal(args, video=True, image=False):
engine_goal = BoxEngine(args.dt, args.state_dim, args.action_dim)
scene_ckp = engine_goal.reset_scene(args.n_particle)
states_rec = np.zeros((args.roll_step, args.n_particle, args.state_dim))
actions_rec = np.zeros((args.roll_step, 1, args.action_dim))
viss_rec = np.zeros((args.roll_step, args.n_particle))
for t in range(args.roll_step):
engine_goal.set_action(rand_float(-600., 100.))
states_rec[t] = engine_goal.get_state()
actions_rec[t] = engine_goal.get_action()
viss_rec[t] = engine_goal.get_vis(states_rec[t])
engine_goal.step()
state_goal_full = engine_goal.get_state()
vis_goal_full = engine_goal.get_vis(state_goal_full)
assert state_goal_full.shape[0] == args.n_particle
assert state_goal_full.shape[1] == args.state_dim
print('states_goal_full', state_goal_full.shape)
render_Box(args.mpcf, 'mpc_Box_goal', lim, states_rec, actions_rec, viss_rec, video=video, image=image)
return state_goal_full[vis_goal_full], state_goal_full, vis_goal_full, scene_ckp
def test_gen_Cradle(info):
thread_idx, n_balls, n_rollout, time_step, dt, video, image, data_dir = \
info['thread_idx'], info['n_balls'], info['n_rollout'], info['time_step'], info['dt'], \
info['video'], info['image'], info['data_dir']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2 ** 32)
lim = 400
attr_dim = 2
state_dim = 4
relation_dim = 4
engine = CradleEngine(dt)
n_objects = n_balls * 2
states = np.zeros((n_rollout, time_step, n_objects, state_dim))
bar = ProgressBar()
for i in bar(range(n_rollout)):
theta = rand_float(0, 90)
engine.reset_scene(n_balls, theta)
for j in range(time_step):
states[i, j] = engine.get_state()
if j > 0:
states[i, j, :, 2:] = (states[i, j, :, :2] - states[i, j - 1, :, :2]) / dt
engine.step()
if video or image:
render_Cradle(data_dir, 'test_cradle_%d' % i, lim, states[i], video=video, image=image)
def add_rels(self, param_load=None):
param = np.zeros((self.n_ball * (self.n_ball - 1) // 2, 2))
self.param_dim = param.shape[0]
if param_load is not None:
print("Load param for init env")
cnt = 0
rels_idx = []
for i in range(self.n_ball):
for j in range(i):
rel_type = rand_int(
0,
self.n_rel_type) if param_load is None else param_load[cnt,
0]
param[cnt, 0] = rel_type
rels_idx.append([i, j])
pos_i = self.balls[i].position
pos_j = self.balls[j].position
if rel_type == 0:
# no relation
pass
elif rel_type == 1:
# spring
rest_length = rand_float(
20, 120) if param_load is None else param_load[cnt, 1]
param[cnt, 1] = rest_length
c = pymunk.DampedSpring(self.balls[i],
self.balls[j], (0, 0), (0, 0),
rest_length=rest_length,
stiffness=20,
damping=0.)
self.space.add(c)
elif rel_type == 2:
# string
rest_length = calc_dis(
pos_i,
pos_j) if param_load is None else param_load[cnt, 1]
param[cnt, 1] = rest_length
c = pymunk.SlideJoint(self.balls[i], self.balls[j], (0, 0),
(0, 0), rest_length - 5,
rest_length + 5)
self.space.add(c)
else:
raise AssertionError("Unknown relation type")
cnt += 1
if param_load is not None:
assert ((param == param_load).all())
self.rels_idx = rels_idx
self.param = param
def test_gen_Box(info):
thread_idx, data_dir = info['thread_idx'], info['data_dir']
n_rollout, n_particle, time_step = info['n_rollout'], info['n_particle'], info['time_step']
dt, video, image = info['dt'], info['video'], info['image']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2**32)
state_dim = 6 # x, y, angle, xdot, ydot, angledot
action_dim = 2 # x, xdot
stats = [init_stat(6), init_stat(2)]
engine = BoxEngine(dt, state_dim, action_dim)
bar = ProgressBar()
for i in bar(range(n_rollout)):
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
# os.system('mkdir -p ' + rollout_dir)
engine.reset_scene(n_particle)
states = np.zeros((time_step, n_particle, state_dim), dtype=np.float32)
actions = np.zeros((time_step, 1, action_dim), dtype=np.float32)
vis = np.zeros((time_step, n_particle), dtype=np.bool)
for j in range(time_step):
engine.set_action(rand_float(-600., 100.))
states[j] = engine.get_state()
actions[j] = engine.get_action()
vis[j] = engine.get_vis(states[j])
if j > 0:
actions[j, :, 1] = (actions[j, :, 0] - actions[j - 1, :, 0]) / dt
# data = [states[j], actions[j], vis[j]]
# store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
engine.step()
# datas = [states.astype(np.float64), actions.astype(np.float64)]
'''
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0] * datas[j].shape[1]
stats[j] = combine_stat(stats[j], stat)
'''
if video:
lim = [-600, 600, -15, 400]
if video or image:
render_Box(data_dir, 'test_Box_%d' % i, lim, states, actions, vis, video=video, image=image)
def add_balls(self, center=(0., 0.), p_range=(-60, 60)):
inertia = pymunk.moment_for_circle(self.mass, 0, self.radius, (0, 0))
for i in range(self.n_ball):
while True:
x = rand_float(p_range[0], p_range[1])
y = rand_float(p_range[0], p_range[1])
flag = True
for j in range(i):
if calc_dis([x, y], self.balls[j].position) < 30:
flag = False
if flag:
break
body = pymunk.Body(self.mass, inertia)
body.position = Vec2d(x, y)
shape = pymunk.Circle(body, 0., (0, 0))
shape.elasticity = 1
self.space.add(body, shape)
self.balls.append(body)
def gen_PyFleX(info):
env, env_idx = info['env'], info['env_idx']
thread_idx, data_dir, data_names = info['thread_idx'], info['data_dir'], info['data_names']
n_rollout, time_step = info['n_rollout'], info['time_step']
shape_state_dim, dt = info['shape_state_dim'], info['dt']
gen_vision = info['gen_vision']
vision_dir, vis_width, vis_height = info['vision_dir'], info['vis_width'], info['vis_height']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2 ** 32)
# positions
stats = [init_stat(3)]
import pyflex
pyflex.init()
for i in range(n_rollout):
if i % 10 == 0:
print("%d / %d" % (i, n_rollout))
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
os.system('mkdir -p ' + rollout_dir)
if env == 'RigidFall':
g_low, g_high = info['physics_param_range']
gravity = rand_float(g_low, g_high)
print("Generated RigidFall rollout {} with gravity {} from range {} ~ {}".format(
i, gravity, g_low, g_high))
n_instance = 3
draw_mesh = 1
scene_params = np.zeros(n_instance * 3 + 3)
scene_params[0] = n_instance
scene_params[1] = gravity
scene_params[-1] = draw_mesh
low_bound = 0.09
for j in range(n_instance):
x = rand_float(0., 0.1)
y = rand_float(low_bound, low_bound + 0.01)
z = rand_float(0., 0.1)
scene_params[j * 3 + 2] = x
scene_params[j * 3 + 3] = y
scene_params[j * 3 + 4] = z
low_bound += 0.21
pyflex.set_scene(env_idx, scene_params, thread_idx)
pyflex.set_camPos(np.array([0.2, 0.875, 2.0]))
n_particles = pyflex.get_n_particles()
n_shapes = 1 # the floor
positions = np.zeros((time_step, n_particles + n_shapes, 3), dtype=np.float32)
shape_quats = np.zeros((time_step, n_shapes, 4), dtype=np.float32)
for j in range(time_step):
positions[j, :n_particles] = pyflex.get_positions().reshape(-1, 4)[:, :3]
ref_positions = positions[0]
for k in range(n_instance):
XX = ref_positions[64*k:64*(k+1)]
YY = positions[j, 64*k:64*(k+1)]
X = XX.copy().T
Y = YY.copy().T
mean_X = np.mean(X, 1, keepdims=True)
mean_Y = np.mean(Y, 1, keepdims=True)
X = X - mean_X
Y = Y - mean_Y
C = np.dot(X, Y.T)
U, S, Vt = np.linalg.svd(C)
D = np.eye(3)
D[2, 2] = np.linalg.det(np.dot(Vt.T, U.T))
R = np.dot(Vt.T, np.dot(D, U.T))
t = mean_Y - np.dot(R, mean_X)
YY_fitted = (np.dot(R, XX.T) + t).T
# print("MSE fit", np.mean(np.square(YY_fitted - YY)))
positions[j, 64*k:64*(k+1)] = YY_fitted
if gen_vision:
pyflex.step(capture=True, path=os.path.join(rollout_dir, str(j) + '.tga'))
else:
pyflex.step()
data = [positions[j], shape_quats[j], scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
if gen_vision:
images = np.zeros((time_step, vis_height, vis_width, 3), dtype=np.uint8)
for j in range(time_step):
img_path = os.path.join(rollout_dir, str(j) + '.tga')
img = scipy.misc.imread(img_path)[:, :, :3][:, :, ::-1]
img = cv2.resize(img, (vis_width, vis_height), interpolation=cv2.INTER_AREA)
images[j] = img
os.system('rm ' + img_path)
store_data(['positions', 'images', 'scene_params'], [positions, images, scene_params],
os.path.join(vision_dir, str(rollout_idx) + '.h5'))
elif env == 'MassRope':
s_low, s_high = info['physics_param_range']
stiffness = rand_float(s_low, s_high)
print("Generated MassRope rollout {} with gravity {} from range {} ~ {}".format(
i, stiffness, s_low, s_high))
x = 0.
y = 1.0
z = 0.
length = 0.7
draw_mesh = 1.
scene_params = np.array([x, y, z, length, stiffness, draw_mesh])
pyflex.set_scene(env_idx, scene_params, 0)
pyflex.set_camPos(np.array([0.13, 2.0, 3.2]))
action = np.zeros(3)
# the last particle is the pin, regarded as shape
n_particles = pyflex.get_n_particles() - 1
n_shapes = 1 # the mass at the top of the rope
positions = np.zeros((time_step + 1, n_particles + n_shapes, 3), dtype=np.float32)
shape_quats = np.zeros((time_step + 1, n_shapes, 4), dtype=np.float32)
action = np.zeros(3)
for j in range(time_step + 1):
positions[j] = pyflex.get_positions().reshape(-1, 4)[:, :3]
if j >= 1:
# append the action (position of the pin) to the previous time step
positions[j - 1, -1, :] = positions[j, -1, :]
ref_positions = positions[0]
# apply rigid projection to the rigid object
# cube: [0, 81)
# rope: [81, 95)
# pin: [95, 96)
XX = ref_positions[:81]
YY = positions[j, :81]
X = XX.copy().T
Y = YY.copy().T
mean_X = np.mean(X, 1, keepdims=True)
mean_Y = np.mean(Y, 1, keepdims=True)
X = X - mean_X
Y = Y - mean_Y
C = np.dot(X, Y.T)
U, S, Vt = np.linalg.svd(C)
D = np.eye(3)
D[2, 2] = np.linalg.det(np.dot(Vt.T, U.T))
R = np.dot(Vt.T, np.dot(D, U.T))
t = mean_Y - np.dot(R, mean_X)
YY_fitted = (np.dot(R, XX.T) + t).T
positions[j, :81] = YY_fitted
scale = 0.1
action[0] += rand_float(-scale, scale) - positions[j, -1, 0] * 0.1
action[2] += rand_float(-scale, scale) - positions[j, -1, 2] * 0.1
if gen_vision:
pyflex.step(action * dt, capture=True, path=os.path.join(rollout_dir, str(j) + '.tga'))
else:
pyflex.step(action * dt)
if j >= 1:
data = [positions[j - 1], shape_quats[j - 1], scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j - 1) + '.h5'))
if gen_vision:
images = np.zeros((time_step, vis_height, vis_width, 3), dtype=np.uint8)
for j in range(time_step):
img_path = os.path.join(rollout_dir, str(j) + '.tga')
img = scipy.misc.imread(img_path)[:, :, :3][:, :, ::-1]
img = cv2.resize(img, (vis_width, vis_height), interpolation=cv2.INTER_AREA)
images[j] = img
os.system('rm ' + img_path)
store_data(['positions', 'images', 'scene_params'], [positions, images, scene_params],
os.path.join(vision_dir, str(rollout_idx) + '.h5'))
else:
raise AssertionError("Unsupported env")
# change dtype for more accurate stat calculation
# only normalize positions
datas = [positions[:time_step].astype(np.float64)]
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0] * datas[j].shape[1]
stats[j] = combine_stat(stats[j], stat)
pyflex.clean()
return stats
def gen_PyFleX(info):
env, root_num = info['env'], info['root_num']
thread_idx, data_dir, data_names = info['thread_idx'], info['data_dir'], info['data_names']
n_rollout, n_instance = info['n_rollout'], info['n_instance']
time_step, time_step_clip = info['time_step'], info['time_step_clip']
shape_state_dim, dt = info['shape_state_dim'], info['dt']
env_idx = info['env_idx']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2**32) ### NOTE: we might want to fix the seed for reproduction
# positions, velocities
if env_idx == 5: # RiceGrip
stats = [init_stat(6), init_stat(6)]
else:
stats = [init_stat(3), init_stat(3)]
import pyflex
pyflex.init()
for i in range(n_rollout):
if i % 10 == 0:
print("%d / %d" % (i, n_rollout))
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
os.system('mkdir -p ' + rollout_dir)
if env == 'FluidFall':
scene_params = np.zeros(1)
pyflex.set_scene(env_idx, scene_params, thread_idx)
n_particles = pyflex.get_n_particles()
positions = np.zeros((time_step, n_particles, 3), dtype=np.float32)
velocities = np.zeros((time_step, n_particles, 3), dtype=np.float32)
for j in range(time_step_clip):
p_clip = pyflex.get_positions().reshape(-1, 4)[:, :3]
pyflex.step()
for j in range(time_step):
positions[j] = pyflex.get_positions().reshape(-1, 4)[:, :3]
if j == 0:
velocities[j] = (positions[j] - p_clip) / dt
else:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
data = [positions[j], velocities[j]]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
elif env == 'BoxBath':
# BoxBath
scene_params = np.zeros(1)
pyflex.set_scene(env_idx, scene_params, thread_idx)
n_particles = pyflex.get_n_particles()
positions = np.zeros((time_step, n_particles, 3), dtype=np.float32)
velocities = np.zeros((time_step, n_particles, 3), dtype=np.float32)
for j in range(time_step_clip):
pyflex.step()
p = pyflex.get_positions().reshape(-1, 4)[:64, :3]
clusters = []
st_time = time.time()
kmeans = MiniBatchKMeans(n_clusters=root_num[0][0], random_state=0).fit(p)
# print('Time on kmeans', time.time() - st_time)
clusters.append([[kmeans.labels_]])
# centers = kmeans.cluster_centers_
ref_rigid = p
for j in range(time_step):
positions[j] = pyflex.get_positions().reshape(-1, 4)[:, :3]
# apply rigid projection to ground truth
XX = ref_rigid
YY = positions[j, :64]
# print("MSE init", np.mean(np.square(XX - YY)))
X = XX.copy().T
Y = YY.copy().T
mean_X = np.mean(X, 1, keepdims=True)
mean_Y = np.mean(Y, 1, keepdims=True)
X = X - mean_X
Y = Y - mean_Y
C = np.dot(X, Y.T)
U, S, Vt = np.linalg.svd(C)
D = np.eye(3)
D[2, 2] = np.linalg.det(np.dot(Vt.T, U.T))
R = np.dot(Vt.T, np.dot(D, U.T))
t = mean_Y - np.dot(R, mean_X)
YY_fitted = (np.dot(R, XX.T) + t).T
# print("MSE fit", np.mean(np.square(YY_fitted - YY)))
positions[j, :64] = YY_fitted
if j > 0:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
data = [positions[j], velocities[j], clusters]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
elif env == 'FluidShake':
# if env is FluidShake
height = 1.0
border = 0.025
dim_x = rand_int(10, 12)
dim_y = rand_int(15, 20)
dim_z = 3
x_center = rand_float(-0.2, 0.2)
x = x_center - (dim_x-1)/2.*0.055
y = 0.055/2. + border + 0.01
z = 0. - (dim_z-1)/2.*0.055
box_dis_x = dim_x * 0.055 + rand_float(0., 0.3)
box_dis_z = 0.2
scene_params = np.array([x, y, z, dim_x, dim_y, dim_z, box_dis_x, box_dis_z])
pyflex.set_scene(env_idx, scene_params, 0)
boxes = calc_box_init_FluidShake(box_dis_x, box_dis_z, height, border)
for i in range(len(boxes)):
halfEdge = boxes[i][0]
center = boxes[i][1]
quat = boxes[i][2]
pyflex.add_box(halfEdge, center, quat)
n_particles = pyflex.get_n_particles()
n_shapes = pyflex.get_n_shapes()
# print("n_particles", n_particles)
# print("n_shapes", n_shapes)
positions = np.zeros((time_step, n_particles + n_shapes, 3), dtype=np.float32)
velocities = np.zeros((time_step, n_particles + n_shapes, 3), dtype=np.float32)
shape_quats = np.zeros((time_step, n_shapes, 4), dtype=np.float32)
x_box = x_center
v_box = 0.
for j in range(time_step_clip):
x_box_last = x_box
x_box += v_box * dt
shape_states_ = calc_shape_states_FluidShake(
x_box, x_box_last, scene_params[-2:], height, border)
pyflex.set_shape_states(shape_states_)
pyflex.step()
for j in range(time_step):
x_box_last = x_box
x_box += v_box * dt
v_box += rand_float(-0.15, 0.15) - x_box * 0.1
shape_states_ = calc_shape_states_FluidShake(
x_box, x_box_last, scene_params[-2:], height, border)
pyflex.set_shape_states(shape_states_)
positions[j, :n_particles] = pyflex.get_positions().reshape(-1, 4)[:, :3]
shape_states = pyflex.get_shape_states().reshape(-1, shape_state_dim)
for k in range(n_shapes):
positions[j, n_particles + k] = shape_states[k, :3]
shape_quats[j, k] = shape_states[k, 6:10]
if j > 0:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
# NOTE: 1) particle + glass wall positions, 2) particle + glass wall velocitys, 3) glass wall rotations, 4) scenen parameters
data = [positions[j], velocities[j], shape_quats[j], scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
elif env == 'RiceGrip':
# if env is RiceGrip
# repeat the grip for R times
R = 3
gripper_config = sample_control_RiceGrip()
if i % R == 0:
### set scene
# x, y, z: [8.0, 10.0]
# clusterStiffness: [0.3, 0.7]
# clusterPlasticThreshold: [0.00001, 0.0005]
# clusterPlasticCreep: [0.1, 0.3]
x = rand_float(8.0, 10.0)
y = rand_float(8.0, 10.0)
z = rand_float(8.0, 10.0)
clusterStiffness = rand_float(0.3, 0.7)
clusterPlasticThreshold = rand_float(0.00001, 0.0005)
clusterPlasticCreep = rand_float(0.1, 0.3)
scene_params = np.array([x, y, z, clusterStiffness, clusterPlasticThreshold, clusterPlasticCreep])
pyflex.set_scene(env_idx, scene_params, thread_idx)
scene_params[4] *= 1000.
halfEdge = np.array([0.15, 0.8, 0.15])
center = np.array([0., 0., 0.])
quat = np.array([1., 0., 0., 0.])
pyflex.add_box(halfEdge, center, quat)
pyflex.add_box(halfEdge, center, quat)
n_particles = pyflex.get_n_particles()
n_shapes = pyflex.get_n_shapes()
positions = np.zeros((time_step, n_particles + n_shapes, 6), dtype=np.float32)
velocities = np.zeros((time_step, n_particles + n_shapes, 6), dtype=np.float32)
shape_quats = np.zeros((time_step, n_shapes, 4), dtype=np.float32)
for j in range(time_step_clip):
shape_states = calc_shape_states_RiceGrip(0, dt, shape_state_dim, gripper_config)
pyflex.set_shape_states(shape_states)
pyflex.step()
p = pyflex.get_positions().reshape(-1, 4)[:, :3]
clusters = []
st_time = time.time()
kmeans = MiniBatchKMeans(n_clusters=root_num[0][0], random_state=0).fit(p)
# print('Time on kmeans', time.time() - st_time)
clusters.append([[kmeans.labels_]])
# centers = kmeans.cluster_centers_
for j in range(time_step):
shape_states = calc_shape_states_RiceGrip(j * dt, dt, shape_state_dim, gripper_config)
pyflex.set_shape_states(shape_states)
positions[j, :n_particles, :3] = pyflex.get_rigidGlobalPositions().reshape(-1, 3)
positions[j, :n_particles, 3:] = pyflex.get_positions().reshape(-1, 4)[:, :3]
shape_states = pyflex.get_shape_states().reshape(-1, shape_state_dim)
for k in range(n_shapes):
positions[j, n_particles + k, :3] = shape_states[k, :3]
positions[j, n_particles + k, 3:] = shape_states[k, :3]
shape_quats[j, k] = shape_states[k, 6:10]
if j > 0:
velocities[j] = (positions[j] - positions[j - 1]) / dt
pyflex.step()
data = [positions[j], velocities[j], shape_quats[j], clusters, scene_params]
store_data(data_names, data, os.path.join(rollout_dir, str(j) + '.h5'))
else:
raise AssertionError("Unsupported env")
# change dtype for more accurate stat calculation
# only normalize positions and velocities
datas = [positions.astype(np.float64), velocities.astype(np.float64)]
# NOTE: stats is of length 2, for positions and velocities
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0] * datas[j].shape[1]
stats[j] = combine_stat(stats[j], stat)
pyflex.clean()
return stats
def gen_Cloth(info):
env, env_idx = info['env'], info['env_idx']
thread_idx, data_dir, data_names = info['thread_idx'], info['data_dir'], info['data_names']
n_rollout, time_step = info['n_rollout'], info['time_step']
dt, args, phase = info['dt'], info['args'], info['phase']
vis_width, vis_height = info['vis_width'], info['vis_height']
state_dim = args.state_dim
action_dim = args.action_dim
dt = 1. / 60.
np.random.seed(round(time.time() * 1000 + thread_idx) % 2 ** 32)
stats = [init_stat(state_dim), init_stat(action_dim)]
engine = ClothEngine(dt, state_dim, action_dim)
import pyflex
pyflex.init()
# bar = ProgressBar()
for i in range(n_rollout):
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
os.system('mkdir -p ' + rollout_dir)
engine.init(pyflex)
scene_params = engine.scene_params
action = np.zeros(4)
states_all = np.zeros((time_step, engine.n_particles, state_dim))
actions_all = np.zeros((time_step, 1, action_dim))
# drop the cloth down
engine.set_action(action)
engine.step()
for j in range(time_step):
positions = pyflex.get_positions().reshape(-1, 4)[:, :3]
# sample the action
if j % 5 == 0:
ctrl_pts = rand_int(0, 8)
act_lim = 0.05
dx = rand_float(-act_lim, act_lim)
dz = rand_float(-act_lim, act_lim)
dy = 0.05
action = np.array([ctrl_pts, dx, dy, dz])
else:
action[2] = 0.
# store the rollout information
state = engine.get_state()
states_all[j] = state
tga_path = os.path.join(rollout_dir, '%d.tga' % j)
pyflex.render(capture=True, path=tga_path)
tga = Image.open(tga_path)
tga = np.array(tga)[:, 60:780, :3][:, :, ::-1]
tga = cv2.resize(tga, (vis_width, vis_height), interpolation=cv2.INTER_AREA)
os.system('rm ' + tga_path)
jpg_path = os.path.join(rollout_dir, 'fig_%d.jpg' % j)
cv2.imwrite(jpg_path, tga)
actions_all[j, 0] = action.copy()
engine.set_action(action)
engine.step()
datas = [states_all, actions_all, scene_params]
store_data(data_names, datas, rollout_dir + '.h5')
datas = [datas[j].astype(np.float64) for j in range(len(datas))]
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0]
stats[j] = combine_stat(stats[j], stat)
pyflex.clean()
return stats
control_v = to_var(actions_roll, use_gpu, requires_grad=True)
elif args.env == 'Box':
state_goal, state_goal_full, vis_goal_full, scene_ckp = generate_Box_goal(args)
state_goal_v = to_var(state_goal, use_gpu=use_gpu)[None, :, :]
latent_goal = encode_partial(state_goal_v)
engine = BoxEngine(args.dt, args.state_dim, args.action_dim)
ckp = engine.reset_scene(args.n_particle, ckp=scene_ckp)
states_roll = np.zeros((args.roll_step, args.n_particle, args.state_dim))
actions_roll = np.zeros((args.roll_step, 1, args.action_dim))
viss_roll = np.zeros((args.roll_step, args.n_particle))
# !!! need tuning
sample_force = rand_float(-600., -450.)
sample_force = -300.
sample_force = -600.
control = np.ones(args.roll_step) * sample_force
control_v = to_var(control, use_gpu, requires_grad=True)
else:
raise AssertionError("Unsupported env")
criterionMSE = nn.MSELoss()
criterionChamfer = ChamferLoss()
optimizer = optim.Adam([control_v], lr=args.lr, betas=(args.beta1, 0.999))
for step in range(args.roll_step):
def init(self, pyflex, scene_params=None):
self.pyflex = pyflex
if scene_params is None:
# offset
radius = 0.05
offset_x = -1.
offset_y = 0.06
offset_z = -1.
# fabrics
fabric_type = rand_int(0, 3) # 0: Cloth, 1: shirt, 2: pants
if fabric_type == 0:
# parameters of the shape
dimx = rand_int(25, 35) # dimx, width
dimy = rand_int(25, 35) # dimy, height
dimz = 0
# the actuated points
ctrl_idx = np.array([
0, dimx // 2,
dimx - 1, dimy // 2 * dimx, dimy // 2 * dimx + dimx - 1,
(dimy - 1) * dimx, (dimy - 1) * dimx + dimx // 2,
(dimy - 1) * dimx + dimx - 1
])
offset_x = -dimx * radius / 2.
offset_y = 0.06
offset_z = -dimy * radius / 2.
elif fabric_type == 1:
# parameters of the shape
dimx = rand_int(16, 25) # width of the body
dimy = rand_int(30, 35) # height of the body
dimz = 7 # size of the sleeves
# the actuated points
ctrl_idx = np.array([
dimx * dimy,
dimx * dimy + dimz * (dimz + dimz // 2) + (1 + dimz) * (dimz + 1) // 4,
dimx * dimy + (1 + dimz) * dimz // 2 + dimz * (dimz - 1),
dimx * dimy + dimz * (dimz + dimz // 2) + (1 + dimz) * (dimz + 1) // 4 + \
(1 + dimz) * dimz // 2 + dimz * dimz - 1,
dimy // 2 * dimx,
dimy // 2 * dimx + dimx - 1,
(dimy - 1) * dimx,
dimy * dimx - 1])
offset_x = -(dimx + dimz * 4) * radius / 2.
offset_y = 0.06
offset_z = -dimy * radius / 2.
elif fabric_type == 2:
# parameters of the shape
dimx = rand_int(9, 13) * 2 # width of the pants
dimy = rand_int(6, 11) # height of the top part
dimz = rand_int(24, 31) # height of the leg
# the actuated points
ctrl_idx = np.array([
0, dimx - 1,
(dimy - 1) * dimx,
(dimy - 1) * dimx + dimx - 1,
dimx * dimy + dimz // 2 * (dimx - 4) // 2,
dimx * dimy + (dimz - 1) * (dimx - 4) // 2,
dimx * dimy + dimz * (dimx - 4) // 2 + 3 + \
dimz // 2 * (dimx - 4) // 2 + (dimx - 4) // 2 - 1,
dimx * dimy + dimz * (dimx - 4) // 2 + 3 + \
dimz * (dimx - 4) // 2 - 1])
offset_x = -dimx * radius / 2.
offset_y = 0.06
offset_z = -(dimy + dimz) * radius / 2.
# physical param
stiffness = rand_float(0.4, 1.0)
stretchStiffness = stiffness
bendStiffness = stiffness
shearStiffness = stiffness
dynamicFriction = 0.6
staticFriction = 1.0
particleFriction = 0.6
invMass = 1.0
# other parameters
windStrength = 0.0
draw_mesh = 1.
# set up environment
self.scene_params = np.array([
offset_x, offset_y, offset_z, fabric_type, dimx, dimy, dimz,
ctrl_idx[0], ctrl_idx[1], ctrl_idx[2], ctrl_idx[3],
ctrl_idx[4], ctrl_idx[5], ctrl_idx[6], ctrl_idx[7],
stretchStiffness, bendStiffness, shearStiffness,
dynamicFriction, staticFriction, particleFriction, invMass,
windStrength, draw_mesh
])
else:
self.scene_params = scene_params
scene_idx = 15
self.pyflex.set_scene(scene_idx, self.scene_params, 0)
# set up camera pose
camPos = np.array([0., 3.5, 0.])
camAngle = np.array([0., -90. / 180. * np.pi, 0.])
pyflex.set_camPos(camPos)
pyflex.set_camAngle(camAngle)
self.n_particles = self.pyflex.get_n_particles()
# let the cloth drop
action_zero = np.zeros(4)
for i in range(5):
pyflex.step(action_zero)
def add_impulse(self, p_range=(-200, 200)):
for i in range(self.n_ball):
impulse = (rand_float(p_range[0], p_range[1]),
rand_float(p_range[0], p_range[1]))
self.balls[i].apply_impulse_at_local_point(impulse=impulse,
point=(0, 0))
def __init__(self, lhs, rhs):
self.lhs = lhs
self.rhs = rhs
self.frc = rand_float(0, 0.1)
self.system = []
def gen_Swim(info):
thread_idx, data_dir, data_names = info['thread_idx'], info[
'data_dir'], info['data_names']
n_rollout, time_step = info['n_rollout'], info['time_step']
dt, video, args, phase = info['dt'], info['video'], info['args'], info[
'phase']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2**32)
attr_dim = args.attr_dim # actuated, soft, rigid
state_dim = args.state_dim # x, y, xdot, ydot
action_dim = args.action_dim
param_dim = args.param_dim # n_box, k, damping, init_p
act_scale = 500.
act_delta = 250.
# attr, state, action
stats = [init_stat(attr_dim), init_stat(state_dim), init_stat(action_dim)]
engine = SwimEngine(dt, state_dim, action_dim, param_dim)
group_size = args.group_size
sub_dataset_size = n_rollout * args.num_workers // args.n_splits
print('group size', group_size, 'sub_dataset_size', sub_dataset_size)
assert n_rollout % group_size == 0
assert args.n_rollout % args.n_splits == 0
bar = ProgressBar()
for i in bar(range(n_rollout)):
rollout_idx = thread_idx * n_rollout + i
group_idx = rollout_idx // group_size
sub_idx = rollout_idx // sub_dataset_size
num_obj_range = args.num_obj_range if phase in {
'train', 'valid'
} else args.extra_num_obj_range
num_obj = num_obj_range[sub_idx]
rollout_dir = os.path.join(data_dir, str(rollout_idx))
param_file = os.path.join(data_dir, str(group_idx) + '.param')
os.system('mkdir -p ' + rollout_dir)
if rollout_idx % group_size == 0:
init_p = None if not args.regular_data else sample_init_p_flight(
n_box=num_obj, aug=True, train=phase == 'train')
engine.init(param=(num_obj, None, None, init_p))
torch.save(engine.get_param(), param_file)
else:
while not os.path.isfile(param_file):
time.sleep(0.5)
param = torch.load(param_file)
engine.init(param=param)
act_t_param = np.zeros((engine.n_box, 3))
for j in range(time_step):
box_type = engine.init_p[:, 2]
act_t = np.zeros((engine.n_box, action_dim))
for k in range(engine.n_box):
if box_type[k] == 0:
# if this is an actuated box
if j == 0:
act_t_param[k] = np.array([
rand_float(0., 1.),
rand_float(1., 2.5),
rand_float(0, np.pi * 2)
])
if act_t_param[k, 0] < 0.3:
# using smooth action
if j == 0:
act_t[k] = rand_float(-act_delta, act_delta)
else:
lo = max(actions_all[j - 1, k] - act_delta,
-act_scale - 20)
hi = min(actions_all[j - 1, k] + act_delta,
act_scale + 20)
act_t[k] = rand_float(lo, hi)
act_t[k] = np.clip(act_t[k], -act_scale, act_scale)
elif act_t_param[k, 0] < 0.6:
# using random action
act_t[k] = rand_float(-act_scale, act_scale)
else:
# using sin action
act_t[k] = np.sin(j / act_t_param[k, 1] + act_t_param[k, 2]) * \
rand_float(act_scale / 2., act_scale)
engine.set_action(act_t)
states = engine.get_state()
actions = engine.get_action()
pos = states[:, :8].copy()
vec = states[:, 8:].copy()
'''reset velocity'''
if j > 0:
vec = (pos - states_all[j - 1, :, :8]) / dt
if j == 0:
attrs_all = np.zeros((time_step, num_obj, attr_dim))
states_all = np.zeros((time_step, num_obj, state_dim))
actions_all = np.zeros((time_step, num_obj, action_dim))
'''attrs: actuated/soft/rigid'''
assert attr_dim == 3
attrs = np.zeros((num_obj, attr_dim))
for k in range(engine.n_box):
attrs[k, int(engine.init_p[k, 2])] = 1
assert np.sum(attrs[:, 0]) == np.sum(engine.init_p[:, 2] == 0)
assert np.sum(attrs[:, 1]) == np.sum(engine.init_p[:, 2] == 1)
assert np.sum(attrs[:, 2]) == np.sum(engine.init_p[:, 2] == 2)
attrs_all[j] = attrs
states_all[j, :, :8] = pos
states_all[j, :, 8:] = vec
actions_all[j] = actions
data = [attrs, states_all[j], actions_all[j]]
store_data(data_names, data,
os.path.join(rollout_dir,
str(j) + '.h5'))
engine.step()
datas = [
attrs_all.astype(np.float64),
states_all.astype(np.float64),
actions_all.astype(np.float64)
]
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0]
stats[j] = combine_stat(stats[j], stat)
return stats
def gen_Box(info):
thread_idx, data_dir, data_names = info['thread_idx'], info[
'data_dir'], info['data_names']
n_rollout, n_particle, time_step = info['n_rollout'], info[
'n_particle'], info['time_step']
dt, args = info['dt'], info['args']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2**32)
state_dim = args.state_dim # x, y, angle, xdot, ydot, angledot
action_dim = args.action_dim # x, xdot
assert state_dim == 6
assert action_dim == 2
stats = [init_stat(state_dim), init_stat(action_dim)]
engine = BoxEngine(dt, state_dim, action_dim)
states = np.zeros((n_rollout, time_step, n_particle, state_dim))
actions = np.zeros((n_rollout, time_step, 1, action_dim))
viss = np.zeros((n_rollout, time_step, n_particle))
bar = ProgressBar()
for i in bar(range(n_rollout)):
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
os.system('mkdir -p ' + rollout_dir)
engine.reset_scene(n_particle)
for j in range(time_step):
engine.set_action(rand_float(-600., 100.))
states[i, j] = engine.get_state()
actions[i, j] = engine.get_action()
viss[i, j] = engine.get_vis(states[i, j])
if j > 0:
states[i, j, :, 3:] = (states[i, j, :, :3] -
states[i, j - 1, :, :3]) / dt
actions[i, j, :, 1] = (actions[i, j, :, 0] -
actions[i, j - 1, :, 0]) / dt
data = [states[i, j], actions[i, j], viss[i, j]]
store_data(data_names, data,
os.path.join(rollout_dir,
str(j) + '.h5'))
engine.step()
datas = [states[i].astype(np.float64), actions[i].astype(np.float64)]
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0]
stats[j] = combine_stat(stats[j], stat)
return stats
def gen_Cradle(info):
thread_idx, data_dir, data_names = info['thread_idx'], info[
'data_dir'], info['data_names']
n_particle, n_rollout, time_step = info['n_particle'], info[
'n_rollout'], info['time_step']
dt, args = info['dt'], info['args']
np.random.seed(round(time.time() * 1000 + thread_idx) % 2**32)
attr_dim = args.attr_dim # ball, anchor
state_dim = args.state_dim # x, y, xdot, ydot
assert attr_dim == 2
assert state_dim == 4
lim = 300
attr_dim = 2
state_dim = 4
relation_dim = 4
stats = [init_stat(attr_dim), init_stat(state_dim)]
engine = CradleEngine(dt)
n_objects = n_particle * 2 # add the same number of anchor points
attrs = np.zeros((n_rollout, time_step, n_objects, attr_dim))
states = np.zeros((n_rollout, time_step, n_objects, state_dim))
bar = ProgressBar()
for i in bar(range(n_rollout)):
rollout_idx = thread_idx * n_rollout + i
rollout_dir = os.path.join(data_dir, str(rollout_idx))
os.system('mkdir -p ' + rollout_dir)
theta = rand_float(0, 90)
engine.reset_scene(n_particle, theta)
for j in range(time_step):
states[i, j] = engine.get_state()
if j > 0:
states[i, j, :, 2:] = (states[i, j, :, :2] -
states[i, j - 1, :, :2]) / dt
attrs[i, j, :n_particle, 0] = 1 # balls
attrs[i, j, n_particle:, 1] = 1 # anchors
data = [attrs[i, j], states[i, j]]
store_data(data_names, data,
os.path.join(rollout_dir,
str(j) + '.h5'))
engine.step()
datas = [attrs[i].astype(np.float64), states[i].astype(np.float64)]
for j in range(len(stats)):
stat = init_stat(stats[j].shape[0])
stat[:, 0] = np.mean(datas[j], axis=(0, 1))[:]
stat[:, 1] = np.std(datas[j], axis=(0, 1))[:]
stat[:, 2] = datas[j].shape[0]
stats[j] = combine_stat(stats[j], stat)
return stats