Long formulas are to be expected. I actually have derived such equations of motion for a planar RRRR manipulator. I assumed real symbolic variables assume(A, 'real')
and run simplify()
. Generating code with MATLAB also creates intermediate variables. For the code output (in C) of a lower-triangular column-major order D(q)
matrix, I got the following (after some manual clean-up):
void Mlt (
const double *restrict in,
const double *restrict pc,
const double *restrict ma,
const double *restrict a,
const double *restrict q,
const double *restrict g,
double *restrict M)
{
/* inputs *
* ****** *
Inertia and mass
double in[24]; [Ixx, Iyy, Izz, Ixy, Ixz, Iyz]
double pc[12]; [cog_x, cog_y, cog_z]
double ma[4]; [mass]
DH parameters
a is distance along x-axis (linkage length)
double a[4];
generalized coordinates
double q[4];
gravitational constant
double g[1];
* ****** */
/* outputs *
* ******* *
double M[16];
* ******* */
double t[64];
t[0] = a[0]*a[0];
t[1] = a[1]*a[1];
t[2] = a[2]*a[2];
t[3] = q[1]+q[2];
t[4] = cos(t[3]);
t[5] = q[2]+q[3];
t[6] = cos(t[5]);
t[7] = cos(q[1]);
t[8] = cos(q[2]);
t[9] = cos(q[3]);
t[10] = q[1]+q[2]+q[3];
t[11] = cos(t[10]);
t[12] = ma[1]*t[1];
t[13] = ma[2]*t[1];
t[14] = ma[3]*t[1];
t[15] = ma[2]*t[2];
t[16] = ma[3]*t[2];
t[17] = a[3]*a[3];
t[18] = ma[3]*t[17];
t[19] = pc[3]*pc[3];
t[20] = ma[1]*t[19];
t[21] = pc[6]*pc[6];
t[22] = ma[2]*t[21];
t[23] = pc[9]*pc[9];
t[24] = ma[3]*t[23];
t[25] = pc[4]*pc[4];
t[26] = ma[1]*t[25];
t[27] = pc[7]*pc[7];
t[28] = ma[2]*t[27];
t[29] = pc[10]*pc[10];
t[30] = ma[3]*t[29];
t[31] = a[1]*ma[1]*pc[3]*2.0;
t[32] = a[2]*ma[2]*pc[6]*2.0;
t[33] = a[3]*ma[3]*pc[9]*2.0;
t[34] = a[1]*a[3]*ma[3]*t[6]*2.0;
t[35] = a[1]*ma[3]*pc[9]*t[6]*2.0;
t[36] = sin(t[3]);
t[37] = sin(t[5]);
t[38] = a[1]*a[2]*ma[2]*t[8]*2.0;
t[39] = a[1]*a[2]*ma[3]*t[8]*2.0;
t[40] = a[2]*a[3]*ma[3]*t[9]*2.0;
t[41] = a[1]*ma[2]*pc[6]*t[8]*2.0;
t[42] = a[2]*ma[3]*pc[9]*t[9]*2.0;
t[43] = sin(q[1]);
t[44] = sin(q[2]);
t[45] = sin(q[3]);
t[46] = sin(t[10]);
t[47] = a[0]*a[2]*ma[2]*t[4];
t[48] = a[0]*a[2]*ma[3]*t[4];
t[49] = a[0]*ma[2]*pc[6]*t[4];
t[50] = a[0]*a[3]*ma[3]*t[11];
t[51] = a[0]*ma[3]*pc[9]*t[11];
t[52] = a[1]*a[3]*ma[3]*t[6];
t[53] = a[1]*ma[3]*pc[9]*t[6];
t[54] = a[1]*a[2]*ma[2]*t[8];
t[55] = a[1]*a[2]*ma[3]*t[8];
t[56] = a[1]*ma[2]*pc[6]*t[8];
t[57] = a[2]*a[3]*ma[3]*t[9];
t[58] = a[2]*ma[3]*pc[9]*t[9];
t[59] = q[0]+q[1]+q[2]+q[3];
t[60] = cos(t[59]);
t[62] = sin(t[59]);
t[61] = a[3]*t[60]+pc[9]*t[60]-pc[10]*t[62];
t[63] = a[3]*t[62]+pc[9]*t[62]+pc[10]*t[60];
M[0] = in[2]+in[8]+in[14]+in[20]+t[12]+t[13]+t[14]+t[15]+t[16]+t[18]+t[20]+t[22]+t[24]+t[26]+t[28]+t[30]+t[31]+t[32]+t[33]+t[34]+t[35]+t[38]+t[39]+t[40]+t[41]+t[42]+ma[0]*t[0]+ma[1]*t[0]+ma[2]*t[0]+ma[3]*t[0]+ma[0]*(pc[0]*pc[0])+ma[0]*(pc[1]*pc[1])+a[0]*ma[0]*pc[0]*2.0+a[0]*a[2]*ma[2]*t[4]*2.0+a[0]*a[1]*ma[1]*t[7]*2.0+a[0]*a[2]*ma[3]*t[4]*2.0+a[0]*a[1]*ma[2]*t[7]*2.0+a[0]*a[1]*ma[3]*t[7]*2.0+a[0]*a[3]*ma[3]*t[11]*2.0+a[0]*ma[2]*pc[6]*t[4]*2.0+a[0]*ma[1]*pc[3]*t[7]*2.0+a[0]*ma[3]*pc[9]*t[11]*2.0-a[0]*ma[2]*pc[7]*t[36]*2.0-a[1]*ma[3]*pc[10]*t[37]*2.0-a[0]*ma[1]*pc[4]*t[43]*2.0-a[1]*ma[2]*pc[7]*t[44]*2.0-a[0]*ma[3]*pc[10]*t[46]*2.0-a[2]*ma[3]*pc[10]*t[45]*2.0;
M[1] = in[8]+in[14]+in[20]+t[12]+t[13]+t[14]+t[15]+t[16]+t[18]+t[20]+t[22]+t[24]+t[26]+t[28]+t[30]+t[31]+t[32]+t[33]+t[34]+t[35]+t[38]+t[39]+t[40]+t[41]+t[42]+t[47]+t[48]+t[49]+t[50]+t[51]+a[0]*a[1]*ma[1]*t[7]+a[0]*a[1]*ma[2]*t[7]+a[0]*a[1]*ma[3]*t[7]+a[0]*ma[1]*pc[3]*t[7]-a[0]*ma[2]*pc[7]*t[36]-a[1]*ma[3]*pc[10]*t[37]*2.0-a[0]*ma[1]*pc[4]*t[43]-a[1]*ma[2]*pc[7]*t[44]*2.0-a[0]*ma[3]*pc[10]*t[46]-a[2]*ma[3]*pc[10]*t[45]*2.0;
M[2] = in[14]+in[20]+t[15]+t[16]+t[18]+t[22]+t[24]+t[28]+t[30]+t[32]+t[33]+t[40]+t[42]+t[47]+t[48]+t[49]+t[50]+t[51]+t[52]+t[53]+t[54]+t[55]+t[56]-a[0]*ma[2]*pc[7]*t[36]-a[1]*ma[3]*pc[10]*t[37]-a[1]*ma[2]*pc[7]*t[44]-a[0]*ma[3]*pc[10]*t[46]-a[2]*ma[3]*pc[10]*t[45]*2.0;
M[3] = in[20]+t[18]+t[24]+t[30]+t[33]+t[50]+t[51]+t[52]+t[53]+t[57]+t[58]-a[1]*ma[3]*pc[10]*t[37]-a[0]*ma[3]*pc[10]*t[46]-a[2]*ma[3]*pc[10]*t[45];
M[5] = in[8]+in[14]+in[20]+t[12]+t[13]+t[14]+t[15]+t[16]+t[18]+t[20]+t[22]+t[24]+t[26]+t[28]+t[30]+t[31]+t[32]+t[33]+t[34]+t[35]+t[38]+t[39]+t[40]+t[41]+t[42]-a[1]*ma[3]*pc[10]*t[37]*2.0-a[1]*ma[2]*pc[7]*t[44]*2.0-a[2]*ma[3]*pc[10]*t[45]*2.0;
M[6] = in[14]+in[20]+t[15]+t[16]+t[18]+t[22]+t[24]+t[28]+t[30]+t[32]+t[33]+t[40]+t[42]+t[52]+t[53]+t[54]+t[55]+t[56]-a[1]*ma[3]*pc[10]*t[37]-a[1]*ma[2]*pc[7]*t[44]-a[2]*ma[3]*pc[10]*t[45]*2.0;
M[7] = in[20]+t[18]+t[24]+t[30]+t[33]+t[52]+t[53]+t[57]+t[58]-a[1]*ma[3]*pc[10]*t[37]-a[2]*ma[3]*pc[10]*t[45];
M[10] = in[14]+in[20]+t[15]+t[16]+t[18]+t[22]+t[24]+t[28]+t[30]+t[32]+t[33]+t[40]+t[42]-a[2]*ma[3]*pc[10]*t[45]*2.0;
M[11] = in[20]+t[18]+t[24]+t[30]+t[33]+t[57]+t[58]-a[2]*ma[3]*pc[10]*t[45];
M[15] = in[20]+ma[3]*(t[61]*t[61])+ma[3]*(t[63]*t[63]);
}
sin(a+b)
out of its expanded form, etc), or otherwise the solution could actually just be long. Note also that sometimes it's better to have the answer in the form of multiplication of a couple matrices, than to do the multiplication by formula. In the later case, you may end up recalculating the same terms over and over. $\endgroup$