Inverse kinematics problem for CRS F3

Matlab is useful for number crunching, not symbolic manipulation.

For this, try Mathematica; the Robotica package can generate the
kinematics (and optionally dynamics) directly from the
Denavit-Hartenberg parameters.
Grab a copy of Robotica from http://www-cvr.ai.uiuc.edu/~lab/ece470/

Does this arm use a spherical wrist?  If so, the kinematics can easily
be decoupled and solved as two 3DOF problems.

Here's another text that may help:
http://www-cvr.ai.uiuc.edu/~seth/index.php?u=spongbook

Later,
Daniel

help:http://www-cvr.ai.uiuc.edu/~seth/index.php?u=spongbook

that was very helpful, thanks Daniel. All the joints are revolute. I
will try everything and hopefully it would work.

help:http://www-cvr.ai.uiuc.edu/~seth/index.php?u=spongbook

I don't believe it is uses spherical wrist, here is the picture of the
manipulator
http://www.engga.uwo.ca/research/robo/CRSF3/pic8.jpg

help:http://www-cvr.ai.uiuc.edu/~seth/index.php?u=spongbook

The Robotica is nice to get the Forward kinematics, I have used it and
it works nicely. But it doesn't do any inverse kinematics.

 > The Robotica is nice to get the Forward kinematics, I have used it
 > and it works nicely. But it doesn't do any inverse kinematics.

Technically, Mathematica's Solve[] could give you the inverse
kinematics -- though they might not be pretty.


 > I don't believe it is uses spherical wrist, here is the picture of
 > the manipulator
 > http://www.engga.uwo.ca/research/robo/CRSF3/pic8.jpg

Hmm... its hard to tell from that photo.  Anyway, the trick with the
spherical wrist is approximately
- calculate where the center of the wrist will be when the manipulator
is at the desired position
- solve for the first three links to position the wrist at this location
- solve for the wrist links to achieve the desired manipulator
orientation (possible since the spherical wrist can accommodate any
orientation between the arm and the manipulator)

With another wrist configuration, you may still be able to modify this
procedure.  If not, the output of something like
Simplify[Solve[{fx(joints)==x, fy(joints)==y, fz(joints)==z}, joints]]
may be the best you can hope for.

Later,
Daniel

Thanks for the help again. I did ensure that this manipluator doesn't
have spherical wrist, I wasn't clear what you meant by this but it is
basically means that the last three axes intersect at one point and
hence can divide the problem into 2 sections. There is an offset
between joint 5 and 6. I solved the forward kinematics through
Mathematica and chose 3 equations for the position and 3 independent
equations for the rotation and used the solve. I left it running for
10 minutes but Mathematica wasn't responding, I don't know what was
the problem. This is my dh.txt (DH parameters that were loaded into
Mathematica)

DOF = 6

joint1 = revolute
a1 = 0
alpha1 = 0
d1 = 0
theta1 = q1
joint2 = revolute
a2 = 0.1
alpha2 = -Pi/2
d2 = 0
theta2 = q2
joint3 = revolute
a3 = 0.265
alpha3 = 0
d3 = 0
theta3 = q3
joint4 = revolute
a4 = 0
alpha4 = Pi/2
d4 = 0.27
theta4 = q4
joint5 = revolute
a5 = 0
alpha5 = -Pi/2
d5 = 0
theta5 = q5
joint6 = revolute
a6 = 0
alpha6 = Pi/2
d6 = 0.075
theta6 = q6

One thing to try: replace all decimal values with their fractional
equivalents (e.g. 0.075 -> 75/1000) or with placeholder constants
(e.g. k1, k2, k3).  This helps Mathematica greatly, as it deals poorly
with numerical roundoff errors when symbolically solving large
algebraic systems.

If that doesn't fix it, I'll try to look at your DH parameters later
this evening.

- Daniel

I did replace it with fraction and also with constants, left it
running for like 30 minutes. There is certainly something wrong. I
will appreciate if you can have a look at the DH parameters and
suggest something.
Appreciate the help, thanks.

I have read example 3.9 for the Elbow Manipulator from the recommended
book you suggested but it talks about the spherical wrist. I'm still
not convinced that my robot has a spherical wrist because of the joint
offset d6=0.075. This is a link of the pdf file I uploaded that shows
what I'm talking about.
http://files-upload.com/399388/Joint5_and_6.pdf.html
Correct me if I'm wrong.
Thanks

As you've drawn it, axis x6 is neither perpendicular to nor
intersecting axis z5; thus its not a valid DH frame...

Also note that the book's spherical wrist DH params have a nonzero d6.

Hope that helps,
Daniel

