Skip to content

folded

Folded

Bases: AbsoluteObjectState, BooleanState

Source code in object_states/folded.py 
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
class Folded(AbsoluteObjectState, BooleanState):

    def calculate_projection_area_and_diagonal(self, dims):
        """
        Calculate the projection area and the diagonal length when projecting to the plane defined by the input dims
        E.g. if dims is [0, 1], the points will be projected onto the x-y plane.

        Args:
            dims (2-array): Global axes to project area onto. Options are {0, 1, 2}.
                E.g. if dims is [0, 1], project onto the x-y plane.

        Returns:
            area (float): 
        """
        cloth = self.obj.links["base_link"]
        points = cloth.particle_positions[:, dims]

        hull = ConvexHull(points)

        diagonal = distance_matrix(points[hull.vertices], points[hull.vertices]).max()

        if m.DEBUG_VISUALIZATION:
            import matplotlib.pyplot as plt
            ax = plt.gca()
            ax.set_aspect('equal')

            plt.plot(points[:, dims[0]], points[:, dims[1]], 'o')
            for simplex in hull.simplices:
                plt.plot(points[simplex, dims[0]], points[simplex, dims[1]], 'k-')
            plt.plot(points[hull.vertices, dims[0]], points[hull.vertices, dims[1]], 'r--', lw=2)
            plt.plot(points[hull.vertices[0], dims[0]], points[hull.vertices[0], dims[1]], 'ro')
            plt.show()

        return hull.area, diagonal

    def calculate_projection_area_and_diagonal_unfolded(self):
        """
        Calculate the maximum projection area and the diagonal length along different axes in the unfolded state.
        Should be called in the initialize function. Assume the object's default pose is unfolded.
        """
        # use the largest projection area as the unfolded area
        area_unfolded = 0.0
        diagonal_unfolded = 0.0
        dims_list = [[0, 1], [0, 2], [1, 2]]  # x-y plane, x-z plane, y-z plane

        for dims in dims_list:
            area, diagonal = self.calculate_projection_area_and_diagonal(dims=dims)
            if area > area_unfolded:
                area_unfolded = area
                diagonal_unfolded = diagonal

        return area_unfolded, diagonal_unfolded

    def check_projection_area_and_diagonal(self):
        """
        Check whether the current projection area and diagonal length satisfy the thresholds
        """
        area, diagonal = self.calculate_projection_area_and_diagonal([0, 1])

        # Check area reduction ratio
        flag_area_reduction = (area / self.area_unfolded) < m.AREA_REDUCTION_THRESHOLD

        # Check the diagonal reduction ratio
        flag_diagonal_reduction = (diagonal / self.diagonal_unfolded) < m.DIAGONAL_REDUCTION_THRESHOLD

        return flag_area_reduction, flag_diagonal_reduction

    def check_smoothness(self):
        """
        Check the smoothness of the cloth; the face normals of the cloth need to be close to the z-axis.
        """
        cloth = self.obj.links["base_link"]
        face_vertex_counts = np.array(cloth.get_attribute("faceVertexCounts"))
        assert (face_vertex_counts == 3).all(), "cloth prim is expected to only contain triangle faces"
        face_vertex_indices = np.array(cloth.get_attribute("faceVertexIndices"))
        points = cloth.particle_positions[face_vertex_indices]
        # Shape [F, 3, 3] where F is the number of faces
        points = points.reshape((face_vertex_indices.shape[0] // 3, 3, 3))

        # Shape [F, 3]
        v1 = points[:, 2, :] - points[:, 0, :]
        v2 = points[:, 1, :] - points[:, 0, :]
        normals = np.cross(v1, v2)
        normals_norm = np.linalg.norm(normals, axis=1)

        valid_normals = normals[normals_norm.nonzero()] / np.expand_dims(normals_norm[normals_norm.nonzero()], axis=1)
        assert valid_normals.shape[0] > 0

        # projection onto the z-axis
        proj = np.abs(np.dot(valid_normals, np.array([0.0, 0.0, 1.0])))
        percentage = np.mean(proj > np.cos(m.NORMAL_Z_ANGLE_DIFF))
        return percentage > m.NORMAL_Z_PERCENTAGE

    def _initialize(self):
        self.area_unfolded, self.diagonal_unfolded = self.calculate_projection_area_and_diagonal_unfolded()

    def _get_value(self):
        # Check the smoothness of the cloth
        flag_smoothness = self.check_smoothness()

        # Early stopping
        if not flag_smoothness:
            return False

        # Calculate the area and the diagonal of the current state
        flag_area_reduction, flag_diagonal_reduction = self.check_projection_area_and_diagonal()

        return flag_diagonal_reduction and flag_smoothness

    def _set_value(self, new_value):
        """
        Set the folded state. Currently, it's not supported yet.
        """
        raise NotImplementedError("_set_value of the Folded state has not been implemented")

calculate_projection_area_and_diagonal(dims)

Calculate the projection area and the diagonal length when projecting to the plane defined by the input dims E.g. if dims is [0, 1], the points will be projected onto the x-y plane.

Parameters:

Name Type Description Default
dims 2-array

Global axes to project area onto. Options are {0, 1, 2}. E.g. if dims is [0, 1], project onto the x-y plane.

 required

Returns:

Name Type Description
area float
Source code in object_states/folded.py 
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
def calculate_projection_area_and_diagonal(self, dims):
    """
    Calculate the projection area and the diagonal length when projecting to the plane defined by the input dims
    E.g. if dims is [0, 1], the points will be projected onto the x-y plane.

    Args:
        dims (2-array): Global axes to project area onto. Options are {0, 1, 2}.
            E.g. if dims is [0, 1], project onto the x-y plane.

    Returns:
        area (float): 
    """
    cloth = self.obj.links["base_link"]
    points = cloth.particle_positions[:, dims]

    hull = ConvexHull(points)

    diagonal = distance_matrix(points[hull.vertices], points[hull.vertices]).max()

    if m.DEBUG_VISUALIZATION:
        import matplotlib.pyplot as plt
        ax = plt.gca()
        ax.set_aspect('equal')

        plt.plot(points[:, dims[0]], points[:, dims[1]], 'o')
        for simplex in hull.simplices:
            plt.plot(points[simplex, dims[0]], points[simplex, dims[1]], 'k-')
        plt.plot(points[hull.vertices, dims[0]], points[hull.vertices, dims[1]], 'r--', lw=2)
        plt.plot(points[hull.vertices[0], dims[0]], points[hull.vertices[0], dims[1]], 'ro')
        plt.show()

    return hull.area, diagonal

calculate_projection_area_and_diagonal_unfolded()

Calculate the maximum projection area and the diagonal length along different axes in the unfolded state. Should be called in the initialize function. Assume the object's default pose is unfolded.

Source code in object_states/folded.py 
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
def calculate_projection_area_and_diagonal_unfolded(self):
    """
    Calculate the maximum projection area and the diagonal length along different axes in the unfolded state.
    Should be called in the initialize function. Assume the object's default pose is unfolded.
    """
    # use the largest projection area as the unfolded area
    area_unfolded = 0.0
    diagonal_unfolded = 0.0
    dims_list = [[0, 1], [0, 2], [1, 2]]  # x-y plane, x-z plane, y-z plane

    for dims in dims_list:
        area, diagonal = self.calculate_projection_area_and_diagonal(dims=dims)
        if area > area_unfolded:
            area_unfolded = area
            diagonal_unfolded = diagonal

    return area_unfolded, diagonal_unfolded

check_projection_area_and_diagonal()

Check whether the current projection area and diagonal length satisfy the thresholds

Source code in object_states/folded.py 
76
77
78
79
80
81
82
83
84
85
86
87
88
def check_projection_area_and_diagonal(self):
    """
    Check whether the current projection area and diagonal length satisfy the thresholds
    """
    area, diagonal = self.calculate_projection_area_and_diagonal([0, 1])

    # Check area reduction ratio
    flag_area_reduction = (area / self.area_unfolded) < m.AREA_REDUCTION_THRESHOLD

    # Check the diagonal reduction ratio
    flag_diagonal_reduction = (diagonal / self.diagonal_unfolded) < m.DIAGONAL_REDUCTION_THRESHOLD

    return flag_area_reduction, flag_diagonal_reduction

check_smoothness()

Check the smoothness of the cloth; the face normals of the cloth need to be close to the z-axis.

Source code in object_states/folded.py 
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
def check_smoothness(self):
    """
    Check the smoothness of the cloth; the face normals of the cloth need to be close to the z-axis.
    """
    cloth = self.obj.links["base_link"]
    face_vertex_counts = np.array(cloth.get_attribute("faceVertexCounts"))
    assert (face_vertex_counts == 3).all(), "cloth prim is expected to only contain triangle faces"
    face_vertex_indices = np.array(cloth.get_attribute("faceVertexIndices"))
    points = cloth.particle_positions[face_vertex_indices]
    # Shape [F, 3, 3] where F is the number of faces
    points = points.reshape((face_vertex_indices.shape[0] // 3, 3, 3))

    # Shape [F, 3]
    v1 = points[:, 2, :] - points[:, 0, :]
    v2 = points[:, 1, :] - points[:, 0, :]
    normals = np.cross(v1, v2)
    normals_norm = np.linalg.norm(normals, axis=1)

    valid_normals = normals[normals_norm.nonzero()] / np.expand_dims(normals_norm[normals_norm.nonzero()], axis=1)
    assert valid_normals.shape[0] > 0

    # projection onto the z-axis
    proj = np.abs(np.dot(valid_normals, np.array([0.0, 0.0, 1.0])))
    percentage = np.mean(proj > np.cos(m.NORMAL_Z_ANGLE_DIFF))
    return percentage > m.NORMAL_Z_PERCENTAGE