MTB_Robot

組長40823131先做出手臂程式，我再和組長想出要如何在coppeliasim內新增UI介面，但介面操控上還有一些問題需要改進。

場景檔:

手臂程式:(沒有UI介面)

function sysCall_init()
    corout=coroutine.create(coroutineMain)
end
 
function sysCall_actuation()
    if coroutine.status(corout)~='dead' then
        local ok,errorMsg=coroutine.resume(corout)
        if errorMsg then
            error(debug.traceback(corout,errorMsg),2)
        end
    else
        corout=coroutine.create(coroutineMain)
    end
end
 
function movCallback(config,vel,accel,handles)
    for i=1,#handles,1 do
        if sim.getJointMode(handles[i])==sim.jointmode_force and sim.isDynamicallyEnabled(handles[i]) then
            sim.setJointTargetPosition(handles[i],config[i])
        else   
            sim.setJointPosition(handles[i],config[i])
        end
    end
end
 
function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf,enable)
    local currentConf={}
    for i=1,#handles,1 do
        currentConf[i]=sim.getJointPosition(handles[i])
        targetConf[i]=targetConf[i]*math.pi/180
    end
    sim.moveToConfig(-1,currentConf,nil,nil,maxVel,maxAccel,maxJerk,targetConf,nil,movCallback,handles)
 
    if enable then
        sim.writeCustomDataBlock(gripperHandle,'activity','on')
    else
        sim.writeCustomDataBlock(gripperHandle,'activity','off')
    end
end
 
function coroutineMain()
    modelBase=sim.getObjectHandle(sim.handle_self)
    gripperHandle=sim.getObjectHandle('suctionPad')
    modelName=sim.getObjectName(modelBase)
    motorHandles = {}
    for i=1,4,1 do
        motorHandles[i]=sim.getObjectHandle('MTB_axis'..i)
    end
    local vel=60
    local accel=10
    local jerk=10
    local maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
    local maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    local maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
    moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{0,0,0,0},true)
    moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{0,0,1.9,0},true)
    moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{0,0,-1.9,0},true)
    moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{-160,-43.5,0,203.5},false)
    moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{160,43.5,0,203.5},false)
    moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{160,43.5,1.90,-203.5},true)
    moveToConfig(motorHandles,maxVel,maxAccel,maxJerk,{160,43.5,-1.90,-203.5},true)
end

手臂程式:(包含UI介面)

function sysCall_init()
  --Notes can refer to the previous chapter
xml = [[
<ui closeable="true" onclose="closeEventHandler" resizable="true" size="220,200">
    <label text="MTB_Robot control" wordwrap="true" />
    
    <group>
        <button text="Start move" onclick = "Start_move" />
        <button text="Stop move" onclick = "Stop_move" />
        <stretch />
    </group>   
 
</ui>
]] 
    ui=simUI.create(xml)

    
    --ui=simUI.create(xml)

    Start_flag = false
    Stop_flag = false
    modelBase=sim.getObjectHandle(sim.handle_self)
    gripperHandle=sim.getObjectHandle('suctionPad')
    modelName=sim.getObjectName(modelBase)
    motorHandles = {}
    local vel=10
    local accel=10
    local jerk=10
    local maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
    local maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
    local maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
end
function closeEventHandler(h)
    sim.addStatusbarMessage('Window '..h..' is closing...')
    simUI.hide(h)
end
function Start_move(h)
    Start_flag = true
end
function Stop_move(h)
    Stop_flag = true   
end

function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf,enable)
    local currentConf={}
    for i=1,#handles,1 do
        currentConf[i]=sim.getJointPosition(handles[i])
        targetConf[i]=targetConf[i]*math.pi/180
    end
    sim.moveToConfig(-1,currentConf,nil,nil,maxVel,maxAccel,maxJerk,targetConf,nil,movCallback,handles)

    if enable then
        sim.writeCustomDataBlock(gripperHandle,'activity','on')
    else
        sim.writeCustomDataBlock(gripperHandle,'activity','off')
    end
