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