Solve System of Equations to calculate the inverse kinematics

Hello,

Im trying to calculate the analytical sollution to the inverse kinematics of a Robot. In this case its a 'RRR' Robot, but im trying to set it up so the exact axis configuration can be changed as long as no redundancies occur. In this example im using the following robot geometry:

that should be able to reach every point in a 2-meter sphere around the base in four different ways (elbow-up, elbow-down and the same if q1 is rotated by 180°)

so far i have the forward transformation

syms q1 q2 q3;

X = cos(q1)*(cos(q2 + q3) + cos(q2));
Y = sin(q1)*(cos(q2 + q3) + cos(q2));
Z = sin(q2 + q3) + sin(q2) + 1/5;

and would like to calculate the inverse.

I tried using solve(), which works for the numerical solution for a specific point in space (although it returns a warning along with the result)

E1 = X == 1;
E2 = Y == 0.5;
E3 = Z == 1;

A = [E1, E2, E3];
B = [q1, q2, q3];

B = solve(A, B)

Warning: Unable to solve symbolically. Returning a numeric solution using vpasolve.

B = struct with fields:
    q1: 0.46364760900080611621425623146121
    q2: -6.4750166608951144183900526055952
    q3: 14.192194708135170455822564270209

When i try to calculate the analytical solution, it doesnt work:

syms x y z;
E1 = X == x;
E2 = Y == y;
E3 = Z == z;

A = [E1, E2, E3];
B = [q1, q2, q3];

B = solve(A, B)

B = struct with fields:
    q1: [0×1 sym]
    q2: [0×1 sym]
    q3: [0×1 sym]

Even though it seems to work fine for a similar problem in https://de.mathworks.com/help/symbolic/solve-a-system-of-algebraic-equations.html?s_tid=srchtitle_Troubleshoot%2520Equation%2520Solutions%2520from%2520solve%2520Function_5#SolveSystemOfAlgebraicEquationsExample-3

I also tried the analyticalInverseKinematics() Funktion, but this one seems to be limited to 6dof 'RRRSSS' and 'SSSSSS' Type robots.

Is there any way of solving the system of eqations for q1 - q3 in Matlab?

I Apreciate any Help,

Christoph

Answer 1

[Hannes Daepp]

Hi Christoph,

As you noted, the analyticalInverseKinematics solver is presently limited to 6-DoF robots with a wrist. Other solutions (general 6-DoF solutions and solutions for robots with less than 6 DoF) are certainly possible, and we are looking to include those in future enhancements. We chose to focus initially on the 6-DoF + wrist because it is a prominent configuration in industrial serial manipulators, and because for many users, a numerical solver that is not limited by robot topology is more appropriate. You can learn more about our numerical solvers here.

While your robot is not technically a valid form for analyticalInverseKinematics, since you are looking at a 3-DoF revolute robots and care only about position (not orientation), this is a subset of the supported robots and can be made to work with some light modifications. To do this, add 3 intersecting revolute joints to the end. Then the resultant robot can be passed to analyticalInverseKinematics.

Here's an example: We start by building a 3-DoF robot. To minimize lines of code, I'll just start with a 6-DoF robot and remove the last three joints.

robot = loadrobot('fanucM16ib',DataFormat="row");
removeBody(robot, 'link_4');
tgtConfig = [pi/4 pi/4 pi/4];
show(robot, tgtConfig, Visuals="off"); % Preview robot

This is your typical 3-DoF robot. Now make a copy of this and add the 3 placeholder revolute joints at the end.

robotWithExtraBodies = copy(robot);
jtRotations =[0 pi/2 -pi/2];
for i = 1:3
    rb{i}=rigidBody("extraBody"+num2str(i));
    rb{i}.Joint = rigidBodyJoint("J"+num2str(i), "revolute");
    setFixedTransform(rb{i}.Joint, axang2tform([1 0 0 jtRotations(i)]));
    addBody(robotWithExtraBodies, rb{i}, robotWithExtraBodies.BodyNames{end});
end

% Visualize and see that it's the same except there are some extra
% frames at the end
show(robotWithExtraBodies, [tgtConfig 0 0 0], Visuals="off");

You can now call analyticalInverseKinematics on the robot with the the extra DoF and generate a solver:

aik = analyticalInverseKinematics(robotWithExtraBodies);
ik=generateIKFunction(aik, "ikFcn");

Given the solver, compute an IK solution for a pose

