1
$\begingroup$

I'm working on the dynamics model of a RRRR articulated robot, I'm following Euler-Lagrange approach and developing my code in m-file in matlab; I'm looking for dynamic model of this form: $$ D(q) \ddot{q} + C(q,\dot{q})\dot{q}+ g(q) = \tau $$ where $D$ and $C$ are $4 \times4$ matrices and $g$ and $\tau$ (torque) are $4\times1$ vectors; by formulating the kinetic and potential energies;;

The problem is that, I'm getting very long equations, and the term in $D$ matrix are very huge and nonlinear, involving sin and cos; I'm talking about a several pages per equation; After I published the code - 7 pages - and the output I got around 45 pages in total; I searched around there was some guy he faced the same problem before, but there was no helpful proposal.

Any Suggestions ??

Improve this question
$\endgroup$
3
  • 2
    $\begingroup$ It would be very hard for anyone to be helpful given that we don't have any idea how you derived those long formulae. There could be many issues; you could have made a mistake in the middle, there could be room for optimization (e.g., factoring out terms to get 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$
    – Shahbaz
    Mar 9, 2015 at 12:22
  • $\begingroup$ I'm following the popular form of Lagrange Equation, Taking Lagrangian L=K-V, (kinetic energy)-(potential energy), it's not too long for the potential energy, but the kinetic energy I think cause the problem , because we need both the translational part and the rotational part, and the later involve the inertia matrices and consequently the rotation matrices, to bring all the stuff to the global frame, hopefully, this clarify my point .... $\endgroup$
    – AlFagera
    Mar 10, 2015 at 5:48
  • $\begingroup$ Can you post a diagram of your setup along with the code you're using? Can you post your derivation of the Lagrangian? Are you then using the Langrangian correctly? $\frac{\partial L}{\partial q_i}=\frac{d}{dt} \left( \frac{\partial L}{\partial \dot{q}} \right) $? Note that, for the left side, you take the partial derivative with respect to $q$, and on the right side you take the partial derivative with respect to the derivative of q, $\dot{q}$. $\endgroup$
    – Chuck ♦
    May 10, 2016 at 12:41

4 Answers 4

Reset to default
4
$\begingroup$

You can use the 'simplify' command , e.g : a=simplify(A*B) .This command is calculating some trigonometric functions thus making your expression shorter .

Improve this answer
$\endgroup$
3
$\begingroup$

It really doesn't sound very unexpected for your equations to become this long. Something that might help a little, is using assumptions for defining symbolic variables.

x = sym('x','real');
y = sym('y','positive');
z = sym('z','integer');
t = sym('t','rational');
Improve this answer
$\endgroup$
2
  • $\begingroup$ I can tell you've been coding in Matlab a lot, from all the semicolons in your text :) . $\endgroup$
    – BarzinM
    Mar 9, 2015 at 17:11
  • $\begingroup$ Not too much, but you see, I usually like to separate my sentences, Anyway, I already get 'real', before that I used to get conj. and very huge stuff; however, I don't think I need positive and integer. $\endgroup$
    – AlFagera
    Mar 10, 2015 at 6:15
2
$\begingroup$

When working with robotic matrices, such as transforms or dynamics, long-long-long equations are usually expected when everything is numerical. Using a symbolic platform such as MuPad or Maple will allow you to easily simplify the matrices symbolically (usually by using trigonometric identities).

Cheers,

Improve this answer
$\endgroup$
2
  • $\begingroup$ I tried using MuPad, after I got my equations, copied one of them, and ask MuPad to simplify, combine or rewrite, but still very long. Furthermore, I noticed there are sum similar terms involving sin and cos; could be combined, but there are staying separately; I don't why this is happen, Do I need to make some assumptions in MuPad first or what, to simplify those trigonometric functions. I did not try Maple. $\endgroup$
    – AlFagera
    Mar 15, 2015 at 5:00
  • $\begingroup$ Definitely use f2 := combine(f, sincos). Where f2 is the simplified function, f is the original function, "sincos" is a modifier which tells MuPad to combine trig terms. Check out mathworks.com/help/symbolic/mupad_ug/… for more info. $\endgroup$
    – nnadeau
    Mar 15, 2015 at 14:56
2
$\begingroup$

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]);

}
Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge that you have read and understand our privacy policy and code of conduct.

Not the answer you're looking for? Browse other questions tagged or ask your own question.