Ejemplo n.º 1
0
def getJointAngle( model , joint_name ):
    """ Returns a list with the rotation in euler angles of the given joint. """
    jointAngle = []

    ### Calculate every frame
    for frame_index in range( model.numberOfFrames ):
        model.setFrame( frame_index )
        jointAngle.append( model.model_eulerAngles[ joint_name ] )

    return jointAngle
Ejemplo n.º 2
0
def getJointPosition( model , joint_name ):
    """ Calculates the position of the given joint in every frame."""
    jointPosition = []

    ### Calculate every frame
    for frame_index in range( model.numberOfFrames ):
        model.setFrame( frame_index )
        jointPosition.append( model.model_position[ joint_name ] )

    return jointPosition
Ejemplo n.º 3
0
def getJointSpeed( model , joint_name ):
        """ Calculates the average speed of the joint between each succesive frames. """
        jointSpeed = []

        ### Initialize model to the position in the first frame
        model.setFrame( 0 )  
        current_position = model.model_position[ joint_name ]  

        ### Calculate average speed of succesive joint positions
        for frame_index in range( 1 , model.numberOfFrames ):
            model.setFrame( frame_index )
            previous_position = current_position
            current_position = model.model_position[ joint_name ]
            
            jointSpeed.append( np.linalg.norm( current_position - previous_position ) / model.frameTime )

        return jointSpeed
Ejemplo n.º 4
0
def getJointLocalWorkspace( model , joint_name ):
    """ Returns the observed positions of the joint in the local workspace. """
    
    ### Get joint node
    joint = model.getJointByName( joint_name )

    ### Local workspace positions
    jointLocalWorkspace = []

    ### Calculate every frame
    for frame_index in range( model.numberOfFrames ):
        model.setFrame( frame_index )
        
        local_position = np.matrix( joint.transformation ) * np.matrix( [ [0] ,[0] ,[0] ,[1] ] )

        jointLocalWorkspace.append( [ local_position[0,0] , local_position[1,0] , local_position[2,0] ] )

    return jointLocalWorkspace
Ejemplo n.º 5
0
def plotFrame( model , frameNumber ):
    """ Plots a frame of the model """

    ## Set frame
    model.setFrame( frameNumber )

    ## Get every joint coordinates in the frame
    x = [ model.model_position[ key ][ 0 , 0 ] for key in model.model_position ] 
    y = [ model.model_position[ key ][ 1 , 0 ] for key in model.model_position ]
    z = [ model.model_position[ key ][ 2 , 0 ] for key in model.model_position ]

    ## 

    ## Plot model
    fig = plt.figure( figsize=plt.figaspect( 1 ) )
    ax = fig.add_subplot( 111 , projection='3d' )
    ax.set_aspect('equal')

    ax.scatter( x , y , z )

    ax.set_xlabel('X Label')
    ax.set_ylabel('Y Label')
    ax.set_zlabel('Z Label')

    ## Set bounding box for the plot
    max_range = max( [ max( x ) - min( x ), max( y ) - min( y ) , max( z ) - min( z ) ] ) / 2.0

    mean_x = np.mean( x )
    mean_y = np.mean( y )
    mean_z = np.mean( z )
    
    ax.set_xlim( mean_x - max_range, mean_x + max_range )
    ax.set_ylim( mean_y - max_range, mean_y + max_range )
    ax.set_zlim( mean_z - max_range, mean_z + max_range )
    
    plt.show()