12. Roboter-Kraftsteuerung

12.1. Kraftsensor konfigurieren

1/**
2* @brief  Konfiguriert den Kraftsensor.
3* @param  config.company: Kraftsensor-Hersteller, 17-Kunwei Technology, 19-Aerospace 11th Institute, 20-ATI Sensor, 21-Zhongke Midian, 22-Weihang Minxin, 23-NBIT, 24-Xinjingcheng (XJC), 26-NSR
4* @param  config.device: Gerätenummer, Kunwei(0-KWR75B), Aerospace 11th Institute(0-MCS6A-200-4), ATI(0-AXIA80-M8), Zhongke Midian(0-MST2010), Weihang Minxin(0-WHC6L-YB-10A), NBIT(0-XLH93003ACS), Xinjingcheng XJC(0-XJC-6F-D82), NSR(0-NSR-FTSensorA)
5* @param  config.softvesion: Softwareversionsnummer, derzeit nicht verwendet, Standard 0.
6* @param  config.bus: Position des Geräts am Flanschbus, derzeit nicht verwendet, Standard 0.
7* @return  Fehlercode.
8*/
9int FT_SetConfig(DeviceConfig config);

12.2. Kraftsensor-Konfiguration abrufen

1/**
2* @brief Gibt die Kraftsensor-Konfiguration zurück.
3* @param [out] config company: Kraftsensor-Hersteller.
4* @param [out] config device: Gerätenummer.
5* @param [out] config softvesion: Softwareversionsnummer.
6* @param [out] config bus: Busposition.
7* @return Fehlercode.
8*/
9int FT_GetConfig(DeviceConfig config);

12.3. Kraftsensor aktivieren

1/**
2* @brief  Aktiviert den Kraftsensor.
3* @param  [in] act  0-Reset, 1-Aktivieren.
4* @return  Fehlercode.
5*/
6int FT_Activate(int act);

12.4. Kraftsensor Nullpunktkorrektur

1/**
2* @brief  Nullpunktkorrektur des Kraftsensors.
3* @param  [in] act  0-Nullpunkt entfernen, 1-Nullpunkt korrigieren.
4* @return  Fehlercode.
5*/
6int FT_SetZero(int act);

12.5. Referenzkoordinatensystem für Kraftsensor einstellen

1/**
2* @brief  Stellt das Referenzkoordinatensystem für den Kraftsensor ein.
3* @param  [in] type  0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem, 2-freies Koordinatensystem.
4* @param  [in] coord  Werte des freien Koordinatensystems (falls type=2).
5* @return  Fehlercode.
6*/
7int FT_SetRCS(int type, DescPose coord);

12.6. Nutzlastgewicht unter dem Kraftsensor einstellen

1/**
2* @brief Stellt das Nutzlastgewicht unter dem Kraftsensor ein.
3* @param [in] weight Nutzlastgewicht in kg.
4* @return Fehlercode.
5*/
6int SetForceSensorPayLoad(double weight);

12.7. Nutzlastschwerpunkt unter dem Kraftsensor einstellen

1/**
2* @brief Stellt den Nutzlastschwerpunkt unter dem Kraftsensor ein.
3* @param [in] cog Nutzlastschwerpunkt in mm.
4* @return Fehlercode.
5*/
6int SetForceSensorPayLoadCog(DescTran cog);

12.8. Nutzlastgewicht unter dem Kraftsensor abrufen

1/**
2* @brief Gibt das Nutzlastgewicht unter dem Kraftsensor zurück.
3* @return List[0]: Fehlercode; List[1]: weight Nutzlastgewicht in kg.
4*/
5List<Number> GetForceSensorPayLoad();

12.9. Nutzlastschwerpunkt unter dem Kraftsensor abrufen

1/**
2* @brief Gibt den Nutzlastschwerpunkt unter dem Kraftsensor zurück.
3* @param [out] cog Nutzlastschwerpunkt in mm.
4* @return Fehlercode.
5*/
6int GetForceSensorPayLoadCog(DescTran cog);

12.10. Automatische Nullpunktkorrektur des Kraftsensors

1/**
2* @brief Automatische Nullpunktkorrektur des Kraftsensors.
3* @param [in,out] massCenter Enthält Sensormasse (kg) und Schwerpunkt (mm). Die berechneten Werte werden hier zurückgegeben.
4* @return Fehlercode.
5*/
6int ForceSensorAutoComputeLoad(MassCenter massCenter);

12.11. Kraft-/Drehmomentdaten im Referenzkoordinatensystem abrufen

1/**
2* @brief Gibt Kraft-/Drehmomentdaten im Referenzkoordinatensystem zurück.
3* @param [in] flag 0-blockierend, 1-nicht blockierend.
4* @param [out] ft  Kraft/Drehmoment, fx, fy, fz, tx, ty, tz.
5* @return Fehlercode.
6*/
7int FT_GetForceTorqueRCS(int flag, ForceTorque ft);

12.12. Rohdaten des Kraftsensors (Kraft/Drehmoment) abrufen

