graduation-design/robot_control/examples/force_rot_insertion.py
2024-03-30 15:52:26 +08:00

34 lines
1.8 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
# 与机器人控制器建立连接,连接成功返回一个机器人对象
robot = Robot.RPC('192.168.58.2')
P = [36.794,-675.119, 65.379, -176.938, 2.535, -179.829]
#恒力参数
status = 1 #恒力控制开启标志0-关1-开
sensor_num = 1 #力传感器编号
is_select = [0,0,1,0,0,0] #六个自由度选择[fx,fy,fz,mx,my,mz]0-不生效1-生效
force_torque = [0.0,0.0,-10.0,0.0,0.0,0.0] #碰撞检测力和力矩检测范围force_torque-min_threshold,force_torque+max_threshold
gain = [0.0001,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 = 100.0 #最大调整距离
max_ang = 5.0 #最大调整角度
#旋转插入参数
rcs = 0 #参考坐标系0-工具坐标系1-基坐标系
forceInsertion = 2.0 #力或力矩阈值0~100单位N或Nm
orn = 1 #力的方向1-fz,2-mz
#默认参数 angVelRot旋转角速度单位 °/s 默认 3
#默认参数 angleMax最大旋转角度单位 ° 默认 5
#默认参数 angAccmax最大旋转加速度单位 °/s^2暂不使用 默认0
#默认参数 rotorn旋转方向1-顺时针2-逆时针 默认1
error = robot.MoveL(P,1,0) #笛卡尔空间直线运动至初始点
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_RotInsertion(rcs,1,orn)
print("旋转插入错误码",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)