# Solve System of Equations to calculate the inverse kinematics

28 views (last 30 days)

Show older comments

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)

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)

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

##### 0 Comments

### Accepted Answer

Hannes Daepp
on 26 Jan 2023

Edited: Hannes Daepp
on 26 Jan 2023

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)

% 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)

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.

### More Answers (1)

Torsten
on 24 Jan 2023

Edited: Torsten
on 24 Jan 2023

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)

which works as you can see.

##### 5 Comments

Hannes Daepp
on 26 Jan 2023

Edited: Hannes Daepp
on 26 Jan 2023

### See Also

### Categories

### Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!