9. Robot trajectory reproduction
9.1. Set the parameters for TPD trajectory recording
1/ * *
2* @brief Set the TPD trajectory recording parameters
3* @param [in] type records data type, 1- joint position
4* @param [in] name Trajectory file name
5* @param [in] period_ms data sampling period, fixed value 2ms, 4ms or 8ms
6* @param [in] di_choose DI selection,bit0 to bit7 correspond to control box DI0 to DI7, bit8 to bit9 correspond to end DI0 to DI1, 0- no selection, 1- selection
7* @param [in] do_choose DO selection,bit0 to bit7 correspond to control box DO0 to DO7, bit8 to bit9 correspond to end DO0 to DO1, 0- do not select, 1- select
8* @return error code
9* /
10int SetTPDParam(int type, string name, int period_ms, UInt16 di_choose, UInt16 do_choose);
9.2. Start TPD trajectory recording
1/ * *
2* @brief Starts TPD trajectory recording
3* @param [in] type records data type, 1- joint position
4* @param [in] name Trajectory file name
5* @param [in] period_ms data sampling period, fixed value 2ms, 4ms or 8ms
6* @param [in] di_choose DI selection,bit0 to bit7 correspond to control box DI0 to DI7, bit8 to bit9 correspond to end DI0 to DI1, 0- no selection, 1- selection
7* @param [in] do_choose DO selection,bit0 to bit7 correspond to control box DO0 to DO7, bit8 to bit9 correspond to end DO0 to DO1, 0- do not select, 1- select
8* @return error code
9* /
10int SetTPDStart(int type, string name, int period_ms, UInt16 di_choose, UInt16 do_choose);
9.3. Stop TPD trajectory recording
1/ * *
2* @brief Stop TPD trajectory recording
3* @return error code
4* /
5int SetWebTPDStop();
9.4. Delete the TPD trajectory record
1/ * *
2* @brief Delete TPD trajectory records
3* @param [in] name Trajectory file name
4* @return error code
5* /
6int SetTPDDelete(string name);
9.5. TPD trajectory preloading
1/ * *
2* @brief Trajectory Preloading
3* @param [in] name Trajectory file name
4* @return error code
5* /
6int LoadTPD(string name);
9.6. Obtain the starting pose of the TPD trajectory
1/ * *
2* @brief Obtain the starting pose of the trajectory
3* @param [in] name Trajectory file name
4* @param [out] desc_pose trajectory starting pose
5* @return error code
6* /
7int GetTPDStartPose(string name, ref DescPose desc_pose);
9.7. TPD trajectory reproduction
1/ * *
2* @brief Trajectory Reproduction
3* @param [in] name Trajectory file name
4* @param [in] blend 0- not smooth, 1- smooth
5* @param [in] ovl speed scaling percentage, range [0 to 100]
6* @return error code
7* /
8int MoveTPD(string name, byte blend, float ovl);
9.8. A sample code for robot TPD trajectory recording
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. Preprocessing of external trajectory files
1/ * *
2* @brief External Trajectory File Preprocessing
3* @param [in] name Trajectory file name
4* @param [in] ovl speed scaling percentage, range [0 to 100]
5* @param [in] opt 1- Control point, default is 1
6* @return error code
7* /
8int LoadTrajectoryJ(string name, float ovl, int opt);
9.10. External trajectory file trajectory reproduction
1/ * *
2* @brief External Trajectory file trajectory reproduction
3* @return error code
4* /
5int MoveTrajectoryJ();
9.11. Obtain the starting position of the trajectory file’s trajectory
1/ * *
2* @brief Get the starting position of the trajectory file trajectory
3* @param [in] name Trajectory file name
4* @param [out] desc_pose trajectory starting pose
5* @return error code
6* /
7int GetTrajectoryStartPose(string name, ref DescPose desc_pose);
9.12. Obtain the trajectory point number in the trajectory file
1/ * *
2* @brief Get the trajectory point number
3* @param [out] pnum trajectory point number
4* @return error code
5* /
6int GetTrajectoryPointNum(ref int pnum);
9.13. Set Speed During Trajectory Execution
1/**
2* @brief Set speed during trajectory execution
3* @param [in] ovl Speed percentage [0-100.0]
4* @param [in] mode Mode; 0-speed reduction mode; 1-direct switching
5* @return Error code
6*/
7errno_t SetTrajectoryJSpeed(float ovl, int mode = 0);
9.14. Code Example for Setting Robot Speed During Trajectory Execution
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. Set the force and torque during the trajectory file’s trajectory operation
1/ * *
2* @brief Set the force and torque during the trajectory operation of the trajectory file
3* @param [in] ft forces and torques in three directions, units N and Nm
4* @return error code
5* /
6int SetTrajectoryJForceTorque(ForceTorque ft);
9.16. Set the force along the x direction during the trajectory’s operation
1/ * *
2* @brief Set the force along the x direction during the trajectory operation
3* @param [in] fx the force along the x direction, unit N
4* @return error code
5* /
6int SetTrajectoryJForceFx(double fx);
9.17. Set the force along the y direction during the trajectory’s operation
1/ * *
2* @brief Set the force along the y direction during the trajectory operation
3* @param [in] fy the force along the y direction, unit N
4* @return error code
5* /
6int SetTrajectoryJForceFy(double fy);
9.18. Set the force along the z direction during the trajectory’s operation
1/ * *
2* @brief Set the force along the z direction during the trajectory operation
3* @param [in] fz force along the z direction, unit N
4* @return error code
5* /
6int SetTrajectoryJForceFz(double fz);
9.19. Set the torque around the X-axis during the trajectory’s operation
1/ * *
2* @brief Set the torque around the X-axis during the trajectory operation
3* @param [in] tx Torque around the X-axis, in Nm
4* @return error code
5* /
6int SetTrajectoryJTorqueTx(double tx);
9.20. Set the torque around the Y-axis during the trajectory’s operation
1/ * *
2* @brief Set the torque around the Y-axis during the trajectory operation
3* @param [in] ty Torque around the Y-axis, in Nm
4* @return error code
5* /
6int SetTrajectoryJTorqueTy(double ty);
9.21. Set the torque around the Z-axis during the trajectory’s operation
1/ * *
2* @brief Set the torque around the Z-axis during the trajectory operation
3* @param [in] tz the torque around the Z-axis, in Nm
4* @return error code
5* /
6int SetTrajectoryJTorqueTz(double tz);
9.22. Upload the trajectory J file
1/ * *
2* @brief Upload the trajectory J file
3* @param [in] filePath upload the full path name of the trajectory file C://test/testJ.txt
4* @return error code
5* /
6int TrajectoryJUpLoad(string filePath);
9.23. Delete the trajectory J file
1/ * *
2* @brief Delete the trajectory J file
3* @param [in] fileName file name testJ.txt
4* @return error code
5* /
6int TrajectoryJDelete(string fileName);
9.24. Sample code for reproducing robot trajectory J files
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. Trajectory preprocessing (trajectory forward-looking)
1/ * *
2* @brief Trajectory Preprocessing (Trajectory Preview)
3* @param [in] name Trajectory file name
4* @param [in] mode Sampling mode, 0- no sampling; 1- Equal data interval sampling; 2- Equal error limiting sampling
5* @param [in] errorLim error limit, effective with linear fitting
6* @param [in] type smoothing method, 0-Bezier smoothing
7* @param [in] precision Smoothing accuracy, effective when using Bezier smoothing
8* @param [in] The maximum speed set by vamx, mm/s
9* @param [in] The maximum acceleration set by amax, mm/s2
10* @param [in] The maximum acceleration set by jmax, mm/s3
11* @return error code
12* /
13int LoadTrajectoryLA(string name, int mode, double errorLim, int type, double precision, double vamx, double amax, double jmax);
9.26. Trajectory reproduction (Trajectory Foresight)
1/ * *
2* @brief Trajectory Reproduction (Trajectory Preview)
3* @return error code
4* /
5int MoveTrajectoryLA();
9.27. Trajectory reproduction (trajectory forward-looking) code example
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); // 直线拟合
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. Move to TPD Trajectory Recording Start Point
1/**
2* @brief Move to TPD trajectory recording start point
3* @param [in] name Trajectory file name
4* @param [in] moveType Motion type; 0-PTP; 1-LIN
5* @param [in] ovl Speed scaling percentage, range [0~100]
6* @return Error code
7*/
8public int MoveToTPDStart(string name, int moveType, double ovl)
9.29. SDK Code Example for Moving to TPD Trajectory Recording Start Point
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}