12. Controllo di Forza del Robot

12.1. Configurare il sensore di forza

1/**
2* @brief  Configurare il sensore di forza
3* @param  config company: produttore del sensore di forza, 17-Kunwei Technology, 19-Academy 11, 20-Sensore ATI, 21-Zhongke Midian, 22-Weihang Minxin, 23-NBIT, 24-Xin Jingcheng (XJC), 26-NSR
4* @param  config device: numero dispositivo, Kunwei (0-KWR75B), Academy 11 (0-MCS6A-200-4), ATI (0-AXIA80-M8), Zhongke Midian (0-MST2010), Weihang Minxin (0-WHC6L-YB-10A), NBIT (0-XLH93003ACS), Xin Jingcheng XJC (0-XJC-6F-D82), NSR (0-NSR-FTSensorA)
5* @param  config softvesion: numero versione software, non utilizzato attualmente, default 0
6* @param  config bus: posizione bus terminale del dispositivo, non utilizzato attualmente, default 0
7* @return  Codice errore
8*/
9int FT_SetConfig(DeviceConfig config);

12.2. Ottenere la configurazione del sensore di forza

1/**
2* @brief Ottenere la configurazione del sensore di forza
3* @param [out] config company: produttore del sensore di forza, 17-Kunwei Technology, 19-Academy 11, 20-Sensore ATI, 21-Zhongke Midian, 22-Weihang Minxin
4* @param [out] config device: numero dispositivo, Kunwei (0-KWR75B), Academy 11 (0-MCS6A-200-4), ATI (0-AXIA80-M8), Zhongke Midian (0-MST2010), Weihang Minxin (0-WHC6L-YB-10A)
5* @param [out] config softvesion: numero versione software, non utilizzato attualmente, default 0
6* @param [out] config bus: posizione bus terminale del dispositivo, non utilizzato attualmente, default 0
7* @return Codice errore
8*/
9int FT_GetConfig(DeviceConfig config);

12.3. Attivare il sensore di forza

1/**
2* @brief  Attivare il sensore di forza
3* @param  [in] act  0-reset, 1-attiva
4* @return  Codice errore
5*/
6int FT_Activate(int act);

12.4. Calibrazione zero del sensore di forza

1/**
2* @brief  Calibrazione zero del sensore di forza
3* @param  [in] act  0-rimuovi zero, 1-calibrazione zero
4* @return  Codice errore
5*/
6int FT_SetZero(int act);

12.5. Impostare il sistema di riferimento del sensore di forza

1/**
2* @brief  Impostare il sistema di riferimento del sensore di forza
3* @param  [in] type  0-sistema coordinato utensile, 1-sistema coordinato base, 2-sistema coordinato libero
4* @param  [in] coord  valore del sistema coordinato libero
5* @return  Codice errore
6*/
7int FT_SetRCS(int type, DescPose coord);

12.6. Impostare il peso del carico sotto il sensore di forza

1/**
2* @brief Impostare il peso del carico sotto il sensore di forza
3* @param [in] weight peso del carico kg
4* @return Codice errore
5*/
6int SetForceSensorPayLoad(double weight);

12.7. Impostare il centro di gravità del carico sotto il sensore di forza

1/**
2* @brief Impostare il centro di gravità del carico sotto il sensore di forza
3* @param [in] cog centro di gravità del carico mm
4* @return Codice errore
5*/
6int SetForceSensorPayLoadCog(DescTran cog);

12.8. Ottenere il peso del carico sotto il sensore di forza

1/**
2* @brief Ottenere il peso del carico sotto il sensore di forza
3* @return List[0]: codice errore; List[1] : weight peso del carico kg
4*/
5List<Number> GetForceSensorPayLoad();

12.9. Ottenere il centro di gravità del carico sotto il sensore di forza

1/**
2* @brief Ottenere il centro di gravità del carico sotto il sensore di forza
3* @param [out] cog centro di gravità del carico mm
4* @return Codice errore
5*/
6int GetForceSensorPayLoadCog(DescTran cog);

12.10. Calibrazione zero automatica del sensore di forza

1/**
2* @brief Calibrazione zero automatica del sensore di forza
3* @param [in] massCenter massa del sensore (kg) e centro di gravità (mm)
4* @return Codice errore
5*/
6int ForceSensorAutoComputeLoad(MassCenter massCenter);

12.11. Ottenere dati forza/coppia nel sistema di riferimento

1/**
2* @brief  Ottenere dati forza/coppia nel sistema di riferimento
3* @param  [in] flag 0-bloccante, 1-non bloccante
4* @param  [out] ft  forza/coppia, fx,fy,fz,tx,ty,tz
5* @return  Codice errore
6*/
7int FT_GetForceTorqueRCS(int flag, ForceTorque ft);

