10. 机器人力控

10.1. 获取力传感器配置

原型

FT_GetConfig()

描述

获取力传感器配置

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode - 返回值（调用成功返回）[number,company,device,softversion,bus] - number 传感器编号; - company 力传感器厂商，17-坤维科技，19-航天十一院，20-ATI 传感器，21-中科米点，22-伟航敏芯; - device 设备号，坤维 (0-KWR75B)，航天十一院 (0-MCS6A-200-4)，ATI(0-AXIA80-M8)，中科米点 (0-MST2010)，伟航敏芯 (0-WHC6L-YB10A); - softvesion 软件版本号，暂不使用，默认为0

10.1.1. 代码示例

 1from fairino import Robot
 2import time
 3# 与机器人控制器建立连接，连接成功返回一个机器人对象
 4robot = Robot.RPC('192.168.58.2')
 5company = 17    #传感器厂商，17-坤维科技
 6device = 0      #传感器设备号
 7error = robot.FT_SetConfig(company, device)   #配置力传感器
 8print("配置力传感器错误码",error)
 9config = robot.FT_GetConfig() #获取力传感器配置信息
10print('获取力传感器配置信息',config)
11time.sleep(1)
12error = robot.FT_Activate(0)  #传感器复位
13print("传感器复位错误码",error)
14time.sleep(1)
15error = robot.FT_Activate(1)  #传感器激活
16print("传感器激活错误码",error)
17time.sleep(1)
18error = robot.SetLoadWeight(0.0)    #末端负载设置为零
19print("末端负载设置为零错误码",error)
20time.sleep(1)
21error = robot.SetLoadCoord(0.0,0.0,0.0)  #末端负载质心设置为零
22print("末端质心设置为零错误码",error)
23time.sleep(1)
24error = robot.FT_SetZero(0)   #传感器去除零点
25print("传感器去除零点错误码",error)
26time.sleep(1)
27error = robot.FT_GetForceTorqueOrigin()   #查询传感器原始数据
28print("查询传感器原始数据",error)
29error = robot.FT_SetZero(1)   #传感器零点矫正,注意此时末端不能安装工具，只有力传感器
30print("传感器零点矫正",error)
31time.sleep(1)
32error = robot.FT_GetForceTorqueRCS()  #查询传感器坐标系下数据
33print("查询传感器坐标系下数据",error)

10.2. 力传感器配置

原型

FT_SetConfig(company,device,softversion=0,bus=0)

描述

力传感器配置

必选参数

  • company：传感器厂商，17-坤维科技，19-航天十一院，20-ATI传感器，21-中科米点，22-伟航敏芯；

  • device：设备号，坤维(0-KWR75B)，航天十一院(0-MCS6A-200-4)，ATI(0-AXIA80-M8)，中科米点(0-MST2010)，伟航敏芯(0-WHC6L-YB-10A)；

默认参数

  • softversion：软件版本号，暂不使用，默认为0；

  • bus：设备挂载末端总线位置，暂不使用，默认为 0；

返回值

  • 成功：[0]

  • 失败：[errcode]

10.3. 力传感器激活

原型

FT_Activate(state)

描述

力传感器激活

必选参数

  • state：0-复位，1-激活

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.4. 力传感器校零

原型

FT_SetZero(state)

描述

力传感器校零

必选参数

  • state：0-去除零点，1-零点矫正

默认参数

无

返回值

错误码 成功-0 失败- errcode

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

原型

FT_SetRCS(ref)

描述

设置力传感器参考坐标系

必选参数

  • ref：0-工具坐标系，1-基坐标系

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.5.1. 代码示例

 1from fairino import Robot
 2import time
 3# 与机器人控制器建立连接，连接成功返回一个机器人对象
 4robot = Robot.RPC('192.168.58.2')
 5#负载辨识，此时末端安装要辨识的工具，工具安装在力传感器下方,末端竖直向下
 6error = robot.FT_SetRCS(0)    #设置参考坐标系为工具坐标系，0-工具坐标系，1-基坐标系
 7print('设置参考坐标系错误码',error)
 8time.sleep(1)
 9tool_id = 10  #传感器坐标系编号
