fipm Posted November 9, 2017 Posted November 9, 2017 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
Omron Forums Support Posted November 9, 2017 Posted November 9, 2017 Take a look at the scara robot kinematics on the last page of our user manual. http://forums.deltatau.com/filedepot/download.php?f=Power%20PMAC/Manuals/Power%20PMAC%20Users%20Manual.pdf [FILE REMOVED]
Richard Naddaf Posted November 11, 2017 Posted November 11, 2017 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.
Recommended Posts