9. Riproduzione Traiettoria Robot

9.1. Impostare Parametri Registrazione Traiettoria TPD

 1/**
 2* @brief  Impostare Parametri Registrazione Traiettoria TPD
 3* @param  [in] type  Tipo dati registrazione, 1-posizione articolare
 4* @param  [in] name  Nome file traiettoria
 5* @param  [in] period_ms  Periodo campionamento dati, valori fissi 2ms o 4ms o 8ms
 6* @param  [in] di_choose  Selezione DI, bit0~bit7 corrispondono DI0~DI7 quadro controllo, bit8~bit9 corrispondono DI0~DI1 estremità, 0-non selezionato, 1-selezionato
 7* @param  [in] do_choose  Selezione DO, bit0~bit7 corrispondono DO0~DO7 quadro controllo, bit8~bit9 corrispondono DO0~DO1 estremità, 0-non selezionato, 1-selezionato
 8* @return   Codice errore
 9*/
10int SetTPDParam(int type, string name, int period_ms, UInt16 di_choose, UInt16 do_choose);

9.2. Iniziare Registrazione Traiettoria TPD

 1/**
 2* @brief  Iniziare Registrazione Traiettoria TPD
 3* @param  [in] type  Tipo dati registrazione, 1-posizione articolare
 4* @param  [in] name  Nome file traiettoria
 5* @param  [in] period_ms  Periodo campionamento dati, valori fissi 2ms o 4ms o 8ms
 6* @param  [in] di_choose  Selezione DI, bit0~bit7 corrispondono DI0~DI7 quadro controllo, bit8~bit9 corrispondono DI0~DI1 estremità, 0-non selezionato, 1-selezionato
 7* @param  [in] do_choose  Selezione DO, bit0~bit7 corrispondono DO0~DO7 quadro controllo, bit8~bit9 corrispondono DO0~DO1 estremità, 0-non selezionato, 1-selezionato
 8* @return   Codice errore
 9*/
10int SetTPDStart(int type, string name, int period_ms, UInt16 di_choose, UInt16 do_choose);

9.3. Arrestare Registrazione Traiettoria TPD

1/**
2* @brief  Arrestare Registrazione Traiettoria TPD
3* @return   Codice errore
4*/
5int SetWebTPDStop();

9.4. Eliminare Registrazione Traiettoria TPD

1/**
2* @brief  Eliminare Registrazione Traiettoria TPD
3* @param  [in] name  Nome file traiettoria
4* @return   Codice errore
5*/
6int SetTPDDelete(string name);

9.5. Precaricamento Traiettoria TPD

1/**
2* @brief  Precaricamento Traiettoria
3* @param  [in] name  Nome file traiettoria
4* @return   Codice errore
5*/
6int LoadTPD(string name);

9.6. Ottenere Posa Iniziale Traiettoria TPD

1/**
2* @brief Ottenere Posa Iniziale Traiettoria
3* @param [in] name  Nome file traiettoria
4* @param [out] desc_pose Posa iniziale traiettoria
5* @return Codice errore
6*/
7int GetTPDStartPose(string name, ref DescPose desc_pose);

9.7. Riproduzione Traiettoria TPD

1/**
2* @brief  Riproduzione Traiettoria
3* @param  [in] name  Nome file traiettoria
4* @param  [in] blend 0-non smoothing, 1-smoothing
5* @param  [in] ovl  Percentuale scala velocità, range [0~100]
6* @return   Codice errore
7*/
8int MoveTPD(string name, byte blend, float ovl);

