(I probably need a better name for this control strategy.)

## Overview

This control method is an attempt to combine the benefits of state-machines, trajectory control, phase regulation and feed-forward motor signals. The entire system is built upon a series of double-linked nodes which contain a desired state for feedback, a motor signal for feedforward and a gain matrix. Each node is spaced by a particular phase length. The traversal along the trajectory is dictated by the first motor output, reserved as the phase differential term. Between nodes, the motor commands are linearly-interpolated.

## Open Questions

Currently, the system has problems regarding the - to branch cut problem. The problem crops up in funny places due if a differential mode is being used. Currently, I believe the crank angle should reside on a cross-screw manifold, and jump from layer to layer at prescribed times, but thus far, implementation seems difficult. (Initial attempts did not seem to solve the problem, but instead create new ones.)

The method of linearly interpolating between nodes is unresolved. Currently, the method solves for the motor commands for each node and then creates a weighted sum of the two outputs. The alternative is to provide weighted sums of each term in the equation and then produce a single output. I'm not entirely sure what will happen if I linearly interpolate two different gain matrices.

## File format

• Version 1.3
• Filename hard coded to Trajectory.dat

### Input vector

• (19 elements):
• Four legs (16 elements total, 4 per leg)
• Front left
• Back left
• Front right
• Back right
• Accelerometer (3 elements)

##### Leg elements
• Wing angle position
• Crank angle position
• Wing angle velocity
• Crank angle velocity

• X-axis
• Y-axis
• Z-axis

### Output vector

• (10 element):
• Phase/time differential (two elements for each chain)
• Four legs (8 elements total, 2 per leg)
• Front left
• Back left
• Front right
• Back right

##### Chains
• Main locomotion chain
• Yaw control chain

##### Leg elements
• Motor A voltage command
• Motor B voltage command

### File layout

• Pointer to Startup Event
• Assorted segment, node and event descriptions
• The order is not fixed, each object will reference its neighbors using file addresses (4 bytes)

• Text of unspecified length, terminated by a null character. (this is chosen so that a standard text reader will display the header properly.)
• Two bytes: first byte is version major, second byte is version minor.
• Immediately followed by the start-up event

#### Trajectory Segment

• Start segment magic byte = 0x55 = 85
• 1 byte: Length of segment name
• N bytes: Name (no terminating character)
• 4 byte file pointer to segment start node
• 4 byte file pointer to segment end node
• 4 byte file pointer to previous segment
• 4 byte file pointer to next segment
• 4 bytes float, phase at the segment start
• 4 bytes float, phase at the segment end
• 1 byte: transistion flags
• bit 0 - entry-at-segment-start event exists
• bit 1 - entry-at-segment-end event exists
• bit 2 - exit-at-segment-start event exists
• bit 3 - exit-at-segment-end event exists
• bit 4 - high if excess phase should be truncate when transitioning to previous segment
• bit 5 - high if excess phase should be truncate when transitioning to next segment
• bit 6 - high if transition to previous segment should enter at start of segment
• bit 7 - high if transition to next segment should enter at end of segment
• 0 or 4 byte file pointer to entry-at-segment-start event
• 0 or 4 byte file pointer to entry-at-segment-end event
• 0 or 4 byte file pointer to exit-at-segment-start event
• 0 or 4 byte file pointer to exit-at-segment-end event
• These four file pointer existence depends on the transition flags
• End segment magic byte = 0xAA = 170

#### Trajectory Node

• Start node magic byte = 0x33 = 51
• 19 element vector of 4 byte floats, of the desired state.
• 10 element vector of 4 byte floats, of the feedforward command.
• 4 byte file pointer to gain matrix
• End node magic byte = 0xCC = 204

#### Gain matrix

• Start gain matrix magic byte = 0x66 = 102
• 19 x 10 element matrix of 4 byte floats, of the gain matrix (written column-wise, so adjacent bytes are in the same column)
• End gain matrix magic byte = 0x99 = 153

#### Events

• Start event magic byte = 0x0F = 15
• 1 byte: Event command
• Multiple bytes depending on event command
• End event magic byte = 0xF0 = 240

##### Event splitter
• Command byte: 0xFF
• 1 byte: number of events
• 4 x N bytes: file pointer to events
• Events are execute in the order they are listed.

##### Spawn new trajectory chain
• Command byte: 0x10
• 4 bytes: file pointer to starting node
• 1 byte: Chain index
• 1 byte: Index of output vector to retrieve phase differential element from.
• 1 byte: Length of chain name
• N bytes: Name (no terminating character)

##### Kill trajectory chain
• Command byte: 0x11
• 1 byte: Chain index

### Differences from previous versions

#### version 1.2

• In the old version, the main entity is the node, and contained a phase length to the next node. This made discontinuous jump harder, and is discarded in place of the segment as the main entity.

#### version 1.1

• Input vector is 16 elements (missing accelerometer terms)
• Output vector is 9 elements
• Multipe chains are not handled
• No events are handled, the code jumps from gain matrix to end magic

#### version 1.0

• Input vector is 8 elements (missing velocity terms)
• Output vector is 9 elements
• Gain is 9x8 elements
• No start and end magic bytes.

## Future Improvements/Ideas

1. Divide chains into nodes and segments, the nodes will only describe the state, gain and command terms and pointers to next and previos segmnts. The segments connect the node together and provide the phase length and phase offsets. This allows for better control when providing bi-directional phasing control and backward-pointing one-way transitions.
2. For better memory, only let the robot have a finite number of chains.
3. Add force sensors into input vector
4. Add low-pass and high-pass filtered version of sensors as needed
5. Add a mapping function to turn on and off input vector elements such that the full gain need not be stored on disk. (For example, the full gain matrix may have entire columns zeroed out.)
6. Add a file pointer system, such that nodes using identical gains, vectors, whatnot, may use the same block of disk memory.