VREP中的力触觉设备接口（CHAI3D）

addCuboid=function(siz,pos,orient)
    if CHAI3DPluginInitialized then
        local cuboidHandle=simCreatePureShape(0,4+16,siz,1)
        simSetObjectParent(cuboidHandle,modelHandle,true)
        simSetObjectPosition(cuboidHandle,modelHandle,pos)
        simSetObjectOrientation(cuboidHandle,modelHandle,orient)
        return addMesh(cuboidHandle,true), cuboidHandle
    end
    return -1, -1
end


-- copy V-REP objects to the CHAI3D scene so that they can be "felt" with the haptic device
addMesh=function(shapeHandle,whenRemovedEraseMesh)
    if CHAI3DPluginInitialized then
        if not objCont then
            objCont={}
        end
        objCont[#objCont+1]=shapeHandle
        local vertices,indices=simGetShapeMesh(shapeHandle)
        local pos=simGetObjectPosition(shapeHandle,modelHandle)
        local orient=simGetObjectOrientation(shapeHandle,modelHandle)
        local hapticObjId=simExtCHAI3D_addShape(vertices,indices,pos,orient,1000)
        objCont[#objCont+1]=hapticObjId
        objCont[#objCont+1]=whenRemovedEraseMesh
        return hapticObjId
    end
    return -1
end


removeAllObjects=function()
    if CHAI3DPluginInitialized then
        if objCont then
            for i=1,(#objCont)/3,1 do
                local obj=objCont[3*(i-1)+1]
                if objCont[3*(i-1)+3] then
                    simRemoveObject(obj)
                end
                simExtCHAI3D_removeObject(objCont[3*(i-1)+2])
            end
        end
        objCont=nil
    end
end



if (sim_call_type==sim_childscriptcall_initialization) then

    modelHandle=simGetObjectAssociatedWithScript(sim_handle_self)

    -- Check if the plugin is loaded:
    moduleName=0
    moduleVersion=0
    index=0
    pluginNotFound=true
    while moduleName do
        moduleName,moduleVersion=simGetModuleName(index)
        if (moduleName==‘CHAI3D‘) then
            pluginNotFound=false
        end
        index=index+1
    end

    if (pluginNotFound) then
        simDisplayDialog(‘Error‘,‘CHAI3D plugin was not found, or was not correctly initialized (v_repExtCHAI3D.dll).‘,sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
    else

        -- Start the device:
        local toolRadius=0.005 -- the radius of the tool
        local workspaceRadius=0.2 -- the workspace radius
        if simExtCHAI3D_start(0,toolRadius,workspaceRadius)~=1 then
            simDisplayDialog(‘Error‘,‘Device failed to initialize.‘,sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})
        else
            CHAI3DPluginInitialized=true

            toolSphereHandle=simCreatePureShape(1,4+16,{toolRadius*2,toolRadius*2,toolRadius*2},1)
            simSetObjectParent(toolSphereHandle,modelHandle,true)
            simSetShapeColor(toolSphereHandle,‘‘,0,{1,1,1})
         
            -- Create a haptic cuboid:
            local cuboidSize={0.1,0.1,0.1}
            local cuboidPosition={0.01,-0.01,0.05}
            local cuboidOrientation={0,0,0}
            cuboidId,cuboidHandle=addCuboid(cuboidSize,cuboidPosition,cuboidOrientation)

--[[
            -- Create a haptic mesh from an existing V-REP shape:
            meshHandle=simGetObjectHandle(‘mesh‘)
            local removeOriginalShapeWhenDone=false
            meshId=addMesh(meshHandle,removeOriginalShapeWhenDone)
--]]

        end
    end
end


if (sim_call_type==sim_childscriptcall_actuation) then

end


if (sim_call_type==sim_childscriptcall_sensing) then
    if CHAI3DPluginInitialized then

        -- Read the position of the cursor:
        local p=simExtCHAI3D_readPosition(0)

        -- Reflect the position of the cursor in V-REP:
        simSetObjectPosition(toolSphereHandle,modelHandle,p)

        -- Read the the interaction force:
        local f=simExtCHAI3D_readForce(0)
        
        -- Read the buttons of the device:
        local b = simExtCHAI3D_readButtons(0)

        local info = string.format("Position:%.2f,%.2f,%.2f Force:%.2f,%.2f,%.2f Button State:%d",p[1],p[2],p[3],f[1],f[2],f[3],b)  
        simAddStatusbarMessage(info)
    end
end


if (sim_call_type==sim_childscriptcall_cleanup) then
    if CHAI3DPluginInitialized then
        -- Remove all Haptic objects (objects and forces):
        removeAllObjects()

        -- Remove all objects in the haptic scene:
        simExtCHAI3D_reset()
    end
end