W14 << Previous Next >> gitlab

MTB_Robot

手臂程式:

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

吸盤程式:

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 >> gitlab