10tool_coord = [0.0,0.0,35.0,0.0,0.0,0.0]   # 传感器相对末端法兰位姿
11tool_type = 1  # 0-工具，1-传感器
12tool_install = 0 # 0-安装末端，1-机器人外部
13error = robot.SetToolCoord(tool_id,tool_coord,tool_type,tool_install)     #设置传感器坐标系，传感器相对末端法兰位姿
14print('设置传感器坐标系错误码',error)
15time.sleep(1)
16error = robot.FT_PdIdenRecord(tool_id)   #记录辨识数据
17print('记录负载重量错误码',error)
18time.sleep(1)
19error = robot.FT_PdIdenRecord()  #计算负载重量，单位kg
20print('计算负载重量错误码',error)
21#负载质心辨识，机器人需要示教三个不同的姿态，然后记录辨识数据，最后计算负载质心
22robot.Mode(1)
23ret = robot.DragTeachSwitch(1)  #机器人切入拖动示教模式，必须在手动模式下才能切入拖动示教模式
24time.sleep(5)
25ret = robot.DragTeachSwitch(0)
26time.sleep(1)
27error = robot.FT_PdCogIdenRecord(tool_id,1)
28print('负载质心1错误码',error)#记录辨识数据
29ret = robot.DragTeachSwitch(1)  #机器人切入拖动示教模式，必须在手动模式下才能切入拖动示教模式
30time.sleep(5)
31ret = robot.DragTeachSwitch(0)
32time.sleep(1)
33error = robot.FT_PdCogIdenRecord(tool_id,2)
34print('负载质心2错误码',error)
35ret = robot.DragTeachSwitch(1)  #机器人切入拖动示教模式，必须在手动模式下才能切入拖动示教模式
36time.sleep(5)
37ret = robot.DragTeachSwitch(0)
38time.sleep(1)
39error = robot.FT_PdCogIdenRecord(tool_id,3)
40print('负载质心3错误码',error)
41time.sleep(1)
42error = robot.FT_PdCogIdenCompute()
43print('负载质心计算错误码',error)

10.6. 负载重量辨识计算

原型

FT_PdIdenCompute()

描述

负载重量辨识计算

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode - 返回值（调用成功返回）weight-负载重量，单位 kg

10.7. 负载重量辨识记录

原型

FT_PdIdenRecord(tool_id)

描述

负载重量辨识记录

必选参数

  • tool_id：传感器坐标系编号，范围[0~14]

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.8. 负载质心辨识计算

原型

FT_PdCogIdenCompute ()

描述

负载质心辨识计算

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode - 返回值（调用成功返回）cog=[cogx,cogy,cogz] ，负载质心，单位 mm

10.9. 负载质心辨识记录

原型

FT_PdCogIdenRecord(tool_id,index)

描述

负载质心辨识记录

必选参数

  • tool_id：传感器坐标系编号，范围[0~14];

  • index：点编号[1~3]

默认参数

无

返回值

错误码 成功-0 失败- errcode

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

原型

FT_GetForceTorqueRCS()

描述

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

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode - 返回值（调用成功返回）data=[fx,fy,fz,tx,ty,tz]

10.10.1. 代码示例

1import frrpc
2# 与机器人控制器建立连接，连接成功返回一个机器人对象
3robot = frrpc.RPC('192.168.58.2')
4rcs = robot.FT_GetForceTorqueRCS()  #查询传感器坐标系下数据
5print(rcs)

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

原型

FT_GetForceTorqueOrigin()

描述

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

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode - 返回值（调用成功返回）data=[fx,fy,fz,tx,ty,tz]

10.11.1. 代码示例

1import frrpc
2# 与机器人控制器建立连接，连接成功返回一个机器人对象
3robot = frrpc.RPC('192.168.58.2')
4origin = robot.FT_GetForceTorqueOrigin()   #查询传感器原始数据
5print(origin)

10.12. 碰撞守护

原型

FT_Guard(flag,sensor_num,select,force_torque,max_threshold,min_threshold)

描述

碰撞守护