12.12. Ottenere dati forza/coppia originali del sensore di forza

1/**
2* @brief  Ottenere dati forza/coppia originali del sensore di forza
3* @param  [in] flag 0-bloccante, 1-non bloccante
4* @param  [out] ft  forza/coppia, fx,fy,fz,tx,ty,tz
5* @return  Codice errore
6*/
7int FT_GetForceTorqueOrigin(int flag, ForceTorque ft);

12.13. Esempio di codice per configurazione e calibrazione zero automatica sensore di forza

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

12.14. Registrazione identificazione peso carico

1/**
2* @brief  Registrazione identificazione peso carico
3* @param  [in] id  numero sistema coordinato sensore, range [1~14]
4* @return  Codice errore
5*/
6int FT_PdIdenRecord(int id);

12.15. Calcolo identificazione peso carico

1/**
2* @brief  Calcolo identificazione peso carico
3* @return  List[0]: codice errore; List[1] : double weight  peso carico, unità kg
4*/
5List<Number> FT_PdIdenCompute();

12.16. Registrazione identificazione centro di gravità carico

1/**
2* @brief  Registrazione identificazione centro di gravità carico
3* @param  [in] id  numero sistema coordinato sensore, range [1~14]
4* @param  [in] index numero punto, range [1~3]
5* @return  Codice errore
6*/
7int FT_PdCogIdenRecord(int id, int index);

12.17. Calcolo identificazione centro di gravità carico

1/**
2* @brief  Calcolo identificazione centro di gravità carico
3* @param  [out] cog  centro di gravità carico, unità mm
4* @return  Codice errore
5*/
6int FT_PdCogIdenCompute(DescTran cog);

12.18. Esempio di codice identificazione carico sensore di forza

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

12.19. Protezione collisione

 1/**
 2* @brief  Protezione collisione
 3* @param  [in] flag 0-disattiva protezione collisione, 1-attiva protezione collisione
 4* @param  [in] sensor_id numero sensore di forza
 5* @param  [in] select  selezione sei gradi di libertà per rilevamento collisione, 0-non rilevare, 1-rileva
 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 rilevamento forza/coppia: (ft-min_threshold, ft+max_threshold)
10* @return  Codice errore
11*/
12int FT_Guard(int flag, int sensor_id, Object[] select, ForceTorque ft, Object[] max_threshold, Object[] min_threshold);

