Assignment #2
US: 1.099868
FS: 0.894989
S_OFFSET: -0.102421
E_OFFSET: 0.098853
Tuesday, February 16, 2010
Learning a model.
Using the same 2-link manipulator simulation as Assignment #1, learn a model that can compensate for deviations in kinematic and dynamic parameters from the assumed model parameters.
Kinematic Calibration

#define US 1.1
#define FS 0.9

#define S_OFFSET -0.1
#define E_OFFSET 0.1
The same principle of using Nelder-Mead was applied to the task of learning the dynamic parameters.  This didn’t work nearly as well.  The goal was to have a method that could learn any extra forces in the dynamics model, as well as scaled values for parameters like moment of inertia and viscous damping.  While this was in fact achieved (although with not nearly as much accuracy as the kinematic calibration), there were two main problems that made this difficult.
1.    In an attempt to compensate for extra x and z forces acting on the arm, a total of 16 parameters were being optimized.  This resulted the algorithm finding local minima among parameters that still resulted in significant amounts of total error.  Additionally, the model would also solve for parameters that are impossible to achieve in real-life (like negative moments of inertia).
2.    The gains on velocity in the control created oscillations at the elbow, especially when the arm was sitting still.  Even when the simulation did not blow up, the oscillation added significant amounts of error to the elbow dynamic score.
The scoring function for Nelder-Mead was a 10-second simulation where torques at the elbow and shoulder were cycled periodically following a sine function.  2 simulations were actually performed in parallel, one with the dynamics from dynamics.c, and the other with the forward dynamics model that used a ‘dummy’ sim model with variable parameters.  The difference between the two models at each time step were compared at each iteration and the summed error is what was minimized.  Parameters in the dynamic optimization were:
Mass ( upper arm / forearm )
Center of mass ( upper arm / forearm )
Moment of inertia ( upper arm / forearm )
External force x-direction ( upper arm / forearm )
External force z-direction ( upper arm / forearm )
Force moment ( upper arm / forearm )
Viscous friction ( shoulder / elbow )
To optimize the parameters, Nelder-Mead was initialized to start with the parameters having the values that get assigned with init_default_parameters().  Initial step sizes for optimization are chosen with respect to the relative scales of each parameter.  After one iteration if the solution does not converge to a small enough error the algorithm restarts with random perturbations from the nominal parameter values.  This repeats up to 10 times if necessary, and at the end the best set of parameters is chosen to serve as the feed-forward model in inverse_dynamics().
Blowups in move_to_start() were mitigated by implementing the minimum jerk trajectory and allowing the arm to sit for 1 second at steady state before starting the dynamic test.  The point at the start of dyn_test() served as the reading for steady-state error.  As can be seen below the oscillations at the elbow resulted in a higher average error.  The oscillation can be confirmed by observing that the angular velocity at the elbow at the end of move_to_start() is frequently non-zero.
Avg. SS Error:    0.0720938     0.133966
Even though the learned dynamic model provided a reasonably set of feed-forward torques, and the trajectories in dyn_test() were generally followed.  The arm did so with significantly more error.  Additionally, there were more blowups with these parameters than in the unmodified code (25 vs 15).  The dynamic test score for the default parameters modified by the same scaling factors as the kinematic calibration above is shown.
total_dyn_score:  353.582
Overall, this method worked ok, but a method that used the known structure and dynamics of the problem would probably have been more effective in this case (if you have structure you should probably use it).  Lastly, at least 3 redundant parameters could have been eliminated from the dynamic optimization (gravity, and the 2 centers of mass) and their effects incorporated into the external forces, although other than making the optimizations run slightly faster, I’m not sure if it would have helped.
To learn the kinematic model, parameters were assumed to be the nominal values from init_default_parameters().  The Nelder-Mead minimization algorithm (from the GNU Scientific Library) was used to minimize the forward kinematic error by varying the parameters for scaling the upper and lower arms, and adjusting the angular offset at the joints and testing over a series of 625 randomly selected angle combinations.  This method reliably identified these parameters for a range of values.  The default scaling values from dynamics.c, and the learned values from optimization are below.  During the forward kinematic calibration, errors of approximately 0.003 and 0.006 in the x - and z - directions respectively were achieved (~1% of full arm length).
Since these values are within about 2% of the true scaling and offset factors, the inverse kinematic solver was able to produce very accurate scores for the kinematic test.  The inverse kinematics solver uses the same method as Assignment #1 (law of cosines and atan2), except that the lengths of the arm are scaled by the appropriate factor, and the angle offsets are subtracted from the calculated angles.
Avg. Kin Error:   0.00150111     0.00150111
kin_score:        0.00169648
It should be noted that the kinematic scoring function was modified slightly to ignore any scores that were reported as NaN.  Because the arms are now different lengths because of the scaling factor, some points near the origin become impossible to reach but are not caught by the out or range checking in the test loop.
Dynamic Calibration (Mac OS X - 10.6, assumes a standard MacPorts install of GSL)