必选参数

  • flag：0-关闭碰撞守护，1-开启碰撞守护；

  • sensor_num：力传感器编号；

  • select：六个自由度是否检测碰撞[fx,fy,fz,mx,my,mz]，0-不生效，1-生效；

  • force_torque：碰撞检测力/力矩，单位N或Nm；

  • max_threshold：最大阈值；

  • min_threshold：最小阈值；

  • 力/力矩检测范围:(force_torque-min_threshold,force_torque+max_threshold)

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.12.1. 代码示例

 1from fairino import Robot
 2# 与机器人控制器建立连接，连接成功返回一个机器人对象
 3robot = Robot.RPC('192.168.58.2')
 4#碰撞守护
 5actFlag = 1   #开启标志，0-关闭碰撞守护，1-开启碰撞守护
 6sensor_num = 1  #力传感器编号
 7is_select = [1,1,1,1,1,1]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
 8force_torque = [0.0,0.0,0.0,0.0,0.0,0.0]  #碰撞检测力和力矩，检测范围（force_torque-min_threshold,force_torque+max_threshold）
 9max_threshold = [10.0,10.0,10.0,10.0,10.0,10.0]  #最大阈值
10min_threshold = [5.0,5.0,5.0,5.0,5.0,5.0]   #最小阈值
11P1=[-160.619,-586.138,384.988,-170.166,-44.782,169.295]
12P2=[-87.615,-606.209,556.119,-102.495,10.118,178.985]
13P3=[41.479,-557.243,484.407,-125.174,46.995,-132.165]
14error = robot.FT_Guard(actFlag, sensor_num, is_select, force_torque, max_threshold, min_threshold)    #开启碰撞守护
15print("开启碰撞守护错误码",error)
16error = robot.MoveL(P1,1,0)         #笛卡尔空间直线运动
17print("笛卡尔空间直线运动错误码",error)
18error = robot.MoveL(P2,1,0)
19print("笛卡尔空间直线运动错误码",error)
20error = robot.MoveL(P3,1,0)
21print("笛卡尔空间直线运动错误码",error)
22actFlag = 0
23error = robot.FT_Guard(actFlag, sensor_num, is_select, force_torque, max_threshold, min_threshold)    #关闭碰撞守护
24print("关闭碰撞守护错误码",error)

10.13. 恒力控制

原型

FT_Control(flag,sensor_num,select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)

描述

恒力控制

必选参数

  • flag：恒力控制开启标志，0-关，1-开；

  • sensor_num：力传感器编号；

  • select：六个自由度是否检测 [fx,fy,fz,mx,my,mz]，0-不生效，1-生效；

  • force_torque：检测力/力矩，单位N或Nm；

  • gain：[f_p,f_i,f_d,m_p,m_i,m_d],力PID参数，力矩PID参数；

  • adj_sign：自适应启停状态，0-关闭，1-开启；

  • ILC_sign: ILC控制启停状态，0-停止，1-训练，2-实操；

  • max_dis：最大调整距离；

  • max_ang：最大调整角度；

默认参数

无

返回值

  • 成功：[0]

  • 失败：[errcode]

10.13.1. 代码示例

 1from fairino import Robot
 2# 与机器人控制器建立连接，连接成功返回一个机器人对象
 3robot = Robot.RPC('192.168.58.2')
 4#恒力控制
 5status = 1  #恒力控制开启标志，0-关，1-开
 6sensor_num = 1 #力传感器编号
 7is_select = [0,0,1,0,0,0]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
 8force_torque = [0.0,0.0,-10.0,0.0,0.0,0.0]
 9gain = [0.0005,0.0,0.0,0.0,0.0,0.0]  #力PID参数，力矩PID参数
10adj_sign = 0  #自适应启停状态，0-关闭，1-开启
11ILC_sign = 0  #ILC控制启停状态，0-停止，1-训练，2-实操
12max_dis = 100.0  #最大调整距离
13max_ang = 0.0  #最大调整角度
14J1=[70.395, -46.976, 90.712, -133.442, -87.076, -27.138]
15P2=[-123.978, -674.129, 44.308, -178.921, 2.734, -172.449]
16P3=[123.978, -674.129, 42.308, -178.921, 2.734, -172.449]
17error = robot.MoveJ(J1,1,0)
18print("关节空间运动指令错误码",error)
19error = robot.MoveL(P2,1,0)
20print("笛卡尔空间直线运动指令错误码",error)
21error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
22print("恒力控制开启错误码",error)
23error = robot.MoveL(P3,1,0)   #笛卡尔空间直线运动
24print("笛卡尔空间直线运动指令错误码",error)
25status = 0
26error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
27print("恒力控制结束错误码",error)