12.20. Esempio di codice protezione collisione

 1public static void main(String[] args)
 2{
 3    Robot robot = new Robot();
 4    robot.SetReconnectParam(true,20,500);//imposta tentativi riconnessione, intervallo
 5    robot.LoggerInit(FrLogType.DIRECT, FrLogLevel.INFO, "D://log", 10, 10);
 6    int rtn = robot.RPC("192.168.58.2");
 7    if(rtn == 0)
 8    {
 9        System.out.println("connessione rpc successo");
10    }
11    else
12    {
13        System.out.println("connessione rpc fallita");
14        return ;
15    }
16    byte flag = 1;
17    byte sensor_id = 8;
18    Object[] select = { 1, 0, 0, 0, 0, 0 };//abilita solo protezione collisione asse x
19    Object[] max_threshold = { 5.0, 0.01, 0.01, 0.01, 0.01, 0.01 };
20    Object[] min_threshold = { 3.0, 0.01, 0.01, 0.01, 0.01, 0.01 };
21
22    ForceTorque ft = new ForceTorque(0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
23    DescPose  desc_p1, desc_p2, desc_p3;
24    desc_p1 = new DescPose(-14.404,-455.283,319.847,-172.935,25.141,-68.097);
25    desc_p2 = new DescPose(-107.999,-599.174,285.939,153.472,12.686,-71.284);
26    desc_p3 = new DescPose(6.586,-704.897,309.638,178.909,-27.759,-70.479);
27
28    int rtn =  robot.FT_Guard(flag, sensor_id, select, ft, max_threshold, min_threshold);
29    System.out.println("FT_Guard avvio rtn {rtn}");
30    robot.MoveCart(desc_p1, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
31    robot.MoveCart(desc_p2, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
32    robot.MoveCart(desc_p3, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
33}

12.21. Controllo forza costante

 1/**
 2* @brief  Controllo forza costante
 3* @param  flag 0-disattiva controllo forza costante, 1-attiva controllo forza costante
 4* @param  sensor_id numero sensore di forza
 5* @param  select  selezione sei gradi di libertà per rilevamento collisione, 0-non rilevare, 1-rileva
 6* @param  ft  forza/coppia collisione, fx,fy,fz,tx,ty,tz
 7* @param  ft_pid parametri pid forza, parametri pid coppia
 8* @param  adj_sign controllo avvio/arresto adattativo, 0-disattiva, 1-attiva
 9* @param  ILC_sign controllo avvio/arresto ILC, 0-arresta, 1-addestra, 2-operazione
10* @param  max_dis distanza regolazione massima, unità mm
11* @param  max_ang angolo regolazione massimo, unità deg
12* @param  M parametri massa
13* @param  B parametri smorzamento
14* @param  polishRadio raggio levigatura, unità mm
15* @param  filter_Sign flag attivazione filtro 0-off; 1-on, default off
16* @param  posAdapt_sign flag attivazione adattamento posa 0-off; 1-on, default off
17* @param  isNoBlock flag bloccante, 0-bloccante; 1-non bloccante
18* @return  Codice errore
19*/
20public int FT_Control(int flag, int sensor_id, int[] select, ForceTorque ft, double[] ft_pid, int adj_sign, int ILC_sign, double max_dis, double max_ang,double[] M,double[] B, double polishRadio,int filter_Sign, int posAdapt_sign, int isNoBlock)

12.22. Esempio di codice controllo forza costante con smorzamento

 1public static void TestFTControlWithDamping(Robot robot)
 2{
 3    int sensor_id = 10;
 4    int[] select = { 0,0,1,0,0,0 };
 5    double[] ft_pid = { 0.0008, 0.0, 0.0, 0.0, 0.0, 0.0 };
 6    int adj_sign = 0;
 7    int ILC_sign = 0;
 8    double max_dis = 100.0;
 9    double max_ang = 20;
10    ForceTorque ft =new ForceTorque(0,0,0,0,0,0);
11    ft.fz = -10.0;
12    ExaxisPos epos=new ExaxisPos(0, 0, 0, 0);
13    JointPos j1=new JointPos(-118.985, -86.882, -118.139, -65.019, 90.002, 54.951);
14    JointPos j2=new JointPos(-77.055, -77.218, -126.219, -66.591, 90.028, 96.881);
15    DescPose desc_p1=new DescPose(-300.856, -332.618, 309.240, 179.976, -0.031, 96.065);
16    DescPose desc_p2=new DescPose(-16.399, -383.760, 309.312, 179.975, -0.031, 96.064);
17    DescPose offset_pos=new DescPose(0, 0, 0, 0, 0, 0);
18    double[] M = {2.0, 2.0};
19    double[] B = {8.0, 8.0};
20    double polishRadio;
21    int filter_Sign;
22    int posAdapt_sign;
23    int isNoBlock;
24    DescPose ftCoord =new DescPose();
25    robot.FT_SetRCS(2, ftCoord);
26    int rtn = robot.FT_Control(1, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, 0, 0, 1, 0);
27    System.out.printf("FT_Control avvio rtn è %d\n", rtn);
28    rtn = robot.MoveL(j1, desc_p1, 0, 0, 100.0, 100.0, 20.0, -1.0,0, epos, 0, 0, offset_pos,0,0,10);
29    rtn = robot.MoveL(j2, desc_p2, 0, 0, 100.0, 100.0, 20.0, -1.0, 0,epos, 0, 0, offset_pos,0,0,10);
30    rtn = robot.FT_Control(1, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, 0, 0, 1, 0);
31    System.out.printf("FT_Control fine rtn è %d\n", rtn);
32    robot.CloseRPC();
33}

12.23. Avvio controllo conformità

1/**
2* @brief  Avvio controllo conformità
3* @param  [in] p coefficiente regolazione posizione o coefficiente conformità
4* @param  [in] force soglia forza attivazione conformità, unità N
5* @return  Codice errore
6*/
7int FT_ComplianceStart(double p, double force);

12.24. Arresto controllo conformità

1/**
2* @brief  Arresto controllo conformità
3* @return  Codice errore
4*/
5int FT_ComplianceStop();

12.25. Esempio di codice controllo conformità

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

12.26. Inizializzazione identificazione carico

1/**
2* @brief Inizializzazione identificazione carico
3* @return Codice errore
4*/
5int LoadIdentifyDynFilterInit();

12.27. Inizializzazione variabili identificazione carico

1/**
2* @brief Inizializzazione variabili identificazione carico
3* @return Codice errore
4*/
5int LoadIdentifyDynVarInit();

12.28. Programma principale identificazione carico

1/**
2* @brief Programma principale identificazione carico
3* @param [in] joint_torque coppia giunti
4* @param [in] joint_pos posizione giunti
5* @param [in] t periodo campionamento
6* @return Codice errore
7*/
8int LoadIdentifyMain(Object[] joint_torque, Object[] joint_pos, double t);

12.29. Ottenere risultati identificazione carico

1/**
2* @brief Ottenere risultati identificazione carico
3* @param [in] gain
4* @return List[0]: codice errore; List[1] : double weight peso carico; List[2]: x centro di gravità carico X mm; List[3] : y centro di gravità carico Y mm; List[2]: z centro di gravità carico Z mm
5*/
6List<Number> LoadIdentifyGetResult(Object[] gain);

12.30. Esempio di codice identificazione carico robot

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

12.31. Trascinamento assistito da sensore di forza

Cambiato nella versione Java: SDK-v1.0.6-3.8.3

 1/**
 2* @brief Trascinamento assistito da sensore di forza
 3* @param [in] status stato controllo, 0-disattiva; 1-attiva
 4* @param [in] asaptiveFlag flag attivazione adattativo, 0-disattiva; 1-attiva
 5* @param [in] interfereDragFlag flag trascinamento zona interferenza, 0-disattiva; 1-attiva
 6* @param [in] ingularityConstraintsFlag strategia punti singolari, 0-evitare; 1-attraversare
 7* @param [in] forceCollisionFlag flag rilevamento collisione robot durante trascinamento assistito; 0-disattiva; 1-attiva
 8* @param [in] M coefficiente inerzia
 9* @param [in] B coefficiente smorzamento
10* @param [in] K coefficiente rigidezza
11* @param [in] F soglia forza sei dimensioni trascinamento
12* @param [in] Fmax limite forza trascinamento massima Nm
13* @param [in] Vmax limite velocità giunti massima °/s
14* @return Codice errore
15*/
16int EndForceDragControl(int status, int asaptiveFlag, int interfereDragFlag,int ingularityConstraintsFlag, int forceCollisionFlag, Object[] M, Object[] B, Object[] K, Object[] F, double Fmax, double Vmax)

12.32. Ottenere stato interruttore trascinamento sensore di forza

1/**
2* @brief Ottenere stato interruttore trascinamento sensore di forza
3* @return List[0]: codice errore; List[1] : dragState stato controllo trascinamento assistito da sensore di forza, 0-disattiva; 1-attiva; List[1] : sixDimensionalDragState stato controllo trascinamento assistito sei dimensioni, 0-disattiva; 1-attiva
4*/
5List<Integer> GetForceAndTorqueDragState();

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-disattiva; 1-attiva
4* @return Codice errore
5*/
6int SetForceSensorDragAutoFlag(int status)

12.34. Esempio di codice trascinamento assistito da sensore di forza

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

12.35. Impostare interruttore e parametri trascinamento misto sei dimensioni e impedenza giunti

 1/**
 2* @brief Impostare interruttore e parametri trascinamento misto sei dimensioni e impedenza giunti
 3* @param [in] status stato controllo, 0-disattiva; 1-attiva
 4* @param [in] impedanceFlag flag attivazione impedenza, 0-disattiva; 1-attiva
 5* @param [in] lamdeGain guadagno trascinamento
 6* @param [in] KGain guadagno rigidezza
 7* @param [in] BGain guadagno smorzamento
 8* @param [in] dragMaxTcpVel limite velocità lineare massima terminale trascinamento
 9* @param [in] dragMaxTcpOriVel limite velocità angolare massima terminale trascinamento
10* @return Codice errore
11*/
12int ForceAndJointImpedanceStartStop(int status, int impedanceFlag, Object[] lamdeGain, Object[] KGain, Object[] BGain, double dragMaxTcpVel, double dragMaxTcpOriVel);

12.36. Esempio di codice trascinamento misto sei dimensioni e impedenza giunti

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

12.37. Controllo avvio/arresto impedenza

 1/**
 2* @brief Controllo avvio/arresto impedenza
 3* @param [in] status 0: disattiva; 1-attiva
 4* @param [in] workSpace 0-spazio giunti; 1-spazio cartesiano
 5* @param [in] forceThreshold soglia forza attivazione (N)
 6* @param [in] m parametri massa
 7* @param [in] b parametri smorzamento
 8* @param [in] k parametri rigidezza
 9* @param [in] maxV velocità lineare massima (mm/s)
10* @param [in] maxVA accelerazione lineare massima (mm/s2)
11* @param [in] maxW velocità angolare massima (°/s)
12* @param [in] maxWA accelerazione angolare massima (°/s2)
13* @return Codice 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 controllo avvio/arresto impedenza robot

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