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 _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