Exemplo n.º 1
0
    def set_scene(self, config, state=None):
        if self.render_mode == 'particle':
            render_mode = 1
        else:
            render_mode = 2

        camera_params = config['camera_params'][config['camera_name']]
        params = np.array([
            *config['init_pos'], config['stretchstiffness'],
            config['bendingstiffness'], config['radius'], config['segment'],
            config['mass'], config['scale'], *camera_params['pos'][:],
            *camera_params['angle'][:], camera_params['width'],
            camera_params['height'], render_mode
        ])

        env_idx = 2

        if self.version == 2:
            robot_params = [1.] if self.action_mode in ['sawyer', 'franka'
                                                        ] else []
            self.params = (params, robot_params)
            pyflex.set_scene(env_idx, params, 0, robot_params)
        elif self.version == 1:
            pyflex.set_scene(env_idx, params, 0)

        num_particles = pyflex.get_n_particles()
        # print("with {} segments, the number of particles are {}".format(config['segment'], num_particles))
        # exit()
        self.update_camera(config['camera_name'],
                           config['camera_params'][config['camera_name']])

        if state is not None:
            self.set_state(state)
        self.current_config = deepcopy(config)
Exemplo n.º 2
0
    def set_scene(self, config, states=None):
        '''
        child envs can pass in specific fluid params through fluid param dic.
        '''
        # sample fluid properties.
        fluid_params = self.sample_fluid_params(config['fluid'])

        # set camera parameters.
        self.initialize_camera()
        camera_name = config.get('camera_name', self.camera_name)
        camera_params = np.array([
            *self.camera_params[camera_name]['pos'],
            *self.camera_params[camera_name]['angle'], self.camera_width,
            self.camera_height, self.render_mode
        ])

        # create fluid
        scene_params = np.concatenate((fluid_params, camera_params))

        env_idx = 1
        if self.version == 2:
            robot_params = []
            self.params = (scene_params, robot_params)
            pyflex.set_scene(env_idx, scene_params, 0, robot_params)
        elif self.version == 1:
            pyflex.set_scene(env_idx, scene_params, 0)

        self.particle_num = pyflex.get_n_particles()
Exemplo n.º 3
0
    def reset(self, config=None, initial_state=None, config_id=None):
        if config is None:
            if config_id is None:
                if self.eval_flag:
                    eval_beg = int(0.8 * len(self.cached_configs))
                    config_id = np.random.randint(
                        low=eval_beg, high=len(self.cached_configs)
                    ) if not self.deterministic else eval_beg
                else:
                    train_high = int(0.8 * len(self.cached_configs))
                    config_id = np.random.randint(
                        low=0, high=max(train_high,
                                        1)) if not self.deterministic else 0

            self.current_config = self.cached_configs[config_id]
            self.current_config_id = config_id
            self.set_scene(self.cached_configs[config_id],
                           self.cached_init_states[config_id])
        else:
            self.current_config = config
            self.set_scene(config, initial_state)
        self.particle_num = pyflex.get_n_particles()
        self.prev_reward = 0.
        self.time_step = 0
        obs = self._reset()
        if self.recording:
            self.video_frames.append(self.render(mode='rgb_array'))
        return obs
Exemplo n.º 4
0
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
Exemplo n.º 5
0
boxes = calc_box_init(box_dis_x, box_dis_z)

for i in range(len(boxes)):
    halfEdge = boxes[i][0]
    center = boxes[i][1]
    quat = boxes[i][2]
    pyflex.add_box(halfEdge, center, quat)

### read scene info
print("Scene Upper:", pyflex.get_scene_upper())
print("Scene Lower:", pyflex.get_scene_lower())
print("Num particles:", pyflex.get_phases().reshape(-1, 1).shape[0])
print("Phases:", np.unique(pyflex.get_phases()))

n_particles = pyflex.get_n_particles()
n_shapes = pyflex.get_n_shapes()
n_rigids = pyflex.get_n_rigids()
n_rigidPositions = pyflex.get_n_rigidPositions()

print("n_particles", n_particles)
print("n_shapes", n_shapes)
print("n_rigids", n_rigids)
print("n_rigidPositions", n_rigidPositions)

positions = np.zeros((time_step, n_particles, dim_position))
velocities = np.zeros((time_step, n_particles, dim_velocity))
shape_states = np.zeros((time_step, n_shapes, dim_shape_state))