% Use a pose that we know will work
feasiblePose=getTransform(robot, tgtConfig,robot.BodyNames{end});
computedConfigs = ik(feasiblePose,false)

computedConfigs = 4×6

0.7854 0.7854 -1.5708 -6.2832 2.3562 0.0000 0.7854 0.7854 1.5708 -3.1416 0.7854 -3.1416 0.7854 0.7854 -1.5708 -3.1416 -2.3562 -3.1416 0.7854 0.7854 1.5708 -0.0000 -0.7854 0.0000

% Since we only care about the first three joints, get the unique values
% and throw out the extra dimensions
[~, isUniqueValidRowIdx] = unique(computedConfigs(:,1:3), 'rows');
configs = computedConfigs(isUniqueValidRowIdx,1:3)

configs = 2×3

0.7854 0.7854 -1.5708 0.7854 0.7854 1.5708

This works because the method we employ is based on Pieper [1968] (see the citation at the bottom of the doc page), which recognizes that if the last three joints form a wrist, then the orientation and pose problem can be decoupled.

You could actually look at the generated solver and extract just the pose problem -- it's the output of a function called solveFirstThreeDHJoints, but be aware that there is some additional mapping that you will need to do to succeed.

If you do want a completely symbolic solution for a 3DoF manipulator, this is a solved problem. I would recommend looking at Pieper [1968] or a similar but more specialized paper. I gave some additional comments to this effect in the above comment thread.

[Christoph]

Hey, Thank you very much

Answer 2

[Torsten]

X = cos(q1)*(cos(q2 + q3) + cos(q2));
Y = sin(q1)*(cos(q2 + q3) + cos(q2));
Z = sin(q2 + q3) + sin(q2) + 1/5;

Dividing equation 2 by equation 1 gives

q1 = atan(Y/X)

Thus

X/cos(atan(Y/X)) = cos(q2 + q3) + cos(q2)
Z-1/5 = sin(q2 + q3) + sin(q2)

Squaring both equations and adding gives

(X/cos(atan(Y/X)))^2 + (Z-1/5)^2 = 2 + 2*(cos(q2)*cos(q2+q3)+sin(q2)*sin(q2+q3)) = 2 + 2*cos(q3)

thus

q3 = acos ( ( (X/cos(atan(Y/X)))^2 + (Z-1/5)^2 - 2 ) / 2 )

Thus you have to solve (for given values of q3 and Z)

syms q2 q3 Z
eqn = Z ==  sin(q2 + q3) + sin(q2) + 1/5;
solve(eqn,q2)

ans = 

which works as you can see.

[Christoph]

Thank you very much for the detailed solution. However, this is specific to this axis configuration and i would have to calculate it manually for any other robot. Is there no way to let Matlab solve the system?

[Torsten]

Do you see any problem typing these three lines of code for the inverse transformation:

q1 = atan(Y/X)
q3 = acos ( ( (X/cos(q1))^2 + (Z-1/5)^2 - 2 ) / 2 )
q2 = asin((Z-1/5)/(2*cos(q3/2))) - q3/2

?

Code for your test example:

X = 1;
Y = 0.5;
Z = 1;
q1 = atan(Y/X)

q1 = 0.4636

q3 = acos ( ( (X/cos(q1))^2 + (Z-1/5)^2 - 2 ) / 2 )

q3 = 1.6258

q2 = asin((Z-1/5)/(2*cos(q3/2))) - q3/2

q2 = -0.1918

cos(q1)*(cos(q2 + q3) + cos(q2))

ans = 1

sin(q1)*(cos(q2 + q3) + cos(q2))

ans = 0.5000

sin(q2 + q3) + sin(q2) + 1/5

ans = 1

[Walter Roberson]

Inverse kinematics has a closed form solution only in trivial cases (no movement), or one simple 2D movement. As soon you have 2 linked movements that include rotations around different points, it starts becoming unsolvable symbolically if I recall correctly.

[Christoph]

Thats unfortunate, but would explain why analyticalInverseKinematics() is limited to one specific robot type. Thanks a lot

[Hannes Daepp]

Walter is mistaken here (but is in general very trustworthy across MATLAB answers :-)) -- there are analytical (closed-form) solutions for many serial manipulators. There are some prominent papers on this topic, but for a 3R robot, an early paper like Pieper [1968] will suffice if the aim is to find a set of symbolic solutions that will work for your applications (it's also a fairly straightforward paper to follow). I provided more information on how to use analyticalInverseKinematics to solve your problem in the solution below.