The nominal kinematics model for such an arm is:
I chose to learn a calibration that handles changes in link lengths, an angle offset for each joint, and which uses a sort of catch-all set of Cartesian offsets for the elbow and wrist joints in a primitive attempt to model various other affects. The resulting model is thus:
To make this linear in the parameters to be learned, I apply the small angle approximation:
So that this becomes:
I then pose this as a linear system, for which a single observation of a pair of joint angles and the resulting Cartesian joint position provides 4 equations:
By moving the joints to a variety of angles and observing the resulting Cartesian positions of the joints, we may accumulate the A matrix, b vector, and solve for the parameter vector x. The model parameters may then be derived uniquely from the values in this vector.
Given that this represents learning a function from one 2D space to another, it would be entirely reasonable to use a brute-force method that fills in a table representing a discretization of the input space, for which interpolated lookups provide the output. Though this would provide much better fitting to bizarre models that aren't simply a perturbation of the basic 2R model, I chose to pursue the linear model fitting becuase I tend to use such brute-force methods all too often and need experience with other alternatives.
The dynamics model for such a system is conventionally written in the form:
which in the case of a 2R planar arm would be:
To calibrate this, I attempt to learn a model that handles changes in link lengths, center of mass locations, moments of inertia, joint angle offsets, viscous friction, torque offsets (constant opposing torques), and gravitational acceleration. In practice, torque scaling (or more generally a function that maps desired to actual torque) would be important, and it may be noted that the model I propose handles linear torque mapping functions.
Towards this end, I first extend the dynamics model as follows:
Then, I apply the small angle approximation as before, to reduce the trigonometric functions to be just functions of joint angles and not learned parameters:
Note that by collecting terms, we may rewrite this as:
This may then be posed as a large linear system, for which each observation of a pair of joint positions, joint velocities, applied torques, and resulting joint accelerations provides 2 equations:
By moving the joints to a variety of combinations of angles, angular velocities, and applying a variety of torques while observing the resulting angular accelerations of the joints, we may accumulate the A matrix, b vector, and solve for the parameter vector x. The values from x may then be used to fill in the M, C, and G matrices, completing the dynamics model. Certainly, constraints exist between the elements of the parameter vector that are not explicitly maintained by a least squares minimization, but if it is successful (and the model form is accurate), then these constaints should be automagically approximately maintained, without having to resort to explicit constrained optimization.
This model is slightly more general than needed, which should hopefully allow it to handle at least some other kinds of perturbations. Flexible links and/or additional joints complicate the model sufficiently (basically, just greatly enlarging it) that I deemed this unworthwhile.
To learn any of these linear models, a reasonably large set of observations is required. To accomplish this, I control the arm to execute a joint position trajectory that is the sum of several sinusoids of varying amplitudes and periods (resulting in a net period that is "very large") with small random position and torque deltas added. This results in an overall trajectory that represents a reasonably uniform sampling of the joint position space (and hence reachable Cartesian space) as well as joint angular velocity, angular acceleration, and torque space, as briefly verified by plotting these. This trajectory is followed using a PD controller with feedforward torques provided by the ideal model for the provided arm parameters. Though tracking may be poor if the PD gains or feedforward torques are inappropriate for the actual model, sampling should still be successful as long as a diversity of states is maintained across the trajectory.
An example of such a trajectory:
As these trajectories are executed, the current state is captured, and the A and y matrices are built up for the two linear systems to calibrate the kinematics and dynamics.
To solve the resulting linear systems, I use a recursive least squares algorithm. This has the nice properties of a small constant-size memory footprint and of making the calibration an anytime algorithm, requiring no batch computation after the sampling trajectory has been performed.
A summary of the algorithm is as follows. Assume that each observation provides values x and y, and we seek to be estimate a parameter vector theta such that:
that minimizes the objective function:
Start with an initial estimate of the parameter vector and an initial covariance:
and then with each observation execute the update:
In the case of solving for these calibrations, I set the initial value of the parameter vectors to be what the correct values would be if the provided ideal parameters for the robot were actually accurate. Thus, is the actual model is only a slight perturbation from the ideal model, the calibration starts with a nearly correct value. As for the covariance matrix, traditionally one starts with diagonal matrices containing large values. In this case, I chose these diagonal values to simply be the square of several times a value "on the order of the values to be learned," so that scaling shouldn't present an issue.
Once the kinematic calibration has been learned, we may apply the model to implement a forward and inverse kinematics functions. As the kinematics model I'm using is very similar to the nominal model for this arm, only small modifications to the original forward and reverse kinematics functions are required.
Forward kinematics function:
void my_forward_kinematics(const Vec2d &theta, Vec2d *x)
{
double l1 = kinematics_model.l1;
double l2 = kinematics_model.l2;
double dth1 = kinematics_model.dth1;
double dth2 = kinematics_model.dth2;
Vec2d d1 = kinematics_model.d1;
Vec2d d2 = kinematics_model.d2;
double s1 = sin(theta.n[0] + dth1);
double c1 = cos(theta.n[0] + dth1);
double s12 = sin(theta.n[0] + dth1 + theta.n[1] + dth2);
double c12 = cos(theta.n[0] + dth1 + theta.n[1] + dth2);
Vec2d x1 = Vec2d(l1*c1, l1*s1) + d1;
Vec2d x2 = x1 + Vec2d(l2*c12, l2*s12) + d2;
x->n[0] = x2.y(); //x_sim
x->n[1] = -x2.x(); //z_sim
}
Inverse kinematics function:
void my_inverse_kinematics(const Vec2d &x, Vec2d *theta, const Vec2d *curr_theta = NULL)
{
double l1 = kinematics_model.l1;
double l2 = kinematics_model.l2;
double dth1 = kinematics_model.dth1;
double dth2 = kinematics_model.dth2;
Vec2d d1 = kinematics_model.d1;
Vec2d d2 = kinematics_model.d2;
double x_des = -x.n[1] - d1.x() - d2.x();
double y_des = x.n[0] - d1.y() - d2.y();
theta->n[1] = acos(MAX(-1.0,MIN(1.0,(x_des*x_des+y_des*y_des - l1*l1 - l2*l2)/(2.0*l1*l2)))); //elbow
if (curr_theta != NULL &&
fabs(sub_angled(theta->n[0],curr_theta->n[0])) > M_PI_2)
theta->n[1] *= -1.0;
theta->n[0] = atan2(y_des,x_des) - atan2(l2*sin(theta->n[1]),
l2*cos(theta->n[1])+l1); //shoulder
theta->n[0] -= dth1;
theta->n[1] -= dth2;
}
Note that I originally computed these for the canonical arrangement of an arm (in the X-Y plane, with the zero position pointing straight out along X, etc.), so the relevant swapping and negation of axes is included.
As with kinematics, we may compute forward and inverse dynamics once the dynamics parameters have been found using the least squares algorithm. These are first used to fill in the M, C, and G matrices as defined previously for the dynamic model.
Forward dynamics function:
void my_forward_dynamics(const Vec2d &theta, const Vec2d &thetad, const Vec2d &tau, Vec2d *thetadd)
{
*thetadd = dynamics_model.M.inverse()*(tau - dynamics_model.C*thetad - dynamics_model.G);
}
Inverse dynamics function:
void my_inverse_dynamics(const Vec2d &theta, const Vec2d &thetad, const Vec2d &thetadd, Vec2d *tau)
{
*tau = dynamics_model.M*thetadd + dynamics_model.C*thetad + dynamics_model.G;
}
To test this calibration learner against an uncalibrated model, I implemented an easy way to switch between using either a calibrated (learned) or uncalibrated (ideal) model and added several tests to the simulate.c file. These were tested on an actual model matching the nominal one, the provided slight perturbation therefrom (1.1/0.9 link scaling and -0.1/0.1 joint offset), and a nastier one (3.1/0.9 link scaling, -0.1/0.1 joint offset, and 10x gravity).
The tests performed were:
The following table summarizes the results achieved:
| Avg fwd kin err | Avg inv kin err | Avg steady state err | Avg traj err | |||||
| Learner | Ideal model | Learner | Ideal model | Learner | Ideal model | Learner | Ideal model | |
| Nominal model | 0 | 8.08e-4 | 0 | 0 | 0 | 0 | 3.4972 | 3.4972 |
| Provided example perturbation | 2.7471e-6 | 0.0019 | 2.7448e-6 | 0.0044 | 0 | 0.0177 | 19.1876 | 947.989 |
| Nasty model | 5.4354e-4 | 0.4024 | 5.0381e-4 | inf/nan | 1.2622e-4 | 5.3246 | 8.3769 | 44350.6 |
Note: in this table, values less than 1e-6 are rounded down to 0.
Some observations that may be made from this:
My implementation of the calibration training and testing adheres to the specification provided on 2/7. When train() is called, a simulation is run that executes the sampling trajectory described above, presently collecting 50,000 observations, which takes about 10s on my machine. The models derived are saved to kinematics_model.txt and dynamics_model.txt respectively for offline study.
As for the dyn_test function, only small tweaks were made, and I did not replace the provided trajectory() function as it is a perfectly good quintic spline generator. I did modify move_to_start() to execute a spline from the current position to the desired one, then stabilize for a while, so that the start point is more likely to be reached properly.
I modified main() in simulate.c to clean-up and add additional tests, but I did not change its fundamental operation. I added an "inner reachability" limit and ignore NaN inverse kinematic results so that some kinematic score can be computed in any event. I also observed that 100 trajectory samples is far too small a number to provide a consistent score, so I increased this to 10,000 and print an average on the same scale as 100 tests to produce results consistent with the provided metric. That is, the 100*avg_dyn_score value should be compared with the original total_dyn_score.
Source code may be found here.