x_box = x_center
v_box = 0
Exemplo n.º 6
0
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
Exemplo n.º 7
0
def get_world_coords(rgb, depth, env):
	height, width, _ = rgb.shape
	K = intrinsic_from_fov(height, width, 45) # the fov is 90 degrees

	# Apply back-projection: K_inv @ pixels * depth
	cam_coords = np.ones((height, width, 4))
	u0 = K[0, 2]
	v0 = K[1, 2]
	fx = K[0, 0]
	fy = K[1, 1]
	# Loop through each pixel in the image
	for v in range(height):
		for u in range(width):
			# Apply equation in fig 3
			x = (u - u0) * depth[v, u] / fx
			y = (v - v0) * depth[v, u] / fy
			z = depth[v, u]
			cam_coords[v][u][:3] = (x, y, z)

	particle_pos = pyflex.get_positions().reshape((-1, 4))
	print('cloth pixels: ', np.count_nonzero(depth))
	print("cloth particle num: ", pyflex.get_n_particles())

	# debug: print camera coordinates
	# print(cam_coords.shape)
	# cnt = 0
	# for v in range(height):
	#     for u in range(width):
	#         if depth[v][u] > 0:
	#             print("v: {} u: {} cnt: {} cam_coord: {} approximate particle pos: {}".format(
	#                     v, u, cnt, cam_coords[v][u], particle_pos[cnt]))
	#             rgb = rgbd[:, :, :3].copy()
	#             rgb[v][u][0] = 255
	#             rgb[v][u][1] = 0
	#             rgb[v][u][2] = 0
	#             cv2.imshow('rgb', rgb[:, :, ::-1])
	#             cv2.waitKey()
	#             cnt += 1

	# from cam coord to world coord
	cam_x, cam_y, cam_z = env.camera_params['default_camera']['pos'][0], env.camera_params['default_camera']['pos'][1], env.camera_params['default_camera']['pos'][2]
	cam_x_angle, cam_y_angle, cam_z_angle = env.camera_params['default_camera']['angle'][0], env.camera_params['default_camera']['angle'][1], env.camera_params['default_camera']['angle'][2]

	# get rotation matrix: from world to camera
	matrix1 = get_rotation_matrix(- cam_x_angle, [0, 1, 0]) 
	# matrix2 = get_rotation_matrix(- cam_y_angle - np.pi, [np.cos(cam_x_angle), 0, np.sin(cam_x_angle)])
	matrix2 = get_rotation_matrix(- cam_y_angle - np.pi, [1, 0, 0])
	rotation_matrix = matrix2 @ matrix1
	
	# get translation matrix: from world to camera
	translation_matrix = np.zeros((4, 4))
	translation_matrix[0][0] = 1
	translation_matrix[1][1] = 1
	translation_matrix[2][2] = 1
	translation_matrix[3][3] = 1
	translation_matrix[0][3] = - cam_x
	translation_matrix[1][3] = - cam_y
	translation_matrix[2][3] = - cam_z

	# debug: from world to camera
	cloth_x, cloth_y = env.current_config['ClothSize'][0], env.current_config['ClothSize'][1]
	# cnt = 0
	# for u in range(height):
	#     for v in range(width):
	#         if depth[u][v] > 0:
	#             world_coord = np.ones(4)
	#             world_coord[:3] = particle_pos[cnt][:3]
	#             convert_cam_coord =  rotation_matrix @ translation_matrix @ world_coord
	#             # convert_cam_coord =  translation_matrix  @ matrix2 @ matrix1 @ world_coord
	#             print("u {} v {} \n world coord {} \n convert camera coord {} \n real camera coord {}".format(
	#                 u, v, world_coord, convert_cam_coord, cam_coords[u][v]
	#             ))
	#             cnt += 1
	#             input('wait...')


	# convert the camera coordinate back to the world coordinate using the rotation and translation matrix
	cam_coords = cam_coords.reshape((-1, 4)).transpose() # 4 x (height x width)
	world_coords = np.linalg.inv(rotation_matrix @ translation_matrix) @ cam_coords # 4 x (height x width)
	world_coords = world_coords.transpose().reshape((height, width, 4))

	# roughly check the final world coordinate with the actual coordinate
	# firstu = 0
	# firstv = 0
	# for u in range(height):
	#     for v in range(width):
	#         if depth[u][v]:
	#             if u > firstu: # move to a new line
	#                 firstu = u
	#                 firstv = v

	#             cnt = (u - firstu) * cloth_x + (v - firstv)  
	#             print("u {} v {} cnt{}\nworld_coord\t{}\nparticle coord\t{}\nerror\t{}".format(
	#                 u, v, cnt, world_coords[u][v], particle_pos[cnt], np.linalg.norm( world_coords[u][v] - particle_pos[cnt])))
	#             rgb = rgbd[:, :, :3].copy()
	#             rgb[u][v][0] = 255
	#             rgb[u][v][1] = 0
	#             rgb[u][v][2] = 0
	#             cv2.imshow('rgb', rgb[:, :, ::-1])
	#             cv2.waitKey()
	# exit()
	return world_coords