10.14. 螺旋线探索

原型

FT_SpiralSearch(rcs,ft, dr = 0.7,max_t_ms = 60000, max_vel = 5)

描述

螺旋线探索

必选参数

  • rcs：参考坐标系，0-工具坐标系，1-基坐标系

  • ft：力或力矩阈值 (0~100)，单位 N 或 Nm;

默认参数

  • dr：每圈半径进给量，单位 mm 默认0.7;

  • max_t_ms：最大探索时间，单位 ms 默认 60000;

  • max_vel：线速度最大值，单位 mm/s 默认 5

返回值

  • 成功：[0]

  • 失败：[errcode]

10.14.1. 代码示例

 1from fairino import Robot
 2# 与机器人控制器建立连接，连接成功返回一个机器人对象
 3robot = Robot.RPC('192.168.58.2')
 4P = [36.794,-675.119, 65.379, -176.938, 2.535, -179.829]
 5#恒力参数
 6status = 1  #恒力控制开启标志，0-关，1-开
 7sensor_num = 1 #力传感器编号
 8is_select = [0,0,1,0,0,0]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
 9force_torque = [0.0,0.0,-10.0,0.0,0.0,0.0]
10gain = [0.0001,0.0,0.0,0.0,0.0,0.0]  #力PID参数，力矩PID参数
11adj_sign = 0  #自适应启停状态，0-关闭，1-开启
12ILC_sign = 0  #ILC控制启停状态，0-停止，1-训练，2-实操
13max_dis = 100.0  #最大调整距离
14max_ang = 5.0  #最大调整角度
15#螺旋线探索参数
16rcs = 0  #参考坐标系，0-工具坐标系，1-基坐标系
17fFinish = 10 #力或力矩阈值（0~100），单位N或Nm
18error = robot.MoveL(P,1,0) #笛卡尔空间直线运动至初始点
19print("笛卡尔空间直线运动至初始点",error)
20is_select = [0,0,1,1,1,0]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
21error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign, max_dis,max_ang)
22print("恒力控制开启错误码",error)
23error = robot.FT_SpiralSearch(rcs,fFinish,max_vel=3)
24print("螺旋线探索错误码",error)
25status = 0
26error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign, max_dis,max_ang)
27print("恒力控制关闭错误码",error)

10.15. 旋转插入

原型

FT_RotInsertion(rcs,ft, orn, angVelRot = 3, angleMax = 45, angAccmax = 0, rotorn =1)

描述

旋转插入

必选参数

  • rcs：参考坐标系，0-工具坐标系，1-基坐标系；

  • ft：力或力矩阈值 (0~100)，单位 N 或 Nm;

  • orn：力/扭矩方向，1-沿z轴方向，2-绕z轴方向;

默认参数

  • angVelRot：旋转角速度，单位°/s,默认 3;

  • angleMax：最大旋转角度，单位 ° 默认 5;

  • angAccmax：最大旋转加速度，单位 °/s^2，暂不使用 默认0;

  • rotorn：旋转方向，1-顺时针，2-逆时针 默认1

返回值

  • 成功：[0]

  • 失败：[errcode]

10.15.1. 代码示例

 1from fairino import Robot
 2# 与机器人控制器建立连接，连接成功返回一个机器人对象
 3robot = Robot.RPC('192.168.58.2')
 4P = [36.794,-675.119, 65.379, -176.938, 2.535, -179.829]
 5#恒力参数
 6status = 1  #恒力控制开启标志，0-关，1-开
 7sensor_num = 1 #力传感器编号
 8is_select = [0,0,1,0,0,0]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
 9force_torque = [0.0,0.0,-10.0,0.0,0.0,0.0]