end
function sysCall_actuation()
        if Start_flag then
            for i=1,#handles,1 do
                if sim.getJointMode(handles[i])==sim.jointmode_force and sim.isDynamicallyEnabled(handles[i]) then
                    sim.setJointTargetPosition(handles[i],config[i])
                else    
                    sim.setJointPosition(handles[i],config[i])
                end
            end
        end
        if Stop_flag then

            sim.setJointTargetPosition('MTB_axis1',0)
            
        end
        
end

function movCallback(config,vel,accel,handles)
    
end
 

 
function sysCall_cleanup()
    -- do some clean-up here
    simUI.destroy(ui)
end

吸盤程式:

function sysCall_init()
    modelBase=sim.getObjectHandle(sim.handle_self)
    s=sim.getObjectHandle('suctionPadSensor')
    l=sim.getObjectHandle('suctionPadLoopClosureDummy1')
    l2=sim.getObjectHandle('suctionPadLoopClosureDummy2')
    b=sim.getObjectHandle('suctionPadBodyRespondable')
    sim.setLinkDummy(l,-1)
    sim.setObjectParent(l,b,true)
    m=sim.getObjectMatrix(l2,-1)
    sim.setObjectMatrix(l,-1,m)
    suctionPadLink=sim.getObjectHandle('suctionPadLink')
    local gripperBase=sim.getObjectHandle(sim.handle_self)
    infiniteStrength=true
    maxPullForce=3
    maxShearForce=1
    maxPeelTorque=0.1
 
end
 
function sysCall_cleanup() 
    sim.setLinkDummy(l,-1)
    sim.setObjectParent(l,b,true)
    m=sim.getObjectMatrix(l2,-1)
    sim.setObjectMatrix(l,-1,m)
end
 
function sysCall_sensing() 
    parent=sim.getObjectParent(l)
    local on=sim.readCustomDataBlock(modelBase,'activity')=='on'
    if not on then
        if (parent~=b) then
            sim.setLinkDummy(l,-1)
            sim.setObjectParent(l,b,true)
            m=sim.getObjectMatrix(l2,-1)
            sim.setObjectMatrix(l,-1,m)
        end
    else
        if (parent==b) then
            index=0
            while true do
                shape=sim.getObjects(index,sim.object_shape_type)
                if (shape==-1) then
                    break
                end
                local res,val=sim.getObjectInt32Parameter(shape,sim.shapeintparam_respondable)
                if (shape~=b) and (val~=0) and (sim.checkProximitySensor(s,shape)==1) then
                    -- Ok, we found a respondable shape that was detected
                    -- We connect to that shape:
                    -- Make sure the two dummies are initially coincident:
                    sim.setObjectParent(l,b,true)
                    m=sim.getObjectMatrix(l2,-1)
                    sim.setObjectMatrix(l,-1,m)
                    -- Do the connection:
                    sim.setObjectParent(l,shape,true)
                    sim.setLinkDummy(l,l2)
                    break
                end
                index=index+1
            end
        else
            -- Here we have an object attached
            if (infiniteStrength==false) then
                -- We might have to conditionally beak it apart!
                result,force,torque=sim.readForceSensor(suctionPadLink) -- Here we read the median value out of 5 values (check the force sensor prop. dialog)
                if (result>0) then
                    breakIt=false
                    if (force[3]>maxPullForce) then breakIt=true end
                    sf=math.sqrt(force[1]*force[1]+force[2]*force[2])
                    if (sf>maxShearForce) then breakIt=true end
                    if (torque[1]>maxPeelTorque) then breakIt=true end
                    if (torque[2]>maxPeelTorque) then breakIt=true end
                    if (breakIt) then
                        -- We break the link:
                        sim.setLinkDummy(l,-1)
                        sim.setObjectParent(l,b,true)
                        m=sim.getObjectMatrix(l2,-1)
                        sim.setObjectMatrix(l,-1,m)
                    end
                end
            end
        end
    end
end

W14 << Previous Next >> W15