def _couple_axes(command_dicts):
"""
If axis 2 and 3 are coupled (as they are with FANUC, for example),
Modify A3 accordingly
"""
# Get axes, if they exist
command_axes = []
for command_dict in command_dicts:
axes = command_dict[
postproc.AXES] if postproc.AXES in command_dict else None
command_axes.append(list(axes))
# Make sure the user has selected use of axes
if not all(x is None for x in command_axes):
# Get indices for command and axis
for command_index in range(len(command_dicts)):
theta_2 = command_axes[command_index][1]
theta_3 = command_axes[command_index][2]
theta_3_coupled = theta_3 - theta_2
# Replace theta_3 in the command_axes list with the coupled value
command_axes[command_index][2] = theta_3_coupled
# Replace the original commands with the new commands
coupled_commands = postproc.Axes(*command_axes[command_index])
command_dicts[command_index][postproc.AXES] = coupled_commands
return command_dicts
def _reconcile_command_rotations(robot_name, command_dicts):
"""
Check commands that used a sample rate.
This is primarily used to ensure that rotation is accumulated when an axis
rotates beyond +/- 180 degrees to avoid discontinuities
:param robot_name:
:param animation_settings:
:param command_dicts:
:return:
"""
# Get axes, if they exist
command_axes = []
for command_dict in command_dicts:
axes = command_dict[
postproc.AXES] if postproc.AXES in command_dict else None
command_axes.append(list(axes))
reconcile_axes = mimic_utils.get_reconcile_axes(robot_name)
# Make sure the user has selected use of axes
if not all(x is None for x in command_axes):
# Get indices for command and axis
for command_index in range(len(command_dicts)):
for axis_index in range(6):
# Get the initial value
value = command_axes[command_index][axis_index]
reconcile_axis = reconcile_axes[axis_index]
# Operate on the value depending on conditional
if reconcile_axis:
if command_index == 0:
continue
else: # Perform the check
previous_value = command_axes[command_index -
1][axis_index]
value = mimic_utils.accumulate_rotation(
value, previous_value)
# Replace original value with new value
command_axes[command_index][axis_index] = value
else: # Not a problem axis
pass
# Replace the original commands with the new commands
reconciled_axes = postproc.Axes(*command_axes[command_index])
command_dicts[command_index][postproc.AXES] = reconciled_axes
return command_dicts
def _check_command_rotations(robot, animation_settings, command_dicts):
"""
Check commands that used a sample rate.
:param robot:
:param animation_settings:
:param command_dicts:
:return:
"""
# TODO: Do this check using userOptions instead...
# Get axes, if they exist
command_axes = []
for command_dict in command_dicts:
axes = command_dict[postproc.AXES] if postproc.AXES in command_dict else None
command_axes.append(list(axes))
# Make sure the user has selected use of axes
if not all(x is None for x in command_axes):
start_frame = animation_settings['Start Frame']
# Get indices for command and axis
for command_index in range(len(command_dicts)):
for axis_index in range(6):
# Get the initial value
value = command_axes[command_index][axis_index]
# Operate on the value depending on conditional
if axis_index == 3 or axis_index == 5: # zero-indexed
rotation_axis = 'Z'
if command_index == 0: # Get initial value
axis_number = axis_index + 1
value = mimic_utils.get_reconciled_rotation_value(
robot,
axis_number,
rotation_axis,
start_frame)[0]
else: # Perform the check
previous_value = command_axes[command_index - 1][axis_index]
value = mimic_utils.accumulate_rotation(
value,
previous_value)
# Replace original value with new value
command_axes[command_index][axis_index] = value
else: # Not a problem axis
pass
# Replace the original commands with the new commands
reconciled_axes = postproc.Axes(*command_axes[command_index])
command_dicts[command_index][postproc.AXES] = reconciled_axes
return command_dicts
def _sample_frames_get_command_dicts(robot_name, frames, animation_settings,
time_interval_in_seconds, user_options):
"""
Sample robot commands using a list of frames and user options.
:param robot_name:
:param frames:
:param animation_settings:
:param user_options:
:return:
"""
# Initialize output array.
command_dicts = []
time_index_count = 0
for frame in frames:
# Set the background to the current frame
# TODO: Implement this! This rocks:
# pm.currentTime(frame)
# Create a dict of datatypes per frame
command_dict = {}
# Add this frame number/step/index to the dictionary
command_dict['Frame'] = frame
command_dict['Framerate'] = animation_settings['Framerate']
command_dict[
'Time Index'] = time_index_count * time_interval_in_seconds
time_index_count += 1
# Get motion parameters
if not user_options.Ignore_motion:
if user_options.Include_axes:
axes = _sample_frame_get_axes(robot_name, frame)
command_dict[postproc.AXES] = postproc.Axes(*axes)
if user_options.Include_pose:
pose = _sample_frame_get_pose(robot_name, frame)
command_dict[postproc.POSE] = postproc.Pose(*pose)
if user_options.Include_external_axes:
external_axes = _sample_frame_get_external_axes(
robot_name, frame)
command_dict[postproc.EXTERNAL_AXES] = postproc.ExternalAxes(
*external_axes)
if user_options.Include_configuration:
configuration = _sample_frame_get_configuration(
robot_name, frame)
command_dict[postproc.CONFIGURATION] = postproc.Configuration(
*configuration)
# Get IO parameters
if not user_options.Ignore_IOs:
if user_options.Include_digital_outputs:
# TODO: Implement digital outputs
# digital_output = None
# command_dict[postproc.DIGITAL_OUTPUT] = postproc.DigitalOutput(*digital_output)
pass
if user_options.Include_digital_inputs:
# TODO: Implement digital inputs
# digital_input = None
# command_dict[postproc.DIGITAL_INPUT'] = postproc.DigitalOutput(*digital_input)
pass
if user_options.Include_analog_outputs:
# TODO: Implement analog outputs
# analog_output = None
# command_dict[postproc.ANALOG_OUTPUT] = postproc.DigitalOutput(*analog_output)
pass
if user_options.Include_analog_inputs:
# TODO: Implement analog inputs
# analog_input = None
# command_dict[postproc.ANALOG_INPUT] = postproc.DigitalOutput(*analog_input)
pass
command_dicts.append(command_dict)
# Reset current frame (just in case)
pm.currentTime(frames[0])
return command_dicts
def _bound_accumulated_rotations(robot_name, command_dicts):
"""
Checks axes whose rotations have been accumulated to ensure they've not
exceeded the stated limits. If they have, this function attempts to slide
the commands by +/- 360 degrees. If the limits are still exceeded, this
function returns the commands that exceed the limits by the least amount
:param robot_name:
:param animation_settings:
:param command_dicts:
:return:
"""
# TODO: Do this check using userOptions instead...
# Get axes, if they exist
command_axes = []
for command_dict in command_dicts:
axes = command_dict[
postproc.AXES] if postproc.AXES in command_dict else None
command_axes.append(list(axes))
reconcile_axes = mimic_utils.get_reconcile_axes(robot_name)
rotation_limits = mimic_utils.get_all_limits(robot_name)['Position']
# Make sure the user has selected use of axes
if not all(x is None for x in command_axes):
for i, reconcile_axis in enumerate(reconcile_axes):
if reconcile_axis:
axis_number = i + 1 # Axis numbers are 1-indexed
axis_name = 'Axis {}'.format(axis_number)
# Get the axis limits
limit_min = rotation_limits[axis_name]['Min Limit']
limit_max = rotation_limits[axis_name]['Max Limit']
# Create a list of commands for the axis to be checked
axis_vals_init = [axis[i] for axis in command_axes]
axis_min = min(axis_vals_init)
axis_max = max(axis_vals_init)
'''
print "#######################################################"
print "Initial Axis {} vals: ".format(i+1), axis_vals_init
print "Axis Min Limit: ", limit_min
print "Axis Max Limit: ", limit_max
print "Axis Min: ", axis_min
print "Axis Max: ", axis_max
'''
## Perform conditional checks
# If no limits are violated, continue to the next axis without
# modifying the commands
if axis_min >= limit_min and axis_max <= limit_max:
# print '## No limits exceeded, no shift'
continue
# If both the max and min axis values exceed their respective
# limits, then there's nothing we can do about it, so we don't
# modify the commands
if axis_min < limit_min and axis_max > limit_max:
# print '## Both limits exceeded, but no shift'
continue
## Try bounding the values between the limits by shifting
## the commands by +/- 360 degrees
coeff = -1 if axis_min < limit_min else 1
# Check if the adjusted values would still violate the limits
# and if so, only shift them if the violation is smaller
axis_min_shift = axis_min - (coeff * 360)
axis_max_shift = axis_max - (coeff * 360)
# print "Axis Min Shifted: ", axis_min_shift
# print "Axis Max Shifted: ", axis_max_shift
if axis_min_shift < limit_min:
if abs(axis_min - limit_min) < abs(axis_min_shift -
limit_min):
# print '## Min limit exceeded, but no shift'
continue
elif axis_max_shift > limit_max:
if abs(axis_max - limit_max) < abs(axis_max_shift -
limit_max):
# print '## Max limit exceeded, but no shift'
continue
# If we've mad it this far it means we should shift all of the
# rotation values of the current axis by +/- 360
# print '## Limit exceeded and values shifted'
axis_vals_shift = [
val - (coeff * 360) for val in axis_vals_init
]
# print "Shifted Axis {} vals: ".format(i+1), axis_vals_shift
# Drop the shifted values back into the command_dicts
for command_index in range(len(command_dicts)):
command_axes[command_index][i] = axis_vals_shift[
command_index]
reconciled_axes = postproc.Axes(
*command_axes[command_index])
command_dicts[command_index][
postproc.AXES] = reconciled_axes
return command_dicts
def _sample_frames_get_command_dicts(robot_name, frames, animation_settings,
user_options, postproc_settings):
"""
Sample robot commands using a list of frames and user options.
:param robot_name:
:param frames:
:param animation_settings:
:param user_options:
:return:
"""
# Initialize output array.
command_dicts = []
start_frame = animation_settings['Start Frame']
end_frame = animation_settings['End Frame']
preview_in_viewport = postproc_settings['Preview in Viewport']
for frame in frames:
if preview_in_viewport:
# Set the background to the current frame
pm.currentTime(frame)
# Create a dict of datatypes per frame
command_dict = {}
# Add this frame number/step/index to the dictionary
command_dict['Frame'] = frame
command_dict['Framerate'] = animation_settings['Framerate']
command_dict[postproc.TIME_INDEX] = (
frame - start_frame) / animation_settings['Framerate']
# Get motion parameters
if not user_options.Ignore_motion:
if user_options.Include_axes:
axes = _sample_frame_get_axes(robot_name, frame)
command_dict[postproc.AXES] = postproc.Axes(*axes)
if user_options.Include_pose:
pose = _sample_frame_get_pose(robot_name, frame)
command_dict[postproc.POSE] = postproc.Pose(*pose)
if user_options.Include_external_axes:
external_axes = _sample_frame_get_external_axes(
robot_name, frame)
command_dict[postproc.EXTERNAL_AXES] = postproc.ExternalAxes(
*external_axes)
if user_options.Include_configuration:
configuration = _sample_frame_get_configuration(
robot_name, frame)
command_dict[postproc.CONFIGURATION] = postproc.Configuration(
*configuration)
# Get IO parameters
if not user_options.Ignore_IOs:
if user_options.Include_digital_outputs:
digital_output = _sample_frame_get_outs(
robot_name, frame, 'digital')
command_dict[postproc.DIGITAL_OUTPUT] = digital_output
if user_options.Include_digital_inputs:
# TODO: Implement digital inputs
# digital_input = None
# command_dict[postproc.DIGITAL_INPUT'] = postproc.DigitalOutput(*digital_input)
pass
if user_options.Include_analog_outputs:
analog_output = _sample_frame_get_outs(robot_name, frame,
'analog')
command_dict[postproc.ANALOG_OUTPUT] = analog_output
if user_options.Include_analog_inputs:
# TODO: Implement analog inputs
# analog_input = None
# command_dict[postproc.ANALOG_INPUT] = postproc.DigitalOutput(*analog_input)
pass
command_dicts.append(command_dict)
pm.refresh()
_update_export_progress_bar(start_frame, end_frame, frame)
# Reset current frame (just in case)
pm.currentTime(frames[0])
return command_dicts
def _generate_derivative_dicts(command_dicts, order):
"""
Generate a command dicts list that is the derivitive of the input
command_dicts
:param command_dicts: list formatted by mimic_program containing dicts of
program info at each program timestep
:param order: int specifying the order of the derivative (e.g. 1, 2, 3)
:return derivative_dicts: list containing command dicts for the current
derivative. Follows the same structure as command_dicts
"""
import copy
derivative_dicts = copy.deepcopy(command_dicts)
num_axes = _get_num_primary_axes(derivative_dicts)
previous_axes = []
for command_index, current_command in enumerate(command_dicts):
current_axes = current_command[postproc.AXES]
current_axes_derivative = [0] * num_axes
if command_index > 0: # skip zeroth
for axis_index, current_axis in enumerate(current_axes):
previous_axis = previous_axes[axis_index]
displacement = current_axis - previous_axis
# Do we need to compute displacement_time between every frame? Does this change?
displacement_time = current_command[
postproc.TIME_INDEX] - previous_command[
postproc.TIME_INDEX]
derivative = displacement / displacement_time
current_axes_derivative[axis_index] = derivative
derivative_dicts[command_index][postproc.AXES] = postproc.Axes(
*current_axes_derivative)
previous_axes = current_axes
previous_command = current_command
# Replace order-th value with with zeros; this is not a derivitive
# We do this to ensure the lengths of our command remain constant
for i in range(order):
try:
derivative_dicts[i][postproc.AXES] = postproc.Axes(*[0] * num_axes)
except IndexError:
pm.warning('Insufficient number of sample points to generate derivatives. ' \
'Increase sample rate or add additional IK/FK keys for proper ' \
'analysis if using time-based post-processor')
# If external axes exist, repeat derivative process on them
if 'external_axes' in derivative_dicts[0]:
num_external_axes = len(command_dicts[0][postproc.EXTERNAL_AXES])
# Get the indeces of external axes that are being used
active_axes = [
i
for i, x in enumerate(derivative_dicts[0][postproc.EXTERNAL_AXES])
if not x is None
]
num_active_external_axes = len(active_axes)
previous_axes = []
for command_index, current_command in enumerate(command_dicts):
current_axes = [
current_command[postproc.EXTERNAL_AXES][x] for x in active_axes
]
current_axes_derivative = [0] * num_active_external_axes
if command_index > 0: # skip zeroth
for derivative_index, current_axis in enumerate(current_axes):
previous_axis = previous_axes[derivative_index]
displacement = current_axis - previous_axis
displacement_time = current_command[
postproc.TIME_INDEX] - previous_command[
postproc.TIME_INDEX]
derivative = displacement / displacement_time
current_axes_derivative[derivative_index] = derivative
# We have to pass the same amount of axes that are in the
# Postprocess EXTERNAL_AXIS structure, so we have to build an array
# That matches the number of axis, with 'None' values for
# inactive axes.
external_axis_derivatives = [None] * num_external_axes
for derivative_index, axis_index in enumerate(active_axes):
external_axis_derivatives[
axis_index] = current_axes_derivative[derivative_index]
# Replace the current command entry with the command's derivatives
derivative_dicts[command_index][
postproc.EXTERNAL_AXES] = postproc.ExternalAxes(
*external_axis_derivatives)
previous_axes = current_axes
previous_command = current_command
# Replace order-th value with with zeros; this is not a derivitive
# We do this to ensure the lengths of our command remain constant
axis_pop = [None] * num_external_axes
for a, _ in enumerate(axis_pop):
if not derivative_dicts[i][postproc.EXTERNAL_AXES][a] is None:
axis_pop[a] = 0
for i in range(order):
derivative_dicts[i][
postproc.EXTERNAL_AXES] = postproc.ExternalAxes(*axis_pop)
return derivative_dicts
