8. 机器人轨迹复现
8.1. 设置TPD轨迹记录参数
1/**
2* @brief 设置TPD轨迹记录参数
3* @param [in] type 记录数据类型，1-关节位置
4* @param [in] name 轨迹文件名
5* @param [in] period_ms 数据采样周期，固定值2ms或4ms或8ms
6* @param [in] di_choose DI选择,bit0~bit7对应控制箱DI0~DI7，bit8~bit9对应末端DI0~DI1，0-不选择，1-选择
7* @param [in] do_choose DO选择,bit0~bit7对应控制箱DO0~DO7，bit8~bit9对应末端DO0~DO1，0-不选择，1-选择
8* @return 错误码
9*/
10errno_t SetTPDParam(int type, char name[30], int period_ms, uint16_t di_choose, uint16_t do_choose);
8.2. 开始TPD轨迹记录
1/**
2* @brief 开始TPD轨迹记录
3* @param [in] type 记录数据类型，1-关节位置
4* @param [in] name 轨迹文件名
5* @param [in] period_ms 数据采样周期，固定值2ms或4ms或8ms
6* @param [in] di_choose DI选择,bit0~bit7对应控制箱DI0~DI7，bit8~bit9对应末端DI0~DI1，0-不选择，1-选择
7* @param [in] do_choose DO选择,bit0~bit7对应控制箱DO0~DO7，bit8~bit9对应末端DO0~DO1，0-不选择，1-选择
8* @return 错误码
9*/
10errno_t SetTPDStart(int type, char name[30], int period_ms, uint16_t di_choose, uint16_t do_choose);
8.3. 停止TPD轨迹记录
1/**
2* @brief 停止TPD轨迹记录
3* @return 错误码
4*/
5errno_t SetWebTPDStop();
8.4. 删除TPD轨迹记录
1/**
2* @brief 删除TPD轨迹记录
3* @param [in] name 轨迹文件名
4* @return 错误码
5*/
6errno_t SetTPDDelete(char name[30]);
8.5. 代码示例
1#include <cstdlib>
2#include <iostream>
3#include <stdio.h>
4#include <cstring>
5#include <unistd.h>
6#include "FRRobot.h"
7#include "RobotTypes.h"
8
9using namespace std;
10
11int main(void)
12{
13 FRRobot robot; //实例化机器人对象
14 robot.RPC("192.168.58.2"); //与机器人控制器建立通信连接
15
16 int type = 1;
17 char name[30] = "tpd2023";
18 int period_ms = 4;
19 uint16_t di_choose = 0;
20 uint16_t do_choose = 0;
21
22 robot.SetTPDParam(type, name, period_ms, di_choose, do_choose);
23
24 robot.Mode(1);
25 sleep(1);
26 robot.DragTeachSwitch(1);
27 robot.SetTPDStart(type, name, period_ms, di_choose, do_choose);
28 sleep(30);
29 robot.SetWebTPDStop();
30 robot.DragTeachSwitch(0);
31
32 //robot.SetTPDDelete(name);
33
34 return 0;
35}
8.6. TPD轨迹预加载
1/**
2* @brief TPD轨迹预加载
3* @param [in] name 轨迹文件名
4* @return 错误码
5*/
6errno_t LoadTPD(char name[30]);
8.7. TPD轨迹复现
1/**
2* @brief TPD轨迹复现
3* @param [in] name 轨迹文件名
4* @param [in] blend 0-不平滑，1-平滑
5* @param [in] ovl 速度缩放百分比，范围[0~100]
6* @return 错误码
7*/
8errno_t MoveTPD(char name[30], uint8_t blend, float ovl);
8.8. 获取TPD起始位姿
1/**
2 * @brief 获取TPD起始位姿
3 * @param [in] name TPD文件名,不需要文件后缀
4 * @return 错误码
5 */
6errno_t GetTPDStartPose(char name[30], DescPose *desc_pose);
8.9. 代码示例
在 C++SDK-v2.1.2.0 版本发生变更.
1#include "libfairino/robot.h"
2
3//如果使用Windows，包含下面的头文件
4#include <string.h>
5#include <windows.h>
6//如果使用linux，包含下面的头文件
7/*
8#include <cstdlib>
9#include <iostream>
10#include <stdio.h>
11#include <cstring>
12#include <unistd.h>
13*/
14#include <chrono>
15#include <thread>
16#include <string>
17using namespace std;
18
19int main(void)
20{
21 FRRobot robot;
22 robot.RPC("192.168.58.2");
23
24 char name[30] = "tpd2023";
25 int tool = 0;
26 int user = 0;
27 float vel = 50.0;
28 float acc = 100.0;
29 float ovl = 100.0;
30 float blendT = -1.0;
31 int config = -1;
32 uint8_t blend = 0;
33 int retval = 0;
34
35 DescPose desc_pose;
36 memset(&desc_pose, 0, sizeof(DescPose));
37 DescPose start_pose;
38 memset(&start_pose, 0, sizeof(DescPose));
39
40 desc_pose.tran.x = 358.820099;
41 desc_pose.tran.y = -419.684113;
42 desc_pose.tran.z = 525.055115;
43 desc_pose.rpy.rx = -85.994499;
44 desc_pose.rpy.ry = -28.797600;
45 desc_pose.rpy.rz = -133.960007;
46
47 retval = robot.LoadTPD(name);
48 printf("LoadTPD retval is: %d\n", retval);
49 //robot.MoveCart(&desc_pose, tool, user, vel, acc, ovl, blendT, config);
50
51 robot.GetTPDStartPose(name, &start_pose);
52 printf("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);
53 std::this_thread::sleep_for(std::chrono::milliseconds(5000));
54
55 retval = robot.MoveTPD(name, blend, ovl);
56 printf("MoveTPD retval is: %d\n", retval);
57 std::this_thread::sleep_for(std::chrono::milliseconds(5000));
58 return 0;
59}
8.10. 轨迹预处理
1/**
2 * @brief 轨迹预处理
3 * @param [in] name 轨迹文件名
4 * @param [in] ovl 速度缩放百分比，范围[0~100]
5 * @param [in] opt 1-控制点，默认为1
6 * @return 错误码
7 */
8errno_t LoadTrajectoryJ(char name[30], float ovl, int opt);
8.11. 轨迹复现
1/**
2 * @brief 轨迹复现
3 * @return 错误码
4 */
5errno_t MoveTrajectoryJ();
8.12. 获取轨迹起始位姿
1/**
2 * @brief 获取轨迹起始位姿
3 * @param [in] name 轨迹文件名
4 * @return 错误码
5 */
6errno_t GetTrajectoryStartPose(char name[30], DescPose *desc_pose);
8.13. 获取轨迹点编号
1/**
2 * @brief 获取轨迹点编号
3 * @return 错误码
4 */
5errno_t GetTrajectoryPointNum(int *pnum);
8.14. 设置轨迹运行中的速度
1/**
2 * @brief 设置轨迹运行中的速度
3 * @param [in] ovl 速度百分比
4 * @return 错误码
5 */
6errno_t SetTrajectoryJSpeed(float ovl);
8.15. 设置轨迹运行中的力和扭矩
1/**
2 * @brief 设置轨迹运行中的力和扭矩
3 * @param [in] ft 三个方向的力和扭矩，单位N和Nm
4 * @return 错误码
5 */
6errno_t SetTrajectoryJForceTorque(ForceTorque *ft);
8.16. 设置轨迹运行中的沿x方向的力
1/**
2 * @brief 设置轨迹运行中的沿x方向的力
3 * @param [in] fx 沿x方向的力，单位N
4 * @return 错误码
5 */
6errno_t SetTrajectoryJForceFx(double fx);
8.17. 设置轨迹运行中的沿y方向的力
1/**
2 * @brief 设置轨迹运行中的沿y方向的力
3 * @param [in] fy 沿y方向的力，单位N
4 * @return 错误码
5 */
6errno_t SetTrajectoryJForceFy(double fy);
8.18. 设置轨迹运行中的沿z方向的力
1/**
2 * @brief 设置轨迹运行中的沿z方向的力
3 * @param [in] fz 沿x方向的力，单位N
4 * @return 错误码
5 */
6errno_t SetTrajectoryJForceFz(double fz);
8.19. 设置轨迹运行中的绕x轴的扭矩
1/**
2 * @brief 设置轨迹运行中的绕x轴的扭矩
3 * @param [in] tx 绕x轴的扭矩，单位Nm
4 * @return 错误码
5 */
6errno_t SetTrajectoryJTorqueTx(double tx);
8.20. 设置轨迹运行中的绕y轴的扭矩
1/**
2 * @brief 设置轨迹运行中的绕y轴的扭矩
3 * @param [in] ty 绕y轴的扭矩，单位Nm
4 * @return 错误码
5 */
6errno_t SetTrajectoryJTorqueTy(double ty);
8.21. 设置轨迹运行中的绕z轴的扭矩
1/**
2 * @brief 设置轨迹运行中的绕z轴的扭矩
3 * @param [in] tz 绕z轴的扭矩，单位Nm
4 * @return 错误码
5 */
6errno_t SetTrajectoryJTorqueTz(double tz);
8.22. 代码示例
在 C++SDK-v2.1.2.0 版本加入.
1#include "libfairino/robot.h"
2
3//如果使用Windows，包含下面的头文件
4#include <string.h>
5#include <windows.h>
6//如果使用linux，包含下面的头文件
7/*
8#include <cstdlib>
9#include <iostream>
10#include <stdio.h>
11#include <cstring>
12#include <unistd.h>
13*/
14#include <chrono>
15#include <thread>
16#include <string>
17using namespace std;
18
19int main(void)
20{
21 FRRobot robot;
22 robot.RPC("192.168.58.2");
23
24 int retval = 0;
25 char traj_file_name[30] = "/fruser/traj/tra我.txt";
26 retval = robot.LoadTrajectoryJ(traj_file_name, 100, 1);
27 printf("LoadTrajectoryJ %s, retval is: %d\n",traj_file_name, retval);
28
29 DescPose traj_start_pose;
30 memset(&traj_start_pose, 0, sizeof(DescPose));
31 retval = robot.GetTrajectoryStartPose(traj_file_name, &traj_start_pose);
32 printf("GetTrajectoryStartPose is: %d\n", retval);
33 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);
34
35 std::this_thread::sleep_for(std::chrono::seconds(5));
36
37 robot.SetSpeed(50);
38 robot.MoveCart(&traj_start_pose, 0, 0, 100, 100, 100, -1, -1);
39
40 int traj_num = 0;
41 retval = robot.GetTrajectoryPointNum(&traj_num);
42 printf("GetTrajectoryStartPose retval is: %d, traj num is: %d\n", retval, traj_num);
43
44 retval = robot.SetTrajectoryJSpeed(50.0);
45 printf("SetTrajectoryJSpeed is: %d\n", retval);
46
47 ForceTorque traj_force;
48 memset(&traj_force, 0, sizeof(ForceTorque));
49 traj_force.fx = 10;
50 retval = robot.SetTrajectoryJForceTorque(&traj_force);
51 printf("SetTrajectoryJForceTorque retval is: %d\n", retval);
52
53 retval = robot.SetTrajectoryJForceFx(10.0);
54 printf("SetTrajectoryJForceFx retval is: %d\n", retval);
55
56 retval = robot.SetTrajectoryJForceFy(0.0);
57 printf("SetTrajectoryJForceFy retval is: %d\n", retval);
58
59 retval = robot.SetTrajectoryJForceFz(0.0);
60 printf("SetTrajectoryJForceFz retval is: %d\n", retval);
61
62 retval = robot.SetTrajectoryJTorqueTx(10.0);
63 printf("SetTrajectoryJTorqueTx retval is: %d\n", retval);
64
65 retval = robot.SetTrajectoryJTorqueTy(10.0);
66 printf("SetTrajectoryJTorqueTy retval is: %d\n", retval);
67
68 retval = robot.SetTrajectoryJTorqueTz(10.0);
69 printf("SetTrajectoryJTorqueTz retval is: %d\n", retval);
70
71 retval = robot.MoveTrajectoryJ();
72 printf("MoveTrajectoryJ retval is: %d\n", retval);
73}