1/**
2* @brief Gibt die rohen Kraft-/Drehmomentdaten des Kraftsensors zurück.
3* @param [in] flag 0-blockierend, 1-nicht blockierend.
4* @param [out] ft  Kraft/Drehmoment, fx, fy, fz, tx, ty, tz.
5* @return Fehlercode.
6*/
7int FT_GetForceTorqueOrigin(int flag, ForceTorque ft);

12.13. Codebeispiel für Kraftsensor-Konfiguration und automatische Nullpunktkorrektur

 1public static int TestFTInit(Robot robot)
 2{
 3    DescTran tr1=new DescTran(0,0,0);
 4    robot.SetForceSensorPayload(0);
 5    robot.SetForceSensorPayloadCog(tr1);
 6
 7    int company = 24;
 8    int device = 0;
 9    int softversion = 0;
10    int bus = 1;
11    int index = 1;
12
13    DeviceConfig con=new DeviceConfig(company,device,softversion,bus);
14    robot.FT_SetConfig(con);
15    robot.Sleep(1000);
16    robot.FT_GetConfig(con);
17    robot.Sleep(1000);
18
19    robot.FT_Activate(0);
20    robot.Sleep(1000);
21    robot.FT_Activate(1);
22    robot.Sleep(1000);
23
24    robot.Sleep(1000);
25    robot.FT_SetZero(0);
26    robot.Sleep(1000);
27
28    ForceTorque ft=new ForceTorque(0,0,0,0,0,0);
29    robot.FT_GetForceTorqueOrigin(0, ft);
30    robot.FT_SetZero(1);
31    robot.Sleep(1000);
32
33    DescPose ftCoord = new DescPose();
34    robot.FT_SetRCS(0, ftCoord);
35
36    robot.SetForceSensorPayload(0.824);
37
38    DescTran tr=new DescTran(0.778, 2.554, 48.765);
39    robot.SetForceSensorPayloadCog(tr);
40    List<Number> weight = new ArrayList<>();
41    double x = 0, y = 0, z = 0;
42    weight=robot.GetForceSensorPayload();
43    robot.GetForceSensorPayloadCog(tr);
44    tr.x=0;
45    tr.y=0;
46    tr.z=0;
47    robot.SetForceSensorPayload(0);
48    robot.SetForceSensorPayloadCog(tr);
49
50    double computeWeight = 0;
51    DescTran tran = new DescTran();
52    MassCenter mass=new MassCenter();
53    mass.weight=weight.get(1).doubleValue();
54    mass.cog=tran;
55    robot.ForceSensorAutoComputeLoad(mass);
56    return 0;
57}

12.14. Nutzlastgewichts-Identifikation aufzeichnen

1/**
2* @brief  Zeichnet die Nutzlastgewichts-Identifikation auf.
3* @param  [in] id  Sensorkoordinatensystem-Nummer, Bereich [1~14].
4* @return  Fehlercode.
5*/
6int FT_PdIdenRecord(int id);

12.15. Nutzlastgewichts-Identifikation berechnen

1/**
2* @brief  Berechnet die Nutzlastgewichts-Identifikation.
3* @return  List[0]: Fehlercode; List[1]: double weight  Nutzlastgewicht, Einheit kg.
4*/
5List<Number> FT_PdIdenCompute();

12.16. Nutzlastschwerpunkt-Identifikation aufzeichnen

1/**
2* @brief  Zeichnet die Nutzlastschwerpunkt-Identifikation auf.
3* @param  [in] id  Sensorkoordinatensystem-Nummer, Bereich [1~14].
4* @param  [in] index Punktnummer, Bereich [1~3].
5* @return  Fehlercode.
6*/
7int FT_PdCogIdenRecord(int id, int index);

12.17. Nutzlastschwerpunkt-Identifikation berechnen

1/**
2* @brief  Berechnet die Nutzlastschwerpunkt-Identifikation.
3* @param  [out] cog  Nutzlastschwerpunkt, Einheit mm.
4* @return  Fehlercode.
5*/
6int FT_PdCogIdenCompute(DescTran cog);

