12. Controllo di forza del robot
12.1. Configurazione sensore di forza
1/**
2* @brief Configura sensore di forza
3* @param [in] company Produttore sensore di forza, 17-Kunwei Technology
4* @param [in] device Numero dispositivo, attualmente non utilizzato, default 0
5* @param [in] softvesion Numero versione software, attualmente non utilizzato, default 0
6* @param [in] bus Posizione bus terminale dove è collegato il dispositivo, attualmente non utilizzato, default 0
7* @return Codice di errore
8*/
9int FT_SetConfig(int company, int device, int softvesion, int bus);
12.2. Ottieni la configurazione del sensore di forza
1/**
2* @brief Ottieni la configurazione del sensore di forza
3* @param [out] deviceID Numero sensore di forza
4* @param [out] company Produttore sensore di forza, 17-Kunwei Technology, 19-CASC 11th Academy, 20-Sensore ATI, 21-Zhongke Midian, 22-Weihang Minxin
5* @param [out] device Numero dispositivo, Kunwei(0-KWR75B), CASC 11th Academy(0-MCS6A-200-4), ATI (0-AXIA80 -M8), Zhongke Midian(0-MST2010), Weihang Minxin(0-WHC6L-YB-10A)
6* @param [out] softvesion Numero versione software, attualmente non utilizzato, default 0
7* @return Codice di errore
8*/
9int FT_GetConfig(ref int deviceID, ref int company, ref int device, ref int softvesion);
12.3. Attivazione sensore di forza
1/**
2* @brief Attivazione sensore di forza
3* @param [in] act 0-Ripristina, 1-Attiva
4* @return Codice di errore
5*/
6int FT_Activate(byte act);
12.4. Azzeramento sensore di forza
1/**
2* @brief Azzeramento sensore di forza
3* @param [in] act 0-Rimuovi offset zero, 1-Correggi offset zero
4* @return Codice di errore
5*/
6int FT_SetZero(byte act);
12.5. Imposta il sistema di coordinate di riferimento del sensore di forza
1/**
2* @brief Imposta il sistema di coordinate di riferimento del sensore di forza
3* @param [in] ref 0-Sistema di coordinate utensile, 1-Sistema di coordinate base
4* @return Codice di errore
5*/
6int FT_SetRCS(byte type);
12.6. Imposta il peso del carico sotto il sensore di forza
1/**
2* @brief Imposta il peso del carico sotto il sensore di forza
3* @param [in] weight Peso del carico kg
4* @return Codice di errore
5*/
6int SetForceSensorPayLoad(double weight);
12.7. Imposta il centro di massa del carico sotto il sensore di forza
1/**
2* @brief Imposta il centro di massa del carico sotto il sensore di forza
3* @param [in] x Centro di massa carico x mm
4* @param [in] y Centro di massa carico y mm
5* @param [in] z Centro di massa carico z mm
6* @return Codice di errore
7*/
8int SetForceSensorPayLoadCog(double x, double y, double z);
12.8. Ottieni il peso del carico sotto il sensore di forza
1/**
2* @brief Ottieni il peso del carico sotto il sensore di forza
3* @param [in] weight Peso del carico kg
4* @return Codice di errore
5*/
6int GetForceSensorPayLoad(ref double weight);
12.9. Ottieni il centro di massa del carico sotto il sensore di forza
1/**
2* @brief Ottieni il centro di massa del carico sotto il sensore di forza
3* @param [out] x Centro di massa carico x mm
4* @param [out] y Centro di massa carico y mm
5* @param [out] z Centro di massa carico z mm
6* @return Codice di errore
7*/
8int GetForceSensorPayLoadCog(ref double x, ref double y, ref double z);
12.10. Azzeramento automatico sensore di forza
1/**
2* @brief Azzeramento automatico sensore di forza
3* @param [out] weight Massa sensore kg
4* @param [out] pos Centro di massa sensore mm
5* @return Codice di errore
6*/
7int ForceSensorAutoComputeLoad(ref double weight, ref DescTran pos);
12.11. Ottieni i dati forza/coppia nel sistema di coordinate di riferimento
1/**
2* @brief Ottieni i dati forza/coppia nel sistema di coordinate di riferimento
3* @param [out] ft Forza/Coppia, fx,fy,fz,tx,ty,tz
4* @return Codice di errore
5*/
6int FT_GetForceTorqueRCS(byte flag, ref ForceTorque ft);
12.12. Ottieni i dati forza/coppia originali del sensore di forza
1/**
2* @brief Ottieni i dati forza/coppia originali del sensore di forza
3* @param [out] ft Forza/Coppia, fx,fy,fz,tx,ty,tz
4* @return Codice di errore
5*/
6int FT_GetForceTorqueOrigin(byte flag, ref ForceTorque ft);
12.13. Esempio di codice per la configurazione e l’azzeramento automatico del sensore di forza
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. Registra identificazione peso carico
1/**
2* @brief Registra identificazione peso carico
3* @param [in] id Numero sistema di coordinate sensore, intervallo[1~14]
4* @return Codice di errore
5*/
6int FT_PdIdenRecord(int id);
12.15. Calcola identificazione peso carico
1/**
2* @brief Calcola identificazione peso carico
3* @param [out] weight Peso carico, unità kg
4* @return Codice di errore
5*/
6int FT_PdIdenCompute(ref double weight);
12.16. Registra identificazione centro di massa carico
1/**
2* @brief Registra identificazione centro di massa carico
3* @param [in] id Numero sistema di coordinate sensore, intervallo[1~14]
4* @param [in] index Numero punto, intervallo[1~3]
5* @return Codice di errore
6*/
7int FT_PdCogIdenRecord(int id, int index);
12.17. Calcola identificazione centro di massa carico
1/**
2* @brief Calcola identificazione centro di massa carico
3* @param [out] cog Centro di massa carico, unità mm
4* @return Codice di errore
5*/
6int FT_PdCogIdenCompute(ref DescTran cog);
12.18. Esempio di codice per l’identificazione del carico del sensore di forza
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. Guardia collisione
1/**
2* @brief Guardia collisione
3* @param [in] flag 0-Disabilita guardia collisione, 1-Abilità guardia collisione
4* @param [in] sensor_id Numero sensore di forza
5* @param [in] select Seleziona quali sei gradi di libertà controllare per collisione, 0-Non controllare, 1-Controlla
6* @param [in] ft Forza/coppia collisione, fx,fy,fz,tx,ty,tz
7* @param [in] max_threshold Soglia massima
8* @param [in] min_threshold Soglia minima
9* @note Intervallo di rilevamento forza/coppia: (ft-min_threshold, ft+max_threshold)
10* @return Codice di errore
11*/
12int FT_Guard(int flag, int sensor_id, int[] select, ForceTorque ft, double[] max_threshold, double[] min_threshold);
12.20. Esempio di codice per la guardia collisione
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. Controllo forza costante
Nuovo nella versione C#SDK-V1.1.9: Web-3.8.7
1/**
2* @brief Controllo forza costante
3* @param [in] flag 0-Disabilita controllo forza costante, 1-Abilità controllo forza costante
4* @param [in] sensor_id Numero sensore di forza
5* @param [in] select Seleziona quali sei gradi di libertà controllare, 0-Non controllare, 1-Controlla
6* @param [in] ft Forza/coppia target, fx,fy,fz,tx,ty,tz
7* @param [in] ft_pid Parametri PID forza, parametri PID coppia
8* @param [in] adj_sign Controllo avvio/arresto adattativo, 0-Disabilita, 1-Abilita
9* @param [in] ILC_sign Controllo avvio/arresto ILC, 0-Arresta, 1-Addestra, 2-Operazione reale
10* @param [in] max_dis Massima distanza di regolazione, unità mm
11* @param [in] max_ang Massimo angolo di regolazione, unità deg
12* @param [in] filter_Sign Flag abilitazione filtro 0-Disabilitato; 1-Abilitato, default disabilitato
13* @param [in] posAdapt_sign Flag abilitazione adattamento postura 0-Disabilitato; 1-Abilitato, default disabilitato
14* @param [in] isNoBlock Flag bloccante, 0-Bloccante; 1-Non bloccante
15* @return Codice di errore
16*/
17public 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, int filter_Sign = 0, int posAdapt_sign = 0, int isNoBlock = 0);
12.22. Esempio di codice per il controllo forza costante con smorzamento
Nuovo nella versione C#SDK-V1.1.9: Web-3.8.7
1public void TestFTControlWithDamping()
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 = 100.0f;
10 float max_ang = 20.0f;
11 ForceTorque ft = new ForceTorque();
12 ft.fz = -10.0;
13 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
14 JointPos j1 = new JointPos(-118.985, -86.882, -118.139, -65.019, 90.002, 54.951);
15 JointPos j2 = new JointPos(-77.055, -77.218, -126.219, -66.591, 90.028, 96.881);
16 DescPose desc_p1 = new DescPose(-300.856, -332.618, 309.240, 179.976, -0.031, 96.065);
17 DescPose desc_p2 = new DescPose(-16.399, -383.760, 309.312, 179.975, -0.031, 96.064);
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] { 8.0, 8.0 };
21 double polishRadio = 0.0;
22 int filter_Sign = 0;
23 int posAdapt_sign = 1;
24 int isNoBlock = 0;
25 DescPose ftCoord = new DescPose();
26 robot.FT_SetRCS(2, ftCoord);
27 rtn = robot.FT_Control(1, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, polishRadio, filter_Sign, posAdapt_sign, isNoBlock);
28 Console.WriteLine($"FT_Control start rtn is {rtn}");
29 int tool = 0;
30 int user = 0;
31 float vel = 100.0f;
32 float acc = 100.0f;
33 float ovl = 20.0f;
34 float blendT = -1.0f;
35 byte offset_flag = 0;
36 rtn = robot.MoveL(j1, desc_p1, tool, user, vel, acc, ovl, blendT, epos, offset_flag, 0, offset_pos, 0, 0, 10);
37 rtn = robot.MoveL(j2, desc_p2, tool, user, vel, acc, ovl, blendT, epos, offset_flag, 0, offset_pos, 0, 0, 10);
38 rtn = robot.FT_Control(0, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, polishRadio, filter_Sign, posAdapt_sign, isNoBlock);
39 Console.WriteLine($"FT_Control end rtn is {rtn}");
40 robot.CloseRPC();
41}
12.23. Avvia controllo compliance
1/**
2* @brief Avvia controllo compliance
3* @param [in] p Coefficiente di regolazione posizione o coefficiente compliance
4* @param [in] force Soglia forza per attivare compliance, unità N
5* @return Codice di errore
6*/
7int FT_ComplianceStart(float p, float force);
12.24. Arresta controllo compliance
1/**
2* @brief Arresta controllo compliance
3* @return Codice di errore
4*/
5int FT_ComplianceStop();
12.25. Esempio di codice per il controllo compliance
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.26. Inizializza identificazione carico
Nuovo nella versione C#SDK-v1.0.4.
1/**
2* @brief Inizializza identificazione carico
3* @return Codice di errore
4*/
5int LoadIdentifyDynFilterInit();
12.27. Inizializza variabili identificazione carico
Nuovo nella versione C#SDK-v1.0.4.
1/**
2* @brief Inizializza variabili identificazione carico
3* @return Codice di errore
4*/
5int LoadIdentifyDynVarInit();
12.28. Programma principale identificazione carico
Nuovo nella versione C#SDK-v1.0.4.
1/**
2* @brief Programma principale identificazione carico
3* @param [in] joint_torque Coppia giunto
4* @param [in] joint_pos Posizione giunto
5* @param [in] t Periodo campionamento
6* @return Codice di errore
7*/
8int LoadIdentifyMain(double[] joint_torque, double[] joint_pos, double t);
12.29. Ottieni risultato identificazione carico
Nuovo nella versione C#SDK-v1.0.4.
1/**
2* @brief Ottieni risultato identificazione carico
3* @param [in] gain Coefficiente termine gravità double[6], coefficiente termine centrifugo double[6]
4* @param [out] weight Peso carico
5* @param [out] cog Centro di massa carico
6* @return Codice di errore
7*/
8int LoadIdentifyGetResult(double[] gain, ref double weight, ref DescTran cog);
12.30. Esempio di codice per l’identificazione del carico del robot
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.31. Trascinamento assistito da sensore di forza
Nuovo nella versione C#SDK-V1.1.4: Web-3.8.3
1/**
2* @brief Trascinamento assistito da sensore di forza
3* @param [in] status Stato controllo, 0-Disabilita; 1-Abilita
4* @param [in] asaptiveFlag Flag abilitazione adattamento, 0-Disabilita; 1-Abilita
5* @param [in] interfereDragFlag Flag trascinamento area di interferenza, 0-Disabilita; 1-Abilita
6* @param [in] ingularityConstraintsFlag Strategia punto singolare, 0-Evita; 1-Attraversa
7* @param [in] forceCollisionFlag Flag rilevamento collisione robot durante trascinamento assistito; 0-Disabilita; 1-Abilita
8* @param [in] M Coefficiente inerzia
9* @param [in] B Coefficiente smorzamento
10* @param [in] K Coefficiente rigidità
11* @param [in] F Soglia forza sei dimensioni per trascinamento
12* @param [in] Fmax Limite massimo forza trascinamento Nm
13* @param [in] Vmax Limite massimo velocità giunto °/s
14* @return Codice di errore
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.32. Ottieni lo stato dell’interruttore di trascinamento del sensore di forza
1/**
2* @brief Ottieni lo stato dell'interruttore di trascinamento del sensore di forza
3* @param [out] dragState Stato controllo trascinamento assistito da sensore di forza, 0-Disabilita; 1-Abilita
4* @param [out] sixDimensionalDragState Stato controllo trascinamento assistito a sei dimensioni, 0-Disabilita; 1-Abilita
5* @return Codice di errore
6*/
7int GetForceAndTorqueDragState(ref int dragState, ref int sixDimensionalDragState);
12.33. Attivazione automatica sensore di forza dopo cancellazione errore
1/**
2* @brief Attivazione automatica sensore di forza dopo cancellazione errore
3* @param [in] status Stato controllo, 0-Disabilita; 1-Abilita
4* @return Codice di errore
5*/
6int SetForceSensorDragAutoFlag(int status);
12.34. Esempio di codice per il trascinamento assistito da sensore di forza
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.35. Imposta interruttore e parametri per trascinamento ibrido a sei dimensioni e impedenza giunto
1/**
2* @brief Imposta interruttore e parametri per trascinamento ibrido a sei dimensioni e impedenza giunto
3* @param [in] status Stato controllo, 0-Disabilita; 1-Abilita
4* @param [in] impedanceFlag Flag abilitazione impedenza, 0-Disabilita; 1-Abilita
5* @param [in] lamdeDain Guadagno trascinamento
6* @param [in] KGain Guadagno rigidità
7* @param [in] BGain Guadagno smorzamento
8* @param [in] dragMaxTcpVel Limite massimo velocità lineare TCP durante trascinamento
9* @param [in] dragMaxTcpOriVel Limite massimo velocità angolare TCP durante trascinamento
10* @return Codice di errore
11*/
12int ForceAndJointImpedanceStartStop(int status, int impedanceFlag, double[] lamdeDain, double[] KGain, double[] BGain, double dragMaxTcpVel, double dragMaxTcpOriVel);
12.36. Esempio di codice per il trascinamento assistito da sensore di forza
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.37. Controllo avvio/arresto impedenza
Nuovo nella versione C#SDK-V1.1.8: Web-3.8.6
1/**
2* @brief Controllo avvio/arresto impedenza
3* @param [in] status 0: Disabilita; 1-Abilità
4* @param [in] workSpace 0-Spazio giunto; 1-Spazio cartesiano
5* @param [in] forceThreshold Soglia forza di attivazione (N)
6* @param [in] m Parametro massa
7* @param [in] b Parametro smorzamento
8* @param [in] k Parametro rigidità
9* @param [in] maxV Massima velocità lineare (mm/s)
10* @param [in] maxVA Massima accelerazione lineare (mm/s2)
11* @param [in] maxW Massima velocità angolare (°/s)
12* @param [in] maxWA Massima accelerazione angolare (°/s2)
13* @return Codice di errore
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.38. Esempio di codice per il controllo avvio/arresto impedenza del robot
Nuovo nella versione 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}