# Inverse kinematics problem for CRS F3

I'm working on finding the inverse kinematics of the CRS F3 manipulator. It is a 6 DOF and I believe there should be a closed
algebraic solution for it. I have used the methods suggested in "Introduction to Robotics:Mechanics and Control" by J. Craig and implemented them in MATLAB but I get really complex equations that I don't know how to simplify. I have tried to use "solve" in MATLAB which worked for a 3-DOF before but now it notifies me there is no explicit solution. I have found some methods in the IEEE like the Raghavan and Roth Formulation but it didn't really help. Any help suggestions would be appreciated.
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
crazygrey wrote:

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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

that was very helpful, thanks Daniel. All the joints are revolute. I will try everything and hopefully it would work.
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

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.
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
crazygrey wrote:

> 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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
crazygrey wrote:

...
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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

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.
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
crazygrey wrote:

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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

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
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
crazygrey wrote:

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)�s; t(2,2)�c; t(2,3)=-sa; t(2,4)=-sad; t(3,1)=sas; t(3,2)=sac; t(3,3)�; t(3,4)�d; 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;
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>

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"
<% if( /^image/.test(type) ){ %>
<% } %>
<%-name%>
wrote:

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