9.8. Esempio Codice Registrazione Traiettoria TPD Robot

 1private void btnTPDMove_Click(object sender, EventArgs e)
 2{
 3    int type = 1;
 4    string name = "tpd2025";
 5    int period_ms = 4;
 6    ushort di_choose = 0;
 7    ushort do_choose = 0;
 8
 9    robot.SetTPDParam(type, name, period_ms, di_choose, do_choose);
10
11    robot.Mode(1);
12    Thread.Sleep(1000);
13    robot.DragTeachSwitch(1);
14    robot.SetTPDStart(type, name, period_ms, di_choose, do_choose);
15    Thread.Sleep(10000);
16    robot.SetWebTPDStop();
17    robot.DragTeachSwitch(0);
18
19    float ovl = 100.0f;
20    byte blend = 0;
21
22    DescPose start_pose = new DescPose();
23
24    int rtn = robot.LoadTPD(name);
25    Console.WriteLine("LoadTPD rtn is: {0}\n", rtn);
26
27    robot.GetTPDStartPose(name, ref start_pose);
28    Console.WriteLine("start pose, xyz is: {0} {1} {2}. rpy is: {3} {4} {5} \n",
29        start_pose.tran.x, start_pose.tran.y, start_pose.tran.z,
30        start_pose.rpy.rx, start_pose.rpy.ry, start_pose.rpy.rz);
31    robot.MoveCart(start_pose, 0, 0, 100, 100, ovl, -1, -1);
32    Thread.Sleep(1000);
33
34    rtn = robot.MoveTPD(name, blend, ovl);
35    Console.WriteLine("MoveTPD rtn is: {0}\n", rtn);
36    Thread.Sleep(5000);
37
38    robot.SetTPDDelete(name);
39}

9.9. Pre-elaborazione File Traiettoria Esterna

1/**
2* @brief Pre-elaborazione File Traiettoria Esterna
3* @param [in] name Nome file traiettoria
4* @param [in] ovl Percentuale scala velocità, range [0~100]
5* @param [in] opt 1-punto controllo, default 1
6* @return Codice errore
7*/
8int LoadTrajectoryJ(string name, float ovl, int opt);

9.10. Riproduzione File Traiettoria Esterna

1/**
2* @brief Riproduzione File Traiettoria Esterna
3* @return Codice errore
4*/
5int MoveTrajectoryJ();

9.11. Ottenere Posizione Iniziale Traiettoria File Traiettoria

1/**
2* @brief Ottenere Posizione Iniziale Traiettoria File Traiettoria
3* @param [in] name Nome file traiettoria
4* @param [out] desc_pose Posa iniziale traiettoria
5* @return Codice errore
6*/
7int GetTrajectoryStartPose(string name, ref DescPose desc_pose);

9.12. Ottenere Numero Punto Traiettoria File Traiettoria

1/**
2* @brief Ottenere Numero Punto Traiettoria
3* @param [out] pnum Numero punto traiettoria
4* @return Codice errore
5*/
6int GetTrajectoryPointNum(ref int pnum);

9.13. Impostare la Velocità Durante l’Esecuzione della Traiettoria

1/**
2* @brief Impostare la velocità durante l'esecuzione della traiettoria
3* @param [in] ovl Percentuale di velocità [0-100.0]
4* @param [in] mode Modalità; 0-modalità di riduzione della velocità; 1-commutazione diretta
5* @return Codice di errore
6*/
7errno_t SetTrajectoryJSpeed(float ovl, int mode = 0);

