Clear Filters
Clear Filters

Solve System of Equations to calculate the inverse kinematics

77 views (last 30 days)
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]
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,

Accepted Answer

Hannes Daepp
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}.Joint = rigidBodyJoint("J"+num2str(i), "revolute");
setFixedTransform(rb{i}.Joint, axang2tform([1 0 0 jtRotations(i)]));
addBody(robotWithExtraBodies, rb{i}, robotWithExtraBodies.BodyNames{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.

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)
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)
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;
ans = 
which works as you can see.
Christoph on 24 Jan 2023
Thats unfortunate, but would explain why analyticalInverseKinematics() is limited to one specific robot type. Thanks a lot
Hannes Daepp
Hannes Daepp on 26 Jan 2023
Edited: Hannes Daepp on 26 Jan 2023
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.

Sign in to comment.

Community Treasure Hunt

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

Start Hunting!