11. 机器人力控

11.1. 力传感器配置

1/**
2* @brief  配置力传感器
3* @param  [in] company  力传感器厂商，17-坤维科技
4* @param  [in] device  设备号，暂不使用，默认为0
5* @param  [in] softvesion  软件版本号，暂不使用，默认为0
6* @param  [in] bus 设备挂在末端总线位置，暂不使用，默认为0
7* @return  错误码
8*/
9int FT_SetConfig(int company, int device, int softvesion, int bus);

11.2. 获取力传感器配置

1/**
2* @brief 获取力传感器配置
3* @param [out] deviceID 力传感器编号
4* @param [out] company 力传感器厂商，，力传感器厂商，17-坤维科技，19-航天十一院，20-ATI传感器，21-中科米点，22-伟航敏芯
5* @param [out] device  设备号，坤维(0-KWR75B)，航天十一院(0-MCS6A-200-4)，ATI (0-AXIA80 -M8)，中科米点(0-MST2010)，伟航敏芯(0-WHC6L-YB-10A)
6* @param [out] softvesion 软件版本号，暂不使用，默认为 0
7* @return 错误码
8*/
9int FT_GetConfig(ref int deviceID, ref int company, ref int device, ref int softvesion);

11.3. 力传感器激活

1/**
2* @brief  力传感器激活
3* @param  [in] act  0-复位，1-激活
4* @return  错误码
5*/
6int FT_Activate(byte act);

11.4. 力传感器校零

1/**
2* @brief  力传感器校零
3* @param  [in] act  0-去除零点，1-零点矫正
4* @return  错误码
5*/
6int FT_SetZero(byte act);

11.5. 代码示例

 1private void btnFT_Click(object sender, EventArgs e)
 2{
 3    Robot robot = new Robot();
 4    robot.RPC("192.168.58.2");
 5
 6    int company = 17;
 7    int device = 0;
 8    int softversion = 0;
 9    int bus = 1;
10    int index = 1;
11    byte act = 0;
12
13    robot.FT_SetConfig(company, device, softversion, bus);
14    Thread.Sleep(1000);
15    company = 0;
16    robot.FT_GetConfig(ref company, ref device, ref softversion, ref bus);
17    Console.WriteLine($"FT config : {company}, {device}, {softversion}, {bus}");
18    Thread.Sleep(1000);
19
20    robot.FT_Activate(act);
21    Thread.Sleep(1000);
22    act = 1;
23    robot.FT_Activate(act);
24    Thread.Sleep(1000);
25
26    robot.SetLoadWeight(0.0f);
27    Thread.Sleep(1000);
28    DescTran coord = new DescTran(0, 0, 0);
29
30    robot.SetLoadCoord(coord);
31    Thread.Sleep(1000);
32    robot.FT_SetZero(0);//0去除零点  1零点矫正
33    Thread.Sleep(1000);
34
35    ForceTorque ft = new ForceTorque(0, 0, 0, 0, 0, 0);
36    int rtn = robot.FT_GetForceTorqueOrigin(1, ref ft);
37    Console.WriteLine($"ft origin : {ft.fx}, {ft.fy}, { ft.fz}, { ft.tx}, { ft.ty}, { ft.tz}    rtn   {rtn}");
38    rtn = robot.FT_SetZero(1);//零点矫正
39    //Console.WriteLine($"set zero rtn {rtn}");
40
41    Thread.Sleep(2000);
42    rtn = robot.FT_GetForceTorqueOrigin(1, ref ft);
43    Console.WriteLine($"ft rcs : {ft.fx}, {ft.fy}, {ft.fz}, {ft.tx}, {ft.ty}, {ft.tz}  rtn  {rtn}");
44
45    robot.FT_GetForceTorqueRCS(1, ref ft);
46    Console.WriteLine($"FT_GetForceTorqueRCS rcs : {ft.fx}, {ft.fy}, {ft.fz}, {ft.tx}, {ft.ty}, {ft.tz}");
47}

11.6. 设置力传感器参考坐标系

