12. Roboter-Kraftsteuerung
12.1. Kraftsensor konfigurieren
1/**
2* @brief Konfiguriert den Kraftsensor
3* @param [in] company Kraftsensor-Hersteller, 17-Kunwei Technology
4* @param [in] device Gerätenummer, derzeit nicht verwendet, Standard 0
5* @param [in] softvesion Softwareversionsnummer, derzeit nicht verwendet, Standard 0
6* @param [in] bus Position des Geräts am Flanschbus, derzeit nicht verwendet, Standard 0
7* @return Fehlercode
8*/
9int FT_SetConfig(int company, int device, int softvesion, int bus);
12.2. Kraftsensor-Konfiguration abrufen
1/**
2* @brief Gibt die Kraftsensor-Konfiguration zurück
3* @param [out] deviceID Kraftsensor-Nummer
4* @param [out] company Kraftsensor-Hersteller, 17-Kunwei Technology, 19-Aerospace 11th Institute, 20-ATI Sensor, 21-Zhongke Midian, 22-Weihang Minxin
5* @param [out] 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)
6* @param [out] softvesion Softwareversionsnummer, derzeit nicht verwendet, Standard 0
7* @return Fehlercode
8*/
9int FT_GetConfig(ref int deviceID, ref int company, ref int device, ref int softvesion);
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(byte 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(byte act);
12.5. Referenzkoordinatensystem für Kraftsensor einstellen
1/**
2* @brief Stellt das Referenzkoordinatensystem für den Kraftsensor ein
3* @param [in] ref 0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem
4* @return Fehlercode
5*/
6int FT_SetRCS(byte type);
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] x Nutzlastschwerpunkt x in mm
4* @param [in] y Nutzlastschwerpunkt y in mm
5* @param [in] z Nutzlastschwerpunkt z in mm
6* @return Fehlercode
7*/
8int SetForceSensorPayLoadCog(double x, double y, double z);
12.8. Nutzlastgewicht unter dem Kraftsensor abrufen
1/**
2* @brief Gibt das Nutzlastgewicht unter dem Kraftsensor zurück
3* @param [out] weight Nutzlastgewicht in kg
4* @return Fehlercode
5*/
6int GetForceSensorPayLoad(ref double weight);
12.9. Nutzlastschwerpunkt unter dem Kraftsensor abrufen
1/**
2* @brief Gibt den Nutzlastschwerpunkt unter dem Kraftsensor zurück
3* @param [out] x Nutzlastschwerpunkt x in mm
4* @param [out] y Nutzlastschwerpunkt y in mm
5* @param [out] z Nutzlastschwerpunkt z in mm
6* @return Fehlercode
7*/
8int GetForceSensorPayLoadCog(ref double x, ref double y, ref double z);
12.10. Automatische Nullpunktkorrektur des Kraftsensors
1/**
2* @brief Automatische Nullpunktkorrektur des Kraftsensors
3* @param [out] weight Sensormasse in kg
4* @param [out] pos Sensorsschwerpunkt in mm
5* @return Fehlercode
6*/
7int ForceSensorAutoComputeLoad(ref double weight, ref DescTran pos);
12.11. Kraft-/Drehmomentdaten im Referenzkoordinatensystem abrufen
1/**
2* @brief Gibt Kraft-/Drehmomentdaten im Referenzkoordinatensystem zurück
3* @param [out] ft Kraft/Drehmoment, fx, fy, fz, tx, ty, tz
4* @return Fehlercode
5*/
6int FT_GetForceTorqueRCS(byte flag, ref ForceTorque ft);
12.12. Rohdaten des Kraftsensors (Kraft/Drehmoment) abrufen
1/**
2* @brief Gibt die rohen Kraft-/Drehmomentdaten des Kraftsensors zurück
3* @param [out] ft Kraft/Drehmoment, fx, fy, fz, tx, ty, tz
4* @return Fehlercode
5*/
6int FT_GetForceTorqueOrigin(byte flag, ref ForceTorque ft);
12.13. Codebeispiel für Kraftsensor-Konfiguration und automatische Nullpunktkorrektur
1private void button54_Click(object sender, EventArgs e)
2{
3 int company = 24;
4 int device = 0;
5 int softversion = 0;
6 int bus = 1;
7 int index = 1;
8
9 robot.FT_SetConfig(company, device, softversion, bus);
10 Thread.Sleep(1000);
11 robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
12 Console.WriteLine($"FT config:{company},{device},{softversion},{bus}");
13 Thread.Sleep(1000);
14
15 robot.FT_Activate(0);
16 Thread.Sleep(1000);
17 robot.FT_Activate(1);
18 Thread.Sleep(1000);
19
20 Thread.Sleep(1000);
21 robot.FT_SetZero(0);
22 Thread.Sleep(1000);
23
24 ForceTorque ft = new ForceTorque(0, 0, 0, 0, 0, 0);
25 robot.FT_GetForceTorqueOrigin(0, ref ft);
26 Console.WriteLine($"ft origin:{ft.fx},{ft.fy},{ft.fz},{ft.tx},{ft.ty},{ft.tz}");
27 robot.FT_SetZero(1);
28 Thread.Sleep(1000);
29
30 DescPose ftCoord = new DescPose(0, 0, 0, 0, 0, 0);
31 robot.FT_SetRCS(0, ftCoord);
32
33 robot.SetForceSensorPayLoad(0.824);
34 robot.SetForceSensorPayLoadCog(0.778, 2.554, 48.765);
35 double weight = 0;
36 double x = 0, y = 0, z = 0;
37 robot.GetForceSensorPayLoad(ref weight);
38 robot.GetForceSensorPayLoadCog(ref x, ref y, ref z);
39 Console.WriteLine($"the FT load is {weight}, {x} {y} {z}");
40
41 robot.SetForceSensorPayLoad(0);
42 robot.SetForceSensorPayLoadCog(0, 0, 0);
43
44 double computeWeight = 0;
45 DescTran tran = new DescTran(0, 0, 0);
46 robot.ForceSensorAutoComputeLoad(ref weight, ref tran);
47 Console.WriteLine($"the result is weight {weight} pos is {tran.x} {tran.y} {tran.z}");
48
49}
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* @param [out] weight Nutzlastgewicht, Einheit kg
4* @return Fehlercode
5*/
6int FT_PdIdenCompute(ref double weight);
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(ref DescTran cog);
12.18. Codebeispiel für Kraftsensor-Nutzlastidentifikation
1private void btnFTPdCog_Click(object sender, EventArgs e)
2{
3 int company = 24, device = 0, softversion = 0, bus = 1;
4
5 robot.FT_SetConfig(company, device, softversion, bus);
6 Thread.Sleep(1000);
7 robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
8 Console.WriteLine($"FT config: {company}, {device}, {softversion}, {bus}");
9 Thread.Sleep(1000);
10
11 robot.FT_Activate(0);
12 Thread.Sleep(1000);
13 robot.FT_Activate(1);
14 Thread.Sleep(1000);
15
16 Thread.Sleep(1000);
17 robot.FT_SetZero(0);
18 Thread.Sleep(1000);
19
20 ForceTorque ft = new ForceTorque(0,0,0,0,0,0);
21 robot.FT_GetForceTorqueOrigin(0, ref ft);
22 Console.WriteLine($"ft origin: {ft.fx}, {ft.fy}, {ft.fz}, {ft.tx}, {ft.ty}, {ft.tz}");
23 robot.FT_SetZero(1);
24 Thread.Sleep(1000);
25
26 DescPose tcoord = new DescPose(0, 0, 35.0, 0, 0, 0);
27 robot.SetToolCoord(10, tcoord, 1, 0, 0, 0);
28
29 robot.FT_PdIdenRecord(10);
30 Thread.Sleep(1000);
31
32 double weight = 0.0f;
33 robot.FT_PdIdenCompute(ref weight);
34 Console.WriteLine($"payload weight: {weight}");
35
36 DescPose desc_p1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
37 DescPose desc_p2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
38 DescPose desc_p3 = new DescPose(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
39
40 robot.MoveCart( desc_p1, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
41 Thread.Sleep(1000);
42 robot.FT_PdCogIdenRecord(10, 1);
43 robot.MoveCart( desc_p2, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
44 Thread.Sleep(1000);
45 robot.FT_PdCogIdenRecord(10, 2);
46 robot.MoveCart( desc_p3, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
47 Thread.Sleep(1000);
48 robot.FT_PdCogIdenRecord(10, 3);
49
50 DescTran cog = new DescTran(0,0,0);
51 robot.FT_PdCogIdenCompute(ref cog);
52 Console.WriteLine($"cog: {cog.x}, {cog.y}, {cog.z}");
53}
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
6* @param [in] ft Kollisionskraft/-drehmoment, fx, fy, fz, tx, ty, tz
7* @param [in] max_threshold Maximalschwelle
8* @param [in] min_threshold Minimalschwelle
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, int[] select, ForceTorque ft, double[] max_threshold, double[] min_threshold);
12.20. Codebeispiel für Kollisionsschutz
1private void btnFTGuard_Click(object sender, EventArgs e)
2{
3 int company = 24, device = 0, softversion = 0, bus = 1;
4
5 robot.FT_SetConfig(company, device, softversion, bus);
6 Thread.Sleep(1000);
7 robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
8 Console.WriteLine($"FT config: {company}, {device}, {softversion}, {bus}");
9 Thread.Sleep(1000);
10
11 robot.FT_Activate(0);
12 Thread.Sleep(1000);
13 robot.FT_Activate(1);
14 Thread.Sleep(1000);
15
16 Thread.Sleep(1000);
17 robot.FT_SetZero(0);
18 Thread.Sleep(1000);
19
20 byte sensor_id = 1;
21 int[] select = { 1, 1, 1, 1, 1, 1 };
22 double[] max_threshold = { 10.0f, 10.0f, 10.0f, 10.0f, 10.0f, 10.0f };
23 double[] min_threshold = { 5.0f, 5.0f, 5.0f, 5.0f, 5.0f, 5.0f };
24
25 ForceTorque ft = new ForceTorque();
26 DescPose desc_p1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
27 DescPose desc_p2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
28 DescPose desc_p3 = new DescPose(-327.622, 402.230, 320.402, -178.067, 2.127, -46.207);
29
30 robot.FT_Guard(1, sensor_id, select, ft, max_threshold, min_threshold);
31 robot.MoveCart( desc_p1, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
32 robot.MoveCart( desc_p2, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
33 robot.MoveCart( desc_p3, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
34
35 robot.FT_Guard(0, sensor_id, select, ft, max_threshold, min_threshold);
36}
12.21. Konstantkraftregelung
Neu in Version C#SDK-V1.1.9: Web-3.8.7
1/**
2* @brief Konstantkraftregelung
3* @param [in] flag 0-Konstantkraftregelung deaktivieren, 1-Konstantkraftregelung aktivieren
4* @param [in] sensor_id Kraftsensor-Nummer
5* @param [in] select Auswahl der sechs Freiheitsgrade für die Erkennung, 0-nicht erkennen, 1-erkennen
6* @param [in] ft Kraft-/Drehmoment-Sollwerte, fx, fy, fz, tx, ty, tz
7* @param [in] ft_pid Kraft-PID-Parameter, Drehmoment-PID-Parameter
8* @param [in] adj_sign Adaptive Start/Stopp-Steuerung, 0-deaktivieren, 1-aktivieren
9* @param [in] ILC_sign ILC Start/Stopp, 0-stopp, 1-Training, 2-Praxis
10* @param [in] max_dis Maximale Anpassungsstrecke, Einheit mm
11* @param [in] max_ang Maximaler Anpassungswinkel, Einheit deg
12* @param [in] M rx, ry Massenparameter [0.1-10], Standard 2
13* @param [in] B rx, ry Dämpfungsparameter [0.1-50], Standard 8
14* @param [in] threshold rx, ry Startschwelle [0-10], Standard 0.2
15* @param [in] adjustCoeff rx, ry Drehmoment-Einstellkoeffizient [0-1], Standard 1
16* @param [in] polishRadio Schleifradius, Einheit mm
17* @param [in] filter_Sign Filteraktivierungsflag 0-aus; 1-ein, Standard aus
18* @param [in] posAdapt_sign Pose-Anpassungsaktivierungsflag 0-aus; 1-ein, Standard aus
19* @param [in] isNoBlock Blockierungsflag, 0-blockierend; 1-nicht blockierend
20* @return Fehlercode
21*/
22public int FT_Control(byte flag, int sensor_id, byte[] select, ForceTorque ft, float[] ft_pid, byte adj_sign, byte ILC_sign, float max_dis, float 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
Neu in Version C#SDK-V1.1.9: Web-3.8.7
1public void TestFTControlWithAdjustCoeff()
2{
3 int rtn;
4 int sensor_id = 10;
5 byte[] select = new byte[6] { 0, 0, 1, 0, 0, 0 };
6 float[] ft_pid = new float[6] { 0.0008f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
7 byte adj_sign = 0;
8 byte ILC_sign = 0;
9 float max_dis = 1000.0f;
10 float max_ang = 20.0f;
11 ForceTorque ft = new ForceTorque();
12 ft.fz = -10.0f;
13 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
14 JointPos j1 = new JointPos(80.765f, -98.795f, 106.548f, -97.734f, -89.999f, 94.842f);
15 JointPos j2 = new JointPos(43.067f, -84.429f, 92.620f, -98.175f, -90.011f, 57.144f);
16 DescPose desc_p1 = new DescPose(5.009f, -547.463f, 262.053f, -179.999f, -0.019f, 75.923f);
17 DescPose desc_p2 = new DescPose(-347.966f, -547.463f, 262.048f, -180.000f, -0.019f, 75.923f);
18 DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
19 double[] M = new double[2] { 2.0, 2.0 };
20 double[] B = new double[2] { 15.0, 15.0 };
21 double[] threshold = new double[2] { 1.0, 1.0 };
22 double[] adjustCoeff = new double[2] { 1.0, 0.8 };
23 double polishRadio = 0.0;
24 int filter_Sign = 0;
25 int posAdapt_sign = 1;
26 int isNoBlock = 0;
27 while (true)
28 {
29 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);
30 Console.WriteLine($"FT_Control start rtn is {rtn}");
31 robot.MoveL(j1, desc_p1, 1, 0, 100, 100, 100, -1, 0, epos, 0, 0, offset_pos, 0, 0, 10);
32 robot.MoveL(j2, desc_p2, 1, 0, 100, 100, 100, -1, 0, epos, 0, 0, offset_pos, 0, 0, 10);
33 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);
34 Console.WriteLine($"FT_Control end rtn is {rtn}");
35 }
36}
12.23. Rotationseinfügung
1/**
2* @brief Rotationseinfügung
3* @param [in] rcs Referenzkoordinatensystem, 0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem
4* @param [in] angVelRot Rotationswinkelgeschwindigkeit, Einheit deg/s
5* @param [in] ft Kraft-/Drehmomentschwelle, fx, fy, fz, tx, ty, tz, Bereich [0~100]
6* @param [in] max_angle Maximaler Rotationswinkel, Einheit deg
7* @param [in] orn Kraft-/Drehmomentrichtung, 1-entlang der z-Achse, 2-um die z-Achse
8* @param [in] max_angAcc Maximale Rotationsbeschleunigung, Einheit deg/s^2, derzeit nicht verwendet, Standard 0
9* @param [in] rotorn Rotationsrichtung, 1-im Uhrzeigersinn, 2-gegen Uhrzeigersinn
10* @param [in] 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. Lineare Einführung
1/**
2* @brief Lineare Einführung
3* @param [in] rcs Referenzkoordinatensystem, 0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem
4* @param [in] ft Kraft-/Drehmomentschwelle, fx,fy,fz,tx,ty,tz, Bereich [0~100]
5* @param [in] lin_v Lineargeschwindigkeit, Einheit mm/s
6* @param [in] lin_a Linearbeschleunigung, Einheit mm/s^2, vorübergehend nicht verwendet
7* @param [in] max_dis Maximale Einführungsdistanz, Einheit mm
8* @param [in] linorn Einführungsrichtung, 0-negative Richtung, 1-positive Richtung
9* @return Fehlercode
10*/
11public int FT_LinInsertion(int rcs, float ft, float lin_v, float lin_a, float max_dis, byte linorn)
12.25. Codebeispiel für Kraftsensor-Rotationseinführung
1public void TestRotInsert()
2{
3 ROBOT_STATE_PKG pkg = new ROBOT_STATE_PKG();
4 int rtn;
5
6 float forceInsertion = 5.0f; // Kraft- oder Drehmomentschwelle (0~100), Einheit N oder Nm
7 int angleMax = 300; // Maximaler Rotationswinkel, Einheit °
8 byte orn = 1; // Kraftrichtung, 1-fz, 2-mz
9 float angAccmax = 0; // Maximale Rotationswinkelbeschleunigung, Einheit °/s^2, vorübergehend nicht verwendet
10 byte status = 1; // Aktivierungsflag für Konstantkraftregelung, 0-aus, 1-ein
11 int sensor_num = 11; // Kraftsensornummer
12 float[] gain = { 0.0001f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f }; // Maximale Schwelle
13 byte adj_sign = 0; // Adaptiver Start-/Stoppstatus, 0-aus, 1-ein
14 byte ILC_sign = 0; // ILC-Regelungs-Start-/Stoppstatus, 0-Stopp, 1-Training, 2-Betrieb
15 float max_dis = 1000.0f; // Maximale Anpassungsdistanz
16 float max_ang = 20.0f; // Maximaler Anpassungswinkel
17 ForceTorque ft = new ForceTorque();
18 int rcs = 0; // Referenzkoordinatensystem, 0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem
19 float angVelRot = 1.0f; // Rotationswinkelgeschwindigkeit, Einheit °/s
20 byte rotorn = 1; // Rotationsrichtung, 1-im Uhrzeigersinn, 2-gegen Uhrzeigersinn
21 JointPos j1 = new JointPos(100.968, -108.678, 126.166, -106.630, -93.253, 19.584);
22 DescPose desc_p1 = new DescPose(159.473, -316.570, 334.560, -179.718, -3.352, 171.400);
23 ExaxisPos epos = new ExaxisPos(0.0f, 0.0f, 0.0f, 0.0f);
24 DescPose offset_pos = new DescPose(0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f);
25
26 robot.MoveL(j1, desc_p1, 2, 0, 100.0f, 180.0f, 100.0f, -1.0f, 0, epos, (byte)0, (byte)1, offset_pos);
27
28 byte[] select3 = { 0, 0, 1, 0, 0, 0 };
29 ft.fz = -5.0f;
30 gain[0] = 0.0001f;
31 status = 1;
32 robot.FT_Control(status, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
33 rtn = robot.FT_LinInsertion(rcs, 10, 1, 1, 100, 1);
34 Console.WriteLine("FT_LinInsertion rtn is " + rtn);
35 robot.FT_Control(0, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
36
37 ft.fz = -30.0f;
38 robot.FT_Control(1, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
39 rtn = robot.FT_RotInsertion(rcs, angVelRot, forceInsertion, angleMax, orn, angAccmax, rotorn, 0);
40 Console.WriteLine("FT_RotInsertion rtn is " + rtn);
41 robot.FT_Control(0, sensor_num, select3, ft, gain, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
42
43 rtn = robot.FT_LinInsertion(0, 40, 3, 0, 100, 1);
44 Console.WriteLine("FT_LinInsertion retract rtn is " + rtn);
45
46 Thread.Sleep(1000);
47 robot.GetRobotRealTimeState(ref pkg);
48 Console.WriteLine("robot errcode " + pkg.main_code + " " + pkg.sub_code);
49}
12.26. Codebeispiel für Rotationseinfügung mit Kraftsensor
1public void TestMove()
2{
3 int rtn;
4 JointPos j1 = new JointPos(-11.904f, -99.669f, 117.473f, -108.616f, -91.726f, 74.256f);
5 JointPos j2 = new JointPos(-45.615f, -106.172f, 124.296f, -107.151f, -91.282f, 74.255f);
6 JointPos j3 = new JointPos(-29.777f, -84.536f, 109.275f, -114.075f, -86.655f, 74.257f);
7 JointPos j4 = new JointPos(-31.154f, -95.317f, 94.276f, -88.079f, -89.740f, 74.256f);
8 DescPose desc_pos1 = new DescPose(-419.524f, -13.000f, 351.569f, -178.118f, 0.314f, 3.833f);
9 DescPose desc_pos2 = new DescPose(-321.222f, 185.189f, 335.520f, -179.030f, -1.284f, -29.869f);
10 DescPose desc_pos3 = new DescPose(-487.434f, 154.362f, 308.576f, 176.600f, 0.268f, -14.061f);
11 DescPose desc_pos4 = new DescPose(-443.165f, 147.881f, 480.951f, 179.511f, -0.775f, -15.409f);
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 float vel = 100.0f;
17 float acc = 100.0f;
18 float ovl = 100.0f;
19 float oacc = 100.0f;
20 float blendT = 0.0f;
21 float blendR = 0.0f;
22 byte flag = 0;
23 byte 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 Console.WriteLine($"movej errcode:{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 Console.WriteLine($"movel errcode:{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 Console.WriteLine($"movec errcode:{rtn}");
33 rtn = robot.MoveJ(j2, desc_pos2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
34 Console.WriteLine($"movej errcode:{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 Console.WriteLine($"circle errcode:{rtn}");
37 rtn = robot.MoveCart(desc_pos4, tool, user, vel, acc, ovl, blendT, -1);
38 Console.WriteLine($"MoveCart errcode:{rtn}");
39 rtn = robot.MoveJ(j1, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
40 Console.WriteLine($"movej errcode:{rtn}");
41 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, epos, search, flag, offset_pos, -1, velAccMode);
42 Console.WriteLine($"movel errcode:{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 Console.WriteLine($"movec errcode:{rtn}");
45 rtn = robot.MoveJ(j2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
46 Console.WriteLine($"movej errcode:{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 Console.WriteLine($"circle errcode:{rtn}");
49}
12.27. 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(float p, float force);
12.28. Nachgiebigkeitsregelung stoppen
1/**
2* @brief Nachgiebigkeitsregelung stoppen
3* @return Fehlercode
4*/
5int FT_ComplianceStop();
12.29. Codebeispiel für Nachgiebigkeitsregelung
1private void btnComplience_Click(object sender, EventArgs e)
2{
3 int company = 24, device = 0, softversion = 0, bus = 1;
4 robot.FT_SetConfig(company, device, softversion, bus);
5 Thread.Sleep(1000);
6 robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
7 Console.WriteLine($"FT config: {company}, {device}, {softversion}, {bus}");
8 Thread.Sleep(1000);
9
10 robot.FT_Activate(0);
11 Thread.Sleep(1000);
12 robot.FT_Activate(1);
13 Thread.Sleep(1000);
14
15 robot.FT_SetZero(0);
16 Thread.Sleep(1000);
17
18 byte flag = 1;
19 int sensor_id = 1;
20 int[] select = { 1, 1, 1, 0, 0, 0 };
21 double[] ft_pid = { 0.0005f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
22 byte adj_sign = 0, ILC_sign = 0;
23 float max_dis = 100.0f, max_ang = 0.0f;
24
25 ForceTorque ft = new ForceTorque { fx = -10.0, fy = -10.0, fz = -10.0 };
26 DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
27 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
28
29 JointPos j1 = new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
30 JointPos j2 = new JointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
31 DescPose desc_p1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
32 DescPose desc_p2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
33
34 robot.FT_Control(flag, (byte)sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
35 float p = 0.00005f;
36 float force = 30.0f;
37 int rtn = robot.FT_ComplianceStart(p, force);
38 Console.WriteLine($"FT_ComplianceStart rtn is {rtn}");
39
40 int count = 5;
41 while (count-- > 0)
42 {
43 robot.MoveL(j1, desc_p1, 0, 0, 100.0f, 180.0f, 100.0f, -1.0f, epos, 0, 1, offset_pos);
44 robot.MoveL(j2, desc_p2, 0, 0, 100.0f, 180.0f, 100.0f, -1.0f, epos, 0, 0, offset_pos);
45 }
46
47 robot.FT_ComplianceStop();
48 Console.WriteLine($"FT_ComplianceStop rtn is {rtn}");
49
50 flag = 0;
51 robot.FT_Control(flag, (byte)sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
52}
12.30. Nutzlastidentifikation initialisieren
Neu in Version C#SDK-v1.0.4.
1/**
2* @brief Initialisiert die Nutzlastidentifikation (Dynamikfilter)
3* @return Fehlercode
4*/
5int LoadIdentifyDynFilterInit();
12.31. Nutzlastidentifikations-Variablen initialisieren
Neu in Version C#SDK-v1.0.4.
1/**
2* @brief Initialisiert die Variablen der Nutzlastidentifikation
3* @return Fehlercode
4*/
5int LoadIdentifyDynVarInit();
12.32. Hauptprogramm der Nutzlastidentifikation
Neu in Version C#SDK-v1.0.4.
1/**
2* @brief Hauptprogramm der Nutzlastidentifikation
3* @param [in] joint_torque Gelenkdrehmomente
4* @param [in] joint_pos Gelenkpositionen
5* @param [in] t Abtastperiode
6* @return Fehlercode
7*/
8int LoadIdentifyMain(double[] joint_torque, double[] joint_pos, double t);
12.33. Ergebnis der Nutzlastidentifikation abrufen
Neu in Version C#SDK-v1.0.4.
1/**
2* @brief Gibt das Ergebnis der Nutzlastidentifikation zurück
3* @param [in] gain Koeffizienten für Gravitationsterme double[6], Koeffizienten für Zentrifugalterme double[6]
4* @param [out] weight Nutzlastgewicht
5* @param [out] cog Nutzlastschwerpunkt
6* @return Fehlercode
7*/
8int LoadIdentifyGetResult(double[] gain, ref double weight, ref DescTran cog);
12.34. Codebeispiel für Roboter-Nutzlastidentifikation
1private void button74_Click(object sender, EventArgs e)
2{
3 int rtn;
4 int retval = 0;
5
6 retval = robot.LoadIdentifyDynFilterInit();
7 Console.WriteLine("LoadIdentifyDynFilterInit retval is: " + retval);
8
9 retval = robot.LoadIdentifyDynVarInit();
10 Console.WriteLine("LoadIdentifyDynVarInit retval is: " + retval);
11
12 JointPos posJ = new JointPos(0,0,0,0,0,0);
13 DescPose posDec = new DescPose(0, 0, 0, 0, 0, 0);
14 double[] joint_toq = new double[6] { 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
15 robot.GetActualJointPosDegree(0, ref posJ);
16 posJ.jPos[1] = posJ.jPos[1] + 10;
17 robot.GetJointTorques(0, joint_toq);
18 joint_toq[1] = joint_toq[1] + 2;
19
20 double[] tmpTorque = new double[6] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
21 for (int i = 0; i < 6; i++)
22 {
23 tmpTorque[i] = joint_toq[i];
24 }
25
26 retval = robot.LoadIdentifyMain(tmpTorque, posJ.jPos, 1);
27 Console.WriteLine("LoadIdentifyMain retval is: " + retval);
28
29 double[] gain = new double[12] { 0, 0.05, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, 0 };
30 double weight = 0;
31 DescTran load_pos = new DescTran(0, 0, 0);
32 retval = robot.LoadIdentifyGetResult(gain, ref weight, ref load_pos);
33 Console.WriteLine("LoadIdentifyGetResult retval is: {0}; weight is {1} cog is {2} {3} {4}", retval, weight, load_pos.x, load_pos.y, load_pos.z);
34}
12.35. Unterstütztes Ziehen mit Kraftsensor
Neu in Version C#SDK-V1.1.4: Web-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
9* @param [in] B Dämpfungskoeffizienten
10* @param [in] K Steifigkeitskoeffizienten
11* @param [in] F Sechsdimensionale Kraftschwellen für das Ziehen
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, double[] M, double[] B, double[] K, double[] F, double Fmax, double Vmax);
12.36. Schaltzustand des kraftunterstützten Ziehens abrufen
1/**
2* @brief Gibt den Schaltzustand des kraftunterstützten Ziehens zurück
3* @param [out] dragState Steuerungsstatus des kraftunterstützten Ziehens, 0-deaktiviert; 1-aktiviert
4* @param [out] sixDimensionalDragState Steuerungsstatus des 6-Achsen-kraftunterstützten Ziehens, 0-deaktiviert; 1-aktiviert
5* @return Fehlercode
6*/
7int GetForceAndTorqueDragState(ref int dragState, ref int sixDimensionalDragState);
12.37. 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.38. Codebeispiel für kraftunterstütztes Ziehen mit Kraftsensor
1private void button61_Click(object sender, EventArgs e)
2{
3 robot.SetForceSensorDragAutoFlag(1);
4 double[] M = { 15.0, 15.0, 15.0, 0.5, 0.5, 0.1 };
5 double[] B = { 150.0, 150.0, 150.0, 5.0, 5.0, 1.0 };
6 double[] K = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
7 double[] F = { 10.0, 10.0, 10.0, 1.0, 1.0, 1.0 };
8
9 robot.EndForceDragControl(1, 0, 0, 0, M, B, K, F, 50, 100);
10 robot.WaitMs(5000);
11
12 int dragState = 0;
13 int sixDimensionalDragState = 0;
14 robot.GetForceAndTorqueDragState(ref dragState, ref sixDimensionalDragState);
15 Console.WriteLine($"the drag state is {dragState} {sixDimensionalDragState}");
16
17 robot.EndForceDragControl(0, 0, 0, 0, M, B, K, F, 50, 100);
18}
12.39. 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] lamdeDain Zugverstärkung
6* @param [in] KGain Steifigkeitsverstärkung
7* @param [in] BGain Dämpfungsverstärkung
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, double[] lamdeDain, double[] KGain, double[] BGain, double dragMaxTcpVel, double dragMaxTcpOriVel);
12.40. Codebeispiel für kraftunterstütztes Ziehen mit Kraftsensor
1private void button62_Click(object sender, EventArgs e)
2{
3 robot.DragTeachSwitch(1);
4 double[] lambdaGain = { 3.0, 2.0, 2.0, 2.0, 2.0, 3.0 };
5 double[] kGain = { 0, 0, 0, 0, 0, 0 };
6 double[] bGain = { 150, 150, 150, 5.0, 5.0, 1.0 };
7 int rtn = robot.ForceAndJointImpedanceStartStop(1, 0, lambdaGain, kGain, bGain, 1000, 180);
8 Console.WriteLine($"ForceAndJointImpedanceStartStop rtn is {rtn}");
9 Thread.Sleep(5000);
10 robot.DragTeachSwitch(0);
11 rtn = robot.ForceAndJointImpedanceStartStop(0, 0, lambdaGain, kGain, bGain, 1000, 180);
12 Console.WriteLine($"ForceAndJointImpedanceStartStop rtn is {rtn}");
13}
12.41. Impedanzregelung Start/Stopp
Neu in Version C#SDK-V1.1.8: Web-3.8.6
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)
6* @param [in] m Massenparameter
7* @param [in] b Dämpfungsparameter
8* @param [in] k Steifigkeitsparameter
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.42. Codebeispiel für Roboter-Impedanzregelung Start/Stopp
Neu in Version C#SDK-V1.1.8: Web-3.8.6
1public void TestImpedanceControl()
2{
3 int[] ctrl = new int[20];
4 int state;
5 int pressValue;
6 int error;
7 int rtn;
8 JointPos j1 = new JointPos(102.622, -135.990, 120.769, -73.950, -90.848, 35.507);
9 JointPos j2 = new JointPos(93.674, -80.062, 82.947, -92.199, -90.967, 26.559);
10 DescPose desc_pos1 = new DescPose(136.552, -149.799, 449.532, 179.817, -1.172, 157.123);
11 DescPose desc_pos2 = new DescPose(136.540, -561.048, 449.542, 179.819, -1.172, 157.122);
12
13 DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
14 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
15
16 int tool = 0;
17 int user = 0;
18 float vel = 100.0f;
19 float acc = 200.0f;
20 float ovl = 100.0f;
21 float blendT = -1.0f;
22 float blendR = -1.0f;
23
24 byte flag = 0;
25
26 byte search = 0;
27 robot.SetSpeed(20);
28 int company = 17;
29 int device = 0;
30 int softversion = 0;
31 int bus = 1;
32 robot.FT_SetConfig(company, device, softversion, bus);
33 Thread.Sleep(1000);
34 robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
35 Console.WriteLine($"FT config:{company},{device},{softversion},{bus}");
36 Thread.Sleep(1000);
37
38 robot.FT_Activate(0);
39 Thread.Sleep(1000);
40 robot.FT_Activate(1);
41 Thread.Sleep(1000);
42 Thread.Sleep(1000);
43 robot.FT_SetZero(0);
44 Thread.Sleep(1000);
45 robot.FT_SetZero(1);
46 Thread.Sleep(1000);
47
48 double[] forceThreshold = new double[] { 30, 30, 30, 5, 5, 5 };
49 double[] m = new double[] { 0.1, 0.1, 0.1, 0.02, 0.02, 0.02 };
50 double[] b = new double[] { 1, 1, 1, 0.08, 0.08, 0.08 };
51 double[] k = new double[] { 0, 0, 0, 0, 0, 0 };
52 rtn = robot.ImpedanceControlStartStop(1, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
53 Console.WriteLine($"ImpedanceControlStartStop errcode:{rtn}");
54 rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
55 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
56 rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
57 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
58 rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
59 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1, 0);
60 Console.WriteLine($"movel errcode:{rtn}");
61 robot.ImpedanceControlStartStop(0, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
62}
12.43. Drehmomentkompensationsfunktion und -koeffizienten aktivieren
1/**
2* @brief Aktiviert die Drehmomentkompensationsfunktion und setzt die Koeffizienten
3* @param [in] status Schalter, 0-deaktivieren; 1-aktivieren
4* @param [in] torqueCoeff J1-J6 Drehmomentkompensationskoeffizienten [0-1]
5* @return Fehlercode
6*/
7public int SetCoderCompenParams(int status, double[] torqueCoeff)
12.44. Oberflächenpositionierung
1/**
2* @brief Oberflächenpositionierung
3* @param [in] rcs Referenzkoordinatensystem, 0-Werkzeugkoordinatensystem, 1-Basiskoordinatensystem
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 Such-Lineargeschwindigkeit, Einheit mm/s
7* @param [in] lin_a Such-Linearbeschleunigung, Einheit mm/s^2, vorübergehend nicht verwendet, Standard 0
8* @param [in] max_dis Maximale Suchdistanz, Einheit mm
9* @param [in] ft Kraft-/Drehmomentschwelle für Bewegungsbeendigung, fx,fy,fz,tx,ty,tz
10* @return Fehlercode
11*/
12public int FT_FindSurface(int rcs, byte dir, byte axis, float lin_v, float lin_a, float max_dis, float ft)
12.45. Start der Berechnung der Mittelebene-Position
1/**
2* @brief Start der Berechnung der Mittelebene-Position
3* @return Fehlercode
4*/
5public int FT_CalCenterStart()
12.46. Ende der Berechnung der Mittelebene-Position
1/**
2* @brief Ende der Berechnung der Mittelebene-Position
3* @param [out] pos Mittelebene-Pose
4* @return Fehlercode
5*/
6public int FT_CalCenterEnd(ref DescPose pos)
12.47. Codebeispiel für Oberflächenpositionierung
1private void button59_Click(object sender, EventArgs e)
2{
3 int company = 22;
4 int device = 0;
5 int softversion = 0;
6 int bus = 1;
7
8 robot.FT_SetConfig(company, device, softversion, bus);
9 Thread.Sleep(1000);
10 robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
11 Console.WriteLine("FT config:" + company + "," + device + "," + softversion + "," + bus);
12 Thread.Sleep(1000);
13
14 robot.FT_Activate(0);
15 Thread.Sleep(1000);
16 robot.FT_Activate(1);
17 Thread.Sleep(1000);
18
19 Thread.Sleep(1000);
20 robot.FT_SetZero(0);
21 Thread.Sleep(1000);
22
23 int rcs = 0;
24 byte dir = 1;
25 byte axis = 1;
26 float lin_v = 15.0f;
27 float lin_a = 0.0f;
28 float maxdis = 500.0f;
29 float ft_goal = 2.0f;
30 DescPose desc_pos = new DescPose(-419.524f, -13.000f, 351.569f, -178.118f, 0.314f, 3.833f);
31 DescPose xcenter = new DescPose(0, 0, 0, 0, 0, 0);
32 DescPose ycenter = new DescPose(0, 0, 0, 0, 0, 0);
33
34 ForceTorque ft = new ForceTorque();
35
36 ft.fx = -2.0f;
37
38 robot.MoveCart(desc_pos, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
39
40 robot.FT_CalCenterStart();
41 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
42 robot.MoveCart(desc_pos, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
43 robot.WaitMs(1000);
44
45 dir = 2;
46 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
47 robot.FT_CalCenterEnd(ref xcenter);
48 Console.WriteLine("xcenter:" + xcenter.tran.x + "," + xcenter.tran.y + "," + xcenter.tran.z + "," + xcenter.rpy.rx + "," + xcenter.rpy.ry + "," + xcenter.rpy.rz);
49 robot.MoveCart(xcenter, 1, 0, 60.0f, 50.0f, 50.0f, -1.0f, -1);
50
51 robot.FT_CalCenterStart();
52 dir = 1;
53 axis = 2;
54 lin_v = 6.0f;
55 maxdis = 150.0f;
56 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
57 robot.MoveCart(desc_pos, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
58 robot.WaitMs(1000);
59
60 dir = 2;
61 robot.FT_FindSurface(rcs, dir, axis, lin_v, lin_a, maxdis, ft_goal);
62 robot.FT_CalCenterEnd(ref ycenter);
63 Console.WriteLine("ycenter:" + ycenter.tran.x + "," + ycenter.tran.y + "," + ycenter.tran.z + "," + ycenter.rpy.rx + "," + ycenter.rpy.ry + "," + ycenter.rpy.rz);
64 robot.MoveCart(ycenter, 1, 0, 60.0f, 50.0f, 50.0f, 0.0f, -1);
65
66}
12.48. Echtzeit-Pendeloffset einstellen
1/**
2* @brief Echtzeit-Pendeloffset einstellen
3* @param [in] offset Echtzeit-Offset [mm, °]
4* @return Fehlercode
5*/
6public int SetWeaveOffsetRT(DescPose offset)
12.49. Codebeispiel für Echtzeit-Pendelgeschwindigkeit und -Offset
1public void TestWeaveSpeedAndOffset()
2{
3 Console.WriteLine("============================================================");
4 Console.WriteLine(" Weave Speed and Offset Test");
5 Console.WriteLine("============================================================");
6
7 if (robot == null)
8 {
9 Console.WriteLine("ERROR: Robot not connected!");
10 return;
11 }
12
13 int rtn;
14 ROBOT_STATE_PKG pkg = new ROBOT_STATE_PKG();
15 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
16 DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
17
18 JointPos j1 = new JointPos(5.027, -84.331, -75.139, -103.690, 86.379, 20.794);
19 DescPose d1 = new DescPose(324.752, -83.339, 366.314, -172.321, -0.936, -106.047);
20
21 JointPos j2 = new JointPos(-35.335, -117.598, -57.174, -95.234, 90.001, -19.560);
22 DescPose d2 = new DescPose(324.999, -355.439, 260.000, 179.995, 0.003, -105.775);
23
24 JointPos j3 = new JointPos(59.787, -117.594, -57.183, -95.222, 90.006, 75.562);
25 DescPose d3 = new DescPose(324.998, 355.441, 260.002, 179.995, 0.003, -105.775);
26
27 // ---- Step 1: MoveJ to start point ----
28 Console.WriteLine("\nStep 1: MoveJ to start point");
29 rtn = robot.MoveJ(j1, d1, 1, 0, 100, 100, 50, epos, -1, 0, offset_pos);
30 Console.WriteLine(" MoveJ(j1) rtn={0}", rtn);
31 Thread.Sleep(500);
32
33 // ---- Step 2: MoveJ to weave entry ----
34 Console.WriteLine("\nStep 2: MoveJ to weave entry point");
35 rtn = robot.MoveJ(j2, d2, 1, 0, 100, 100, 50, epos, -1, 0, offset_pos);
36 Console.WriteLine(" MoveJ(j2) rtn={0}", rtn);
37 Thread.Sleep(500);
38
39 // ---- Step 3: WeaveStart, launch weave MoveL thread ----
40 Console.WriteLine("\nStep 3: WeaveStart + MoveL in background thread");
41 robot.WeaveStart(0);
42
43 bool weaveRunning = true;
44 Thread weaveThread = new Thread(() =>
45 {
46 rtn = robot.MoveL(j3, d3, 1, 0, 100, 100, 5, -1, 0, epos, 0, 0, offset_pos, 5, 0, 0, 10);
47 Console.WriteLine(" MoveL(weave) thread finished, rtn={0}", rtn);
48 weaveRunning = false;
49 });
50 weaveThread.IsBackground = true;
51 weaveThread.Start();
52 Thread.Sleep(500); // Wait for motion to start
53
54 // ---- Step 4: Speed test (main thread, weave MoveL in background) ----
55 Console.WriteLine("\nStep 4: SetSpeed test during weaving");
56 int[] speedValues = { 20, 50, 80, 30, 60, 10 };
57 foreach (int speed in speedValues)
58 {
59 if (!weaveRunning) break;
60 rtn = robot.SetSpeedInstant(speed);
61 robot.GetRobotRealTimeState(ref pkg);
62 Console.WriteLine(" SetSpeed({0}) -> rtn={1}, TCP_CmpSpeed={2}", speed, rtn, pkg.target_TCP_CmpSpeed);
63 Thread.Sleep(5000);
64 }
65
66
67 Thread.Sleep(5000);
68 // ---- Step 5: SetWeaveOffsetRT offset test (main thread, weave MoveL in background) ----
69 Console.WriteLine("\nStep 5: SetWeaveOffsetRT test (50 iterations, delta=0.1)");
70 double accumOffset = 0.0;
71 for (int i = 0; i < 50 && weaveRunning; i++)
72 {
73 accumOffset += 0.1;
74 DescPose weaveOffset = new DescPose(0, 0, accumOffset, 0, 0, 0);
75 rtn = robot.SetWeaveOffsetRT(weaveOffset);
76 robot.GetRobotRealTimeState(ref pkg);
77 Console.WriteLine(" [{0}/50] SetWeaveOffsetRT(x={1:F1}) -> rtn={2}, TCP_pos=({3:F2},{4:F2},{5:F2})",
78 i + 1, accumOffset, rtn,
79 pkg.tl_cur_pos[0], pkg.tl_cur_pos[1], pkg.tl_cur_pos[2]);
80 Thread.Sleep(100);
81 }
82
83 // ---- Step 6: Wait for weave MoveL, then WeaveEnd ----
84 Console.WriteLine("\nStep 6: Wait for weave MoveL, then WeaveEnd");
85 weaveThread.Join();
86 robot.WeaveEnd(0);
87 Thread.Sleep(500);
88
89 // ---- Step 7: MoveL back to start ----
90 Console.WriteLine("\nStep 7: MoveL back to start");
91 rtn = robot.MoveL(j1, d1, 1, 0, 100, 100, 50, -1, 0, epos, 0, 0, offset_pos, 50, 0, 0, 10);
92 Console.WriteLine(" MoveL(back) rtn={0}", rtn);
93
94 robot.GetRobotRealTimeState(ref pkg);
95 Console.WriteLine("\n Final robot state: main_code={0}, sub_code={1}", pkg.main_code, pkg.sub_code);
96 Console.WriteLine("============================================================");
97 Console.WriteLine(" Weave Speed and Offset Test Complete");
98 Console.WriteLine("============================================================");
99}