10gain = [0.0001,0.0,0.0,0.0,0.0,0.0]  #力PID参数，力矩PID参数
11adj_sign = 0  #自适应启停状态，0-关闭，1-开启
12ILC_sign = 0  #ILC控制启停状态，0-停止，1-训练，2-实操
13max_dis = 100.0  #最大调整距离
14max_ang = 5.0  #最大调整角度
15#旋转插入参数
16rcs = 0  #参考坐标系，0-工具坐标系，1-基坐标系
17forceInsertion = 2.0 #力或力矩阈值（0~100），单位N或Nm
18orn = 1 #力的方向，1-fz,2-mz
19#默认参数 angVelRot：旋转角速度，单位 °/s  默认 3
20#默认参数 angleMax：最大旋转角度，单位 ° 默认 5
21#默认参数 angAccmax：最大旋转加速度，单位 °/s^2，暂不使用 默认0
22#默认参数 rotorn：旋转方向，1-顺时针，2-逆时针 默认1
23error = robot.MoveL(P,1,0) #笛卡尔空间直线运动至初始点
24print("笛卡尔空间直线运动至初始点",error)
25error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
26print("恒力控制开启错误码",error)
27error = robot.FT_RotInsertion(rcs,1,orn)
28print("旋转插入错误码",error)
29status = 0
30error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
31print("恒力控制关闭错误码",error)

10.16. 直线插入

原型

FT_LinInsertion(rcs, ft, disMax, linorn, lin_v = 1.0, lin_a = 1.0)

描述

直线插入

必选参数

  • rcs：参考坐标系，0-工具坐标系，1-基坐标系；

  • ft：力或力矩阈值 (0~100)，单位 N 或 Nm;

  • disMax：最大插入距离，单位 mm;

  • linorn：插入方向:0-负方向，1-正方向

默认参数

  • lin_v：直线速度，单位 mm/s 默认1;

  • lin_a：直线加速度，单位 mm/s^2，暂不使用 默认0

返回值

  • 成功：[0]

  • 失败：[errcode]

10.16.1. 代码示例

 1from fairino import Robot
 2# 与机器人控制器建立连接，连接成功返回一个机器人对象
 3robot = Robot.RPC('192.168.58.2')
 4P = [36.794,-675.119, 65.379, -176.938, 2.535, -179.829]
 5#恒力参数
 6status = 1  #恒力控制开启标志，0-关，1-开
 7sensor_num = 1 #力传感器编号
 8is_select = [0,0,1,0,0,0]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
 9force_torque = [0.0,0.0,-10.0,0.0,0.0,0.0]
10gain = [0.0001,0.0,0.0,0.0,0.0,0.0]  #力PID参数，力矩PID参数
11adj_sign = 0  #自适应启停状态，0-关闭，1-开启
12ILC_sign = 0  #ILC控制启停状态，0-停止，1-训练，2-实操
13max_dis = 100.0  #最大调整距离
14max_ang = 5.0  #最大调整角度
15#直线插入参数
16rcs = 0  #参考坐标系，0-工具坐标系，1-基坐标系
17force_goal = 10.0  #力或力矩阈值（0~100），单位N或Nm
18disMax = 100.0 #最大插入距离，单位mm
19linorn = 1 #插入方向，1-正方向，2-负方向
20#默认参数 lin_v：直线速度，单位 mm/s 默认1
21#默认参数 lin_a：直线加速度，单位 mm/s^2，暂不使用 默认0
22error = robot.MoveL(P,1,0) #笛卡尔空间直线运动至初始点
23print("笛卡尔空间直线运动至初始点",error)
24error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
25print("恒力控制开启错误码",error)
26error = robot.FT_LinInsertion(rcs,force_goal,disMax,linorn)
27print("直线插入错误码",error)
28status = 0
29error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
30print("恒力控制关闭错误码",error)

10.17. 计算中间平面位置开始

原型

FT_CalCenterStart()

描述

计算中间平面位置开始

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.18. 计算中间平面位置结束

原型

FT_CalCenterEnd()

描述

计算中间平面位置结束

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode - 返回值（调用成功返回）pos=[x,y,z,rx,ry,rz]

10.19. 表面定位

原型

FT_FindSurface (rcs, dir, axis, disMax, ft, lin_v = 3.0, lin_a = 0.0)

描述

表面定位

必选参数

  • rcs： 参考坐标系，0-工具坐标系，1-基坐标系；

  • dir：移动方向，1-正方向，2-负方向；

  • axis：移动轴，1-x，2-y，3-z；

  • disMax：大探索距离，单位 mm;

  • ft：动作终止力阈值，单位N；

默认参数

  • lin_v：探索直线速度，单位mm/s 默认3;

  • lin_a：探索直线加速度，单位mm/s^2 默认0;

返回值

错误码 成功-0 失败- errcode

