DH transform with prismatic joints

In this paper, the authors model the kinematics of an elephant's trunk by combining a planar curvature model with modified D-H. At the bottom of pg 12 (Section E), they add a prismatic joint to the base of the trunk. To incorporate this extra DOF, they multiply by one additional matrix: I^4 + one off-diagonal term.

Maybe this is similar enough that you can adapt their approach?

Susan

This sounds to me like a modified SCARA configuration. Normal SCARA arms have three parallel-axis joints; two revolute followed by one prismatic. Your system has the prismatic joint first, followed by four revolute joints.

I'm not seeing the problem. For parallel z axes, the x-axis is simply the perpendicular vector between the z axes; since the z axes are parallel, this forms a plane of choices, but any perpendicular on the plane will work. Usually you would choose one which represents the actual manipulator construction.

Establish coordinates: The base frame has axes x[0], y[0], and z[0] Each link, L[n] (n>=1) has three axes: x[n], y[n], and z[n]

DH convention: x[n] must intersect and be perpendicular to both z[n] and z[n-1]

DH parameters: a[n] - distance from z[n-1] to z[n] along the x[n] axis alpha[n] - angle from z[n-1] to z[n] about the x[n] axis d[n] - distance from x[n-1] to x[n] along the z[n] axis theta[n] - angle from x[n-1] to x[n] about the z[n] axis

For prismatic joints, d[n] is the variable; and theta[n] is the variable for revolute joints.

Then the system you describe is of the form

n a alpha d theta

1 ~0 ~0 *d1 ~0 2 r2 0 ~0 *th2 3 r3 0 ~0 *th3 4 r4 0 ~0 *th4 5 r5 0 ~0 *th5 6 ??

Where * indicates a variable term, ~ indicates a resonable default value, and ?? indicates insufficient information given. The terms r# indicate the link lengths between adjacent axes.

The transformation matrices are given by A[n] = Rot(theta[n] about z[n]) * Trans(d[n] along z[n]) * Trans(a[n] along x[n]) * Rot(alpha[n] about x[n])

Hope that helps, Daniel

• D Herring [2005-10-28 06:05]:

Yes, but the ultimate goal is to calibrate the manipulator. So I've an ideal model and a real model, each of them is caracterized by 28 independent parameters (N = 4R + 2P + 6, where R are the number of revolute joints and P are the number of prismatic joints). If the ideal robot geometry indicates parallel axes and I use a conventional DH model and in the process of calibration is determined that the axes are misaligned then the normal between the axes becomes unique. That discontinuity reflects also in the value of d[n] and leads to problem in the convergence of the optimization code which should guess the real parameters from a set of misurations.

I can address the problem by using a modified Denavit-Hartenber but this is not possible if the joint in question is prismatic. Do you know alternative way to express relationship between consecutive frames?

this post (my post) is an outragous offtopic, so treat accordingly.

I don't have any idea of the math in question, but if I understand you correctly, you need to find some model parameters that will allow precise control of that hand, and you are having problem with convergence.

But, let me take another direction in this thread, namely, let me attack whole concept of "calibration". Look at human hand. It grows. It stretches its bones and change proportions. Its musculature gets stronger or weaker in under a minute, when your blood mixture changes. Yet, humans are able to re-"calibrate" it on-the-fly, accordingly. With exception of small fraction of circumstances, where hand gets seriously damaged, or nervous system itself is disfunct, etc.

Please note, this doesn't take any intelligence - even a lizard can do the same. Or take a snake - it strikes its victim quite precisely, while its "joints" structure seem to be quite complex.

/* here everybody yawn and think what's the point... */

All right, well, the point is that perhaps your model is a bit wrong. Just a little bit totaly, absolutely wrong way to go.

Just for reference, almost all the footage of our Hand stuff is done without world models: get the movements right in the right place, and you don't *need* complicated models.

However, we do do control of the other type as well (integrated into the system, so you just send the Hand setpoints and it goes there.)

cheers, Rich.

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.

the inverse kinematics are done by 'twitching' the jacobian rather than solving the matrices for every step...but that is a subject for another day as it gets a bit complicated if you take into account the orientation of the gripper rather than a point, it is still easier and settles just as fast as DH inverse, and will move past singularities!

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 {1} 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 {2} ap=t0n*[ai(1),0,0,1]?; case 2 % Calc end pos of FOR {3} 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;

Ok. So the issue isn't establishing DH frames, but rather calibrating them. It appears that you have some way of controlling the joint variables (e.g. motors) and some way of measuring the end-effector coordinates. If you have enough time, detecting axis alignments shouldn't be too hard.

Simple procedure:

- Send the robot to some default ("home") position.

- Move the prismatic joint back and forth, keeping the revolute joints fixed. Measure the end-effector positions; a least-squares fit should result in a line parallel to the prismatic z axis. Return to home.

- Fix the prismatic joint, and move the first revolute joint back and forth. Measure the end-effector positions; these should form an arc in a plane perpendicular to the joint z axis. Return to home.

- Repeat for the other joints.

- Once you have all the z axes in the home position, solving for the other parameters should be easier.

There are probably better ways of doing this; but, as you say, with all the axes so close to parallel, things are sensitive to error.

On the other hand, have you actually built and tried calibrating this robot? If a visual inspection does not reveal axis misalignment, then accuracy will probably be sufficient if you hard-code the parallel-axis conditions into your optimization code.

Later, Daniel

• D Herring [2005-10-29 18:39]:

This work is my thesis. The goal is to write a program to calibrate a 6dof robot. By now, there is no physical robot, only some papers which describe the geometry.

The program receveis in input a description of the robot geometry by means of a number of parameters coming from a kinematical model. The first problem is to find a model which is complete and proportional. I can use a combination of DH e MDH except between the first and the second joint. The first joint is a prismatic joint and the second a revolute joint, their axes are parallel: this prevents the use of a MDH transformation (the joint is prismatic) and the use of a DH transformation (axes parallel). Which transformation I could use?

Yes, but how this is done is only of limited concern to me. Given there is no physical robot I should *simulate the calibration*. I should:

1. Find a good (proportional and complete) kinematical model and apply it to the robot, this will provide an array of parameters ip[] describing the geometry to the program.
2. Add some deltas to the parameters to simulate the difference between the ideal robot and the real robot. p[] = ip[] + delta[]
3. Simulate a set of calibration measurements by using the forward kinematics with the p[] parameters.
4. Use an optimization algorithm to find the p[] parameters starting with the ip[] parameters and the simulated measurements.

The robot will be (?) used in orthopedic surgical procedures, so it needs a very high accuracy, from the paper describing it: "[...] and this causes the most likely error to be as low as +/-0.08mm [...]"

thanks, Ivano

Then I leave you with my best wishes.

Daniel