src/conntact/assembly_tools.py
def to_homogeneous(quat, point):
        """Takes a quaternion and msg.Point and outputs a homog. tf matrix.
        :param quat: (geometry_msgs.Quaternion) Orientation information.
        :param point: (geometry.msgs.Point) Position information.
        :return: (np.Array()) 4x4 Homogeneous transform matrix.
        """
        #TODO candidate for Utils 
        output = trfm.quaternion_matrix(np.array([quat.x, quat.y, quat.z, quat.w]))
        output[0][3] = point.x
        output[1][3] = point.y
        output[2][3] = point.z
        return output
Takes a quaternion and msg.Point and outputs a homog. tf matrix.
        :param quat: (geometry_msgs.Quaternion) Orientation information.
        :param point: (geometry.msgs.Point) Position information.
        :return: (np.Array()) 4x4 Homogeneous transform matrix.
        
Takes a quaternion and msg.Point and outputs a homog. tf matrix.
Takes a quaternion and msg . Point and outputs a homog . tf matrix .
def to_homogeneous(quat, point):
        output = trfm.quaternion_matrix(np.array([quat.x, quat.y, quat.z, quat.w]))
        output[0][3] = point.x
        output[1][3] = point.y
        output[2][3] = point.z
        return output
def to_homogeneous ( quat , point ) : output = trfm . quaternion_matrix ( np . array ( [ quat . x , quat . y , quat . z , quat . w ] ) ) output [ 0 ] [ 3 ] = point . x output [ 1 ] [ 3 ] = point . y output [ 2 ] [ 3 ] = point . z return output
Takes a quaternion and msg.Point and outputs a homog.
Takes a quaternion and msg . Point and outputs a homog .
"""Takes a quaternion and msg.Point and outputs a homog. tf matrix.
        :param quat: (geometry_msgs.Quaternion) Orientation information.
        :param point: (geometry.msgs.Point) Position information.
        :return: (np.Array()) 4x4 Homogeneous transform matrix.
        """ #TODO candidate for Utils 
(np.Array()) 4x4 Homogeneous transform matrix.
( np . Array () ) 4x4 Homogeneous transform matrix .
quat
(geometry_msgs.Quaternion) Orientation information.
( geometry_msgs . Quaternion ) Orientation information .
point
(geometry.msgs.Point) Position information.
( geometry . msgs . Point ) Position information .