9.14. Esempio di Codice per Impostare la Velocità del Robot Durante l’Esecuzione della Traiettoria

 1int TestSetTrajectoryJSpeed()
 2{
 3    ROBOT_STATE_PKG pkg = {};
 4    FRRobot robot;
 5    robot.LoggerInit();
 6    robot.SetLoggerLevel(1);
 7    robot.SetReConnectParam(true, 30000, 500);
 8    int rtn = robot.RPC("192.168.58.2");
 9    if (rtn != 0)
10    {
11        return -1;
12    }
13
14    rtn = robot.TrajectoryJUpLoad("D://zUP/trajHelix_aima_1.txt");
15    printf("Upload TrajectoryJ A %d\n", rtn);
16    char traj_file_name[90] = "/fruser/traj/trajHelix_aima_1.txt";
17    rtn = robot.LoadTrajectoryJ(traj_file_name, 100, 1);
18    printf("LoadTrajectoryJ %s, rtn is: %d\n", traj_file_name, rtn);
19    DescPose traj_start_pose;
20    memset(&traj_start_pose, 0, sizeof(DescPose));
21    rtn = robot.GetTrajectoryStartPose(traj_file_name, &traj_start_pose);
22    printf("GetTrajectoryStartPose is: %d\n", rtn);
23    printf("desc_pos:%f,%f,%f,%f,%f,%f\n", traj_start_pose.tran.x, traj_start_pose.tran.y, traj_start_pose.tran.z, traj_start_pose.rpy.rx, traj_start_pose.rpy.ry, traj_start_pose.rpy.rz);
24    std::this_thread::sleep_for(std::chrono::seconds(1));
25    robot.SetSpeed(50);
26    robot.MoveCart(&traj_start_pose, 0, 0, 100, 100, 100, -1, -1);
27    int traj_num = 0;
28    rtn = robot.GetTrajectoryPointNum(&traj_num);
29    printf("GetTrajectoryStartPose rtn is: %d, traj num is: %d\n", rtn, traj_num);
30    rtn = robot.MoveTrajectoryJ();
31    printf("MoveTrajectoryJ rtn is: %d\n", rtn);
32    robot.Sleep(1000);
33    robot.GetRobotRealTimeState(&pkg);
34    int trajspeedMode = 1;
35    while (pkg.motion_done == 0)
36    {
37        robot.GetRobotRealTimeState(&pkg);
38        rtn = robot.SetTrajectoryJSpeed(10.0, trajspeedMode);
39        printf("SetTrajectoryJSpeed is: %d\n", rtn);
40        robot.Sleep(1000);
41        rtn = robot.SetTrajectoryJSpeed(80.0, trajspeedMode);
42        printf("SetTrajectoryJSpeed is: %d\n", rtn);
43        robot.Sleep(1000);
44    }
45    robot.CloseRPC();
46    robot.Sleep(1000000);
47    return 0;
48}

9.15. Impostare Forza e Coppia durante Esecuzione Traiettoria File Traiettoria

1/**
2* @brief Impostare Forza e Coppia durante Esecuzione Traiettoria File Traiettoria
3* @param [in] ft Forza e coppia tre direzioni, unità N e Nm
4* @return Codice errore
5*/
6int SetTrajectoryJForceTorque(ForceTorque ft);

9.16. Impostare Forza lungo Direzione x durante Esecuzione Traiettoria

1/**
2* @brief Impostare Forza lungo Direzione x durante Esecuzione Traiettoria
3* @param [in] fx  Forza lungo direzione x, unità N
4* @return Codice errore
5*/
6int SetTrajectoryJForceFx(double fx);

9.17. Impostare Forza lungo Direzione y durante Esecuzione Traiettoria

1/**
2* @brief Impostare Forza lungo Direzione y durante Esecuzione Traiettoria
3* @param [in] fy  Forza lungo direzione y, unità N
4* @return Codice errore
5*/
6int SetTrajectoryJForceFy(double fy);

9.18. Impostare Forza lungo Direzione z durante Esecuzione Traiettoria

1/**
2* @brief Impostare Forza lungo Direzione z durante Esecuzione Traiettoria
3* @param [in] fz  Forza lungo direzione z, unità N
4* @return Codice errore
5*/
6int SetTrajectoryJForceFz(double fz);

9.19. Impostare Coppia attorno Asse x durante Esecuzione Traiettoria

1/**
2* @brief Impostare Coppia attorno Asse x durante Esecuzione Traiettoria
3* @param [in] tx  Coppia attorno asse x, unità Nm
4* @return Codice errore
5*/
6int SetTrajectoryJTorqueTx(double tx);

9.20. Impostare Coppia attorno Asse y durante Esecuzione Traiettoria

1/**
2* @brief Impostare Coppia attorno Asse y durante Esecuzione Traiettoria
3* @param [in] ty  Coppia attorno asse y, unità Nm
4* @return Codice errore
5*/
6int SetTrajectoryJTorqueTy(double ty);

9.21. Impostare Coppia attorno Asse z durante Esecuzione Traiettoria

1/**
2* @brief Impostare Coppia attorno Asse z durante Esecuzione Traiettoria
3* @param [in] tz  Coppia attorno asse z, unità Nm
4* @return Codice errore
5*/
6int SetTrajectoryJTorqueTz(double tz);

9.22. Caricare File Traiettoria J

