ravens/agents/gt_state.py
def extract_x_y_theta(self, object_info, t_worldaug_world=None, preserve_theta=False, softbody=False):
        """Given either object OR action pose info, return stuff to put in GT observation.
        Note: only called from within this class and 2-step case via subclassing.

        During training, there is normally data augmentation applied, so t_worldaug_world
        is NOT None, and augmentation is applied as if the 'image' were adjusted. However,
        for actions, we preserve theta so `object_quat_xyzw` does not change, and we query
        index=2 for the z rotation. For most deformables tasks, we did not use rotations,
        hence theta=0.

        Get t_world_object which is a 4x4 homogeneous matrix, then stick the position there (why?).
        Then we do the matrix multiplication with '@' to get the homogeneous matrix representation
        of the object after augmentation is applied.
        https://stackoverflow.com/questions/34142485/difference-between-numpy-dot-and-python-3-5-matrix-multiplication
        Then the position is extracted from the resulting homogeneous matrix. (And we ignore the
        z coordinate for that, but in fact for cloth vertices I would keep it.)

        Augmentation for cloth vertices? For now I'm following what they do for positions only
        (ignoring orientation), and returning updated (x,y,z) where the last value is the z
        (height) and not a rotation (doesn't make sense with vertrics anyway). Augmentations
        are all in SE(2), so the z coordinates should not change. Indeed that's what I see after
        extracting positions from `t_worldaug_object`.

        And yes, with vertices, they are flattened in the same way regardless of whether data
        augmentation is used, so every third value represents a vertex height. :D So we get
        [ v1_x, v1_y, v1_z, v2_x, v2_y, v2_z, ... ] in the flattened version.

        Args:
            t_worldaug_world: Only True if there's an augmentation and we need to change the
                ground truth pose as if we applied the augmentation. Only happens during (a)
                determining mean/stds, and (b) training. During initialization and the action
                (loading) we do not use this.
            preserve_theta: Only True if calling where `object_info` is an action pose.
            softbody: Only True if `object_info` is a vertex mesh of a PyBullet soft body.

        Returns:
            object_x_y_theta, pos, quat: the first is an SE(2) pose and parameterized by
            three numbers, the xy position and a scalar rotation `theta`. In case we have
            a cloth, I'm not returning anything after that (no pos or quat) so that if we
            use the older API for cloth (which we should not be!), it should crash code.
        """
        # ------------------------------------------------------------------------------------------ #
        # Daniel: sanity checks to make sure we're handling the soft body ID correctly.
        # The last `if` of `softbody` helps to distinguish among actions, which are also length 2.
        # ------------------------------------------------------------------------------------------ #
        if (self.task in TASKS_SOFT) and (len(object_info) == 2) and softbody:
            nb_vertices, vert_pos_l = object_info
            assert nb_vertices == 100, f'We should be using 100 vertices but have: {nb_vertices}'
            assert nb_vertices == len(vert_pos_l), f'{nb_vertices}, {len(vert_pos_l)}'
            vert_pos_np = np.array(vert_pos_l)  # (100, 3)

            if t_worldaug_world is not None:
                augmented_vertices = []

                # For each vertex, apply data augmentation.
                for i in range(vert_pos_np.shape[0]):
                    vertex_position = vert_pos_np[i,:]

                    # Use identity quaternion (w=1, others 0). Othewrise, follow normal augmentation.
                    t_world_object = transformations.quaternion_matrix( (1,0,0,0) )
                    t_world_object[0:3, 3] = np.array(vertex_position)
                    t_worldaug_object = t_worldaug_world @ t_world_object
                    new_v_position = t_worldaug_object[0:3, 3]
                    augmented_vertices.append(new_v_position)

                # Stack everything.
                augmented_flattened = np.concatenate(augmented_vertices).flatten().astype(np.float32)
                return augmented_flattened
            else:
                vert_flattened = vert_pos_np.flatten().astype(np.float32)
                return vert_flattened
        # ------------------------------------------------------------------------------------------ #
        # Now proceed as usual, untouched from normal ravens code except for documentation/assertions.

        object_position = object_info[0]
        object_quat_xyzw = object_info[1]

        if t_worldaug_world is not None:
            object_quat_wxyz = (object_quat_xyzw[3], object_quat_xyzw[0], object_quat_xyzw[1], object_quat_xyzw[2])
            t_world_object = transformations.quaternion_matrix(object_quat_wxyz)
            t_world_object[0:3, 3] = np.array(object_position)

            # Daniel: pretty sure this has to be true. Upper left 3x3 is rotation, upper right 3x1 is position.
            assert t_world_object.shape == (4,4), f'shape is not 4x4: {t_world_object.shape}'
            assert t_worldaug_world.shape == (4,4), f'shape is not 4x4: {t_worldaug_world.shape}'

            # Daniel: data augmentation. Then, extract `object_position` from augmented object.
            t_worldaug_object = t_worldaug_world @ t_world_object
            object_quat_wxyz = transformations.quaternion_from_matrix(t_worldaug_object)
            if not preserve_theta:
                object_quat_xyzw = (object_quat_wxyz[1], object_quat_wxyz[2], object_quat_wxyz[3], object_quat_wxyz[0])
            object_position = t_worldaug_object[0:3, 3]

        object_xy = object_position[0:2]
        object_theta = -np.float32(utils.get_rot_from_pybullet_quaternion(object_quat_xyzw)[2]) / self.THETA_SCALE
        return np.hstack((object_xy, object_theta)).astype(np.float32), object_position, object_quat_xyzw