Sorry to revisit this issue but I was away for holidays and now I'm
back and still trying to solve this problem.
Yes I have noticed the nonzero d6. Yet the Pieper solution can be used
when all three axes intersect, doesn't that mean they shouldn't have
an offset. d6 is defined to be the distance from X5 to X6 along axis
Z6. Correct me if I'm wrong.
Thanks

Here is some kinematics stuff from when I was in school;

if you ignore the whole DH parameter thing robotics is much easier.

use simple transformation matrices at each joint, a 4x4 where the upper
left 3x3 represents rotation about whatever axis, the right 3x1' is
translation, and the bottom row is always 0 0 0 1.

each matrix describes 1 movement, either rotation or translation, keep
frame axis aligned, if a variable distance such as prismatic joint, the
distance stays as a variable.

so that a Z rotation is;

c1 -s1 0 0
s1 c1 0 0
0 0 1 0
0 0 0 1

a Z translation is;
1 0 0 0
0 1 0 0
0 0 1 dZ
0 0 0 1

a Y rotation is;
c2 0 s2 0
0 1 0 0
-s2 0 c2 0
0 0 0 1

a Y translation is;
1 0 0 0
0 1 0 dY
0 0 1 0
0 0 0 1

X rotation;
1 0 0 0
0 c3 -s3 0
0 s3 c3 0
0 0 0 1

X translation;
1 0 0 dX
0 1 0 0
0 0 1 0
0 0 0 1

then multiply 'em all together, notice no pesky frame rotations.




Matlab reverse kinematics simulation of RRR arm;

function []=rrr()
clear
clc
px=6;py=0;pz=0; %set home position

figure('Position',get(0,'ScreenSize'));
botsim(px,py,pz)
while 1
      menu1=menu('Please make a selection','X+','X-','Y+',...
      'Y-','Z+','Z-','Refresh','Quit');
  switch(menu1)
    case 1
        px=px+0.5;
        botsim(px,py,pz);
    case 2
        px=px-0.5;
        botsim(px,py,pz);
    case 3
        py=py+0.5;
        botsim(px,py,pz);
    case 4
        py=py-0.5;
        botsim(px,py,pz);
    case 5
        pz=pz+0.5;
        botsim(px,py,pz);
    case 6
        pz=pz-0.5;
        botsim(px,py,pz);
    case 7
        close figure 1
        figure('Position',get(0,'ScreenSize'));
        botsim(px,py,pz)
    case 8
        close figure 1
        return
end
end

function []=botsim(px,py,pz)
p0=eye(4); p1=0; p2=0; p3=0;
l1=2;l2=3;l3=1;                 % link lengths
theta1=0; theta2=0; theta3=0;   % initialise variables
x=[];y=[];z=[];
xo=0;yo=0;zo=0;
%--------------------------------------------
% Find angles
%--------------------------------------------
theta1=atan2(py,px);
if theta1==0
     k=px-l1;
else
     k=(-pz/sin(theta1))-l1;
end
theta3=atan2((sqrt(4*l2^2*l3^2-(pz^2+k^2-l2^2-l3^2)^2)),(pz^2+k^2-l2^2-l3^2))
theta2=atan2((l3*pz*cos(theta3)+l2*pz-l3*k*sin(theta3)),(l3*pz*sin(theta3)-l2*k-l3*k*cos(theta3)))

