EC基于力控的搜索与装配


1. EC机器人支持力控功能,具体见 https://bbs.elibot.cn/forum/detail/topic/181.html 和 https://bbs.elibot.cn/forum/detail/topic/191.html

2. 以上为机器人使用简易螺旋线,以固定向下2n的力进行搜索,当机器人末端位姿的z小于设定值,可以认为机器人找到探寻的洞。

    机器人在从当前停下位置开始向下走5mm进行装配。装配过程中,设定机器人在base坐标系的x和y方向力均为0(即希望外部力为0,不然就往反方向移动,使得装配成功)

   可以使用以下JBI代码,以及后台监控机器人末端位姿代码

NOP
SETPOSE V000 281.7873,-522.1475,35.6150,3.1248,-0.0028,2.9738
//螺旋线中心点
SET D000 20
//D000 运动速度
SET D001 20
//D001 螺旋线最大半径
SET D002 10
//D002 到达螺旋线最大半径的圈数

//*******************************************
// 以下内容无需修改
SET I010 0
TIMER T=0.5 S
WAIT I010 = 0
SET D020 0
MUL D002 2
SET D011 D001
SET D012 D002
DIV D011 D012
//计算初始半径,存入到D011
SET D012 D011
SET V009 V000
ADD V009(1) D012
MOVEL V=D000MM/S CR=0.0MM V000
//机器人走到螺旋线中心点,并开启力控,即机器人以恒定的z方向-2n进行螺旋运动
STARTFORCEMODE Mode=0 ConstV=[0,0,0,0,0,0] ConstV=[0,0,1,0,0,0] ConstV=[0.000,0.000,-2,0.000,0.000,0.000] ConstV=[20.000,20.000,20.000,5.730,5.730,5.730]
// Mode=0表示参考机器人base坐标系,ConstV=[0,0,1,0,0,0]表示仅z方向进行力控,ConstV=[0.000,0.000,-2,0.000,0.000,0.000]表示z方向的力是-2N
// ConstV=[20.000,20.000,20.000,5.730,5.730,5.730] 表示xyz三个方向最大运动速度mm/s 以及rx ry rz三个旋转方向最大速度rad/s
MOVEL V=D000MM/S CR=0.0MM V009
LABEL *t
SET V010 V000
SET V011 V000
SET V012 V000
SET V013 V000

SUB V010(0) D012
SUB V011(1) D012
ADD D012 D011
ADD V012(0) D012
ADD V013(1) D012

MOVEC V=D000MM/S CR=5.0MM V010 V011 UNTIL I010=1
// 当LUA中监控I010=1,机器人停止运动(LUA中判断当前位姿的z小于设定即触发I010=1)
IF I010 =1
    JUMP *EE
ENDIF
INC D020
MOVEC V=D000MM/S CR=5.0MM V012 V013 UNTIL I010=1
// 当LUA中监控I010=1,机器人停止运动(LUA中判断当前位姿的z小于设定即触发I010=1)
IF I010 =1
    JUMP *EE
ENDIF
ADD D012 D011
INC D020
JUMP *t IF D020<D002
LABEL *EE
ENDFORCEMODE
//停止螺旋线力控搜索

GETPOS P001
//获取当前停下的位置
JOINTTOPOSE P001 V001
SUB V001(2) 5
//再往下5mm
//重新开启力控,此时仅对x和y方向进行力控,即使得机器人能够顺利插入产品
STARTFORCEMODE Mode=0 ConstV=[0,0,0,0,0,0] ConstV=[1,1,0,0,0,0] ConstV=[0.000,0.000,-2,0.000,0.000,0.000] ConstV=[20.000,20.000,20.000,5.730,5.730,5.730]
MOVL V001 V=5MM/S CR=0.000MM 
//向下走5mm
ENDFORCEMODE
END

后台监控LUA如下:

local table_height = 33
-- 设定判断机器人位姿的z高度
sleep(0.3)
set_global_variable('I010',0)
sleep(0.3)
while true do
    local c_pose = get_tcp_pose()
    if c_pose[3] < table_height then
        set_global_variable('I010',1)
    end
    sleep(0.1)
end

点击显示全文
赞同1
发表评论
分享

手机扫码分享
0
753
收藏
举报
收起
登录
  • 密码登录
  • 验证码登录
还没有账号,立即注册
还没有账号,立即注册
注册
已有账号,立即登录
选择发帖板块
举报
请选择举报理由
举报
举报说明