Jump to content
OMRON Forums

Recommended Posts

Posted

Hi,

 

we are experiencing problems performing motion programs. We have implemented the kinematics of a SCARA robot. We have checked both forward and inverse kinematics in order to know they match. The 4 motors are configurated as phantom motors the same way this thread http://forums.deltatau.com/showthread.php?tid=193&highlight=phantom+motor

 

Jogging motors works ok but motion programs disable InPos for a second and then return true without moving then and of course not being in the desired position.

 

Here is the code:

 

Forward Kinematics

open forward (1)      // Put Coordinate System number inside "(cs)"
// --------------------User Code Goes Here------------------------
paramZ(0) = 2,0,0,2,0
paramX(0) = 0,3,2,0,0
angleHome(0)= KinPosMotor1,KinPosMotor2,KinPosMotor3,KinPosMotor4;		

DH(0)= 0,paramZ(0),0,0,angleHome(0),0,paramX(1),0,angleHome(1),0,paramX(2),0,0,-paramZ(3),0,0,angleHome(2),angleHome(3),0,0;		

fT01(0) = cosd(DH(0)), -cosd(DH(3))*sind(DH(0)), sind(DH(3))*sind(DH(0)), DH(2)*cosd(DH(0)), sind(DH(0)), cosd(DH(3))*cosd(DH(0)), -sind(DH(3))*cosd(DH(0)), DH(2)*sind(DH(0)), 0, sind(DH(3)), cosd(DH(3)), DH(1), 0, 0, 0, 1;
fT12(0) = cosd(DH(4)), -cosd(DH(7))*sind(DH(4)), sind(DH(7))*sind(DH(4)), DH(6)*cosd(DH(4)), sind(DH(4)), cosd(DH(7))*cosd(DH(4)), -sind(DH(7))*cosd(DH(4)), DH(6)*sind(DH(4)), 0, sind(DH(7)), cosd(DH(7)), DH(5), 0, 0, 0, 1;
fT23(0) = cosd(DH(8)), -cosd(DH(11))*sind(DH(8)), sind(DH(11))*sind(DH(8)), DH(10)*cosd(DH(8)), sind(DH(8)), cosd(DH(11))*cosd(DH(8)), -sind(DH(11))*cosd(DH(8)), DH(10)*sind(DH(8)), 0, sind(DH(11)), cosd(DH(11)), DH(9), 0, 0, 0, 1;
fT34(0) = cosd(DH(12)), -cosd(DH(15))*sind(DH(12)), sind(DH(15))*sind(DH(12)), DH(14)*cosd(DH(12)), sind(DH(12)), cosd(DH(15))*cosd(DH(12)), -sind(DH(15))*cosd(DH(12)), DH(14)*sind(DH(12)), 0, sind(DH(15)), cosd(DH(15)), DH(13), 0, 0, 0, 1;
fT45(0) = cosd(DH(16)), -cosd(DH(19))*sind(DH(16)), sind(DH(19))*sind(DH(16)), DH(18)*cosd(DH(16)), sind(DH(16)), cosd(DH(19))*cosd(DH(16)), -sind(DH(19))*cosd(DH(16)), DH(18)*sind(DH(16)), 0, sind(DH(19)), cosd(DH(19)), DH(17), 0, 0, 0, 1;

MxMult = mmul(&fT02(0),&fT01(0),&fT12(0),4,4,4)
MxMult = mmul(&fT03(0),&fT02(0),&fT23(0),4,4,4)
MxMult = mmul(&fT04(0),&fT03(0),&fT34(0),4,4,4)
MxMult = mmul(&fT05(0),&fT04(0),&fT45(0),4,4,4)

KinPosAxisX = fT05(3);
KinPosAxisY = fT05(7);
KinPosAxisZ = fT05(11);

KinAxisUsed=$1C0
close

 

Inverse Kinematics

open inverse (1)      // Put Coordinate System number inside "(cs)"
// --------------------User Code Goes Here------------------------

sA1 = (fT02(0) * fT01(1)) + (fT02(4) * fT01(5)) + (fT02(8) * fT01(9));
cA1 = (fT02(1) * fT01(1)) + (fT02(5) * fT01(5)) + (fT02(9) * fT01(9));
qA1 = atan2d(sA1,cA1);

KinPosMotor1 = qA1;

sA2 = (fT03(0) * fT02(1)) + (fT03(4) * fT02(5)) + (fT03(8) * fT02(9));
cA2 = (fT03(1) * fT02(1)) + (fT03(5) * fT02(5)) + (fT03(9) * fT02(9));
qA2 = atan2d(sA2,cA2);

KinPosMotor2 = qA2;

sA3 = (fT05(0) * fT04(1)) + (fT05(4) * fT04(5)) + (fT05(8) * fT04(9));
cA3 = (fT05(1) * fT04(1)) + (fT05(5) * fT04(5)) + (fT05(9) * fT04(9));
qA3 = atan2d(sA3,cA3);

KinPosMotor3 = qA3;


qA4 = fT05(11) - fT04(11);

KinPosMotor4 = qA4;

close

 

Motion Program

open prog 1
// --------------------User Code Goes Here------------------------
pmatch;	
inc;
rapid;
Coord[1].SegMoveTime = 5; // Segmentation time of 5 msec		
X2.63 Y4.05 Z-2 //A-90 B-87 C-90	
close
/****************************************/

 

Any suggestions?

 

Thanks

  • Replies 2
  • Created
  • Last Reply

Top Posters In This Topic

Top Posters In This Topic

Posted

Have you looked at the coordinate system status?

I would do the following:

- Make sure I can run a program in coordinate system 1. And that feedrate override (%) for example is not 0. I would verify this by running a program with just a long dwell, or using the online command CPX; e.g. &1CPX DELAY 36000.

- Make sure that the forward is computing the expected tooltip coordinates by issuing a &1PMATCH followed by &1P. Make sure you include KinAxisUsed in the forward.

- Copy the motor output(s) in the inverse to debugging global variables. Is anything being written to them?

- Add general purpose debugging flags inside the kinematics to find out if any stage is being executed

 

On a side note:

- PMAC performs a PMATCH automatically at the start of a motion program or subroutine. There is no need for you to explicitly issue one. It is misleading for future users.

- I personally would not place Coord[1].SegMoveTime inside a motion program. It does not hurt but it is not conventional unless you plan on changing it per program which is not a standard procedure.

Guest
This topic is now closed to further replies.

×
×
  • Create New...