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

37 lines
1.7 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')
J1=[75.005,-46.434,90.687,-133.708,-90.315,-27.139]
P2=[-77.24,-679.599,38.328,179.373,-0.028,-167.849]
P3=[77.24,-679.599,38.328,179.373,-0.028,-167.849]
#恒力控制参数
status = 1 #恒力控制开启标志0-关1-开
sensor_num = 1 #力传感器编号
is_select = [1,1,1,0,0,0] #六个自由度选择[fx,fy,fz,mx,my,mz]0-不生效1-生效
force_torque = [-10.0,-10.0,-10.0,0.0,0.0,0.0]
gain = [0.0005,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 = 1000.0 #最大调整距离
max_ang = 0.0 #最大调整角度
error = robot.MoveJ(J1,1,0)
print("关节空间运动到点1错误码",error)
#柔顺控制
error = robot.FT_Control(status,sensor_num,is_select,force_torque,gain,adj_sign,ILC_sign,max_dis,max_ang)
print("恒力控制开始错误码",error)
p = 0.00005 #位置调节系数或柔顺系数
force = 30.0 #柔顺开启力阈值单位N
error = robot.FT_ComplianceStart(p,force)
print("柔顺控制开始错误码",error)
error = robot.MoveL(P2,1,0,vel =10) #笛卡尔空间直线运动
print("笛卡尔空间直线运动到点2错误码", error)
error = robot.MoveL(P3,1,0,vel =10)
print("笛卡尔空间直线运动到点3错误码", error)
time.sleep(1)
error = robot.FT_ComplianceStop()
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)