10.19.1. 代码示例

 1from fairino import Robot
 2import time
 3# 与机器人控制器建立连接，连接成功返回一个机器人对象
 4robot = Robot.RPC('192.168.58.2')
 5#恒力控制
 6status = 1  #恒力控制开启标志，0-关，1-开
 7sensor_num = 1 #力传感器编号
 8is_select = [1,0,0,0,0,0]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
 9force_torque = [-2.0,0.0,0.0,0.0,0.0,0.0]
10gain = [0.0002,0.0,0.0,0.0,0.0,0.0]  #力PID参数，力矩PID参数
11adj_sign = 0  #自适应启停状态，0-关闭，1-开启
12ILC_sign = 0  #ILC控制启停状态，0-停止，1-训练，2-实操
13max_dis = 15.0  #最大调整距离
14max_ang = 0.0  #最大调整角度
15#表面定位参数
16rcs = 0 #参考坐标系，0-工具坐标系，1-基坐标系
17direction = 1 #移动方向，1-正方向，2-负方向
18axis = 1 #移动轴，1-X,2-Y,3-Z
19lin_v = 3.0  #探索直线速度，单位mm/s
20lin_a = 0.0  #探索直线加速度，单位mm/s^2
21disMax = 50.0 #最大探索距离，单位mm
22force_goal = 2.0 #动作终止力阈值，单位N
23P1=[-77.24,-679.599,58.328,179.373,-0.028,-167.849]
24Robot.MoveCart(P1,1,0)       #关节空间点到点运动
25#x方向寻找中心
26#第1个表面
27error = robot.FT_CalCenterStart()
28print("计算中间平面开始错误码",error)
29error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
30print("恒力控制开始错误码",error)
31error = robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
32print("寻找X+表面错误码",error)
33status = 0
34error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
35print("恒力控制结束错误码",error)
36time.sleep(2)
37error = robot.MoveCart(P1,1,0)       #关节空间点到点运动
38print("关节空间点到点运动错误码",error)
39time.sleep(5)
40#第2个表面
41error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
42print("恒力控制开始错误码",error)
43direction = 2 #移动方向，1-正方向，2-负方向
44error = robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
45print("寻找X—表面错误码",error)
46status = 0
47error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
48print("恒力控制结束错误码",error)
49#计算x方向中心位置
50error,xcenter = robot.FT_CalCenterEnd()
51print("计算X方向中间平面结束错误码",xcenter)
52error = robot.MoveCart(xcenter,1,0)
53print("关节空间点到点运动错误码",error)
54time.sleep(1)
55#y方向寻找中心
56#第1个表面
57error =robot.FT_CalCenterStart()
58print("计算中间平面开始错误码",error)
59error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
60print("恒力控制开始错误码",error)
61direction = 1 #移动方向，1-正方向，2-负方向
62axis = 2 #移动轴，1-X,2-Y,3-Z
63disMax = 150.0 #最大探索距离，单位mm
64lin_v = 6.0  #探索直线速度，单位mm/s
65error =robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
66print("寻找表面Y+错误码",error)
67status = 0
68error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
69print("恒力控制结束错误码",error)
70error =robot.MoveCart(P1,1,0)       #关节空间点到点运动
71print("关节空间点到点运动错误码",error)
72Robot.WaitMs(1000)
73#第2个表面
74error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
75print("恒力控制开始错误码",error)
76direction = 2 #移动方向，1-正方向，2-负方向
77error =robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
78print("寻找表面Y-错误码",error)
79status = 0
80error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
81print("恒力控制结束错误码",error)
82#计算y方向中心位置
83error,ycenter=robot.FT_CalCenterEnd()
84print("计算中间平面Y方向结束错误码",ycenter)
85error =robot.MoveCart(ycenter,1,0)
86print("关节空间点到点运动错误码",error)

10.20. 柔顺控制关闭

原型

FT_ComplianceStop()

描述

柔顺控制关闭

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.21. 柔顺控制开启

原型

FT_ComplianceStart(p,force)

描述

柔顺控制开启