12.18. Codebeispiel für Kraftsensor-Nutzlastidentifikation

 1public static int TestFTLoadCompute(Robot robot)
 2{
 3    DescTran tr1=new DescTran(0,0,0);
 4    robot.SetForceSensorPayload(0);
 5    robot.SetForceSensorPayloadCog(tr1);
 6
 7    int company = 24;
 8    int device = 0;
 9    int softversion = 0;
10    int bus = 1;
11    int index = 1;
12
13    DeviceConfig con=new DeviceConfig(company, device, softversion, bus);
14    robot.FT_SetConfig(con);
15    robot.Sleep(1000);
16    robot.FT_GetConfig(con);
17    robot.Sleep(1000);
18
19    robot.FT_Activate(0);
20    robot.Sleep(1000);
21    robot.FT_Activate(1);
22    robot.Sleep(1000);
23
24    robot.Sleep(1000);
25    robot.FT_SetZero(0);
26    robot.Sleep(1000);
27
28    ForceTorque ft=new ForceTorque(0,0,0,0,0,0);
29    robot.FT_GetForceTorqueOrigin(0, ft);
30    robot.FT_SetZero(1);
31    robot.Sleep(1000);
32
33    DescPose tcoord = new DescPose();
34    tcoord.tran.z = 35.0;
35    robot.SetToolCoord(10, tcoord, 1, 0, 0, 0);
36
37    robot.FT_PdIdenRecord(10);
38    robot.Sleep(1000);
39
40    List<Number> weight =new ArrayList<>();
41    weight=robot.FT_PdIdenCompute();
42
43    DescPose desc_p1=new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
44    DescPose desc_p2=new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
45    DescPose desc_p3=new DescPose(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
46
47    robot.MoveCart(desc_p1, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
48    robot.Sleep(1000);
49    robot.FT_PdCogIdenRecord(10, 1);
50    robot.MoveCart(desc_p2, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
51    robot.Sleep(1000);
52    robot.FT_PdCogIdenRecord(10, 2);
53    robot.MoveCart(desc_p3, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
54    robot.Sleep(1000);
55    robot.FT_PdCogIdenRecord(10, 3);
56    robot.Sleep(1000);
57    DescTran cog=new DescTran(0,0,0);
58    robot.FT_PdCogIdenCompute(cog);
59
60    robot.CloseRPC();
61    return 0;
62}

12.19. Kollisionsschutz

 1/**
 2* @brief  Kollisionsschutz.
 3* @param  [in] flag 0-Kollisionsschutz deaktivieren, 1-Kollisionsschutz aktivieren.
 4* @param  [in] sensor_id Kraftsensor-Nummer.
 5* @param  [in] select  Auswahl der sechs Freiheitsgrade für die Kollisionserkennung, 0-nicht erkennen, 1-erkennen (als Object[] erwartet, z.B. Integer[]).
 6* @param  [in] ft  Kollisionskraft/-drehmoment, fx, fy, fz, tx, ty, tz.
 7* @param  [in] max_threshold Maximalschwellen (als Object[] erwartet, z.B. Double[]).
 8* @param  [in] min_threshold Minimalschwellen (als Object[] erwartet, z.B. Double[]).
 9* @note   Erkennungsbereich für Kraft/Drehmoment: (ft - min_threshold, ft + max_threshold).
10* @return  Fehlercode.
11*/
12int FT_Guard(int flag, int sensor_id, Object[] select, ForceTorque ft, Object[] max_threshold, Object[] min_threshold);

12.20. Codebeispiel für Kollisionsschutz

 1public static void main(String[] args)
 2{
 3    Robot robot = new Robot();
 4    robot.SetReconnectParam(true,20,500);//设置重连次数、间隔
 5    robot.LoggerInit(FrLogType.DIRECT, FrLogLevel.INFO, "D://log", 10, 10);
 6    int rtn = robot.RPC("192.168.58.2");
 7    if(rtn == 0)
 8    {
 9        System.out.println("RPC-Verbindung erfolgreich");
10    }
11    else
12    {
13        System.out.println("RPC-Verbindung fehlgeschlagen");
14        return;
15    }
16    byte flag = 1;
17    byte sensor_id = 8;
18    Object[] select = { 1, 0, 0, 0, 0, 0 }; // Nur x-Achsen-Kollisionsschutz aktivieren
19    Object[] max_threshold = { 5.0, 0.01, 0.01, 0.01, 0.01, 0.01 };
20    Object[] min_threshold = { 3.0, 0.01, 0.01, 0.01, 0.01, 0.01 };
21
22    ForceTorque ft = new ForceTorque(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
23    DescPose  desc_p1, desc_p2, desc_p3;
24    desc_p1 = new DescPose(-14.404,-455.283,319.847,-172.935,25.141,-68.097);
25    desc_p2 = new DescPose(-107.999,-599.174,285.939,153.472,12.686,-71.284);
26    desc_p3 = new DescPose(6.586,-704.897,309.638,178.909,-27.759,-70.479);
27
28    int rtn =  robot.FT_Guard(flag, sensor_id, select, ft, max_threshold, min_threshold);
29    System.out.println("FT_Guard start rtn {rtn}");
30    robot.MoveCart(desc_p1, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
31    robot.MoveCart(desc_p2, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
32    robot.MoveCart(desc_p3, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
33}

12.21. Konstantkraftregelung

 1/**
 2* @brief  Konstantkraftregelung.
 3* @param  flag 0-Konstantkraftregelung deaktivieren, 1-aktivieren.
 4* @param  sensor_id Kraftsensor-Nummer.
 5* @param  select  Auswahl der sechs Freiheitsgrade, 0-nicht, 1-erkennen (int[]).
 6* @param  ft  Kraft-/Drehmoment-Sollwerte.
 7* @param  ft_pid Kraft-PID-Parameter (double[] der Länge 6).
 8* @param  adj_sign Adaptive Start/Stopp-Steuerung, 0-deaktivieren, 1-aktivieren.
 9* @param  ILC_sign ILC Start/Stopp, 0-stopp, 1-Training, 2-Praxis.
10* @param  max_dis Maximale Anpassungsstrecke, Einheit mm.
11* @param  max_ang Maximaler Anpassungswinkel, Einheit deg.
12* @param  M rx, ry Massenparameter [0.1-10], Standard 2 (double[] der Länge 2).
13* @param  B rx, ry Dämpfungsparameter [0.1-50], Standard 8 (double[] der Länge 2).
14* @param  threshold rx, ry Startschwelle [0-10], Standard 0.2 (double[] der Länge 2).
15* @param  adjustCoeff rx, ry Drehmoment-Einstellkoeffizient [0-1], Standard 1 (double[] der Länge 2).
16* @param  polishRadio Schleifradius, Einheit mm.
17* @param  filter_Sign Filteraktivierungsflag 0-aus; 1-ein, Standard aus.
18* @param  posAdapt_sign Pose-Anpassungsaktivierungsflag 0-aus; 1-ein, Standard aus.
19* @param  isNoBlock Blockierungsflag, 0-blockierend; 1-nicht blockierend.
20* @return  Fehlercode.
21*/
22public int FT_Control(int flag, int sensor_id, int[] select, ForceTorque ft, double[] ft_pid, int adj_sign, int ILC_sign, double max_dis, double max_ang, double[] M, double[] B, double[] threshold, double[] adjustCoeff, double polishRadio, int filter_Sign, int posAdapt_sign, int isNoBlock);

12.22. Codebeispiel für Konstantkraftregelung mit Dämpfung

 1public static int TestFTControlWithAdjustCoeff(Robot robot)
 2{
 3    int sensor_id = 10;
 4    int[] select = { 0,0,1,0,0,0 };
 5    double[] ft_pid = { 0.0008, 0.0, 0.0, 0.0, 0.0, 0.0 };
 6    int adj_sign = 0;
 7    int ILC_sign = 0;
 8    double max_dis = 1000.0;
 9    double max_ang = 20;
10    ForceTorque ft = new ForceTorque(0.0,0,0,0,0,0);
11    ExaxisPos epos=new ExaxisPos(0, 0, 0, 0);
12    JointPos j1=new JointPos(80.765, -98.795, 106.548, -97.734, -89.999, 94.842);
13    JointPos j2=new JointPos(43.067, -84.429, 92.620, -98.175, -90.011, 57.144);
14    DescPose desc_p1=new DescPose(5.009, -547.463, 262.053, -179.999, -0.019, 75.923);
15    DescPose desc_p2=new DescPose(-347.966, -547.463, 262.048, -180.000, -0.019, 75.923);
16    DescPose offset_pos=new DescPose(0, 0, 0, 0, 0, 0);
17    double[] M = { 2.0, 2.0 };
18    double[] B = { 15.0, 15.0 };
19    double[] threshold = {1.0, 1.0};
20    double[] adjustCoeff = {1.0, 0.8};
21    double polishRadio = 0.0;
22    int filter_Sign = 0;
23    int posAdapt_sign = 1;
24    int isNoBlock;
25    ft.fz = -10.0;
26    while(true)
27    {
28        int rtn = robot.FT_Control(1, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
29        System.out.printf("FT_Control start rtn is %d\n", rtn);
30        robot.MoveL(j1, desc_p1, 1, 0, 100.0, 100.0, 100.0, -1.0, 0, epos, 0, 0, offset_pos, 0,0, 0,10);
31        robot.MoveL(j2, desc_p2, 1, 0, 100.0, 100.0, 100.0, -1.0, 0, epos, 0, 0, offset_pos, 0,0, 0,10);
32        rtn = robot.FT_Control(0, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
33        System.out.printf("FT_Control end rtn is %d\n", rtn);
34    }
35}

12.23. Rotationseinfügung

 1/**
 2* @brief Rotationseinfügung.
 3* @param rcs Referenzkoordinatensystem, 0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem.
 4* @param angVelRot Rotationswinkelgeschwindigkeit, Einheit deg/s.
 5* @param ft  Kraft-/Drehmomentschwelle (als einzelner Wert? Im C++ Beispiel war es ein double).
 6* @param max_angle Maximaler Rotationswinkel, Einheit deg.
 7* @param orn Kraft-/Drehmomentrichtung, 1-entlang der z-Achse, 2-um die z-Achse.
 8* @param max_angAcc Maximale Rotationsbeschleunigung, Einheit deg/s^2, derzeit nicht verwendet, Standard 0.
 9* @param rotorn  Rotationsrichtung, 1-im Uhrzeigersinn, 2-gegen Uhrzeigersinn.
10* @param strategy Behandlungsstrategie bei nicht erkannter Kraft, 0-Fehler; 1-Warnung, Bewegung fortsetzen.
11* @return  Fehlercode.
12*/
13public int FT_RotInsertion(int rcs, double angVelRot, double ft, double max_angle, int orn, double max_angAcc, int rotorn, int strategy);

12.24. Codebeispiel für Rotationseinfügung mit Kraftsensor

 1public static int TestMove(Robot robot)
 2{
 3    int rtn=-1;
 4    JointPos j1=new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
 5    JointPos j2=new JointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
 6    JointPos j3=new JointPos(-29.777, -84.536, 109.275, -114.075, -86.655, 74.257);
 7    JointPos j4=new JointPos(-31.154, -95.317, 94.276, -88.079, -89.740, 74.256);
 8    DescPose desc_pos1=new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
 9    DescPose desc_pos2=new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
10    DescPose desc_pos3=new DescPose(-487.434, 154.362, 308.576, 176.600, 0.268, -14.061);
11    DescPose desc_pos4=new DescPose(-443.165, 147.881, 480.951, 179.511, -0.775, -15.409);
12    DescPose offset_pos=new DescPose(0, 0, 0, 0, 0, 0);
13    ExaxisPos epos=new ExaxisPos(0, 0, 0, 0);
14    int tool = 0;
15    int user = 0;
16    double vel = 100.0;
17    double acc = 100.0;
18    double ovl = 100.0;
19    double oacc = 100.0;
20    double blendT = 0.0;
21    double blendR = 0.0;
22    int flag = 0;
23    int search = 0;
24    int blendMode = 0;
25    int velAccMode = 0;
26    robot.SetSpeed(20);
27    rtn = robot.MoveJ(j1, desc_pos1, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
28    System.out.printf("movej errcode:%d\n", rtn);
29    rtn = robot.MoveL(j2, desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, epos, search, flag, offset_pos, oacc, velAccMode,0,10);
30    System.out.printf("movel errcode:%d\n", rtn);
31    rtn = robot.MoveC(j3, desc_pos3, tool, user, vel, acc, epos, flag, offset_pos, j4, desc_pos4, tool, user, vel, acc, epos, flag, offset_pos, ovl, blendR, oacc, velAccMode);
32    System.out.printf("movec errcode:%d\n", rtn);
33    rtn = robot.MoveJ(j2, desc_pos2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
34    System.out.printf("movej errcode:%d\n", rtn);
35    rtn = robot.Circle(j3, desc_pos3, tool, user, vel, acc, epos, j1, desc_pos1, tool, user, vel, acc, epos, ovl, flag, offset_pos, oacc, -1, velAccMode);
36    System.out.printf("circle errcode:%d\n", rtn);
37    rtn = robot.MoveCart(desc_pos4, tool, user, vel, acc, ovl, blendT, -1);
38    System.out.printf("MoveCart errcode:%d\n", rtn);
39    rtn = robot.MoveJ(j1, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
40    System.out.printf("movej errcode:%d\n", rtn);
41    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, epos, search, flag, offset_pos, -1, velAccMode,0,10);
42    System.out.printf("movel errcode:%d\n", rtn);
43    rtn = robot.MoveC(desc_pos3, tool, user, vel, acc, epos, flag, offset_pos, desc_pos4, tool, user, vel, acc, epos, flag, offset_pos, ovl, blendR, -1, velAccMode);
44    System.out.printf("movec errcode:%d\n", rtn);
45    rtn = robot.MoveJ(j2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
46    System.out.printf("movej errcode:%d\n", rtn);
47    rtn = robot.Circle(desc_pos3, tool, user, vel, acc, epos, desc_pos1, tool, user, vel, acc, epos, ovl, flag, offset_pos, oacc, blendR, -1, velAccMode);
48    System.out.printf("circle errcode:%d\n", rtn);
49    return 0;
50}

12.25. Nachgiebigkeitsregelung starten

1/**
2* @brief  Nachgiebigkeitsregelung starten.
3* @param  [in] p Positionseinstellkoeffizient oder Nachgiebigkeitskoeffizient.
4* @param  [in] force Kraftschwelle zum Starten der Nachgiebigkeit, Einheit N.
5* @return  Fehlercode.
6*/
7int FT_ComplianceStart(double p, double force);

12.26. Nachgiebigkeitsregelung stoppen

1/**
2* @brief  Nachgiebigkeitsregelung stoppen.
3* @return  Fehlercode.
4*/
5int FT_ComplianceStop();

12.27. Codebeispiel für Nachgiebigkeitsregelung

 1public static int TestCompliance(Robot robot)
 2{
 3    DescTran tr1=new DescTran(0,0,0);
 4    robot.SetForceSensorPayload(0);
 5    robot.SetForceSensorPayloadCog(tr1);
 6
 7    int company = 24;
 8    int device = 0;
 9    int softversion = 0;
10    int bus = 1;
11    int index = 1;
12
13    DeviceConfig con=new DeviceConfig(company, device, softversion, bus);
14    robot.FT_SetConfig(con);
15    robot.Sleep(1000);
16    robot.FT_GetConfig(con);
17
18    robot.Sleep(1000);
19
20    robot.FT_Activate(0);
21    robot.Sleep(1000);
22    robot.FT_Activate(1);
23    robot.Sleep(1000);
24
25    robot.Sleep(1000);
26    robot.FT_SetZero(0);
27    robot.Sleep(1000);
28
29    int flag = 1;
30    int sensor_id = 1;
31    Object[] select =new Object[] { 1,1,1,0,0,0 };
32    Object[] ft_pid =new Object[] { 0.0005,0.0,0.0,0.0,0.0,0.0 };
33    int adj_sign = 0;
34    int ILC_sign = 0;
35    double max_dis = 100.0;
36    double max_ang = 0.0;
37
38    ForceTorque ft=new ForceTorque(0,0,0,0,0,0);
39    DescPose  offset_pos=new DescPose(0, 0, 0, 0, 0, 0);
40    ExaxisPos epos=new ExaxisPos(0, 0, 0, 0);
41
42
43    JointPos j1=new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
44    JointPos j2=new JointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
45    DescPose desc_p1=new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
46    DescPose desc_p2=new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
47
48    ft.fx = -10.0;
49    ft.fy = -10.0;
50    ft.fz = -10.0;
51    robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
52    double p = 0.00005;
53    double force = 30.0;
54    int rtn = robot.FT_ComplianceStart(p, force);
55
56    int count = 15;
57    while (count>0)
58    {
59        robot.MoveL(j1, desc_p1, 0, 0, 100.0, 180.0, 100.0, -1.0,0, epos, 0, 1, offset_pos,0,10);
60        robot.MoveL(j2, desc_p2, 0, 0, 100.0, 180.0, 100.0, -1.0,0, epos, 0, 0, offset_pos,0,10);
61        count -= 1;
62    }
63    robot.FT_ComplianceStop();
64    flag = 0;
65    robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
66
67    robot.CloseRPC();
68    return 0;
69}

12.28. Nutzlastidentifikation initialisieren

1/**
2* @brief Initialisiert die Nutzlastidentifikation (Dynamikfilter).
3* @return Fehlercode.
4*/
5int LoadIdentifyDynFilterInit();

12.29. Nutzlastidentifikations-Variablen initialisieren

1/**
2* @brief Initialisiert die Variablen der Nutzlastidentifikation.
3* @return Fehlercode.
4*/
5int LoadIdentifyDynVarInit();

12.30. Hauptprogramm der Nutzlastidentifikation

1/**
2* @brief Hauptprogramm der Nutzlastidentifikation.
3* @param [in] joint_torque Gelenkdrehmomente (Object[] erwartet, z.B. Double[]).
4* @param [in] joint_pos Gelenkpositionen (Object[] erwartet, z.B. Double[]).
5* @param [in] t Abtastperiode.
6* @return Fehlercode.
7*/
8int LoadIdentifyMain(Object[] joint_torque, Object[] joint_pos, double t);

12.31. Ergebnis der Nutzlastidentifikation abrufen

1/**
2* @brief Gibt das Ergebnis der Nutzlastidentifikation zurück.
3* @param [in] gain Koeffizienten (Object[] erwartet, z.B. Double[]).
4* @return List[0]: Fehlercode; List[1]: double weight Nutzlastgewicht; List[2]: double x Nutzlastschwerpunkt X (mm); List[3]: double y Nutzlastschwerpunkt Y (mm); List[4]: double z Nutzlastschwerpunkt Z (mm).
5*/
6List<Number> LoadIdentifyGetResult(Object[] gain);

12.32. Codebeispiel für Roboter-Nutzlastidentifikation

 1public static int TestIdentify(Robot robot)
 2{
 3    int retval = 0;
 4
 5    retval = robot.LoadIdentifyDynFilterInit();
 6
 7    retval = robot.LoadIdentifyDynVarInit();
 8
 9    JointPos posJ = new JointPos(0,0,0,0,0,0);
10    DescPose posDec = new DescPose(0,0,0,0,0,0);
11    List<Number> joint_toq=new ArrayList<>();
12    robot.GetActualJointPosDegree( posJ);
13    posJ.J2 = posJ.J2 + 10;
14    joint_toq=robot.GetJointTorques(0);
15
16    Object[] gain =new Object[] { 0,0.05,0,0,0,0,0,0.02,0,0,0,0 };
17    double weight = 0;
18    DescTran load_pos=new DescTran(0,0,0);
19    List<Number> num=new ArrayList<>();
20    num = robot.LoadIdentifyGetResult(gain);
21
22    robot.CloseRPC();
23    return 0;
24
25}

12.33. Unterstütztes Ziehen mit Kraftsensor

Geändert in Version Java: SDK-v1.0.6-3.8.3

 1/**
 2* @brief Unterstütztes Ziehen mit Kraftsensor.
 3* @param [in] status Steuerungsstatus, 0-deaktivieren; 1-aktivieren.
 4* @param [in] asaptiveFlag Adaptivitätsflag, 0-deaktivieren; 1-aktivieren.
 5* @param [in] interfereDragFlag Interferenzbereich-Ziehflag, 0-deaktivieren; 1-aktivieren.
 6* @param [in] ingularityConstraintsFlag Singularitätsstrategie, 0-vermeiden; 1-durchqueren.
 7* @param [in] forceCollisionFlag Kollisionserkennungsflag beim unterstützten Ziehen; 0-deaktivieren; 1-aktivieren.
 8* @param [in] M Trägheitskoeffizienten (Object[] erwartet, z.B. Double[] der Länge 6).
 9* @param [in] B Dämpfungskoeffizienten (Object[] erwartet, z.B. Double[] der Länge 6).
10* @param [in] K Steifigkeitskoeffizienten (Object[] erwartet, z.B. Double[] der Länge 6).
11* @param [in] F Sechsdimensionale Kraftschwellen für das Ziehen (Object[] erwartet, z.B. Double[] der Länge 6).
12* @param [in] Fmax Maximale Zugkraftbegrenzung Nm.
13* @param [in] Vmax Maximale Gelenkgeschwindigkeitsbegrenzung °/s.
14* @return  Fehlercode.
15*/
16int EndForceDragControl(int status, int asaptiveFlag, int interfereDragFlag, int ingularityConstraintsFlag, int forceCollisionFlag, Object[] M, Object[] B, Object[] K, Object[] F, double Fmax, double Vmax);

12.34. Schaltzustand des kraftunterstützten Ziehens abrufen

1/**
2* @brief Gibt den Schaltzustand des kraftunterstützten Ziehens zurück.
3* @return List[0]: Fehlercode; List[1]: dragState Steuerungsstatus des kraftunterstützten Ziehens, 0-deaktiviert; 1-aktiviert; List[2]: sixDimensionalDragState Steuerungsstatus des 6-Achsen-kraftunterstützten Ziehens, 0-deaktiviert; 1-aktiviert.
4*/
5List<Integer> GetForceAndTorqueDragState();

12.35. Automatische Aktivierung des Kraftsensors nach Fehlerlöschung

1/**
2* @brief Automatische Aktivierung des Kraftsensors nach Fehlerlöschung.
3* @param [in] status Steuerungsstatus, 0-deaktivieren; 1-aktivieren.
4* @return Fehlercode.
5*/
6int SetForceSensorDragAutoFlag(int status);

12.36. Codebeispiel für kraftunterstütztes Ziehen mit Kraftsensor

 1public static int TestEndForceDragCtrl(Robot robot)
 2{
 3    DescTran tr1 = new DescTran(0,0,0);
 4    robot.SetForceSensorPayload(0);
 5    robot.SetForceSensorPayloadCog(tr1);
 6
 7    robot.SetForceSensorDragAutoFlag(1);
 8
 9    Object[] M =new Object[] { 15.0, 15.0, 15.0, 0.5, 0.5, 0.1 };
10    Object[] B =new Object[] { 150.0, 150.0, 150.0, 5.0, 5.0, 1.0 };
11    Object[] K =new Object[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
12    Object[] F =new Object[] { 10.0, 10.0, 10.0, 1.0, 1.0, 1.0 };
13    robot.EndForceDragControl(1, 0, 0, 0, M, B, K, F, 50, 100);
14
15    robot.Sleep(10000);
16
17    int dragState = 0;
18    int sixDimensionalDragState = 0;
19    List<Integer> state=new ArrayList<>();
20    state=robot.GetForceAndTorqueDragState();
21
22    robot.EndForceDragControl(0, 0, 0, 0, M, B, K, F, 50, 100);
23    return 0;
24}

12.37. Schalter und Parameter für gemischtes Ziehen mit 6-Achsen-Kraft und Gelenkimpedanz einstellen

 1/**
 2* @brief Stellt Schalter und Parameter für gemischtes Ziehen mit 6-Achsen-Kraft und Gelenkimpedanz ein.
 3* @param [in] status Steuerungsstatus, 0-deaktivieren; 1-aktivieren.
 4* @param [in] impedanceFlag Impedanz-Aktivierungsflag, 0-deaktivieren; 1-aktivieren.
 5* @param [in] lamdeGain Zugverstärkung (Object[] erwartet, z.B. Double[] der Länge 6).
 6* @param [in] KGain Steifigkeitsverstärkung (Object[] erwartet, z.B. Double[] der Länge 6).
 7* @param [in] BGain Dämpfungsverstärkung (Object[] erwartet, z.B. Double[] der Länge 6).
 8* @param [in] dragMaxTcpVel Maximale Endlineargeschwindigkeitsbegrenzung beim Ziehen.
 9* @param [in] dragMaxTcpOriVel Maximale Endwinkelgeschwindigkeitsbegrenzung beim Ziehen.
10* @return  Fehlercode.
11*/
12int ForceAndJointImpedanceStartStop(int status, int impedanceFlag, Object[] lamdeGain, Object[] KGain, Object[] BGain, double dragMaxTcpVel, double dragMaxTcpOriVel);

12.38. Codebeispiel für gemischtes Ziehen mit 6-Achsen-Kraft und Gelenkimpedanz

 1public static int TestForceAndJointImpedance(Robot robot)
 2{
 3    robot.DragTeachSwitch(1);
 4    Object[] lamdeDain =new Object[] { 3.0, 2.0, 2.0, 2.0, 2.0, 3.0 };
 5    Object[] KGain = new Object[]{ 0, 0, 0, 0, 0, 0 };
 6    Object[] BGain =new Object[] { 150, 150, 150, 5.0, 5.0, 1.0 };
 7    int rtn = robot.ForceAndJointImpedanceStartStop(1, 0, lamdeDain, KGain, BGain, 1000.0, 180.0);
 8
 9    robot.Sleep(10000);
10
11    robot.DragTeachSwitch(0);
12    rtn = robot.ForceAndJointImpedanceStartStop(0, 0, lamdeDain, KGain, BGain, 1000.0, 180.0);
13
14    robot.CloseRPC();
15    return 0;
16}

12.39. Impedanzregelung Start/Stopp

 1/**
 2* @brief Impedanzregelung Start/Stopp.
 3* @param [in] status 0: deaktivieren; 1-aktivieren.
 4* @param [in] workSpace 0-Gelenkraum; 1-Kartesischer Raum.
 5* @param [in] forceThreshold Auslösekraftschwelle (N) (double[] der Länge 6).
 6* @param [in] m Massenparameter (double[] der Länge 6).
 7* @param [in] b Dämpfungsparameter (double[] der Länge 6).
 8* @param [in] k Steifigkeitsparameter (double[] der Länge 6).
 9* @param [in] maxV Maximale Lineargeschwindigkeit (mm/s).
10* @param [in] maxVA Maximale Linearbeschleunigung (mm/s²).
11* @param [in] maxW Maximale Winkelgeschwindigkeit (°/s).
12* @param [in] maxWA Maximale Winkelbeschleunigung (°/s²).
13* @return Fehlercode.
14*/
15public int ImpedanceControlStartStop(int status, int workSpace, double[] forceThreshold, double[] m, double[] b, double[] k, double maxV, double maxVA, double maxW, double maxWA);

12.40. Codebeispiel für Roboter-Impedanzregelung Start/Stopp

 1public static int TestImpedanceControl(Robot robot)
 2{
 3    JointPos j1=new JointPos(102.622, -135.990, 120.769, -73.950, -90.848, 35.507);
 4    JointPos j2=new JointPos(93.674, -80.062, 82.947, -92.199, -90.967, 26.559);
 5    DescPose desc_pos1=new DescPose(136.552, -149.799, 449.532, 179.817, -1.172, 157.123);
 6    DescPose desc_pos2=new DescPose(136.540, -561.048, 449.542, 179.819, -1.172, 157.122);
 7    DescPose offset_pos=new DescPose(0, 0, 0, 0, 0, 0);
 8    ExaxisPos epos=new ExaxisPos(0, 0, 0, 0);
 9    int tool = 0;
10    int user = 0;
11    double vel = 100.0;
12    double acc = 200.0;
13    double ovl = 100.0;
14    double blendT = -1.0;
15    double blendR = -1.0;
16    int flag = 0;
17    int search = 0;
18    robot.SetSpeed(20);
19    int company = 17;
20    int device = 0;
21    int softversion = 0;
22    int bus = 1;
23    DeviceConfig con=new DeviceConfig(company, device, softversion, bus);
24    robot.FT_SetConfig(con);
25    robot.Sleep(1000);
26    robot.FT_GetConfig(con);
27    System.out.println("FT config:"+con.company+","+con.device+","+con.softwareVersion+"con"+ con.bus);
28    robot.Sleep(1000);
29    robot.FT_Activate(0);
30    robot.Sleep(1000);
31    robot.FT_Activate(1);
32    robot.Sleep(1000);
33    robot.Sleep(1000);
34    robot.FT_SetZero(0);
35    robot.Sleep(1000);
36    robot.FT_SetZero(1);
37    robot.Sleep(1000);
38    double[] forceThreshold = { 30,30,30,5,5,5 };
39    double[] m= { 0.1,0.1,0.1,0.02,0.02,0.02 };
40    double[] b = { 1,1,1,0.08,0.08,0.08 };
41    double[] k = { 0,0,0,0,0,0 };
42    int rtn = robot.ImpedanceControlStartStop(1, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
43    System.out.println("ImpedanceControlStartStop errcode:"+ rtn);
44    rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
45    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
46    rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
47    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
48    rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
49    rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
50    System.out.println("movel errcode:"+ rtn);
51    robot.ImpedanceControlStartStop(0, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
52    robot.CloseRPC();
53    return 0;
54}

12.41. Drehmomentkompensationsfunktion und -koeffizienten aktivieren

1/**
2* @brief Aktiviert die Drehmomentkompensationsfunktion und setzt die Koeffizienten.
3* @param  status Schalter, 0-deaktivieren; 1-aktivieren.
4* @param  torqueCoeff J1-J6 Drehmomentkompensationskoeffizienten [0-1] (double[] der Länge 6).
5* @return Fehlercode.
6*/
7public int SerCoderCompenParams(int status, double[] torqueCoeff);