12. Roboter-Kraftregelung
12.1. Kraftsensor konfigurieren
1/**
2 * @brief Kraftsensor konfigurieren
3 * @param [in] company Kraftsensor-Hersteller, 17-Kunwei, 19-CASA, 20-ATI, 21-Zhongke Midian, 22-Weihang Minxin, 23-NBIT, 24-Xinjingcheng (XJC), 26-NSR
4 * @param [in] device Gerätenummer, Kunwei(0-KWR75B), CASA(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 [in] softvesion Softwareversionsnummer, vorübergehend nicht verwendet, Standard 0
6 * @param [in] bus Position des Geräts am Endeffektor-Bus, vorübergehend nicht verwendet, Standard 0
7 * @return Fehlercode
8 */
9errno_t FT_SetConfig(int company, int device, int softvesion, int bus);
12.2. Kraftsensor-Konfiguration abrufen
1/**
2* @brief Kraftsensor-Konfiguration abrufen
3* @param [out] company Kraftsensor-Hersteller
4* @param [out] device Gerätenummer
5* @param [out] softvesion Softwareversionsnummer
6* @param [out] bus Bus-Position
7* @return Fehlercode
8*/
9errno_t FT_GetConfig(int *company, int *device, int *softvesion, int *bus);
12.3. Kraftsensor aktivieren
1/**
2* @brief Kraftsensor aktivieren
3* @param [in] act 0-Zurücksetzen, 1-Aktivieren
4* @return Fehlercode
5*/
6errno_t FT_Activate(uint8_t act);
12.4. Kraftsensor Nullpunkt setzen (Tarieren)
1/**
2* @brief Kraftsensor Nullpunkt setzen (Tarieren)
3* @param [in] act 0-Nullpunkt entfernen, 1-Nullpunkt korrigieren
4* @return Fehlercode
5*/
6errno_t FT_SetZero(uint8_t act);
12.5. Referenzkoordinatensystem für Kraftsensor einstellen
1/**
2* @brief Referenzkoordinatensystem für Kraftsensor einstellen
3* @param [in] ref 0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem
4* @return Fehlercode
5*/
6errno_t FT_SetRCS(uint8_t ref);
12.6. Lastgewicht unter dem Kraftsensor einstellen
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Lastgewicht unter dem Kraftsensor einstellen
3 * @param [in] weight Lastgewicht [kg]
4 * @return Fehlercode
5 */
6errno_t SetForceSensorPayload(double weight);
12.7. Lastschwerpunkt unter dem Kraftsensor einstellen
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Lastschwerpunkt unter dem Kraftsensor einstellen
3 * @param [in] x Schwerpunkt x [mm]
4 * @param [in] y Schwerpunkt y [mm]
5 * @param [in] z Schwerpunkt z [mm]
6 * @return Fehlercode
7 */
8errno_t SetForceSensorPayloadCog(double x, double y, double z);
12.8. Lastgewicht unter dem Kraftsensor abrufen
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Lastgewicht unter dem Kraftsensor abrufen
3 * @param [out] weight Lastgewicht [kg]
4 * @return Fehlercode
5 */
6errno_t GetForceSensorPayload(double& weight);
12.9. Lastschwerpunkt unter dem Kraftsensor abrufen
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Lastschwerpunkt unter dem Kraftsensor abrufen
3 * @param [out] x Schwerpunkt x [mm]
4 * @param [out] y Schwerpunkt y [mm]
5 * @param [out] z Schwerpunkt z [mm]
6 * @return Fehlercode
7 */
8errno_t GetForceSensorPayloadCog(double& x, double& y, double& z);
12.10. Automatische Nullpunktberechnung (Tarieren) des Kraftsensors
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Automatische Nullpunktberechnung (Tarieren) des Kraftsensors
3 * @param [out] weight Sensor-eigenes Mass [kg] (oder resultierende Last?)
4 * @param [out] pos Sensor-eigener Schwerpunkt [mm] (oder resultierender Schwerpunkt?)
5 * @return Fehlercode
6 */
7errno_t ForceSensorAutoComputeLoad(double& weight, DescTran& pos);
12.11. Kraft-/Drehmomentdaten im Referenzkoordinatensystem abrufen
1/**
2* @brief Kraft-/Drehmomentdaten im Referenzkoordinatensystem abrufen
3* @param [out] ft Kraft/Drehmoment, fx, fy, fz, tx, ty, tz
4* @return Fehlercode
5*/
6errno_t FT_GetForceTorqueRCS(ForceTorque *ft);
12.12. Rohe Kraft-/Drehmomentdaten des Sensors abrufen
1/**
2* @brief Rohe Kraft-/Drehmomentdaten des Sensors abrufen
3* @param [out] ft Kraft/Drehmoment, fx, fy, fz, tx, ty, tz
4* @return Fehlercode
5*/
6errno_t FT_GetForceTorqueOrigin(ForceTorque *ft);
12.13. Codebeispiel für Kraftsensor-Konfiguration und automatische Nullpunktberechnung
1int TestFTInit(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 int company = 24;
14 int device = 0;
15 int softversion = 0;
16 int bus = 1;
17 int index = 1;
18 robot.FT_SetConfig(company, device, softversion, bus);
19 robot.Sleep(1000);
20 robot.FT_GetConfig(&company, &device, &softversion, &bus);
21 printf("FT config:%d,%d,%d,%d\n", company, device, softversion, bus);
22 robot.Sleep(1000);
23 robot.FT_Activate(0);
24 robot.Sleep(1000);
25 robot.FT_Activate(1);
26 robot.Sleep(1000);
27 robot.Sleep(1000);
28 robot.FT_SetZero(0);
29 robot.Sleep(1000);
30 ForceTorque ft;
31 memset(&ft, 0, sizeof(ForceTorque));
32 robot.FT_GetForceTorqueOrigin(0, &ft);
33 printf("ft origin:%f,%f,%f,%f,%f,%f\n", ft.fx, ft.fy, ft.fz, ft.tx, ft.ty, ft.tz);
34 robot.FT_SetZero(1);
35 robot.Sleep(1000);
36 DescPose ftCoord = {};
37 robot.FT_SetRCS(0, ftCoord);
38 robot.SetForceSensorPayload(0.824);
39 robot.SetForceSensorPayloadCog(0.778, 2.554, 48.765);
40 double weight = 0;
41 double x = 0, y = 0, z = 0;
42 robot.GetForceSensorPayload(weight);
43 robot.GetForceSensorPayloadCog(x, y, z);
44 printf("the FT load is %lf, %lf %lf %lf\n", weight, x, y, z);
45 robot.SetForceSensorPayload(0);
46 robot.SetForceSensorPayloadCog(0, 0, 0);
47 double computeWeight = 0;
48 DescTran tran = {};
49 robot.ForceSensorAutoComputeLoad(weight, tran);
50 cout << "the result is weight " << weight << " pos is " << tran.x << " " << tran.y << " " << tran.z << endl;
51 robot.CloseRPC();
52 return 0;
53}
12.14. Lastgewichtserkennung aufzeichnen
1/**
2* @brief Lastgewichtserkennung aufzeichnen
3* @param [in] id Sensorkoordinatensystem-Nummer, Bereich [1~14]
4* @return Fehlercode
5*/
6errno_t FT_PdIdenRecord(int id);
12.15. Lastgewichtserkennung berechnen
1/**
2* @brief Lastgewichtserkennung berechnen
3* @param [out] weight Lastgewicht [kg]
4* @return Fehlercode
5*/
6errno_t FT_PdIdenCompute(float *weight);
12.16. Lastschwerpunkt-Erkennung aufzeichnen
1/**
2* @brief Lastschwerpunkt-Erkennung aufzeichnen
3* @param [in] id Sensorkoordinatensystem-Nummer, Bereich [1~14]
4* @param [in] index Punktnummer, Bereich [1~3]
5* @return Fehlercode
6*/
7errno_t FT_PdCogIdenRecord(int id, int index);
12.17. Lastschwerpunkt-Erkennung berechnen
1/**
2* @brief Lastschwerpunkt-Erkennung berechnen
3* @param [out] cog Lastschwerpunkt [mm]
4* @return Fehlercode
5*/
6errno_t FT_PdCogIdenCompute(DescTran *cog);
12.18. Codebeispiel für Lastidentifikation mit Kraftsensor
1int TestFTLoadCompute(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 int company = 24;
14 int device = 0;
15 int softversion = 0;
16 int bus = 1;
17 int index = 1;
18 robot.FT_SetConfig(company, device, softversion, bus);
19 robot.Sleep(1000);
20 robot.FT_GetConfig(&company, &device, &softversion, &bus);
21 printf("FT config:%d,%d,%d,%d\n", company, device, softversion, bus);
22 robot.Sleep(1000);
23 robot.FT_Activate(0);
24 robot.Sleep(1000);
25 robot.FT_Activate(1);
26 robot.Sleep(1000);
27 robot.Sleep(1000);
28 robot.FT_SetZero(0);
29 robot.Sleep(1000);
30 ForceTorque ft;
31 memset(&ft, 0, sizeof(ForceTorque));
32 robot.FT_GetForceTorqueOrigin(0, &ft);
33 printf("ft origin:%f,%f,%f,%f,%f,%f\n", ft.fx, ft.fy, ft.fz, ft.tx, ft.ty, ft.tz);
34 robot.FT_SetZero(1);
35 robot.Sleep(1000);
36 DescPose tcoord = {};
37 tcoord.tran.z = 35.0;
38 robot.SetToolCoord(10, &tcoord, 1, 0, 0, 0);
39 robot.FT_PdIdenRecord(10);
40 robot.Sleep(1000);
41 float weight = 0.0;
42 robot.FT_PdIdenCompute(&weight);
43 printf("payload weight:%f\n", weight);
44 DescPose desc_p1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
45 DescPose desc_p2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
46 DescPose desc_p3(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
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;
58 memset(&cog, 0, sizeof(DescTran));
59 robot.FT_PdCogIdenCompute(&cog);
60 printf("cog:%f,%f,%f\n", cog.x, cog.y, cog.z);
61 robot.CloseRPC();
62 return 0;
63}
12.19. Kollisionsüberwachung (Force Guard)
1/**
2* @brief Kollisionsüberwachung (Force Guard)
3* @param [in] flag 0-deaktivieren, 1-aktivieren
4* @param [in] sensor_id Kraftsensor-Nummer
5* @param [in] select Auswahl der sechs Freiheitsgrade für die Kollisionserkennung, 0-nicht prüfen, 1-prüfen
6* @param [in] ft Kollisionskraft/-moment Schwellwerte (Mitte), fx,fy,fz,tx,ty,tz
7* @param [in] max_threshold Maximaler Schwellwert (obere Toleranz)
8* @param [in] min_threshold Minimaler Schwellwert (untere Toleranz)
9* @note Erkennungsbereich: (ft - min_threshold, ft + max_threshold)
10* @return Fehlercode
11*/
12errno_t FT_Guard(uint8_t flag, int sensor_id, uint8_t select[6], ForceTorque *ft, float max_threshold[6], float min_threshold[6]);
12.20. Codebeispiel für Kollisionsüberwachung
1int TestFTGuard(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 int company = 24;
14 int device = 0;
15 int softversion = 0;
16 int bus = 1;
17 int index = 1;
18 robot.FT_SetConfig(company, device, softversion, bus);
19 robot.Sleep(1000);
20 robot.FT_GetConfig(&company, &device, &softversion, &bus);
21 printf("FT config:%d,%d,%d,%d\n", company, device, softversion, bus);
22 robot.Sleep(1000);
23 robot.FT_Activate(0);
24 robot.Sleep(1000);
25 robot.FT_Activate(1);
26 robot.Sleep(1000);
27 robot.Sleep(1000);
28 robot.FT_SetZero(0);
29 robot.Sleep(1000);
30 uint8_t sensor_id = 1;
31 uint8_t select[6] = { 1,1,1,1,1,1 };
32 float max_threshold[6] = { 10.0,10.0,10.0,10.0,10.0,10.0 };
33 float min_threshold[6] = { 5.0,5.0,5.0,5.0,5.0,5.0 };
34 ForceTorque ft;
35 DescPose desc_p1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
36 DescPose desc_p2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
37 DescPose desc_p3(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
38 robot.FT_Guard(1, sensor_id, select, &ft, max_threshold, min_threshold);
39 robot.MoveCart(&desc_p1, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
40 robot.MoveCart(&desc_p2, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
41 robot.MoveCart(&desc_p3, 0, 0, 100.0, 100.0, 100.0, -1.0, -1);
42 robot.FT_Guard(0, sensor_id, select, &ft, max_threshold, min_threshold);
43 robot.CloseRPC();
44 return 0;
45}
12.21. Kraftregelung (Constant Force Control)
1/**
2* @brief Kraftregelung (Constant Force Control)
3* @param [in] flag 0-deaktivieren, 1-aktivieren
4* @param [in] sensor_id Kraftsensor-Nummer
5* @param [in] select Auswahl der sechs Freiheitsgrade für die Kraftregelung, 0-nicht regeln, 1-regeln
6* @param [in] ft Soll-Kraft/-Moment, fx,fy,fz,tx,ty,tz
7* @param [in] ft_pid PID-Parameter für Kraftregelung [6] (Reihenfolge: fx,fy,fz,tx,ty,tz P-Anteil? I,D=0?)
8* @param [in] adj_sign Adaptive Regelung, 0-deaktivieren, 1-aktivieren
9* @param [in] ILC_sign ILC (Iterative Learning Control), 0-stopp, 1-Training, 2-Praxis
10* @param [in] max_dis Maximale Anpassungsdistanz [mm]
11* @param [in] max_ang Maximaler Anpassungswinkel [°]
12* @param [in] M Massenparameter für rx, ry [0.1-10], Standard 2
13* @param [in] B Dämpfungsparameter für rx, ry [0.1-50], Standard 8
14* @param [in] threshold Startschwellen für rx, ry [0-10], Standard 0.2
15* @param [in] adjustCoeff Momenten-Anpassungskoeffizient für rx, ry [0-1], Standard 1
16* @param [in] polishRadio Schleifscheibenradius [mm], für spezielle Anwendungen
17* @param [in] filter_Sign Filter aktivieren, 0-aus; 1-an, Standard aus
18* @param [in] posAdapt_sign Posennachgiebigkeit aktivieren, 0-aus; 1-an, Standard aus
19* @param [in] isNoBlock Blockierungs-Flag, 0-blockierend; 1-nicht blockierend
20* @return Fehlercode
21*/
22errno_t FT_Control(uint8_t flag, int sensor_id, uint8_t select[6], ForceTorque* ft, float ft_pid[6], uint8_t adj_sign,
23uint8_t ILC_sign, float max_dis, float max_ang, double M[2], double B[2], double threshold[2], double adjustCoeff[2], double polishRadio = 0.0, int filter_Sign = 0, int posAdapt_sign = 0, int isNoBlock = 0);
12.22. Codebeispiel für Kraftregelung mit Dämpfung
1int TestFTControlWithAdjustCoeff(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 uint8_t sensor_id = 10;
14 uint8_t select[6] = { 0,0,1,0,0,0 };
15 float ft_pid[6] = { 0.0008, 0.0, 0.0, 0.0, 0.0, 0.0 };
16 uint8_t adj_sign = 0;
17 uint8_t ILC_sign = 0;
18 float max_dis = 1000.0;
19 float max_ang = 20;
20 ForceTorque ft = {0.0};
21 ExaxisPos epos(0, 0, 0, 0);
22 JointPos j1(80.765, -98.795, 106.548, -97.734, -89.999, 94.842);
23 JointPos j2(43.067, -84.429, 92.620, -98.175, -90.011, 57.144);
24 DescPose desc_p1(5.009, -547.463, 262.053, -179.999, -0.019, 75.923);
25 DescPose desc_p2(-347.966, -547.463, 262.048, -180.000, -0.019, 75.923);
26 DescPose offset_pos(0, 0, 0, 0, 0, 0);
27 double M[2] = { 2.0, 2.0 };
28 double B[2] = { 15.0, 15.0 };
29 double threshold[2] = {1.0, 1.0};
30 double adjustCoeff[2] = {1.0, 0.8};
31 double polishRadio = 0.0;
32 int filter_Sign = 0;
33 int posAdapt_sign = 1;
34 int isNoBlock;
35 ft.fz = -10.0;
36 while(true)
37 {
38 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);
39 printf("FT_Control start rtn is %d\n", rtn);
40 robot.MoveL(&j1, &desc_p1, 1, 0, 100, 100, 100, -1, 0, &epos, 0, 0, &offset_pos, 200.0, 0);
41 robot.MoveL(&j2, &desc_p2, 1, 0, 100, 100, 100, -1, 0, &epos, 0, 0, &offset_pos, 200.0, 0);
42 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);
43 printf("FT_Control end rtn is %d\n", rtn);
44 }
45 robot.CloseRPC();
46 return 0;
47}
12.23. Spiralförmige Suche
1/**
2* @brief Spiralförmige Suche (nach Kontakt)
3* @param [in] rcs Referenzkoordinatensystem, 0-Werkzeug, 1-Basis
4* @param [in] dr Radiuszunahme pro Umdrehung [mm]
5* @param [in] ft Kraft-/Momentenschwellwert für Abbruch [N] oder [Nm]
6* @param [in] max_t_ms Maximale Suchzeit [ms]
7* @param [in] max_vel Maximale Lineargeschwindigkeit [mm/s]
8* @return Fehlercode
9*/
10errno_t FT_SpiralSearch(int rcs, float dr, float ft, float max_t_ms, float max_vel);
12.24. Rotatorisches Einführen
1/**
2* @brief Rotatorisches Einführen (mit Kraftsuche)
3* @param [in] rcs Referenzkoordinatensystem, 0-Werkzeug, 1-Basis
4* @param [in] angVelRot Rotationsgeschwindigkeit [°/s]
5* @param [in] ft Kraft-/Momentenschwellwert für Abbruch [N] oder [Nm]
6* @param [in] max_angle Maximaler Rotationswinkel [°]
7* @param [in] orn Kraft-/Momentenrichtung, 1-entlang Z-Achse, 2-um Z-Achse
8* @param [in] max_angAcc Maximale Rotationsbeschleunigung [°/s²], vorübergehend nicht verwendet, Standard 0
9* @param [in] rotorn Rotationsrichtung, 1-im Uhrzeigersinn, 2-gegen Uhrzeigersinn
10* @param [in] strategy Strategie bei fehlender Kraft-/Momentenerkennung, 0-Fehler; 1-Warnung, Bewegung fortsetzen
11* @return Fehlercode
12*/
13errno_t FT_RotInsertion(int rcs, float angVelRot, float ft, float max_angle, uint8_t orn, float max_angAcc, uint8_t rotorn, int strategy = 0);
12.25. Codebeispiel für rotatorisches Einführen mit Kraftsensor
1int TestMove(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
14 JointPos j2(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
15 JointPos j3(-29.777, -84.536, 109.275, -114.075, -86.655, 74.257);
16 JointPos j4(-31.154, -95.317, 94.276, -88.079, -89.740, 74.256);
17 DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
18 DescPose desc_pos2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
19 DescPose desc_pos3(-487.434, 154.362, 308.576, 176.600, 0.268, -14.061);
20 DescPose desc_pos4(-443.165, 147.881, 480.951, 179.511, -0.775, -15.409);
21 DescPose offset_pos(0, 0, 0, 0, 0, 0);
22 ExaxisPos epos(0, 0, 0, 0);
23 int tool = 0;
24 int user = 0;
25 float vel = 100.0;
26 float acc = 100.0;
27 float ovl = 100.0;
28 float oacc = 100.0;
29 float blendT = 0.0;
30 float blendR = 0.0;
31 uint8_t flag = 0;
32 uint8_t search = 0;
33 int blendMode = 0;
34 int velAccMode = 0;
35 robot.SetSpeed(20);
36 rtn = robot.MoveJ(&j1, &desc_pos1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
37 printf("movej errcode:%d\n", rtn);
38 rtn = robot.MoveL(&j2, &desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, &epos, search, flag, &offset_pos, oacc, velAccMode);
39 printf("movel errcode:%d\n", rtn);
40 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);
41 printf("movec errcode:%d\n", rtn);
42 rtn = robot.MoveJ(&j2, &desc_pos2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
43 printf("movej errcode:%d\n", rtn);
44 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);
45 printf("circle errcode:%d\n", rtn);
46 rtn = robot.MoveCart(&desc_pos4, tool, user, vel, acc, ovl, blendT, -1);
47 printf("MoveCart errcode:%d\n", rtn);
48 rtn = robot.MoveJ(&j1, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
49 printf("movej errcode:%d\n", rtn);
50 rtn = robot.MoveL(&desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, &epos, search, flag, &offset_pos, -1, velAccMode);
51 printf("movel errcode:%d\n", rtn);
52 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);
53 printf("movec errcode:%d\n", rtn);
54 rtn = robot.MoveJ(&j2, tool, user, vel, acc, ovl, &epos, blendT, flag, &offset_pos);
55 printf("movej errcode:%d\n", rtn);
56 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);
57 printf("circle errcode:%d\n", rtn);
58 robot.CloseRPC();
59 return 0;
60}
12.26. Lineares Einführen
1/**
2* @brief Lineares Einführen (mit Kraftsuche)
3* @param [in] rcs Referenzkoordinatensystem, 0-Werkzeug, 1-Basis
4* @param [in] ft Kraft-/Momentenschwellwert für Abbruch [N] oder [Nm]
5* @param [in] lin_v Lineare Suchgeschwindigkeit [mm/s]
6* @param [in] lin_a Lineare Suchbeschleunigung [mm/s²], vorübergehend nicht verwendet
7* @param [in] max_dis Maximale Einführdistanz [mm]
8* @param [in] linorn Einführrichtung, 0-negative Richtung, 1-positive Richtung
9* @return Fehlercode
10*/
11errno_t FT_LinInsertion(int rcs, float ft, float lin_v, float lin_a, float max_dis, uint8_t linorn);
12.27. Codebeispiel für Spiral-Suche, Lineares Einführen etc.
1int TestFTSearch(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 int company = 24;
14 int device = 0;
15 int softversion = 0;
16 int bus = 1;
17 int index = 1;
18 robot.FT_SetConfig(company, device, softversion, bus);
19 robot.Sleep(1000);
20 robot.FT_GetConfig(&company, &device, &softversion, &bus);
21 printf("FT config:%d,%d,%d,%d\n", company, device, softversion, bus);
22 robot.Sleep(1000);
23 robot.FT_Activate(0);
24 robot.Sleep(1000);
25 robot.FT_Activate(1);
26 robot.Sleep(1000);
27 robot.Sleep(1000);
28 robot.FT_SetZero(0);
29 robot.Sleep(1000);
30 uint8_t status = 1;
31 int sensor_num = 1;
32 float gain[6] = { 0.0001,0.0,0.0,0.0,0.0,0.0 };
33 uint8_t adj_sign = 0;
34 uint8_t ILC_sign = 0;
35 float max_dis = 100.0;
36 float max_ang = 5.0;
37 ForceTorque ft;
38 memset(&ft, 0, sizeof(ForceTorque));
39 int rcs = 0;
40 float dr = 0.7;
41 float fFinish = 1.0;
42 float t = 60000.0;
43 float vmax = 3.0;
44 float force_goal = 20.0;
45 float lin_v = 0.0;
46 float lin_a = 0.0;
47 float disMax = 100.0;
48 uint8_t linorn = 1;
49 float angVelRot = 2.0;
50 float forceInsertion = 1.0;
51 int angleMax = 45;
52 uint8_t orn = 1;
53 float angAccmax = 0.0;
54 uint8_t rotorn = 1;
55 uint8_t select1[6] = { 0,0,1,1,1,0 };
56 ft.fz = -10.0;
57 robot.FT_Control(status, sensor_num, select1, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
58 rtn = robot.FT_SpiralSearch(rcs, dr, fFinish, t, vmax);
59 printf("FT_SpiralSearch rtn is %d\n", rtn);
60 status = 0;
61 robot.FT_Control(status, sensor_num, select1, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
62 uint8_t select2[6] = { 1,1,1,0,0,0 };
63 gain[0] = 0.00005;
64 ft.fz = -30.0;
65 status = 1;
66 robot.FT_Control(status, sensor_num, select2, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
67 rtn = robot.FT_LinInsertion(rcs, force_goal, lin_v, lin_a, disMax, linorn);
68 printf("FT_LinInsertion rtn is %d\n", rtn);
69 status = 0;
70 robot.FT_Control(status, sensor_num, select2, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
71 uint8_t select3[6] = { 0,0,1,1,1,0 };
72 ft.fz = -10.0;
73 gain[0] = 0.0001;
74 status = 1;
75 robot.FT_Control(status, sensor_num, select3, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
76 rtn = robot.FT_RotInsertion(rcs, angVelRot, forceInsertion, angleMax, orn, angAccmax, rotorn);
77 printf("FT_RotInsertion rtn is %d\n", rtn);
78 status = 0;
79 robot.FT_Control(status, sensor_num, select3, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
80 uint8_t select4[6] = { 1,1,1,0,0,0 };
81 ft.fz = -30.0;
82 status = 1;
83 robot.FT_Control(status, sensor_num, select4, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
84 rtn = robot.FT_LinInsertion(rcs, force_goal, lin_v, lin_a, disMax, linorn);
85 printf("FT_LinInsertion rtn is %d\n", rtn);
86 status = 0;
87 robot.FT_Control(status, sensor_num, select4, &ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
88 robot.CloseRPC();
89 return 0;
90}
12.28. Oberflächenlokalisierung
1/**
2* @brief Oberflächenlokalisierung (FindSurface)
3* @param [in] rcs Referenzkoordinatensystem, 0-Werkzeug, 1-Basis
4* @param [in] dir Bewegungsrichtung, 1-positive Richtung, 2-negative Richtung
5* @param [in] axis Bewegungsachse, 1-x-Achse, 2-y-Achse, 3-z-Achse
6* @param [in] lin_v Lineare Suchgeschwindigkeit [mm/s]
7* @param [in] lin_a Lineare Suchbeschleunigung [mm/s²], vorübergehend nicht verwendet, Standard 0
8* @param [in] max_dis Maximale Suchdistanz [mm]
9* @param [in] ft Kraftschwellwert für Aktionsabbruch [N] (einzelner Wert für die gewählte Achse)
10* @return Fehlercode
11*/
12errno_t FT_FindSurface(int rcs, uint8_t dir, uint8_t axis, float lin_v, float lin_a, float max_dis, float ft);
12.29. Berechnung der mittleren Ebene starten
1/**
2* @brief Berechnung der mittleren Ebene starten (für Zentrierung)
3* @return Fehlercode
4*/
5errno_t FT_CalCenterStart();
12.30. Berechnung der mittleren Ebene beenden
1/**
2* @brief Berechnung der mittleren Ebene beenden
3* @param [out] pos Pose der mittleren Ebene (Zentrumsposition)
4* @return Fehlercode
5*/
6errno_t FT_CalCenterEnd(DescPose *pos);
12.31. Codebeispiel für Oberflächenlokalisierung
1int TestSurface(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 int company = 24;
14 int device = 0;
15 int softversion = 0;
16 int bus = 1;
17 int index = 1;
18 robot.FT_SetConfig(company, device, softversion, bus);
19 robot.Sleep(1000);
20 robot.FT_GetConfig(&company, &device, &softversion, &bus);
21 printf("FT config:%d,%d,%d,%d\n", company, device, softversion, bus);
22 robot.Sleep(1000);
23 robot.FT_Activate(0);
24 robot.Sleep(1000);
25 robot.FT_Activate(1);
26 robot.Sleep(1000);
27 robot.Sleep(1000);
28 robot.FT_SetZero(0);
29 robot.Sleep(1000);
30 int rcs = 0;
31 uint8_t dir = 1;
32 uint8_t axis = 1;
33 float lin_v = 3.0;
34 float lin_a = 0.0;
35 float maxdis = 50.0;
36 float ft_goal = 2.0;
37 DescPose desc_pos(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
38 DescPose xcenter(0, 0, 0, 0, 0, 0);
39 DescPose ycenter(0, 0, 0, 0, 0, 0);
40 ForceTorque ft;
41 memset(&ft, 0, sizeof(ForceTorque));
42 ft.fx = -2.0;
43 robot.MoveCart(&desc_pos, 9, 0, 100.0, 100.0, 100.0, -1.0, -1);
44 robot.FT_CalCenterStart();
45 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
46 robot.MoveCart(&desc_pos, 9, 0, 100.0, 100.0, 100.0, -1.0, -1);
47 robot.WaitMs(1000);
48 dir = 2;
49 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
50 robot.FT_CalCenterEnd(&xcenter);
51 printf("xcenter:%f,%f,%f,%f,%f,%f\n", xcenter.tran.x, xcenter.tran.y, xcenter.tran.z, xcenter.rpy.rx, xcenter.rpy.ry, xcenter.rpy.rz);
52 robot.MoveCart(&xcenter, 9, 0, 60.0, 50.0, 50.0, -1.0, -1);
53 robot.FT_CalCenterStart();
54 dir = 1;
55 axis = 2;
56 lin_v = 6.0;
57 maxdis = 150.0;
58 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
59 robot.MoveCart(&desc_pos, 9, 0, 100.0, 100.0, 100.0, -1.0, -1);
60 robot.WaitMs(1000);
61 dir = 2;
62 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
63 robot.FT_CalCenterEnd(&ycenter);
64 printf("ycenter:%f,%f,%f,%f,%f,%f\n", ycenter.tran.x, ycenter.tran.y, ycenter.tran.z, ycenter.rpy.rx, ycenter.rpy.ry, ycenter.rpy.rz);
65 robot.MoveCart(&ycenter, 9, 0, 60.0, 50.0, 50.0, 0.0, -1);
66 robot.CloseRPC();
67 return 0;
68}
12.32. Nachgiebigkeitsregelung (Compliance) starten
1/**
2* @brief Nachgiebigkeitsregelung (Compliance) starten
3* @param [in] p Positionsregelungsfaktor oder Nachgiebigkeitskoeffizient
4* @param [in] force Kraftschwellwert für Aktivierung [N]
5* @return Fehlercode
6*/
7errno_t FT_ComplianceStart(float p, float force);
12.33. Nachgiebigkeitsregelung (Compliance) stoppen
1/**
2* @brief Nachgiebigkeitsregelung (Compliance) stoppen
3* @return Fehlercode
4*/
5errno_t FT_ComplianceStop();
12.34. Codebeispiel für Nachgiebigkeitsregelung
1int TestCompliance(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 int company = 24;
14 int device = 0;
15 int softversion = 0;
16 int bus = 1;
17 int index = 1;
18 robot.FT_SetConfig(company, device, softversion, bus);
19 robot.Sleep(1000);
20 robot.FT_GetConfig(&company, &device, &softversion, &bus);
21 printf("FT config:%d,%d,%d,%d\n", company, device, softversion, bus);
22 robot.Sleep(1000);
23 robot.FT_Activate(0);
24 robot.Sleep(1000);
25 robot.FT_Activate(1);
26 robot.Sleep(1000);
27 robot.Sleep(1000);
28 robot.FT_SetZero(0);
29 robot.Sleep(1000);
30 uint8_t flag = 1;
31 int sensor_id = 1;
32 uint8_t select[6] = { 1,1,1,0,0,0 };
33 float ft_pid[6] = { 0.0005,0.0,0.0,0.0,0.0,0.0 };
34 uint8_t adj_sign = 0;
35 uint8_t ILC_sign = 0;
36 float max_dis = 100.0;
37 float max_ang = 0.0;
38 ForceTorque ft;
39 DescPose offset_pos(0, 0, 0, 0, 0, 0);
40 ExaxisPos epos(0, 0, 0, 0);
41 memset(&ft, 0, sizeof(ForceTorque));
42 JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
43 JointPos j2(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
44 DescPose desc_p1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
45 DescPose desc_p2(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
46 ft.fx = -10.0;
47 ft.fy = -10.0;
48 ft.fz = -10.0;
49 robot.FT_Control(flag, sensor_id, select, &ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
50 float p = 0.00005;
51 float force = 30.0;
52 rtn = robot.FT_ComplianceStart(p, force);
53 printf("FT_ComplianceStart rtn is %d\n", rtn);
54 int count = 15;
55 while (count)
56 {
57 robot.MoveL(&j1, &desc_p1, 0, 0, 100.0, 180.0, 100.0, -1.0, &epos, 0, 1, &offset_pos);
58 robot.MoveL(&j2, &desc_p2, 0, 0, 100.0, 180.0, 100.0, -1.0, &epos, 0, 0, &offset_pos);
59 count -= 1;
60 }
61 robot.FT_ComplianceStop();
62 printf("FT_ComplianceStop rtn is %d\n", rtn);
63 flag = 0;
64 robot.FT_Control(flag, sensor_id, select, &ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
65 robot.CloseRPC();
66 return 0;
67}
12.35. Lastidentifikation - Dynamikfilter initialisieren
1/**
2* @brief Lastidentifikation - Dynamikfilter initialisieren
3* @return Fehlercode
4*/
5errno_t LoadIdentifyDynFilterInit();
12.36. Lastidentifikation - Variablen initialisieren
1/**
2* @brief Lastidentifikation - Variablen initialisieren
3* @return Fehlercode
4*/
5errno_t LoadIdentifyDynVarInit();
12.37. Lastidentifikation - Hauptprogramm
1/**
2* @brief Lastidentifikation - Hauptprogramm
3* @param [in] joint_torque Gemessene Gelenkmomente [Nm]
4* @param [in] joint_pos Gemessene Gelenkpositionen [rad] oder [°]?
5* @param [in] t Abtastperiode [s]
6* @return Fehlercode
7*/
8errno_t LoadIdentifyMain(double joint_torque[6], double joint_pos[6], double t);
12.38. Lastidentifikationsergebnis abrufen
1/**
2* @brief Lastidentifikationsergebnis abrufen
3* @param [in] gain Verstärkungsfaktor (für Filter?)
4* @param [out] weight Berechnetes Lastgewicht [kg]
5* @param [out] cog Berechneter Lastschwerpunkt [mm]
6* @return Fehlercode
7*/
8errno_t LoadIdentifyGetResult(double gain[12], double *weight, DescTran *cog);
12.39. Codebeispiel für Roboter-Lastidentifikation
1int TestIdentify(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 int retval = 0;
14 retval = robot.LoadIdentifyDynFilterInit();
15 printf("LoadIdentifyDynFilterInit retval is: %d \n", retval);
16 retval = robot.LoadIdentifyDynVarInit();
17 printf("LoadIdentifyDynVarInit retval is: %d \n", retval);
18 JointPos posJ = {};
19 DescPose posDec = {};
20 float joint_toq[6] = { 0.0 };
21 robot.GetActualJointPosDegree(0, &posJ);
22 posJ.jPos[1] = posJ.jPos[1] + 10;
23 robot.GetJointTorques(0, joint_toq);
24 joint_toq[1] = joint_toq[1] + 2;
25 double tmpTorque[6] = { 0.0 };
26 for (int i = 0; i < 6; i++)
27 {
28 tmpTorque[i] = joint_toq[i];
29 }
30 retval = robot.LoadIdentifyMain(tmpTorque, posJ.jPos, 1);
31 printf("LoadIdentifyMain retval is: %d \n", retval);
32 double gain[12] = { 0,0.05,0,0,0,0,0,0.02,0,0,0,0 };
33 double weight = 0;
34 DescTran load_pos;
35 memset(&load_pos, 0, sizeof(DescTran));
36 retval = robot.LoadIdentifyGetResult(gain, &weight, &load_pos);
37 printf("LoadIdentifyGetResult retval is: %d ; weight is %f cog is %f %f %f \n", retval, weight, load_pos.x, load_pos.y, load_pos.z);
38 robot.CloseRPC();
39 return 0;
40}
12.40. Kraftsensor-unterstütztes Führen (Drag & Teach)
Geändert in Version C++SDK-v2.2.0-3.8.0.
1/**
2* @brief Kraftsensor-unterstütztes Führen (Drag & Teach)
3* @param [in] status Steuerstatus, 0-aus; 1-an
4* @param [in] asaptiveFlag Adaptive Regelung aktivieren, 0-aus; 1-an
5* @param [in] interfereDragFlag Führen in Interferenzbereichen, 0-aus; 1-an
6* @param [in] ingularityConstraintsFlag Strategie bei Singularität, 0-vermeiden; 1-durchqueren
7* @param [in] forceCollisionFlag Kollisionserkennung während Führung, 0-aus; 1-an
8* @param [in] M Trägheitskoeffizienten (Vektor der Länge 6)
9* @param [in] B Dämpfungskoeffizienten (Vektor der Länge 6)
10* @param [in] K Steifigkeitskoeffizienten (Vektor der Länge 6)
11* @param [in] F Kraftschwellwerte für sechs Achsen (Vektor der Länge 6)
12* @param [in] Fmax Maximale Kraftbegrenzung
13* @param [in] Vmax Maximale Geschwindigkeitsbegrenzung
14* @return Fehlercode
15*/
16errno_t EndForceDragControl(int status, int asaptiveFlag, int interfereDragFlag, int ingularityConstraintsFlag, int forceCollisionFlag, std::vector<double> M, std::vector<double> B, std::vector<double> K, std::vector<double> F, double Fmax, double Vmax);
12.41. Kraftsensor-Führungsstatus abrufen
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Kraftsensor-Führungsstatus abrufen
3 * @param [out] dragState Kraftsensor-unterstützter Führungsstatus, 0-aus; 1-an
4 * @param [out] sixDimensionalDragState 6D-Kraftführungsstatus, 0-aus; 1-an (möglicherweise identisch)
5 * @return Fehlercode
6 */
7errno_t GetForceAndTorqueDragState(int& dragState, int& sixDimensionalDragState);
12.42. Automatische Aktivierung des Kraftsensors nach Fehlerlöschung
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Automatische Aktivierung des Kraftsensors nach Fehlerlöschung
3 * @param [in] status Steuerstatus, 0-aus; 1-an
4 * @return Fehlercode
5 */
6errno_t SetForceSensorDragAutoFlag(int status);
12.43. Codebeispiel für Kraftsensor-unterstütztes Führen
1 int TestEndForceDragCtrl(void)
2 {
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 robot.SetForceSensorDragAutoFlag(1);
14 vector <double> M = { 15.0, 15.0, 15.0, 0.5, 0.5, 0.1 };
15 vector <double> B = { 150.0, 150.0, 150.0, 5.0, 5.0, 1.0 };
16 vector <double> K = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
17 vector <double> F = { 10.0, 10.0, 10.0, 1.0, 1.0, 1.0 };
18 robot.EndForceDragControl(1, 0, 0, 0, 1, M, B, K, F, 50, 100);
19 robot.Sleep(5000);
20 int dragState = 0;
21 int sixDimensionalDragState = 0;
22 robot.GetForceAndTorqueDragState(dragState, sixDimensionalDragState);
23 printf("the drag state is %d %d \n", dragState, sixDimensionalDragState);
24 robot.EndForceDragControl(0, 0, 0, 0, 1, M, B, K, F, 50, 100);
25 robot.CloseRPC();
26 return 0;
27 }
12.44. Gemischte 6D-Kraft- und Gelenkimpedanz-Führung ein-/ausschalten und Parameter setzen
Neu in Version C++SDK-v2.1.5.0.
1/**
2 * @brief Gemischte 6D-Kraft- und Gelenkimpedanz-Führung ein-/ausschalten und Parameter setzen
3 * @param [in] status Steuerstatus, 0-aus; 1-an
4 * @param [in] impedanceFlag Impedanz aktivieren, 0-aus; 1-an
5 * @param [in] lamdeDain Führungsverstärkung (Vektor der Länge 6)
6 * @param [in] KGain Steifigkeitsverstärkung (Vektor der Länge 6)
7 * @param [in] BGain Dämpfungsverstärkung (Vektor der Länge 6)
8 * @param [in] dragMaxTcpVel Maximale Endeffektor-Lineargeschwindigkeit während Führung [mm/s]
9 * @param [in] dragMaxTcpOriVel Maximale Endeffektor-Winkelgeschwindigkeit während Führung [°/s]
10 * @return Fehlercode
11 */
12errno_t ForceAndJointImpedanceStartStop(int status, int impedanceFlag, std::vector<double> lamdeDain, std::vector<double> KGain, std::vector<double> BGain, double dragMaxTcpVel, double dragMaxTcpOriVel);
12.45. Codebeispiel für gemischte 6D-Kraft- und Gelenkimpedanz-Führung
Neu in Version C++SDK-v2.1.5.0.
1int TestForceAndJointImpedance(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 robot.DragTeachSwitch(1);
14 vector <double> lamdeDain = { 3.0, 2.0, 2.0, 2.0, 2.0, 3.0 };
15 vector <double> KGain = { 0, 0, 0, 0, 0, 0 };
16 vector <double> BGain = { 150, 150, 150, 5.0, 5.0, 1.0 };
17 rtn = robot.ForceAndJointImpedanceStartStop(1, 0, lamdeDain, KGain, BGain, 1000, 180);
18 printf("ForceAndJointImpedanceStartStop rtn is %d\n", rtn);
19 robot.Sleep(5000);
20 robot.DragTeachSwitch(0);
21 rtn = robot.ForceAndJointImpedanceStartStop(0, 0, lamdeDain, KGain, BGain, 1000, 180);
22 printf("ForceAndJointImpedanceStartStop rtn is %d\n", rtn);
23 robot.CloseRPC();
24 return 0;
25}
12.46. Impedanzregelung Start/Stopp
Neu in Version C++SDK-v3.8.6.
1/**
2* @brief Impedanzregelung Start/Stopp
3* @param [in] status 0: aus; 1-an
4* @param [in] workSpace 0-Gelenkraum; 1-Kartesischer Raum
5* @param [in] forceThreshold Kraftschwellwert für Auslösung [N] (Vektor der Länge 6)
6* @param [in] m Massenparameter (Vektor der Länge 6)
7* @param [in] b Dämpfungsparameter (Vektor der Länge 6)
8* @param [in] k Steifigkeitsparameter (Vektor 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*/
15errno_t ImpedanceControlStartStop(int status, int workSpace, double forceThreshold[6], double m[6], double b[6], double k[6], double maxV, double maxVA, double maxW, double maxWA);
12.47. Codebeispiel für Roboter-Impedanzregelung
Neu in Version C++SDK-v3.8.6.
1int TestImpedanceControl()
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 uint8_t ctrl[20];
6 uint8_t state;
7 int pressVlaue;
8 int error;
9 robot.CloseRPC();
10 robot.LoggerInit();
11 robot.SetLoggerLevel(1);
12 int rtn = robot.RPC("192.168.58.2");
13 if (rtn != 0)
14 {
15 return 0;
16 }
17 robot.SetReConnectParam(true, 30000, 500);
18 JointPos j1(102.622, -135.990, 120.769, -73.950, -90.848, 35.507);
19 JointPos j2(93.674, -80.062, 82.947, -92.199, -90.967, 26.559);
20 DescPose desc_pos1(136.552, -149.799, 449.532, 179.817, -1.172, 157.123);
21 DescPose desc_pos2(136.540, -561.048, 449.542, 179.819, -1.172, 157.122);
22 DescPose offset_pos(0, 0, 0, 0, 0, 0);
23 ExaxisPos epos(0, 0, 0, 0);
24 int tool = 0;
25 int user = 0;
26 float vel = 100.0;
27 float acc = 200.0;
28 float ovl = 100.0;
29 float blendT = -1.0;
30 float blendR = -1.0;
31 uint8_t flag = 0;
32 uint8_t search = 0;
33 robot.SetSpeed(20);
34 int company = 17;
35 int device = 0;
36 int softversion = 0;
37 int bus = 1;
38 robot.FT_SetConfig(company, device, softversion, bus);
39 robot.Sleep(1000);
40 robot.FT_GetConfig(&company, &device, &softversion, &bus);
41 printf("FT config:%d,%d,%d,%d\n", company, device, softversion, bus);
42 robot.Sleep(1000);
43 robot.FT_Activate(0);
44 robot.Sleep(1000);
45 robot.FT_Activate(1);
46 robot.Sleep(1000);
47 robot.Sleep(1000);
48 robot.FT_SetZero(0);
49 robot.Sleep(1000);
50 robot.FT_SetZero(1);
51 robot.Sleep(1000);
52 double forceThreshold[6] = { 30,30,30,5,5,5 };
53 double m[6] = { 0.1,0.1,0.1,0.02,0.02,0.02 };
54 double b[6] = { 1,1,1,0.08,0.08,0.08 };
55 double k[6] = { 0,0,0,0,0,0 };
56 rtn = robot.ImpedanceControlStartStop(1, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
57 printf("ImpedanceControlStartStop errcode:%d\n", rtn);
58 rtn = robot.MoveL(&desc_pos1, tool, user, vel, acc, ovl, blendR, 0, &epos, search, flag, &offset_pos, -1, 1);
59 rtn = robot.MoveL(&desc_pos2, tool, user, vel, acc, ovl, blendR, 0, &epos, search, flag, &offset_pos, -1, 1);
60 rtn = robot.MoveL(&desc_pos1, tool, user, vel, acc, ovl, blendR, 0, &epos, search, flag, &offset_pos, -1, 1);
61 rtn = robot.MoveL(&desc_pos2, tool, user, vel, acc, ovl, blendR, 0, &epos, search, flag, &offset_pos, -1, 1);
62 printf("movel errcode:%d\n", rtn);
63 robot.ImpedanceControlStartStop(0, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
64 robot.CloseRPC();
65 return 0;
66}
12.48. Momentenkompensation aktivieren und Koeffizienten setzen
1/**
2* @brief Momentenkompensation aktivieren und Koeffizienten setzen (für Gelenke)
3* @param [in] status Schalter, 0-aus; 1-an
4* @param [in] torqueCoeff Momentenkompensationskoeffizienten J1-J6 [0-1]
5* @return Fehlercode
6*/
7errno_t SerCoderCompenParams(int status, double torqueCoeff[6]);