1/**
2* @brief Caricare File Traiettoria J
3* @param [in] filePath Nome percorso completo file traiettoria caricamento   C://test/testJ.txt
4* @return Codice errore
5*/
6int TrajectoryJUpLoad(string filePath);

9.23. Eliminare File Traiettoria J

1/**
2* @brief Eliminare File Traiettoria J
3* @param [in] fileName Nome file testJ.txt
4* @return Codice errore
5*/
6int TrajectoryJDelete(string fileName);

9.24. Esempio Codice Riproduzione File Traiettoria J Robot

 1private void button33_Click(object sender, EventArgs e)
 2{
 3    int rtn = robot.TrajectoryJUpLoad("D://zUP/spray_traj1.txt");
 4    Console.WriteLine("Upload TrajectoryJ A {0}\n", rtn);
 5
 6    string traj_file_name = "/fruser/traj/spray_traj1.txt";
 7    rtn = robot.LoadTrajectoryJ(traj_file_name, 100, 1);
 8    Console.WriteLine("LoadTrajectoryJ {0}, rtn is: {1}\n", traj_file_name, rtn);
 9
10    DescPose traj_start_pose = new DescPose();
11    rtn = robot.GetTrajectoryStartPose(traj_file_name, ref traj_start_pose);
12    Console.WriteLine("GetTrajectoryStartPose is: {0}\n", rtn);
13    Console.WriteLine("desc_pos:{0},{1},{2},{3},{4},{5}\n",
14        traj_start_pose.tran.x, traj_start_pose.tran.y, traj_start_pose.tran.z,
15        traj_start_pose.rpy.rx, traj_start_pose.rpy.ry, traj_start_pose.rpy.rz);
16
17    Thread.Sleep(1000);
18
19    robot.SetSpeed(50);
20    robot.MoveCart(traj_start_pose, 0, 0, 100, 100, 100, -1, -1);
21
22    int traj_num = 0;
23    rtn = robot.GetTrajectoryPointNum(ref traj_num);
24    Console.WriteLine("GetTrajectoryStartPose rtn is: {0}, traj num is: {1}\n", rtn, traj_num);
25
26    rtn = robot.SetTrajectoryJSpeed(50.0f);
27    Console.WriteLine("SetTrajectoryJSpeed is: {0}\n", rtn);
28
29    ForceTorque traj_force = new ForceTorque();
30    traj_force.fx = 10;
31    rtn = robot.SetTrajectoryJForceTorque(traj_force);
32    Console.WriteLine("SetTrajectoryJForceTorque rtn is: {0}\n", rtn);
33
34    rtn = robot.SetTrajectoryJForceFx(10.0f);
35    Console.WriteLine("SetTrajectoryJForceFx rtn is: {0}\n", rtn);
36
37    rtn = robot.SetTrajectoryJForceFy(0.0f);
38    Console.WriteLine("SetTrajectoryJForceFy rtn is: {0}\n", rtn);
39
40    rtn = robot.SetTrajectoryJForceFz(0.0f);
41    Console.WriteLine("SetTrajectoryJForceFz rtn is: {0}\n", rtn);
42
43    rtn = robot.SetTrajectoryJTorqueTx(10.0f);
44    Console.WriteLine("SetTrajectoryJTorqueTx rtn is: {0}\n", rtn);
45
46    rtn = robot.SetTrajectoryJTorqueTy(10.0f);
47    Console.WriteLine("SetTrajectoryJTorqueTy rtn is: {0}\n", rtn);
48
49    rtn = robot.SetTrajectoryJTorqueTz(10.0f);
50    Console.WriteLine("SetTrajectoryJTorqueTz rtn is: {0}\n", rtn);
51
52    rtn = robot.MoveTrajectoryJ();
53    Console.WriteLine("MoveTrajectoryJ rtn is: {0}\n", rtn);
54}

