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
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
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
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
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()