1/**
2* @brief  设置力传感器参考坐标系
3* @param  [in] ref  0-工具坐标系，1-基坐标系
4* @return  错误码
5*/
6int FT_SetRCS(byte type);

11.7. 负载重量辨识记录

1/**
2* @brief  负载重量辨识记录
3* @param  [in] id  传感器坐标系编号，范围[1~14]
4* @return  错误码
5*/
6int FT_PdIdenRecord(int id);

11.8. 负载重量辨识计算

1/**
2* @brief  负载重量辨识计算
3* @param  [out] weight  负载重量，单位kg
4* @return  错误码
5*/
6int FT_PdIdenCompute(ref double weight);

11.9. 负载质心辨识记录

1/**
2* @brief  负载质心辨识记录
3* @param  [in] id  传感器坐标系编号，范围[1~14]
4* @param  [in] index 点编号，范围[1~3]
5* @return  错误码
6*/
7int FT_PdCogIdenRecord(int id, int index);

11.10. 负载质心辨识计算

1/**
2* @brief  负载质心辨识计算
3* @param  [out] cog  负载质心，单位mm
4* @return  错误码
5*/
6int FT_PdCogIdenCompute(ref DescTran cog);

11.11. 代码示例

 1private void btnFTPdCog_Click(object sender, EventArgs e)
 2{
 3    Robot robot = new Robot();
 4    robot.RPC("192.168.58.2");
 5
 6    double weight = 0.1;
 7    int rtn = -1;
 8    DescPose tcoord, desc_p1, desc_p2, desc_p3;
 9    tcoord = new DescPose(0, 0, 0, 0, 0, 0);
10    desc_p1 = new DescPose(0, 0, 0, 0, 0, 0);
11    desc_p2 = new DescPose(0, 0, 0, 0, 0, 0);
12    desc_p3 = new DescPose(0, 0, 0, 0, 0, 0);
13
14    robot.FT_SetRCS(0);
15    Thread.Sleep(1000);
16
17    tcoord.tran.z = 35.0;
18    robot.SetToolCoord(10, tcoord, 1, 0);
19    Thread.Sleep(1000);
20    robot.FT_PdIdenRecord(10);
21    Thread.Sleep(1000);
22    robot.FT_PdIdenCompute(ref weight);
23    Console.WriteLine($"payload weight : {weight}");
24
25    desc_p1.tran.x = -47.805;
26    desc_p1.tran.y = -362.266;
27    desc_p1.tran.z = 317.754;
28    desc_p1.rpy.rx = -179.496;
29    desc_p1.rpy.ry = -0.255;
30    desc_p1.rpy.rz = 34.948;
31
32    desc_p2.tran.x = -77.805;
33    desc_p2.tran.y = -312.266;
34    desc_p2.tran.z = 317.754;
35    desc_p2.rpy.rx = -179.496;
36    desc_p2.rpy.ry = -0.255;
37    desc_p2.rpy.rz = 34.948;
38
39    desc_p3.tran.x = -167.805;
40    desc_p3.tran.y = -312.266;
41    desc_p3.tran.z = 387.754;
42    desc_p3.rpy.rx = -179.496;
43    desc_p3.rpy.ry = -0.255;
44    desc_p3.rpy.rz = 34.948;
45
46    rtn = robot.MoveCart(desc_p1, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
47    Console.WriteLine($"MoveCart rtn  {rtn}");
48    Thread.Sleep(1000);
49    robot.FT_PdCogIdenRecord(10, 1);
50    robot.MoveCart(desc_p2, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
51    Thread.Sleep(1000);
52    robot.FT_PdCogIdenRecord(10, 2);
53    robot.MoveCart(desc_p3, 0, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
54    Thread.Sleep(1000);
55    robot.FT_PdCogIdenRecord(10, 3);
56    Thread.Sleep(1000);
57    DescTran cog = new DescTran(0, 0, 0);
58
59    robot.FT_PdCogIdenCompute(ref cog);
60    Console.WriteLine($"cog : {cog.x}, {cog.y}, {cog.z}");
61}

11.12. 获取参考坐标系下力/扭矩数据

1/**
2* @brief  获取参考坐标系下力/扭矩数据
3* @param  [out] ft  力/扭矩，fx,fy,fz,tx,ty,tz
4* @return  错误码
5*/
6int FT_GetForceTorqueRCS(byte flag, ref ForceTorque ft);

11.13. 获取力传感器原始力/扭矩数据

1/**
2* @brief  获取力传感器原始力/扭矩数据
3* @param  [out] ft  力/扭矩，fx,fy,fz,tx,ty,tz
4* @return  错误码
5*/
6int FT_GetForceTorqueOrigin(byte flag, ref ForceTorque ft);

11.14. 碰撞守护

 1/**
 2* @brief  碰撞守护
 3* @param  [in] flag 0-关闭碰撞守护，1-开启碰撞守护
 4* @param  [in] sensor_id 力传感器编号
 5* @param  [in] select  选择六个自由度是否检测碰撞，0-不检测，1-检测
 6* @param  [in] ft  碰撞力/扭矩，fx,fy,fz,tx,ty,tz
 7* @param  [in] max_threshold 最大阈值
 8* @param  [in] min_threshold 最小阈值
 9* @note   力/扭矩检测范围：(ft-min_threshold, ft+max_threshold)
10* @return  错误码
11*/
12int FT_Guard(int flag, int sensor_id, int[] select, ForceTorque ft, double[] max_threshold, double[] min_threshold);

11.15. 代码示例

 1private void btnFTGuard_Click(object sender, EventArgs e)
 2{
 3    Robot robot = new Robot();
 4    robot.RPC("192.168.58.2");
 5
 6    byte flag = 1;
 7    byte sensor_id = 1;
 8    int[] select = new int[6]{ 1, 0, 0, 0, 0, 0 };//只启用x轴碰撞守护
 9    double[] max_threshold = new double[6]{ 5.0f, 0.01f, 0.01f, 0.01f, 0.01f, 0.01f };
10    double[] min_threshold = new double[6]{ 3.0f, 0.01f, 0.01f, 0.01f, 0.01f, 0.01f };
11
12    ForceTorque ft = new ForceTorque(0, 0, 0, 0, 0, 0);
13    DescPose desc_p1, desc_p2, desc_p3;
14    desc_p1 = new DescPose(0, 0, 0, 0, 0, 0);
15    desc_p2 = new DescPose(0, 0, 0, 0, 0, 0);
16    desc_p3 = new DescPose(0, 0, 0, 0, 0, 0);
17
18    desc_p1.tran.x = 1.299;
19    desc_p1.tran.y = -719.159;
20    desc_p1.tran.z = 141.314;
21    desc_p1.rpy.rx = 177.999;
22    desc_p1.rpy.ry = -0.715;
23    desc_p1.rpy.rz = -161.937;
24
25    desc_p2.tran.x = 245.047;
26    desc_p2.tran.y = -675.509;
27    desc_p2.tran.z = 139.538;
28    desc_p2.rpy.rx = 177.987;
29    desc_p2.rpy.ry = -0.129;
30    desc_p2.rpy.rz = -142.238;
31
32    desc_p3.tran.x = 157.233;
33    desc_p3.tran.y = -550.088;
34    desc_p3.tran.z = 112.485;
35    desc_p3.rpy.rx = -176.579;
36    desc_p3.rpy.ry = -2.819;
37    desc_p3.rpy.rz = -148.415;
38    robot.SetSpeed(5);
39
40    int rtn =  robot.FT_Guard(flag, sensor_id, select, ft, max_threshold, min_threshold);
41    Console.WriteLine($"FT_Guard start rtn {rtn}");
42    robot.MoveCart(desc_p1, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
43    robot.MoveCart(desc_p2, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
44    robot.MoveCart(desc_p3, 1, 0, 100.0f, 100.0f, 100.0f, -1.0f, -1);
45    flag = 0;
46    rtn = robot.FT_Guard(flag, sensor_id, select, ft, max_threshold, min_threshold);
47    Console.WriteLine($"FT_Guard end rtn {rtn}");
48}

11.16. 恒力控制

 1/**
 2* @brief  恒力控制
 3* @param  [in] flag 0-关闭恒力控制，1-开启恒力控制
 4* @param  [in] sensor_id 力传感器编号
 5* @param  [in] select  选择六个自由度是否检测碰撞，0-不检测，1-检测
 6* @param  [in] ft  碰撞力/扭矩，fx,fy,fz,tx,ty,tz
 7* @param  [in] ft_pid 力pid参数，力矩pid参数
 8* @param  [in] adj_sign 自适应启停控制，0-关闭，1-开启
 9* @param  [in] ILC_sign ILC启停控制， 0-停止，1-训练，2-实操
10* @param  [in] 最大调整距离，单位mm
11* @param  [in] 最大调整角度，单位deg
12* @return  错误码
13*/
14int 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);

11.17. 代码示例

 1private void btnFTConttol_Click(object sender, EventArgs e)
 2{
 3    Robot robot = new Robot();
 4    robot.RPC("192.168.58.2");
 5
 6    byte flag = 1;
 7    byte sensor_id = 1;
 8    int[] select = new int[6]{ 0, 0, 1, 0, 0, 0 };
 9    double[] ft_pid = new double[6]{ 0.0005f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
10    byte adj_sign = 0;
11    byte ILC_sign = 0;
12    float max_dis = 100.0f;
13    float max_ang = 0.0f;
14
15    ForceTorque ft = new ForceTorque(0, 0, 0, 0 ,0 ,0);
16    DescPose desc_p1, desc_p2, offset_pos;
17    JointPos j1, j2;
18    ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
19    desc_p1 = new DescPose(0, 0, 0, 0, 0, 0);
20    desc_p2 = new DescPose(0, 0, 0, 0, 0, 0);
21    offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
22
23    j2 = new JointPos(0, 0, 0, 0, 0, 0);
24    j1 = new JointPos(0, 0, 0, 0, 0, 0);
25
26    desc_p1.tran.x = 1.299;
27    desc_p1.tran.y = -719.159;
28    desc_p1.tran.z = 141.314;
29    desc_p1.rpy.rx = 177.999;
30    desc_p1.rpy.ry = -0.715;
31    desc_p1.rpy.rz = -161.937;
32
33    desc_p2.tran.x = 245.047;
34    desc_p2.tran.y = -675.509;
35    desc_p2.tran.z = 139.538;
36    desc_p2.rpy.rx = 177.987;
37    desc_p2.rpy.ry = -0.129;
38    desc_p2.rpy.rz = -142.238;
39    ft.fz = -10.0;
40
41    robot.GetInverseKin(0, desc_p1, -1, ref j1);
42    robot.GetInverseKin(0, desc_p2, -1, ref j2);
43
44    robot.MoveJ(j1, desc_p1, 1, 0, 100.0f, 180.0f, 100.0f, epos, -1.0f, 0, offset_pos);
45    int rtn = robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
46    Console.WriteLine($"FT_Control start rtn {rtn}");
47
48    robot.MoveL(j2, desc_p2, 1, 0, 100.0f, 180.0f, 20.0f, -1.0f, epos, 0, 0, offset_pos);
49    flag = 0;
50    rtn = robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
51    Console.WriteLine($"FT_Control end rtn {rtn}");
52}

11.18. 柔顺控制开启

1/**
2* @brief  柔顺控制开启
3* @param  [in] p 位置调节系数或柔顺系数
4* @param  [in] force 柔顺开启力阈值，单位N
5* @return  错误码
6*/
7int FT_ComplianceStart(float p, float force);

11.19. 柔顺控制关闭

1/**
2* @brief  柔顺控制关闭
3* @return  错误码
4*/
5int FT_ComplianceStop();

11.20. 代码示例

 1private void btnComplience_Click(object sender, EventArgs e)
 2{
 3    Robot robot = new Robot();
 4    robot.RPC("192.168.58.2");
 5
 6    byte flag = 1;
 7    int sensor_id = 1;
 8    int[] select = new int[6]{ 1, 1, 1, 0, 0, 0 };
 9    double[] ft_pid = new double[6] { 0.0005f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f };
10    byte adj_sign = 0;
11    byte ILC_sign = 0;
12    float max_dis = 100.0f;
13    float max_ang = 0.0f;
14
15    ForceTorque ft = new ForceTorque(0, 0, 0, 0, 0, 0);
16    DescPose desc_p1, desc_p2, offset_pos;
17    JointPos j1, j2;
18
19    ExaxisPos epos = new ExaxisPos(0, 0, 0, 0);
20    desc_p1 = new DescPose(0, 0, 0, 0, 0, 0);
21    desc_p2 = new DescPose(0, 0, 0, 0, 0, 0);
22    offset_pos = new DescPose(0, 0, 0, 0, 0, 0);
23
24    j2 = new JointPos(0, 0, 0, 0, 0, 0);
25    j1 = new JointPos(0, 0, 0, 0, 0, 0);
26
27    desc_p1.tran.x = 1.299;
28    desc_p1.tran.y = -719.159;
29    desc_p1.tran.z = 141.314;
30    desc_p1.rpy.rx = 177.999;
31    desc_p1.rpy.ry = -0.715;
32    desc_p1.rpy.rz = -161.937;
33
34    desc_p2.tran.x = 245.047;
35    desc_p2.tran.y = -675.509;
36    desc_p2.tran.z = 139.538;
37    desc_p2.rpy.rx = 177.987;
38    desc_p2.rpy.ry = -0.129;
39    desc_p2.rpy.rz = -142.238;
40    ft.fz = -10.0;
41
42    robot.GetInverseKin(0, desc_p1, -1, ref j1);
43    robot.GetInverseKin(0, desc_p2, -1, ref j2);
44
45    ft.fx = -10.0;
46    ft.fy = -10.0;
47    ft.fz = -10.0;
48    robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
49    float p = 0.00005f;
50    float force = 30.0f;
51    int rtn = robot.FT_ComplianceStart(p, force);
52    Console.WriteLine($"FT_ComplianceStart rtn {rtn}");
53    int count = 15;
54    while (count > 0)
55    {
56        robot.MoveL(j1, desc_p1, 1, 0, 100.0f, 180.0f, 100.0f, -1.0f, epos, 0, 1, offset_pos);
57        robot.MoveL(j2, desc_p2, 1, 0, 100.0f, 180.0f, 100.0f, -1.0f, epos, 0, 0, offset_pos);
58        count -= 1;
59    }
60    rtn = robot.FT_ComplianceStop();
61    Console.WriteLine($"FT_ComplianceStop rtn {rtn}");
62    flag = 0;
63    robot.FT_Control(flag, sensor_id, select, ft, ft_pid, adj_sign, ILC_sign, max_dis, max_ang);
64}

11.21. 负载辨识初始化

在 C#SDK-v1.0.4 版本加入.

1/**
2* @brief 负载辨识初始化
3* @return 错误码
4*/
5int LoadIdentifyDynFilterInit();

11.22. 负载辨识变量初始化

在 C#SDK-v1.0.4 版本加入.

1/**
2* @brief 负载辨识变量初始化
3* @return 错误码
4*/
5int LoadIdentifyDynVarInit();

11.23. 负载辨识主程序

在 C#SDK-v1.0.4 版本加入.

1/**
2* @brief 负载辨识主程序
3* @param [in] joint_torque 关节扭矩
4* @param [in] joint_pos 关节位置
5* @param [in] t 采样周期
6* @return 错误码
7*/
8int LoadIdentifyMain(double[] joint_torque, double[] joint_pos, double t);

11.24. 获取负载辨识结果

在 C#SDK-v1.0.4 版本加入.

1/**
2* @brief 获取负载辨识结果
3* @param [in] gain
4* @param [out] weight 负载重量
5* @param [out] cog 负载质心
6* @return 错误码
7*/
8int LoadIdentifyGetResult(double[] gain, ref double weight, ref DescTran cog);