9.25. Pre-elaborazione Traiettoria (Look-ahead Traiettoria)

 1/**
 2* @brief  Pre-elaborazione Traiettoria (Look-ahead Traiettoria)
 3* @param  [in] name  Nome file traiettoria
 4* @param  [in] mode Modalità campionamento, 0-non campionamento; 1-campionamento intervallo dati uguale; 2-campionamento limite errore uguale
 5* @param  [in] errorLim Limite errore, effettivo quando usa interpolazione lineare
 6* @param  [in] type Metodo smoothing, 0-smoothing Bezier
 7* @param  [in] precision Precisione smoothing, effettivo quando usa smoothing Bezier
 8* @param  [in] vamx Velocità massima impostata, mm/s
 9* @param  [in] amax Accelerazione massima impostata, mm/s2
10* @param  [in] jmax Jerk massimo impostato, mm/s3
11* @return   Codice errore
12*/
13int LoadTrajectoryLA(string name, int mode, double errorLim, int type, double precision, double vamx, double amax, double jmax);

9.26. Riproduzione Traiettoria (Look-ahead Traiettoria)

1/**
2* @brief  Riproduzione Traiettoria (Look-ahead Traiettoria)
3* @return   Codice errore
4*/
5int MoveTrajectoryLA();

9.27. Esempio Codice Riproduzione Traiettoria (Look-ahead Traiettoria)

 1private void button8_Click(object sender, EventArgs e)
 2{
 3    int rtn = 0;
 4
 5    string nameA = "/fruser/traj/A.txt";
 6    string nameB = "/fruser/traj/B.txt";
 7
 8    rtn = robot.LoadTrajectoryLA(nameB, 0, 0, 0, 1, 100.0, 100.0, 1000.0);    // Interpolazione lineare
 9    Console.WriteLine($"LoadTrajectoryLA rtn is {rtn}");
10
11    DescPose startPos = new DescPose(0, 0, 0, 0, 0, 0);
12    robot.GetTrajectoryStartPose(nameA, ref startPos);
13
14    //
15    robot.MoveCart(startPos, 1, 0, (float)100.0, (float)100.0, (float)100.0, -1, -1);
16
17    rtn = robot.MoveTrajectoryLA();
18    Console.WriteLine($"MoveTrajectoryLA rtn is {rtn}");
19}

9.28. Spostarsi al Punto di Inizio della Registrazione della Traiettoria TPD

1/**
2* @brief Spostarsi al punto di inizio della registrazione della traiettoria TPD
3* @param [in] name Nome del file di traiettoria
4* @param [in] moveType Tipo di movimento; 0-PTP; 1-LIN
5* @param [in] ovl Percentuale di scala della velocità, intervallo [0~100]
6* @return Codice di errore
7*/
8public int MoveToTPDStart(string name, int moveType, double ovl)

9.29. Esempio di Codice SDK per Spostarsi al Punto di Inizio della Registrazione della Traiettoria TPD

 1void testTPDmove()
 2{
 3    string name = "tpd2025";
 4    int type = 1;
 5    int period_ms = 4;
 6    int rtn = 0;
 7    UInt16 di_choose = 0;
 8    UInt16 do_choose = 0;
 9
10    robot.SetTPDParam(type, name, period_ms, di_choose, do_choose);
11
12    robot.Mode(1);
13    Thread.Sleep(3000);
14    robot.DragTeachSwitch(1);
15    robot.SetTPDStart(type, name, period_ms, di_choose, do_choose);
16    Thread.Sleep(3000);
17    robot.SetWebTPDStop();
18    robot.DragTeachSwitch(0);
19
20    Thread.Sleep(1000);
21    float ovl = 100.0f;
22    byte blend = 0;
23    DescPose start_pose = new DescPose();
24    rtn = robot.LoadTPD(name);
25    Console.WriteLine($"LoadTPD rtn is:{rtn}\n");
26
27    robot.GetTPDStartPose(name, ref start_pose);
28    Console.WriteLine($"start pose, xyz is: %f %f %f. rpy is: {start_pose.tran.x},{start_pose.tran.y}, {start_pose.tran.z}, {start_pose.rpy.rx}, {start_pose.rpy.ry}, {start_pose.rpy.rz}");
29
30    rtn = robot.MoveToTPDStart(name, 0, 100.0);
31
32    rtn = robot.MoveTPD(name, blend, ovl);
33    Thread.Sleep(5000*5);
34
35    robot.SetTPDDelete(name);
36}