Given either object OR action pose info, return stuff to put in GT observation.
        Note: only called from within this class and 2-step case via subclassing.

        During training, there is normally data augmentation applied, so t_worldaug_world
        is NOT None, and augmentation is applied as if the 'image' were adjusted. However,
        for actions, we preserve theta so `object_quat_xyzw` does not change, and we query
        index=2 for the z rotation. For most deformables tasks, we did not use rotations,
        hence theta=0.

        Get t_world_object which is a 4x4 homogeneous matrix, then stick the position there (why?).
        Then we do the matrix multiplication with '@' to get the homogeneous matrix representation
        of the object after augmentation is applied.
        https://stackoverflow.com/questions/34142485/difference-between-numpy-dot-and-python-3-5-matrix-multiplication
        Then the position is extracted from the resulting homogeneous matrix. (And we ignore the
        z coordinate for that, but in fact for cloth vertices I would keep it.)

        Augmentation for cloth vertices? For now I'm following what they do for positions only
        (ignoring orientation), and returning updated (x,y,z) where the last value is the z
        (height) and not a rotation (doesn't make sense with vertrics anyway). Augmentations
        are all in SE(2), so the z coordinates should not change. Indeed that's what I see after
        extracting positions from `t_worldaug_object`.

        And yes, with vertices, they are flattened in the same way regardless of whether data
        augmentation is used, so every third value represents a vertex height. :D So we get
        [ v1_x, v1_y, v1_z, v2_x, v2_y, v2_z, ... ] in the flattened version.

        Args:
            t_worldaug_world: Only True if there's an augmentation and we need to change the
                ground truth pose as if we applied the augmentation. Only happens during (a)
                determining mean/stds, and (b) training. During initialization and the action
                (loading) we do not use this.
            preserve_theta: Only True if calling where `object_info` is an action pose.
            softbody: Only True if `object_info` is a vertex mesh of a PyBullet soft body.

        Returns:
            object_x_y_theta, pos, quat: the first is an SE(2) pose and parameterized by
            three numbers, the xy position and a scalar rotation `theta`. In case we have
            a cloth, I'm not returning anything after that (no pos or quat) so that if we
            use the older API for cloth (which we should not be!), it should crash code.
        
Given either object OR action pose info, return stuff to put in GT observation.
Note: only called from within this class and 2-step case via subclassing.

During training, there is normally data augmentation applied, so t_worldaug_world
is NOT None, and augmentation is applied as if the 'image' were adjusted. However,
for actions, we preserve theta so `object_quat_xyzw` does not change, and we query
index=2 for the z rotation. For most deformables tasks, we did not use rotations,
hence theta=0.

Get t_world_object which is a 4x4 homogeneous matrix, then stick the position there (why?).
Then we do the matrix multiplication with '@' to get the homogeneous matrix representation
of the object after augmentation is applied.

Augmentation for cloth vertices. For now I'm following what they do for positions only
(ignoring orientation), and returning updated (x,y,z) where the last value is the z
(height) and not a rotation (doesn't make sense with vertrics anyway). Augmentations
are all in SE(2), so the z coordinates should not change. Indeed that's what I see after
extracting positions from `t_worldaug_object`.

And yes, with vertices, they are flattened in the same way regardless of whether data
augmentation is used, so every third value represents a vertex height.
Given either object OR action pose info return stuff to put in GT observation . Note : only called from within this class and 2 - step case via subclassing . During training there is normally data augmentation applied so t_worldaug_world is NOT None and augmentation is applied as if the ' image ' were adjusted . However for actions we preserve theta so ` object_quat_xyzw ` does not change and we query index = 2 for the z rotation . For most deformables tasks we did not use rotations hence theta = 0 . Get t_world_object which is a 4x4 homogeneous matrix then stick the position there ( why? ) . Then we do the matrix multiplication with ' @ ' to get the homogeneous matrix representation of the object after augmentation is applied . Augmentation for cloth vertices . For now I ' m following what they do for positions only ( ignoring orientation ) and returning updated ( x y z ) where the last value is the z ( height ) and not a rotation ( doesn ' t make sense with vertrics anyway ) . Augmentations are all in SE ( 2 ) so the z coordinates should not change . Indeed that ' s what I see after extracting positions from ` t_worldaug_object ` . And yes with vertices they are flattened in the same way regardless of whether data augmentation is used so every third value represents a vertex height .
def extract_x_y_theta(self, object_info, t_worldaug_world=None, preserve_theta=False, softbody=False):
        if (self.task in TASKS_SOFT) and (len(object_info) == 2) and softbody:
            nb_vertices, vert_pos_l = object_info
            assert nb_vertices == 100, f'We should be using 100 vertices but have: {nb_vertices}'
            assert nb_vertices == len(vert_pos_l), f'{nb_vertices}, {len(vert_pos_l)}'
            vert_pos_np = np.array(vert_pos_l)  
            if t_worldaug_world is not None:
                augmented_vertices = []
                for i in range(vert_pos_np.shape[0]):
                    vertex_position = vert_pos_np[i,:]
                    t_world_object = transformations.quaternion_matrix( (1,0,0,0) )
                    t_world_object[0:3, 3] = np.array(vertex_position)
                    t_worldaug_object = t_worldaug_world @ t_world_object
                    new_v_position = t_worldaug_object[0:3, 3]
                    augmented_vertices.append(new_v_position)
                augmented_flattened = np.concatenate(augmented_vertices).flatten().astype(np.float32)
                return augmented_flattened
            else:
                vert_flattened = vert_pos_np.flatten().astype(np.float32)
                return vert_flattened
        object_position = object_info[0]
        object_quat_xyzw = object_info[1]
        if t_worldaug_world is not None:
            object_quat_wxyz = (object_quat_xyzw[3], object_quat_xyzw[0], object_quat_xyzw[1], object_quat_xyzw[2])
            t_world_object = transformations.quaternion_matrix(object_quat_wxyz)
            t_world_object[0:3, 3] = np.array(object_position)
            assert t_world_object.shape == (4,4), f'shape is not 4x4: {t_world_object.shape}'
            assert t_worldaug_world.shape == (4,4), f'shape is not 4x4: {t_worldaug_world.shape}'
            t_worldaug_object = t_worldaug_world @ t_world_object
            object_quat_wxyz = transformations.quaternion_from_matrix(t_worldaug_object)
            if not preserve_theta:
                object_quat_xyzw = (object_quat_wxyz[1], object_quat_wxyz[2], object_quat_wxyz[3], object_quat_wxyz[0])
            object_position = t_worldaug_object[0:3, 3]
        object_xy = object_position[0:2]
        object_theta = -np.float32(utils.get_rot_from_pybullet_quaternion(object_quat_xyzw)[2]) / self.THETA_SCALE
        return np.hstack((object_xy, object_theta)).astype(np.float32), object_position, object_quat_xyzw
def extract_x_y_theta ( self , object_info , t_worldaug_world = None , preserve_theta = False , softbody = False ) : if ( self . task in TASKS_SOFT ) and ( len ( object_info ) == 2 ) and softbody : nb_vertices , vert_pos_l = object_info assert nb_vertices == 100 , f'We should be using 100 vertices but have: {nb_vertices}' assert nb_vertices == len ( vert_pos_l ) , f'{nb_vertices}, {len(vert_pos_l)}' vert_pos_np = np . array ( vert_pos_l ) if t_worldaug_world is not None : augmented_vertices = [ ] for i in range ( vert_pos_np . shape [ 0 ] ) : vertex_position = vert_pos_np [ i , : ] t_world_object = transformations . quaternion_matrix ( ( 1 , 0 , 0 , 0 ) ) t_world_object [ 0 : 3 , 3 ] = np . array ( vertex_position ) t_worldaug_object = t_worldaug_world @ t_world_object new_v_position = t_worldaug_object [ 0 : 3 , 3 ] augmented_vertices . append ( new_v_position ) augmented_flattened = np . concatenate ( augmented_vertices ) . flatten ( ) . astype ( np . float32 ) return augmented_flattened else : vert_flattened = vert_pos_np . flatten ( ) . astype ( np . float32 ) return vert_flattened object_position = object_info [ 0 ] object_quat_xyzw = object_info [ 1 ] if t_worldaug_world is not None : object_quat_wxyz = ( object_quat_xyzw [ 3 ] , object_quat_xyzw [ 0 ] , object_quat_xyzw [ 1 ] , object_quat_xyzw [ 2 ] ) t_world_object = transformations . quaternion_matrix ( object_quat_wxyz ) t_world_object [ 0 : 3 , 3 ] = np . array ( object_position ) assert t_world_object . shape == ( 4 , 4 ) , f'shape is not 4x4: {t_world_object.shape}' assert t_worldaug_world . shape == ( 4 , 4 ) , f'shape is not 4x4: {t_worldaug_world.shape}' t_worldaug_object = t_worldaug_world @ t_world_object object_quat_wxyz = transformations . quaternion_from_matrix ( t_worldaug_object ) if not preserve_theta : object_quat_xyzw = ( object_quat_wxyz [ 1 ] , object_quat_wxyz [ 2 ] , object_quat_wxyz [ 3 ] , object_quat_wxyz [ 0 ] ) object_position = t_worldaug_object [ 0 : 3 , 3 ] object_xy = object_position [ 0 : 2 ] object_theta = - np . float32 ( utils . get_rot_from_pybullet_quaternion ( object_quat_xyzw ) [ 2 ] ) / self . THETA_SCALE return np . hstack ( ( object_xy , object_theta ) ) . astype ( np . float32 ) , object_position , object_quat_xyzw
Given either object OR action pose info, return stuff to put in GT observation.
Given either object OR action pose info return stuff to put in GT observation .
"""Given either object OR action pose info, return stuff to put in GT observation.
        Note: only called from within this class and 2-step case via subclassing.

        During training, there is normally data augmentation applied, so t_worldaug_world
        is NOT None, and augmentation is applied as if the 'image' were adjusted. However,
        for actions, we preserve theta so `object_quat_xyzw` does not change, and we query
        index=2 for the z rotation. For most deformables tasks, we did not use rotations,
        hence theta=0.

        Get t_world_object which is a 4x4 homogeneous matrix, then stick the position there (why?).
        Then we do the matrix multiplication with '@' to get the homogeneous matrix representation
        of the object after augmentation is applied.
        https://stackoverflow.com/questions/34142485/difference-between-numpy-dot-and-python-3-5-matrix-multiplication
        Then the position is extracted from the resulting homogeneous matrix. (And we ignore the
        z coordinate for that, but in fact for cloth vertices I would keep it.)

        Augmentation for cloth vertices? For now I'm following what they do for positions only
        (ignoring orientation), and returning updated (x,y,z) where the last value is the z
        (height) and not a rotation (doesn't make sense with vertrics anyway). Augmentations
        are all in SE(2), so the z coordinates should not change. Indeed that's what I see after
        extracting positions from `t_worldaug_object`.

        And yes, with vertices, they are flattened in the same way regardless of whether data
        augmentation is used, so every third value represents a vertex height. :D So we get
        [ v1_x, v1_y, v1_z, v2_x, v2_y, v2_z, ... ] in the flattened version.

        Args:
            t_worldaug_world: Only True if there's an augmentation and we need to change the
                ground truth pose as if we applied the augmentation. Only happens during (a)
                determining mean/stds, and (b) training. During initialization and the action
                (loading) we do not use this.
            preserve_theta: Only True if calling where `object_info` is an action pose.
            softbody: Only True if `object_info` is a vertex mesh of a PyBullet soft body.

        Returns:
            object_x_y_theta, pos, quat: the first is an SE(2) pose and parameterized by
            three numbers, the xy position and a scalar rotation `theta`. In case we have
            a cloth, I'm not returning anything after that (no pos or quat) so that if we
            use the older API for cloth (which we should not be!), it should crash code.
        """ # ------------------------------------------------------------------------------------------ # # Daniel: sanity checks to make sure we're handling the soft body ID correctly. # The last `if` of `softbody` helps to distinguish among actions, which are also length 2. # ------------------------------------------------------------------------------------------ # # (100, 3) # For each vertex, apply data augmentation. # Use identity quaternion (w=1, others 0). Othewrise, follow normal augmentation. # Stack everything. # ------------------------------------------------------------------------------------------ # # Now proceed as usual, untouched from normal ravens code except for documentation/assertions. # Daniel: pretty sure this has to be true. Upper left 3x3 is rotation, upper right 3x1 is position. # Daniel: data augmentation. Then, extract `object_position` from augmented object.
object_x_y_theta, pos, quat: the first is an SE(2) pose and parameterized by
three numbers, the xy position and a scalar rotation `theta`. In case we have
a cloth, I'm not returning anything after that (no pos or quat) so that if we
use the older API for cloth (which we should not be!), it should crash code.
object_x_y_theta pos quat : the first is an SE ( 2 ) pose and parameterized by three numbers the xy position and a scalar rotation ` theta ` . In case we have a cloth I ' m not returning anything after that ( no pos or quat ) so that if we use the older API for cloth ( which we should not be! ) it should crash code .
self
object_info
t_worldaug_world
Only True if there's an augmentation and we need to change the
ground truth pose as if we applied the augmentation. Only happens during (a)
determining mean/stds, and (b) training. During initialization and the action
(loading) we do not use this.
Only True if there ' s an augmentation and we need to change the ground truth pose as if we applied the augmentation . Only happens during ( a ) determining mean / stds and ( b ) training . During initialization and the action ( loading ) we do not use this .
preserve_theta
Only True if calling where `object_info` is an action pose.
Only True if calling where ` object_info ` is an action pose .
softbody
Only True if `object_info` is a vertex mesh of a PyBullet soft body.
Only True if ` object_info ` is a vertex mesh of a PyBullet soft body .