12. Controle de Força do Robô
12.1. Configurar Sensor de Força
1/**
2* @brief Configura o sensor de força
3* @param config company: Fabricante do sensor de força, 17-Kunwei Technology, 19-Instituto de Pesquisa 11 da Aeroespacial, 20-Sensor ATI, 21-Zhongke Midian, 22-Weihang Minxin, 23-NBIT, 24-Xin Jingcheng (XJC), 26-NSR
4* @param config device: Número do dispositivo, Kunwei (0-KWR75B), Instituto de Pesquisa 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 softversion: Número da versão do software, não utilizado no momento, padrão 0
6* @param config bus: Posição do barramento onde o dispositivo está conectado, não utilizado no momento, padrão 0
7* @return Código de erro
8*/
9int FT_SetConfig(DeviceConfig config);
12.2. Obter Configuração do Sensor de Força
1/**
2* @brief Obtém a configuração do sensor de força
3* @param [out] config company: Fabricante do sensor de força, 17-Kunwei Technology, 19-Instituto de Pesquisa 11 da Aeroespacial, 20-Sensor ATI, 21-Zhongke Midian, 22-Weihang Minxin
4* @param [out] config device: Número do dispositivo, Kunwei (0-KWR75B), Instituto de Pesquisa 11 (0-MCS6A-200-4), ATI (0-AXIA80-M8), Zhongke Midian (0-MST2010), Weihang Minxin (0-WHC6L-YB-10A)
5* @param [out] config softversion: Número da versão do software, não utilizado no momento, padrão 0
6* @param [out] config bus: Posição do barramento onde o dispositivo está conectado, não utilizado no momento, padrão 0
7* @return Código de erro
8*/
9int FT_GetConfig(DeviceConfig config);
12.3. Ativar Sensor de Força
1/**
2* @brief Ativa o sensor de força
3* @param [in] act 0-reset, 1-ativar
4* @return Código de erro
5*/
6int FT_Activate(int act);
12.4. Calibrar Zero do Sensor de Força
1/**
2* @brief Calibra o zero do sensor de força
3* @param [in] act 0-remover zero, 1-corrigir zero
4* @return Código de erro
5*/
6int FT_SetZero(int act);
12.5. Definir Sistema de Coordenadas de Referência do Sensor de Força
1/**
2* @brief Define o sistema de coordenadas de referência do sensor de força
3* @param [in] type 0-sistema de coordenadas da ferramenta, 1-sistema de coordenadas base, 2-sistema de coordenadas livre
4* @param [in] coord Valor do sistema de coordenadas livre
5* @return Código de erro
6*/
7int FT_SetRCS(int type, DescPose coord);
12.6. Definir Peso da Carga sob o Sensor de Força
1/**
2* @brief Define o peso da carga sob o sensor de força
3* @param [in] weight Peso da carga em kg
4* @return Código de erro
5*/
6int SetForceSensorPayLoad(double weight);
12.7. Definir Centro de Massa da Carga sob o Sensor de Força
1/**
2* @brief Define o centro de massa da carga sob o sensor de força
3* @param [in] cog Centro de massa da carga em mm
4* @return Código de erro
5*/
6int SetForceSensorPayLoadCog(DescTran cog);
12.8. Obter Peso da Carga sob o Sensor de Força
1/**
2* @brief Obtém o peso da carga sob o sensor de força
3* @return List[0]: código de erro; List[1]: weight Peso da carga em kg
4*/
5List<Number> GetForceSensorPayLoad();
12.9. Obter Centro de Massa da Carga sob o Sensor de Força
1/**
2* @brief Obtém o centro de massa da carga sob o sensor de força
3* @param [out] cog Centro de massa da carga em mm
4* @return Código de erro
5*/
6int GetForceSensorPayLoadCog(DescTran cog);
12.10. Calibração Automática do Zero do Sensor de Força
1/**
2* @brief Calibração automática do zero do sensor de força
3* @param [in] massCenter Massa do sensor (kg) e centro de massa (mm)
4* @return Código de erro
5*/
6int ForceSensorAutoComputeLoad(MassCenter massCenter);
12.11. Obter Dados de Força/Torque no Sistema de Coordenadas de Referência
1/**
2* @brief Obtém dados de força/torque no sistema de coordenadas de referência
3* @param [in] flag 0-bloqueante, 1-não bloqueante
4* @param [out] ft Força/torque, fx, fy, fz, tx, ty, tz
5* @return Código de erro
6*/
7int FT_GetForceTorqueRCS(int flag, ForceTorque ft);
12.12. Obter Dados Brutos de Força/Torque do Sensor de Força
1/**
2* @brief Obtém dados brutos de força/torque do sensor de força
3* @param [in] flag 0-bloqueante, 1-não bloqueante
4* @param [out] ft Força/torque, fx, fy, fz, tx, ty, tz
5* @return Código de erro
6*/
7int FT_GetForceTorqueOrigin(int flag, ForceTorque ft);
12.13. Exemplo de Código de Configuração e Calibração Automática do Sensor de Força
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. Registro de Identificação do Peso da Carga
1/**
2* @brief Registro de identificação do peso da carga
3* @param [in] id Número do sistema de coordenadas do sensor, intervalo [1~14]
4* @return Código de erro
5*/
6int FT_PdIdenRecord(int id);
12.15. Cálculo da Identificação do Peso da Carga
1/**
2* @brief Cálculo da identificação do peso da carga
3* @return List[0]: código de erro; List[1]: double weight Peso da carga, em kg
4*/
5List<Number> FT_PdIdenCompute();
12.16. Registro de Identificação do Centro de Massa da Carga
1/**
2* @brief Registro de identificação do centro de massa da carga
3* @param [in] id Número do sistema de coordenadas do sensor, intervalo [1~14]
4* @param [in] index Número do ponto, intervalo [1~3]
5* @return Código de erro
6*/
7int FT_PdCogIdenRecord(int id, int index);
12.17. Cálculo da Identificação do Centro de Massa da Carga
1/**
2* @brief Cálculo da identificação do centro de massa da carga
3* @param [out] cog Centro de massa da carga, em mm
4* @return Código de erro
5*/
6int FT_PdCogIdenCompute(DescTran cog);
12.18. Exemplo de Código de Identificação da Carga do Sensor de Força
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. Proteção contra Colisão
1/**
2* @brief Proteção contra colisão
3* @param [in] flag 0-desativar proteção contra colisão, 1-ativar proteção contra colisão
4* @param [in] sensor_id Número do sensor de força
5* @param [in] select Seleciona quais dos seis graus de liberdade detectam colisão, 0-não detectar, 1-detectar
6* @param [in] ft Força/torque de colisão, fx, fy, fz, tx, ty, tz
7* @param [in] max_threshold Limiar máximo
8* @param [in] min_threshold Limiar mínimo
9* @note Faixa de detecção de força/torque: (ft - min_threshold, ft + max_threshold)
10* @return Código de erro
11*/
12int FT_Guard(int flag, int sensor_id, Object[] select, ForceTorque ft, Object[] max_threshold, Object[] min_threshold);
12.20. Exemplo de Código de Proteção contra Colisão
1public static void main(String[] args)
2{
3 Robot robot = new Robot();
4 robot.SetReconnectParam(true,20,500); // Define parâmetros de reconexão
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("Conexão RPC bem-sucedida");
10 }
11 else
12 {
13 System.out.println("Falha na conexão RPC");
14 return;
15 }
16 byte flag = 1;
17 byte sensor_id = 8;
18 Object[] select = { 1, 0, 0, 0, 0, 0 }; // Ativa apenas a proteção contra colisão no eixo 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 start rtn {rtn}");
30 robot.MoveCart(desc_p1, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
31 robot.MoveCart(desc_p2, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
32 robot.MoveCart(desc_p3, 0, 0, 20, 100.0f, 100.0f, -1.0f, -1);
33}
12.21. Controle de Força Constante
1/**
2* @brief Controle de força constante
3* @param flag 0-desativar controle de força constante, 1-ativar controle de força constante
4* @param sensor_id Número do sensor de força
5* @param select Seleciona quais dos seis graus de liberdade detectam colisão, 0-não detectar, 1-detectar
6* @param ft Força/torque de colisão, fx, fy, fz, tx, ty, tz
7* @param ft_pid Parâmetros PID de força, parâmetros PID de torque
8* @param adj_sign Controle de adaptação liga/desliga, 0-desativar, 1-ativar
9* @param ILC_sign Controle ILC liga/desliga, 0-parar, 1-treinar, 2-praticar
10* @param max_dis Distância máxima de ajuste, em mm
11* @param max_ang Ângulo máximo de ajuste, em graus
12* @param M Parâmetros de massa rx, ry [0.1-10], padrão 2
13* @param B Parâmetros de amortecimento rx, ry [0.1-50], padrão 8
14* @param threshold Limiares de ativação rx, ry [0-10], padrão 0.2
15* @param adjustCoeff Coeficientes de ajuste de torque rx, ry [0-1], padrão 1
16* @param polishRadio Raio de polimento, em mm
17* @param filter_Sign Flag de ativação de filtro 0-desativar; 1-ativar, padrão desativado
18* @param posAdapt_sign Flag de ativação de adaptação de postura 0-desativar; 1-ativar, padrão desativado
19* @param isNoBlock Flag de bloqueio, 0-bloqueante; 1-não bloqueante
20* @return Código de erro
21*/
22public int FT_Control(int flag, int sensor_id, int[] select, ForceTorque ft, double[] ft_pid, int adj_sign, int ILC_sign, double max_dis, double max_ang, double[] M, double[] B, double[] threshold, double[] adjustCoeff, double polishRadio, int filter_Sign, int posAdapt_sign, int isNoBlock)
12.22. Exemplo de Código de Controle de Força Constante com Amortecimento
1public static int TestFTControlWithAdjustCoeff(Robot robot)
2{
3 int sensor_id = 10;
4 int[] select = { 0,0,1,0,0,0 };
5 double[] ft_pid = { 0.0008, 0.0, 0.0, 0.0, 0.0, 0.0 };
6 int adj_sign = 0;
7 int ILC_sign = 0;
8 double max_dis = 1000.0;
9 double max_ang = 20;
10 ForceTorque ft = new ForceTorque(0.0,0,0,0,0,0);
11 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
12 JointPos j1 = new JointPos(80.765, -98.795, 106.548, -97.734, -89.999, 94.842);
13 JointPos j2 = new JointPos(43.067, -84.429, 92.620, -98.175, -90.011, 57.144);
14 DescPose desc_p1 = new DescPose(5.009, -547.463, 262.053, -179.999, -0.019, 75.923);
15 DescPose desc_p2 = new DescPose(-347.966, -547.463, 262.048, -180.000, -0.019, 75.923);
16 DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
17 double[] M = { 2.0, 2.0 };
18 double[] B = { 15.0, 15.0 };
19 double[] threshold = {1.0, 1.0};
20 double[] adjustCoeff = {1.0, 0.8};
21 double polishRadio = 0.0;
22 int filter_Sign = 0;
23 int posAdapt_sign = 1;
24 int isNoBlock;
25 ft.fz = -10.0;
26 while(true)
27 {
28 int rtn = robot.FT_Control(1, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
29 System.out.printf("FT_Control start rtn is %d\n", rtn);
30 robot.MoveL(j1, desc_p1, 1, 0, 100.0, 100.0, 100.0, -1.0, 0, epos, 0, 0, offset_pos, 0,0, 0,10);
31 robot.MoveL(j2, desc_p2, 1, 0, 100.0, 100.0, 100.0, -1.0, 0, epos, 0, 0, offset_pos, 0,0, 0,10);
32 rtn = robot.FT_Control(0, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, M, B, threshold, adjustCoeff, 0, 0, 1, 0);
33 System.out.printf("FT_Control end rtn is %d\n", rtn);
34 }
35}
12.23. Inserção Rotativa
1/**
2* @brief Inserção rotativa
3* @param rcs Sistema de coordenadas de referência, 0-sistema de coordenadas da ferramenta, 1-sistema de coordenadas base
4* @param angVelRot Velocidade angular de rotação, em graus/s
5* @param ft Limiar de força/torque, fx, fy, fz, tx, ty, tz, intervalo [0~100]
6* @param max_angle Ângulo máximo de rotação, em graus
7* @param orn Direção da força/torque, 1-ao longo do eixo Z, 2-em torno do eixo Z
8* @param max_angAcc Aceleração angular máxima, em graus/s^2, não utilizado no momento, padrão 0
9* @param rotorn Direção de rotação, 1-horário, 2-anti-horário
10* @param strategy Estratégia de tratamento quando força/torque não são detectados, 0-reportar erro; 1-avisar, continuar movimento
11* @return Código de erro
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. Exemplo de Código de Inserção Rotativa com Sensor de Força do Robô
1public static int TestMove(Robot robot)
2{
3 int rtn = -1;
4 JointPos j1 = new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
5 JointPos j2 = new JointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
6 JointPos j3 = new JointPos(-29.777, -84.536, 109.275, -114.075, -86.655, 74.257);
7 JointPos j4 = new JointPos(-31.154, -95.317, 94.276, -88.079, -89.740, 74.256);
8 DescPose desc_pos1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
9 DescPose desc_pos2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
10 DescPose desc_pos3 = new DescPose(-487.434, 154.362, 308.576, 176.600, 0.268, -14.061);
11 DescPose desc_pos4 = new DescPose(-443.165, 147.881, 480.951, 179.511, -0.775, -15.409);
12 DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
13 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
14 int tool = 0;
15 int user = 0;
16 double vel = 100.0;
17 double acc = 100.0;
18 double ovl = 100.0;
19 double oacc = 100.0;
20 double blendT = 0.0;
21 double blendR = 0.0;
22 int flag = 0;
23 int search = 0;
24 int blendMode = 0;
25 int velAccMode = 0;
26 robot.SetSpeed(20);
27 rtn = robot.MoveJ(j1, desc_pos1, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
28 System.out.printf("movej errcode:%d\n", rtn);
29 rtn = robot.MoveL(j2, desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, epos, search, flag, offset_pos, oacc, velAccMode,0,10);
30 System.out.printf("movel errcode:%d\n", rtn);
31 rtn = robot.MoveC(j3, desc_pos3, tool, user, vel, acc, epos, flag, offset_pos, j4, desc_pos4, tool, user, vel, acc, epos, flag, offset_pos, ovl, blendR, oacc, velAccMode);
32 System.out.printf("movec errcode:%d\n", rtn);
33 rtn = robot.MoveJ(j2, desc_pos2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
34 System.out.printf("movej errcode:%d\n", rtn);
35 rtn = robot.Circle(j3, desc_pos3, tool, user, vel, acc, epos, j1, desc_pos1, tool, user, vel, acc, epos, ovl, flag, offset_pos, oacc, -1, velAccMode);
36 System.out.printf("circle errcode:%d\n", rtn);
37 rtn = robot.MoveCart(desc_pos4, tool, user, vel, acc, ovl, blendT, -1);
38 System.out.printf("MoveCart errcode:%d\n", rtn);
39 rtn = robot.MoveJ(j1, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
40 System.out.printf("movej errcode:%d\n", rtn);
41 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, blendMode, epos, search, flag, offset_pos, -1, velAccMode,0,10);
42 System.out.printf("movel errcode:%d\n", rtn);
43 rtn = robot.MoveC(desc_pos3, tool, user, vel, acc, epos, flag, offset_pos, desc_pos4, tool, user, vel, acc, epos, flag, offset_pos, ovl, blendR, -1, velAccMode);
44 System.out.printf("movec errcode:%d\n", rtn);
45 rtn = robot.MoveJ(j2, tool, user, vel, acc, ovl, epos, blendT, flag, offset_pos);
46 System.out.printf("movej errcode:%d\n", rtn);
47 rtn = robot.Circle(desc_pos3, tool, user, vel, acc, epos, desc_pos1, tool, user, vel, acc, epos, ovl, flag, offset_pos, oacc, blendR, -1, velAccMode);
48 System.out.printf("circle errcode:%d\n", rtn);
49 return 0;
50}
12.25. Ativar Controle de Compliance
1/**
2* @brief Ativa o controle de compliance
3* @param [in] p Coeficiente de ajuste de posição ou coeficiente de compliance
4* @param [in] force Limiar de força para ativação do compliance, em N
5* @return Código de erro
6*/
7int FT_ComplianceStart(double p, double force);
12.26. Desativar Controle de Compliance
1/**
2* @brief Desativa o controle de compliance
3* @return Código de erro
4*/
5int FT_ComplianceStop();
12.27. Exemplo de Código de Controle de Compliance
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 JointPos j1 = new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
43 JointPos j2 = new JointPos(-45.615, -106.172, 124.296, -107.151, -91.282, 74.255);
44 DescPose desc_p1 = new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
45 DescPose desc_p2 = new DescPose(-321.222, 185.189, 335.520, -179.030, -1.284, -29.869);
46
47 ft.fx = -10.0;
48 ft.fy = -10.0;
49 ft.fz = -10.0;
50 robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
51 double p = 0.00005;
52 double force = 30.0;
53 int rtn = robot.FT_ComplianceStart(p, force);
54
55 int count = 15;
56 while (count > 0)
57 {
58 robot.MoveL(j1, desc_p1, 0, 0, 100.0, 180.0, 100.0, -1.0,0, epos, 0, 1, offset_pos,0,10);
59 robot.MoveL(j2, desc_p2, 0, 0, 100.0, 180.0, 100.0, -1.0,0, epos, 0, 0, offset_pos,0,10);
60 count -= 1;
61 }
62 robot.FT_ComplianceStop();
63 flag = 0;
64 robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang, 0, 0, 0);
65
66 robot.CloseRPC();
67 return 0;
68}
12.28. Inicialização da Identificação da Carga
1/**
2* @brief Inicialização da identificação da carga
3* @return Código de erro
4*/
5int LoadIdentifyDynFilterInit();
12.29. Inicialização das Variáveis de Identificação da Carga
1/**
2* @brief Inicialização das variáveis de identificação da carga
3* @return Código de erro
4*/
5int LoadIdentifyDynVarInit();
12.30. Programa Principal de Identificação da Carga
1/**
2* @brief Programa principal de identificação da carga
3* @param [in] joint_torque Torque das juntas
4* @param [in] joint_pos Posição das juntas
5* @param [in] t Período de amostragem
6* @return Código de erro
7*/
8int LoadIdentifyMain(Object[] joint_torque, Object[] joint_pos, double t);
12.31. Obter Resultado da Identificação da Carga
1/**
2* @brief Obtém o resultado da identificação da carga
3* @param [in] gain
4* @return List[0]: código de erro; List[1]: double weight Peso da carga; List[2]: x Centro de massa X mm; List[3]: y Centro de massa Y mm; List[4]: z Centro de massa Z mm
5*/
6List<Number> LoadIdentifyGetResult(Object[] gain);
12.32. Exemplo de Código de Identificação da Carga do Robô
1public static int TestIdentify(Robot robot)
2{
3 int retval = 0;
4
5 retval = robot.LoadIdentifyDynFilterInit();
6
7 retval = robot.LoadIdentifyDynVarInit();
8
9 JointPos posJ = new JointPos(0,0,0,0,0,0);
10 DescPose posDec = new DescPose(0,0,0,0,0,0);
11 List<Number> joint_toq = new ArrayList<>();
12 robot.GetActualJointPosDegree(posJ);
13 posJ.J2 = posJ.J2 + 10;
14 joint_toq = robot.GetJointTorques(0);
15
16 Object[] gain = new Object[] { 0,0.05,0,0,0,0,0,0.02,0,0,0,0 };
17 double weight = 0;
18 DescTran load_pos = new DescTran(0,0,0);
19 List<Number> num = new ArrayList<>();
20 num = robot.LoadIdentifyGetResult(gain);
21
22 robot.CloseRPC();
23 return 0;
24
25}
12.33. Arrasto Assistido por Sensor de Força
Alterado na versão Java: SDK-v1.0.6-3.8.3
1/**
2* @brief Arrasto assistido por sensor de força
3* @param [in] status Estado de controle, 0-desativar; 1-ativar
4* @param [in] asaptiveFlag Flag de ativação de adaptação, 0-desativar; 1-ativar
5* @param [in] interfereDragFlag Flag de arrasto em zona de interferência, 0-desativar; 1-ativar
6* @param [in] ingularityConstraintsFlag Estratégia de ponto singular, 0-evitar; 1-atravessar
7* @param [in] forceCollisionFlag Flag de detecção de colisão do robô durante arrasto assistido; 0-desativar; 1-ativar
8* @param [in] M Coeficiente de inércia
9* @param [in] B Coeficiente de amortecimento
10* @param [in] K Coeficiente de rigidez
11* @param [in] F Limiar de força de arrasto 6D
12* @param [in] Fmax Limite máximo de força de arrasto Nm
13* @param [in] Vmax Limite máximo de velocidade das juntas °/s
14* @return Código de erro
15*/
16int EndForceDragControl(int status, int asaptiveFlag, int interfereDragFlag, int ingularityConstraintsFlag, int forceCollisionFlag, Object[] M, Object[] B, Object[] K, Object[] F, double Fmax, double Vmax)
12.34. Obter Estado do Interruptor de Arrasto do Sensor de Força
1/**
2* @brief Obtém o estado do interruptor de arrasto do sensor de força
3* @return List[0]: código de erro; List[1]: dragState Estado de controle de arrasto assistido por sensor de força, 0-desativar; 1-ativar; List[2]: sixDimensionalDragState Estado de controle de arrasto assistido por força 6D, 0-desativar; 1-ativar
4*/
5List<Integer> GetForceAndTorqueDragState();
12.35. Ativação Automática do Sensor de Força Após Limpeza de Erro
1/**
2* @brief Ativação automática do sensor de força após limpeza de erro
3* @param [in] status Estado de controle, 0-desativar; 1-ativar
4* @return Código de erro
5*/
6int SetForceSensorDragAutoFlag(int status)
12.36. Exemplo de Código de Arrasto Assistido por Sensor de Força
1public static int TestEndForceDragCtrl(Robot robot)
2{
3 DescTran tr1 = new DescTran(0,0,0);
4 robot.SetForceSensorPayload(0);
5 robot.SetForceSensorPayloadCog(tr1);
6
7 robot.SetForceSensorDragAutoFlag(1);
8
9 Object[] M = new Object[] { 15.0, 15.0, 15.0, 0.5, 0.5, 0.1 };
10 Object[] B = new Object[] { 150.0, 150.0, 150.0, 5.0, 5.0, 1.0 };
11 Object[] K = new Object[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
12 Object[] F = new Object[] { 10.0, 10.0, 10.0, 1.0, 1.0, 1.0 };
13 robot.EndForceDragControl(1, 0, 0, 0, M, B, K, F, 50, 100);
14
15 robot.Sleep(10000);
16
17 int dragState = 0;
18 int sixDimensionalDragState = 0;
19 List<Integer> state = new ArrayList<>();
20 state = robot.GetForceAndTorqueDragState();
21
22 robot.EndForceDragControl(0, 0, 0, 0, M, B, K, F, 50, 100);
23 return 0;
24}
12.37. Definir Interruptor e Parâmetros do Arrasto Híbrido de Força 6D e Impedância Articular
1/**
2* @brief Define o interruptor e parâmetros do arrasto híbrido de força 6D e impedância articular
3* @param [in] status Estado de controle, 0-desativar; 1-ativar
4* @param [in] impedanceFlag Flag de ativação de impedância, 0-desativar; 1-ativar
5* @param [in] lamdeGain Ganho de arrasto
6* @param [in] KGain Ganho de rigidez
7* @param [in] BGain Ganho de amortecimento
8* @param [in] dragMaxTcpVel Velocidade linear máxima do TCP durante arrasto
9* @param [in] dragMaxTcpOriVel Velocidade angular máxima do TCP durante arrasto
10* @return Código de erro
11*/
12int ForceAndJointImpedanceStartStop(int status, int impedanceFlag, Object[] lamdeGain, Object[] KGain, Object[] BGain, double dragMaxTcpVel, double dragMaxTcpOriVel);
12.38. Exemplo de Código de Arrasto Híbrido de Força 6D e Impedância Articular
1public static int TestForceAndJointImpedance(Robot robot)
2{
3 robot.DragTeachSwitch(1);
4 Object[] lamdeDain = new Object[] { 3.0, 2.0, 2.0, 2.0, 2.0, 3.0 };
5 Object[] KGain = new Object[]{ 0, 0, 0, 0, 0, 0 };
6 Object[] BGain = new Object[] { 150, 150, 150, 5.0, 5.0, 1.0 };
7 int rtn = robot.ForceAndJointImpedanceStartStop(1, 0, lamdeDain, KGain, BGain, 1000.0, 180.0);
8
9 robot.Sleep(10000);
10
11 robot.DragTeachSwitch(0);
12 rtn = robot.ForceAndJointImpedanceStartStop(0, 0, lamdeDain, KGain, BGain, 1000.0, 180.0);
13
14 robot.CloseRPC();
15 return 0;
16}
12.39. Controle de Ativação/Desativação de Impedância
1/**
2* @brief Controle de ativação/desativação de impedância
3* @param [in] status 0: desativar; 1-ativar
4* @param [in] workSpace 0-espaço articular; 1-espaço cartesiano
5* @param [in] forceThreshold Limiar de força de ativação (N)
6* @param [in] m Parâmetro de massa
7* @param [in] b Parâmetro de amortecimento
8* @param [in] k Parâmetro de rigidez
9* @param [in] maxV Velocidade linear máxima (mm/s)
10* @param [in] maxVA Aceleração linear máxima (mm/s²)
11* @param [in] maxW Velocidade angular máxima (°/s)
12* @param [in] maxWA Aceleração angular máxima (°/s²)
13* @return Código de erro
14*/
15public int ImpedanceControlStartStop(int status, int workSpace, double[] forceThreshold, double[] m, double[] b, double[] k, double maxV, double maxVA, double maxW, double maxWA)
12.40. Exemplo de Código de Controle de Ativação/Desativação de Impedância do Robô
1public static int TestImpedanceControl(Robot robot)
2{
3 JointPos j1 = new JointPos(102.622, -135.990, 120.769, -73.950, -90.848, 35.507);
4 JointPos j2 = new JointPos(93.674, -80.062, 82.947, -92.199, -90.967, 26.559);
5 DescPose desc_pos1 = new DescPose(136.552, -149.799, 449.532, 179.817, -1.172, 157.123);
6 DescPose desc_pos2 = new DescPose(136.540, -561.048, 449.542, 179.819, -1.172, 157.122);
7 DescPose offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
8 ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
9 int tool = 0;
10 int user = 0;
11 double vel = 100.0;
12 double acc = 200.0;
13 double ovl = 100.0;
14 double blendT = -1.0;
15 double blendR = -1.0;
16 int flag = 0;
17 int search = 0;
18 robot.SetSpeed(20);
19 int company = 17;
20 int device = 0;
21 int softversion = 0;
22 int bus = 1;
23 DeviceConfig con = new DeviceConfig(company, device, softversion, bus);
24 robot.FT_SetConfig(con);
25 robot.Sleep(1000);
26 robot.FT_GetConfig(con);
27 System.out.println("FT config:"+con.company+","+con.device+","+con.softwareVersion+"con"+ con.bus);
28 robot.Sleep(1000);
29 robot.FT_Activate(0);
30 robot.Sleep(1000);
31 robot.FT_Activate(1);
32 robot.Sleep(1000);
33 robot.Sleep(1000);
34 robot.FT_SetZero(0);
35 robot.Sleep(1000);
36 robot.FT_SetZero(1);
37 robot.Sleep(1000);
38 double[] forceThreshold = { 30,30,30,5,5,5 };
39 double[] m = { 0.1,0.1,0.1,0.02,0.02,0.02 };
40 double[] b = { 1,1,1,0.08,0.08,0.08 };
41 double[] k = { 0,0,0,0,0,0 };
42 int rtn = robot.ImpedanceControlStartStop(1, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
43 System.out.println("ImpedanceControlStartStop errcode:"+ rtn);
44 rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
45 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
46 rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
47 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
48 rtn = robot.MoveL(desc_pos1, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
49 rtn = robot.MoveL(desc_pos2, tool, user, vel, acc, ovl, blendR, 0, epos, search, flag, offset_pos, -1,0,-1, 1);
50 System.out.println("movel errcode:"+ rtn);
51 robot.ImpedanceControlStartStop(0, 1, forceThreshold, m, b, k, 1000, 500, 100, 100);
52 robot.CloseRPC();
53 return 0;
54}
12.41. Ativar Função de Compensação de Torque e Coeficientes de Compensação
1/**
2* @brief Ativa a função de compensação de torque e coeficientes de compensação
3* @param status Interruptor, 0-desativar; 1-ativar
4* @param torqueCoeff Coeficientes de compensação de torque J1-J6 [0-1]
5* @return Código de erro
6*/
7public int SetCoderCompenParams(int status, double[] torqueCoeff)