graduation-design/abandon_code/examples/force_find_surface_calcenter.py
2024-04-25 20:10:52 +08:00

89 lines
4.0 KiB
Python
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

from fairino import Robot
import time
# 与机器人控制器建立连接,连接成功返回一个机器人对象
robot = Robot.RPC('192.168.58.2')
#恒力控制
status = 1 #恒力控制开启标志0-关1-开
sensor_num = 1 #力传感器编号
is_select = [1,0,0,0,0,0] #六个自由度选择[fx,fy,fz,mx,my,mz]0-不生效1-生效
force_torque = [-2.0,0.0,0.0,0.0,0.0,0.0]
gain = [0.0002,0.0,0.0,0.0,0.0,0.0] #力PID参数力矩PID参数
adj_sign = 0 #自适应启停状态0-关闭1-开启
ILC_sign = 0 #ILC控制启停状态0-停止1-训练2-实操
max_dis = 15.0 #最大调整距离
max_ang = 0.0 #最大调整角度
#表面定位参数
rcs = 0 #参考坐标系0-工具坐标系1-基坐标系
direction = 1 #移动方向1-正方向2-负方向
axis = 1 #移动轴1-X,2-Y,3-Z
lin_v = 3.0 #探索直线速度单位mm/s
lin_a = 0.0 #探索直线加速度单位mm/s^2
disMax = 50.0 #最大探索距离单位mm
force_goal = 2.0 #动作终止力阈值单位N
P1=[-77.24,-679.599,58.328,179.373,-0.028,-167.849]
Robot.MoveCart(P1,1,0) #关节空间点到点运动
#x方向寻找中心
#第1个表面
error = robot.FT_CalCenterStart()
print("计算中间平面开始错误码",error)
error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制开始错误码",error)
error = robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
print("寻找X+表面错误码",error)
status = 0
error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制结束错误码",error)
time.sleep(2)
error = robot.MoveCart(P1,1,0) #关节空间点到点运动
print("关节空间点到点运动错误码",error)
time.sleep(5)
#第2个表面
error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制开始错误码",error)
direction = 2 #移动方向1-正方向2-负方向
error = robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
print("寻找X—表面错误码",error)
status = 0
error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制结束错误码",error)
#计算x方向中心位置
error,xcenter = robot.FT_CalCenterEnd()
print("计算X方向中间平面结束错误码",xcenter)
error = robot.MoveCart(xcenter,1,0)
print("关节空间点到点运动错误码",error)
time.sleep(1)
#y方向寻找中心
#第1个表面
error =robot.FT_CalCenterStart()
print("计算中间平面开始错误码",error)
error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制开始错误码",error)
direction = 1 #移动方向1-正方向2-负方向
axis = 2 #移动轴1-X,2-Y,3-Z
disMax = 150.0 #最大探索距离单位mm
lin_v = 6.0 #探索直线速度单位mm/s
error =robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
print("寻找表面Y+错误码",error)
status = 0
error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制结束错误码",error)
error =robot.MoveCart(P1,1,0) #关节空间点到点运动
print("关节空间点到点运动错误码",error)
Robot.WaitMs(1000)
#第2个表面
error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制开始错误码",error)
direction = 2 #移动方向1-正方向2-负方向
error =robot.FT_FindSurface(rcs,direction,axis,disMax,force_goal)
print("寻找表面Y-错误码",error)
status = 0
error =robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制结束错误码",error)
#计算y方向中心位置
error,ycenter=robot.FT_CalCenterEnd()
print("计算中间平面Y方向结束错误码",ycenter)
error =robot.MoveCart(ycenter,1,0)
print("关节空间点到点运动错误码",error)