9. Odtwarzanie trajektorii robota
9.1. Ustawianie parametrów rejestracji trajektorii TPD
1/**
2* @brief Ustawia parametry rejestracji trajektorii TPD
3* @param [in] type Typ rejestrowanych danych, 1-pozycja stawów
4* @param [in] name Nazwa pliku trajektorii
5* @param [in] period_ms Okres próbkowania danych, stałe wartości 2 ms, 4 ms lub 8 ms
6* @param [in] di_choose Wybór DI, bity 0~7 odpowiadają DI0~DI7 szafy sterowniczej, bity 8~9 odpowiadają DI0~DI1 końcówki, 0-niewybrany, 1-wybrany
7* @param [in] do_choose Wybór DO, bity 0~7 odpowiadają DO0~DO7 szafy sterowniczej, bity 8~9 odpowiadają DO0~DO1 końcówki, 0-niewybrany, 1-wybrany
8* @return Kod błędu
9*/
10errno_t SetTPDParam(int type, char name[30], int period_ms, uint16_t di_choose, uint16_t do_choose);
9.2. Rozpoczęcie rejestracji trajektorii TPD
1/**
2* @brief Rozpoczyna rejestrację trajektorii TPD
3* @param [in] type Typ rejestrowanych danych, 1-pozycja stawów
4* @param [in] name Nazwa pliku trajektorii
5* @param [in] period_ms Okres próbkowania danych, stałe wartości 2 ms, 4 ms lub 8 ms
6* @param [in] di_choose Wybór DI, bity 0~7 odpowiadają DI0~DI7 szafy sterowniczej, bity 8~9 odpowiadają DI0~DI1 końcówki, 0-niewybrany, 1-wybrany
7* @param [in] do_choose Wybór DO, bity 0~7 odpowiadają DO0~DO7 szafy sterowniczej, bity 8~9 odpowiadają DO0~DO1 końcówki, 0-niewybrany, 1-wybrany
8* @return Kod błędu
9*/
10errno_t SetTPDStart(int type, char name[30], int period_ms, uint16_t di_choose, uint16_t do_choose);
9.3. Zatrzymanie rejestracji trajektorii TPD
1/**
2* @brief Zatrzymuje rejestrację trajektorii TPD
3* @return Kod błędu
4*/
5errno_t SetWebTPDStop();
9.4. Usuwanie rejestracji trajektorii TPD
1/**
2* @brief Usuwa rejestrację trajektorii TPD
3* @param [in] name Nazwa pliku trajektorii
4* @return Kod błędu
5*/
6errno_t SetTPDDelete(char name[30]);
9.5. Wstępne ładowanie trajektorii TPD
1/**
2* @brief Wstępne ładowanie trajektorii TPD
3* @param [in] name Nazwa pliku trajektorii
4* @return Kod błędu
5*/
6errno_t LoadTPD(char name[30]);
9.6. Odtwarzanie trajektorii TPD
1/**
2* @brief Odtwarzanie trajektorii TPD
3* @param [in] name Nazwa pliku trajektorii
4* @param [in] blend 0-niewygładzone, 1-wygładzone
5* @param [in] ovl Procent skalowania prędkości, zakres [0~100]
6* @return Kod błędu
7*/
8errno_t MoveTPD(char name[30], uint8_t blend, float ovl);
9.7. Pobieranie początkowej pozy i orientacji TPD
1/**
2* @brief Pobiera początkową pozy i orientację TPD
3* @param [in] name Nazwa pliku TPD, bez rozszerzenia
4* @return Kod błędu
5*/
6errno_t GetTPDStartPose(char name[30], DescPose *desc_pose);
9.8. Ruch do punktu początkowego rejestracji trajektorii TPD
1/**
2* @brief Ruch do punktu początkowego rejestracji trajektorii TPD
3* @param [in] name Nazwa pliku trajektorii
4* @param [in] moveType Typ ruchu; 0-PTP; 1-LIN
5* @param [in] ovl Procent skalowania prędkości, zakres [0~100]
6* @return Kod błędu
7*/
8errno_t MoveToTPDStart(char name[30], uint8_t moveType, float ovl);
9.9. Przykład kodu rejestracji trajektorii TPD robota
1int TestTPD(void)
2{
3ROBOT_STATE_PKG pkg = {};
4FRRobot robot;
5robot.LoggerInit();
6robot.SetLoggerLevel(1);
7int rtn = robot.RPC("192.168.58.2");
8if (rtn != 0)
9{
10 return -1;
11}
12robot.SetReConnectParam(true, 30000, 500);
13int type = 1;
14char name[30] = "tpd2025";
15int period_ms = 4;
16uint16_t di_choose = 0;
17uint16_t do_choose = 0;
18robot.SetTPDParam(type, name, period_ms, di_choose, do_choose);
19robot.Mode(1);
20robot.Sleep(1000);
21robot.DragTeachSwitch(1);
22robot.SetTPDStart(type, name, period_ms, di_choose, do_choose);
23robot.Sleep(3000);
24robot.SetWebTPDStop();
25robot.DragTeachSwitch(0);
26robot.Sleep(1000);
27float ovl = 100.0;
28uint8_t blend = 0;
29DescPose start_pose = {};
30rtn = robot.LoadTPD(name);
31printf("LoadTPD rtn is: %d\n", rtn);
32robot.GetTPDStartPose(name, &start_pose);
33printf("start pose, xyz is: %f %f %f. rpy is: %f %f %f \n", start_pose.tran.x, start_pose.tran.y, start_pose.tran.z, start_pose.rpy.rx, start_pose.rpy.ry, start_pose.rpy.rz);
34rtn = robot.MoveToTPDStart(name, 0, 100);
35printf("MoveToTPDStart rtn is: %d\n", rtn);
36rtn = robot.MoveTPD(name, blend, ovl);
37printf("MoveTPD rtn is: %d\n", rtn);
38std::this_thread::sleep_for(std::chrono::milliseconds(5000));
39robot.SetTPDDelete(name);
40robot.CloseRPC();
41return 0;
42}
9.10. Wstępne przetwarzanie trajektorii
1/**
2* @brief Wstępne przetwarzanie trajektorii
3* @param [in] name Nazwa pliku trajektorii
4* @param [in] ovl Procent skalowania prędkości, zakres [0~100]
5* @param [in] opt 1-punkt kontrolny, domyślnie 1
6* @return Kod błędu
7*/
8errno_t LoadTrajectoryJ(char name[30], float ovl, int opt);
9.11. Odtwarzanie trajektorii
1/**
2* @brief Odtwarzanie trajektorii
3* @return Kod błędu
4*/
5errno_t MoveTrajectoryJ();
9.12. Pobieranie początkowej pozy i orientacji trajektorii
1/**
2* @brief Pobiera początkową pozy i orientację trajektorii
3* @param [in] name Nazwa pliku trajektorii
4* @return Kod błędu
5*/
6errno_t GetTrajectoryStartPose(char name[30], DescPose *desc_pose);
9.13. Pobieranie numeru punktu trajektorii
1/**
2* @brief Pobiera numer punktu trajektorii
3* @return Kod błędu
4*/
5errno_t GetTrajectoryPointNum(int *pnum);
9.14. Ustawianie prędkości podczas wykonywania trajektorii
1/**
2* @brief Ustawia prędkość podczas wykonywania trajektorii
3* @param [in] ovl Procent prędkości [0-100.0]
4* @param [in] mode Tryb; 0-tryb zmniejszania prędkości; 1-bezpośrednie przełączenie
5* @return Kod błędu
6*/
7errno_t SetTrajectoryJSpeed(float ovl, int mode = 0);
9.15. Przykład kodu ustawiania prędkości podczas wykonywania trajektorii robota
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/horse.txt");
15 printf("Upload TrajectoryJ A %d\n", rtn);
16 char traj_file_name[90] = "horse.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.16. Ustawianie siły i momentu podczas wykonywania trajektorii
1/**
2* @brief Ustawia siłę i moment podczas wykonywania trajektorii
3* @param [in] ft Siły i momenty w trzech kierunkach, jednostka N i Nm
4* @return Kod błędu
5*/
6errno_t SetTrajectoryJForceTorque(ForceTorque *ft);
9.17. Ustawianie siły wzdłuż osi X podczas wykonywania trajektorii
1/**
2* @brief Ustawia siłę wzdłuż osi X podczas wykonywania trajektorii
3* @param [in] fx Siła wzdłuż osi X, jednostka N
4* @return Kod błędu
5*/
6errno_t SetTrajectoryJForceFx(double fx);
9.18. Ustawianie siły wzdłuż osi Y podczas wykonywania trajektorii
1/**
2* @brief Ustawia siłę wzdłuż osi Y podczas wykonywania trajektorii
3* @param [in] fy Siła wzdłuż osi Y, jednostka N
4* @return Kod błędu
5*/
6errno_t SetTrajectoryJForceFy(double fy);
9.19. Ustawianie siły wzdłuż osi Z podczas wykonywania trajektorii
1/**
2* @brief Ustawia siłę wzdłuż osi Z podczas wykonywania trajektorii
3* @param [in] fz Siła wzdłuż osi X, jednostka N
4* @return Kod błędu
5*/
6errno_t SetTrajectoryJForceFz(double fz);
9.20. Ustawianie momentu wokół osi X podczas wykonywania trajektorii
1/**
2* @brief Ustawia moment wokół osi X podczas wykonywania trajektorii
3* @param [in] tx Moment wokół osi X, jednostka Nm
4* @return Kod błędu
5*/
6errno_t SetTrajectoryJTorqueTx(double tx);
9.21. Ustawianie momentu wokół osi Y podczas wykonywania trajektorii
1/**
2* @brief Ustawia moment wokół osi Y podczas wykonywania trajektorii
3* @param [in] ty Moment wokół osi Y, jednostka Nm
4* @return Kod błędu
5*/
6errno_t SetTrajectoryJTorqueTy(double ty);
9.22. Ustawianie momentu wokół osi Z podczas wykonywania trajektorii
1/**
2* @brief Ustawia moment wokół osi Z podczas wykonywania trajektorii
3* @param [in] tz Moment wokół osi Z, jednostka Nm
4* @return Kod błędu
5*/
6errno_t SetTrajectoryJTorqueTz(double tz);
9.23. Przesyłanie pliku trajektorii J
Nowe w wersji V3.7.7.
1/**
2 * @brief Przesyła plik trajektorii J
3 * @param [in] filePath Pełna ścieżka przesyłanego pliku trajektorii C://test/testJ.txt
4 * @return Kod błędu
5 */
6errno_t TrajectoryJUpLoad(const std::string& filePath);
9.24. Usuwanie pliku trajektorii J
Nowe w wersji V3.7.7.
1/**
2 * @brief Usuwa plik trajektorii J
3 * @param [in] fileName Nazwa pliku testJ.txt
4 * @return Kod błędu
5 */
6errno_t TrajectoryJDelete(const std::string& fileName);
9.25. Przykład kodu odtwarzania pliku trajektorii J robota
1int TestTraj(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 rtn = robot.TrajectoryJUpLoad("D://zUP/traj1.txt");
14 printf("Upload TrajectoryJ A %d\n", rtn);
15 char traj_file_name[30] = "/fruser/traj/traj1.txt";
16 rtn = robot.LoadTrajectoryJ(traj_file_name, 100, 1);
17 printf("LoadTrajectoryJ %s, rtn is: %d\n", traj_file_name, rtn);
18 DescPose traj_start_pose;
19 memset(&traj_start_pose, 0, sizeof(DescPose));
20 rtn = robot.GetTrajectoryStartPose(traj_file_name, &traj_start_pose);
21 printf("GetTrajectoryStartPose is: %d\n", rtn);
22 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);
23 std::this_thread::sleep_for(std::chrono::seconds(1));
24 robot.SetSpeed(50);
25 robot.MoveCart(&traj_start_pose, 0, 0, 100, 100, 100, -1, -1);
26 int traj_num = 0;
27 rtn = robot.GetTrajectoryPointNum(&traj_num);
28 printf("GetTrajectoryStartPose rtn is: %d, traj num is: %d\n", rtn, traj_num);
29 rtn = robot.SetTrajectoryJSpeed(50.0);
30 printf("SetTrajectoryJSpeed is: %d\n", rtn);
31 ForceTorque traj_force;
32 memset(&traj_force, 0, sizeof(ForceTorque));
33 traj_force.fx = 10;
34 rtn = robot.SetTrajectoryJForceTorque(&traj_force);
35 printf("SetTrajectoryJForceTorque rtn is: %d\n", rtn);
36 rtn = robot.SetTrajectoryJForceFx(10.0);
37 printf("SetTrajectoryJForceFx rtn is: %d\n", rtn);
38 rtn = robot.SetTrajectoryJForceFy(0.0);
39 printf("SetTrajectoryJForceFy rtn is: %d\n", rtn);
40 rtn = robot.SetTrajectoryJForceFz(0.0);
41 printf("SetTrajectoryJForceFz rtn is: %d\n", rtn);
42 rtn = robot.SetTrajectoryJTorqueTx(10.0);
43 printf("SetTrajectoryJTorqueTx rtn is: %d\n", rtn);
44 rtn = robot.SetTrajectoryJTorqueTy(10.0);
45 printf("SetTrajectoryJTorqueTy rtn is: %d\n", rtn);
46 rtn = robot.SetTrajectoryJTorqueTz(10.0);
47 printf("SetTrajectoryJTorqueTz rtn is: %d\n", rtn);
48 rtn = robot.MoveTrajectoryJ();
49 printf("MoveTrajectoryJ rtn is: %d\n", rtn);
50 robot.CloseRPC();
51 return 0;
52}
9.26. Wstępne przetwarzanie trajektorii (planowanie trajektorii z wyprzedzeniem)
1/**
2* @brief Wstępne przetwarzanie trajektorii (planowanie trajektorii z wyprzedzeniem)
3* @param [in] name Nazwa pliku trajektorii
4* @param [in] mode Tryb próbkowania, 0-bez próbkowania; 1-próbkowanie w równych odstępach danych; 2-próbkowanie z ograniczeniem błędu
5* @param [in] errorLim Ograniczenie błędu, stosowane przy aproksymacji liniowej
6* @param [in] type Sposób wygładzania, 0-wygładzanie Beziera
7* @param [in] precision Dokładność wygładzania, stosowana przy wygładzaniu Beziera
8* @param [in] vamx Ustawiona maksymalna prędkość, mm/s
9* @param [in] amax Ustawione maksymalne przyspieszenie, mm/s2
10* @param [in] jmax Ustawione maksymalne zryw, mm/s3
11* @param [in] flag Przełącznik włączenia wyprzedzenia z jednostajną prędkością 0-niewłączone; 1-włączone
12* @return Kod błędu
13*/
14errno_t LoadTrajectoryLA(char name[30], int mode, double errorLim, int type, double precision, double vamx, double amax, double jmax, int flag = 0);
9.27. Odtwarzanie trajektorii (planowanie trajektorii z wyprzedzeniem)
1/**
2* @brief Odtwarzanie trajektorii (planowanie trajektorii z wyprzedzeniem)
3* @return Kod błędu
4*/
5errno_t MoveTrajectoryLA();
9.28. Przykład kodu odtwarzania trajektorii (planowanie trajektorii z wyprzedzeniem)
1int TestLoadTrajLA(void)
2{
3 ROBOT_STATE_PKG pkg = {};
4 FRRobot robot;
5 robot.LoggerInit();
6 robot.SetLoggerLevel(1);
7 int rtn = robot.RPC("192.168.58.2");
8 if (rtn != 0)
9 {
10 return -1;
11 }
12 robot.SetReConnectParam(true, 30000, 500);
13 rtn = robot.TrajectoryJUpLoad("D://zUP/traj.txt");
14 printf("Upload TrajectoryJ A %d\n", rtn);
15 char traj_file_name[30] = "/fruser/traj/traj.txt";
16 rtn = robot.LoadTrajectoryLA(traj_file_name, 1, 2, 0, 2, 100, 200, 1000);
17 printf("LoadTrajectoryLA %s, rtn is: %d\n", traj_file_name, rtn);
18 DescPose traj_start_pose;
19 memset(&traj_start_pose, 0, sizeof(DescPose));
20 rtn = robot.GetTrajectoryStartPose(traj_file_name, &traj_start_pose);
21 printf("GetTrajectoryStartPose is: %d\n", rtn);
22 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);
23 std::this_thread::sleep_for(std::chrono::seconds(1));
24 robot.SetSpeed(50);
25 robot.MoveCart(&traj_start_pose, 0, 0, 100, 100, 100, -1, -1);
26 rtn = robot.MoveTrajectoryLA();
27 printf("MoveTrajectoryLA rtn is: %d\n", rtn);
28 robot.CloseRPC();
29 return 0;
30}