%------------------------------------------
% Calculate points
%------------------------------------------
     T1=[cos(theta1) -sin(theta1) 0 0; sin(theta1) cos(theta1) 0 0; 0 0
1 0; 0 0 0 1];
     T2=[1 0 0 l1; 0 1 0 0; 0 0 1 0; 0 0 0 1];
     T3=[cos(theta2) 0 sin(theta2) 0; 0 1 0 0; -sin(theta2) 0
cos(theta2) 0; 0 0 0 1];
     T4=[1 0 0 l2; 0 1 0 0; 0 0 1 0; 0 0 0 1];
     T5=[cos(theta3) 0 sin(theta3) 0; 0 1 0 0; -sin(theta3) 0
cos(theta3) 0; 0 0 0 1];
     T6=[1 0 0 l3; 0 1 0 0; 0 0 1 0; 0 0 0 1];

     p3=T1*T2*T3*T4*T5*T6*p0;
     p2=T1*T2*T3*T4*p0;
     p1=T1*T2*p0;

     x=[p0(1,4),p1(1,4),p2(1,4),p3(1,4)];
     y=[p0(2,4),p1(2,4),p2(2,4),p3(2,4)];
     z=[p0(3,4),p1(3,4),p2(3,4),p3(3,4)];

     %----------------------------------------
     % Plotting
     %----------------------------------------
     % Graphical data output
      subplot(2,2,2)
     plot3(x,y,z,xo,yo,zo,'h','LineWidth',2)
     hold on
     grid on
     xlabel('X'), ylabel('Y'), zlabel('Z')
     axis([-6 6,-6 6,-4 4])
     subplot(2,2,3)
     plot(x,y,xo,yo,'h','LineWidth',2)
     axis([-6 6,-6 6])
     xlabel('X'), ylabel('Y')
     subplot(2,2,4)
     plot(x,z,xo,zo,'h','LineWidth',2)
     axis([-6 6,-4 4])
     xlabel('X'), ylabel('Z')
     % Numerical data output
     subplot(2,2,1)
     axis([-4 4, -4 4])
     text(-1,3.5,['\theta Z = ',num2str(theta1)]);
     text(-1,3,['\theta Elbow = ',num2str(theta2)]);
     text(-1,2.5,['\theta Wrist = ',num2str(theta3)]);
     text(-3,1.5,['Effector pos X = ',num2str(p3(1,4),2),'  Y =
',num2str(p3(2,4),2),'  Z = ',num2str(p3(3,4),2)]);
     text(-3,1,['Wrist pos X = ',num2str(p2(1,4),2),'  Y =
',num2str(p2(2,4),2),'  Z = ',num2str(p2(3,4),2)]);
     text(-3,0.5,['Elbow pos X = ',num2str(p1(1,4),2),'  Y =
',num2str(p1(2,4),2),'  Z = ',num2str(p1(3,4),2)]);




here is a Matlab program to do the forward kinematics of a RRR arm, note
that the bulk of the code is to use a menu for user input and
plot/report the position of the arm.

function []=rrr()
clear
clc
theta1=pi;  %initial angle for good view
theta2=0;   % arm level
theta3=0;   % wrist straight
figure('Position',get(0,'ScreenSize'));
botsim(theta1,theta2,theta3)
while 1
      menu1=menu('Please make a selection','Rotate Left','Rotate
Right','Arm Up',...
      'Arm Down','Wrist Up','Wrist Down','Refresh','Quit');
  switch(menu1)
    case 1
        theta1=theta1+0.1;
        botsim(theta1,theta2,theta3)
    case 2
        theta1=theta1-0.1;
        botsim(theta1,theta2,theta3)
    case 3
        theta2=theta2-0.1;
        botsim(theta1,theta2,theta3)
    case 4
        theta2=theta2+0.1;
        botsim(theta1,theta2,theta3)
    case 5
        theta3=theta3-0.1;
        botsim(theta1,theta2,theta3)
    case 6
        theta3=theta3+0.1;
        botsim(theta1,theta2,theta3)
    case 7
        close figure 1
        figure('Position',get(0,'ScreenSize'));
        botsim(theta1,theta2,theta3)
    case 8
        close figure 1
        return
end
end

function []=botsim(theta1,theta2,theta3)
p0=eye(4); p1=0; p2=0; p3=0;
xo=0;yo=0;zo=0;
x=[];y=[];z=[];

l1=2;   % Link lengths
l2=3;
l3=1;
%--------------------------------------------
% Simulation
%--------------------------------------------
     T1=[cos(theta1) -sin(theta1) 0 0; sin(theta1) cos(theta1) 0 0; 0 0
1 0; 0 0 0 1];
     T2=[1 0 0 l1; 0 1 0 0; 0 0 1 0; 0 0 0 1];
     T3=[cos(theta2) 0 sin(theta2) 0; 0 1 0 0; -sin(theta2) 0
cos(theta2) 0; 0 0 0 1];
     T4=[1 0 0 l2; 0 1 0 0; 0 0 1 0; 0 0 0 1];
     T5=[cos(theta3) 0 sin(theta3) 0; 0 1 0 0; -sin(theta3) 0
cos(theta3) 0; 0 0 0 1];
     T6=[1 0 0 l3; 0 1 0 0; 0 0 1 0; 0 0 0 1];

     p3=T1*T2*T3*T4*T5*T6*p0;
     p2=T1*T2*T3*T4*p0;
     p1=T1*T2*p0;

     x=[p0(1,4),p1(1,4),p2(1,4),p3(1,4)];
     y=[p0(2,4),p1(2,4),p2(2,4),p3(2,4)];
     z=[p0(3,4),p1(3,4),p2(3,4),p3(3,4)];
     %----------------------------------------
     % Plotting
     %----------------------------------------
     % Graphical data output
     subplot(2,2,2)
     plot3(x,y,z,xo,yo,zo,'h','LineWidth',1)
     hold on
     grid on
     xlabel('X'), ylabel('Y'), zlabel('Z')
     axis([-6 6,-6 6,-4 4])
     subplot(2,2,3)
     plot(x,y,xo,yo,'h','LineWidth',1)
     axis([-6 6,-6 6])
     xlabel('X'), ylabel('Y')
     subplot(2,2,4)
     plot(x,z,xo,zo,'h','LineWidth',1)
     axis([-6 6,-4 4])
     xlabel('X'), ylabel('Z')
     % Numerical data output
     subplot(2,2,1)
     axis([-4 4, -4 4])
     text(-1,3.5,['\theta Z = ',num2str(theta1)]);
     text(-1,3,['\theta Elbow = ',num2str(theta2)]);
     text(-1,2.5,['\theta Wrist = ',num2str(theta3)]);
     text(-3,1.5,['Effector pos X = ',num2str(p3(1,4),2),'  Y =
',num2str(p3(2,4),2),'  Z = ',num2str(p3(3,4),2)]);
     text(-3,1,['Wrist pos X = ',num2str(p2(1,4),2),'  Y =
',num2str(p2(2,4),2),'  Z = ',num2str(p2(3,4),2)]);
     text(-3,0.5,['Elbow pos X = ',num2str(p1(1,4),2),'  Y =
',num2str(p1(2,4),2),'  Z = ',num2str(p1(3,4),2)]);



here is a Matlab program to do the forward kinematics of a RRR arm using
DH convention, note that the bulk of the code is needed to just make it
work!!!!!

clc
clf
clear
ai(1,:)=input(’Enter link lengths, A, B and C (say [3, 2, 1]): ’ );
disp(’The D H parameters for the RRR robot in the home position are’)
dh=[0,0,0,0;pi/2,ai (1),0,0;0, ai (2),0, pi/2;0,0,0,-pi/2];
disp(dh)
x(1)=0;y(1)=0;z(1)=0; % Origin of FOR getdata % Get coord’s of links and
plotdata % plot at home position.
jointang(1,:)=input(’Enter angle for joints (say [pi/2,-pi/4,pi /2]): ’ );
steps=input(’Enter number of steps to simulate : ’);

% The simulation loop.

for k=1:steps; % Loop steps times
dh(2,4)=dh(2,4)+jointang(1)/steps; % Update joint angles
dh(3,4)=dh(3,4)+jointang(2)/steps;
dh(4,4)=dh(4,4)+jointang(3)/steps;
getdata
plotdata
end

% function to compute the transformation matrix Tˆ(i-1) i
function t=ftransm(alpha,a,d,theta)
c=cos(theta); s=sin(theta);
ca=cos(alpha); sa=sin(alpha);
t(1,1)=c;
t(1,2)=-s;
t(1,3)=0;
t(1,4)=a;
t(2,1)=cas;
t(2,2)=cac;
t(2,3)=-sa;
t(2,4)=-sad;
t(3,1)=sas;
t(3,2)=sac;
t(3,3)=ca;
t(3,4)=cad;
t (4,1:4)=[0 0 0 1];

% calculate the transformation matrices and the end coordinates
% of each of the links and the gripper or point A of the RRR robot.
t=eye(4);
for i=1:3;
alpha = dh(i,1);
a = dh(i,2);
d = dh(i+1,3);
theta = dh(i+1,4);
t0n = t*ftransm(alpha,a,d,theta); % Calc T matrix for successive FOR’s
t = t0n;
switch i
case 1 % Calc end pos of FOR
ap=t0n*[ai(1),0,0,1]’;
case 2 % Calc end pos of FOR
ap=t0n*[ai(2),0,0,1]’;
case 3 % Calc end pos of gripper (A)
ap=t0n*[ai(3),0,0,1]’;
end
x(i+1)=ap(1); % Get x, y and z coord’s for plotting
y(i+1)=ap(2);
z(i+1)=ap(3);
end

% plot the RRR robot links.
plot3(x,y,z)
hold on
grid on
xlabel(’X’)
ylabel(’Y’)
zlabel(’Z’)
xlim([-6,6])
ylim([-6,6])
zlim([-4,4])
end;

I don't know what happened here, apparently someone hacked to my
account and started posting senseless stuff. Can anyone remove all
these posts following this useful post from "Fulliautomatix"

It wasn't just you. The same spammer got most of us on here the same way. It
wasn't your account that was hacked, just the newsgroup was spammed using
quite a few names from here.

It has already died down or been filtered on another group so I am hoping
that less mention of it on here will allow the same to happen. FYI.
jcd