0

Currently I am using the vpasolve command to solve a system of 3 equations with 3 unknowns in it. These 3 equations are being used to model the kinematic motion of a robot manipulator, and the unknowns are either the lengths of the links of the robot or the angles of rotation of the joints. The vpasolve command manages to solve the equations correctly, the problem is that the solutions that I get are not close. What do I mean by not close? If I get a solution of 1.5, 0.3, -0.4 [rad] for one of the angles, the next solution for the next step of the motion would be any other random angle that may solve it, but in reality is not useful, so it could be something like -0.8 -1.5 2 [rad]. Of course it would be pointless for a robot to jump from 1.5 rad to -0.8 rad just to move its end point by 1 cm. Instead I would like to get a solution that is as close as possible to the angles from the previous solution.

The equations look something like this:

x == 1*cos(theta1)*cos(0) + 1*cos(theta1 + theta2)*cos(0 + 0) + 1*cos(theta1 + theta2 + theta3)*cos(0 + 0 + 0)
y == 1*cos(theta1)*sin(0) + 1*cos(theta1 + theta2)*sin(0 + 0) + 1*cos(theta1 + theta2 + theta3)*sin(0 + 0 + 0)
z == 1*sin(theta1) + 1*sin(theta1 + theta2) + 1*sin(theta1 + theta2 + theta3)

I give them different values for x y and z and solve the thetas for these values. For example:

t = 0:0.1:1;
x = 2 * t;
y = 0;
z = 1;

So here we have 10 different positions, I know that there are "consecutive" solutions, I just dont know how to make vpasolve give me these solutions, here is how I use it:

syms x y z theta1 theta2 theta3    

xEquation = 'x == 1*cos(theta1)*cos(0) + 1*cos(theta1 + theta2)*cos(0 + 0) + 1*cos(theta1 + theta2 + theta3)*cos(0 + 0 + 0)';
yEquation = 'y == 1*cos(theta1)*sin(0) + 1*cos(theta1 + theta2)*sin(0 + 0) + 1*cos(theta1 + theta2 + theta3)*sin(0 + 0 + 0)';
zEquation = 'z == 1*sin(theta1) + 1*sin(theta1 + theta2) + 1*sin(theta1 + theta2 + theta3)';

t = 0:0.1:1;
x = 2 * t;
y = 0;
z = 1;

xEquationEv = eval(xEquation);
yEquationEv = eval(yEquation);
zEquationEv = eval(zEquation);

for f = 1:size(x, 2)
    sol(f) = vpasolve([xEquationEv(f), yEquationEv, zEquationEv], [theta1, theta2, theta3], [-pi pi; -pi pi; -pi pi]);
end

sol.theta1
sol.theta2
sol.theta3

You will see how the values are not smoothly incrementing or decrementing all the time, but sometimes they make big jumps.

Note: I am starting off with strings and eval, because the equations may change, I have another algorithm that is generating them.

Darkbound
  • 3,026
  • 7
  • 34
  • 72
  • Why do have constant arguments to some of your periodic functions? The whole problem seems to me like you rather should look into solve differential equations. – Sven-Eric Krüger May 16 '19 at 09:30
  • These constant arguments are the lengths of the links and other joint angles that are not changing, I am just starting off with these generalized equations and substituting my values in them – Darkbound May 16 '19 at 09:44

2 Answers2

0

I think I managed to solve it by recalculating the limits after obtaining the first solution. Something like:

limits = [previousTheta1 - step previousTheta1 + step;
          previousTheta2 - step previousTheta2 + step;
          previousTheta3 - step previousTheta3 + step];

That gave me much smoother motion (solutions).

Sardar Usama
  • 19,536
  • 9
  • 36
  • 58
Darkbound
  • 3,026
  • 7
  • 34
  • 72
0

Sounds like you have already solved this, but you might also try using the unwrap function on your original result.

For non-polynomials, vpasolve only returns one answer (of two, in your case) to the equations. unwrap should tend to reduce the number of times that the variables jump between the different solutions to your equations, though you will need to reduce the default tolerance (as that was set up to catch 2*pi jumps).

I would try plotting unwrap(sol.theta1,pi/2), for example, and see if that looks more like what you were expecting.

Jim Quirk
  • 606
  • 5
  • 18