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