必选参数

  • p: 位置调节系数或柔顺系数

  • force：柔顺开启力阈值，单位N

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.21.1. 代码示例

 1from fairino import Robot
 2import time
 3# 与机器人控制器建立连接，连接成功返回一个机器人对象
 4robot = Robot.RPC('192.168.58.2')
 5J1=[75.005,-46.434,90.687,-133.708,-90.315,-27.139]
 6P2=[-77.24,-679.599,38.328,179.373,-0.028,-167.849]
 7P3=[77.24,-679.599,38.328,179.373,-0.028,-167.849]
 8#恒力控制参数
 9status = 1  #恒力控制开启标志，0-关，1-开
10sensor_num = 1 #力传感器编号
11is_select = [1,1,1,0,0,0]  #六个自由度选择[fx,fy,fz,mx,my,mz]，0-不生效，1-生效
12force_torque = [-10.0,-10.0,-10.0,0.0,0.0,0.0]
13gain = [0.0005,0.0,0.0,0.0,0.0,0.0]  #力PID参数，力矩PID参数
14adj_sign = 0  #自适应启停状态，0-关闭，1-开启
15ILC_sign = 0  #ILC控制启停状态，0-停止，1-训练，2-实操
16max_dis = 1000.0  #最大调整距离
17max_ang = 0.0  #最大调整角度
18error = robot.MoveJ(J1,1,0)
19print("关节空间运动到点1错误码",error)
20#柔顺控制
21error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
22print("恒力控制开始错误码",error)
23p = 0.00005  #位置调节系数或柔顺系数
24force = 30.0 #柔顺开启力阈值，单位N
25error = robot.FT_ComplianceStart(p,force)
26print("柔顺控制开始错误码",error)
27error = robot.MoveL(P2,1,0,vel =10)   #笛卡尔空间直线运动
28print("笛卡尔空间直线运动到点2错误码", error)
29error = robot.MoveL(P3,1,0,vel =10)
30print("笛卡尔空间直线运动到点3错误码", error)
31time.sleep(1)
32error = robot.FT_ComplianceStop()
33print("柔顺控制结束错误码",error)
34status = 0
35error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign, ILC_sign,max_dis,max_ang)
36print("恒力控制关闭错误码",error)

10.22. 负载辨识滤波初始化

在 python 版本加入: SDK-v2.0.1

原型

LoadIdentifyDynFilterInit()

描述

负载辨识滤波初始化

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.22.1. 代码示例

 1from fairino import Robot
 2
 3# 与机器人控制器建立连接，连接成功返回一个机器人对象
 4robot = Robot.RPC('192.168.58.2')
 5
 6#负载辨识滤波初始化
 7error = robot.LoadIdentifyDynFilterInit()
 8print("LoadIdentifyDynFilterInit:",error)
 9#负载辨识变量初始化
10error = robot.LoadIdentifyDynVarInit()
11print("LoadIdentifyDynVarInit:",error)
12
13joint_torque= [0,0,0,0,0,0]
14joint_pos= [0,0,0,0,0,0]
15gain=[0,0.05,0,0,0,0,0,0.02,0,0,0,0]
16t =10
17error,joint_pos=robot.GetActualJointPosDegree(1)
18joint_pos[1] = joint_pos[1]+10
19error,joint_torque=robot.GetJointTorques(1)
20joint_torque[1] = joint_torque[1]+2
21#负载辨识主程序
22error = robot.LoadIdentifyMain(joint_torque, joint_pos, t)
23print("LoadIdentifyMain:",error)
24#获取负载辨识结果
25error = robot.LoadIdentifyGetResult(gain)
26print("LoadIdentifyGetResult:",error)

10.23. 负载辨识变量初始化

在 python 版本加入: SDK-v2.0.1

原型

LoadIdentifyDynVarInit()

描述

负载辨识变量初始化

必选参数

无

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.24. 负载辨识主程序

在 python 版本加入: SDK-v2.0.1

原型

LoadIdentifyMain(joint_torque, joint_pos, t)

描述

负载辨识主程序

必选参数

  • joint_torque： 关节扭矩 j1-j6；

  • joint_pos：关节位置 j1-j6

默认参数

无

返回值

错误码 成功-0 失败- errcode

10.25. 获取负载辨识结果

在 python 版本加入: SDK-v2.0.1

原型

LoadIdentifyGetResult(gain)

描述

获取负载辨识结果

必选参数

  • gain

默认参数

无

返回值

  • 错误码 成功-0 失败- errcode

  • Return:（if success）weight 负载重量，cog 负载质心 [x,y,z]