CoppeliaSim forums

Code: Select all

function sysCall_init()
        
    -- Following are the joints that we control when in FK mode:
    local fkDrivingJoints={-1,-1,-1,-1}
    fkDrivingJoints[1] = sim.getObject('./drivingJoint1')
    fkDrivingJoints[2] = sim.getObject('./drivingJoint2')
    fkDrivingJoints[3] = sim.getObject('./drivingJoint3')
    fkDrivingJoints[4] = sim.getObject('./motor')

    -- Following are the joints that we control when in IK mode:
    local ikDrivingJoints={-1,-1,-1,-1}
    ikDrivingJoints[1] = sim.getObject('./cartesianX')
    ikDrivingJoints[2] = sim.getObject('./cartesianY')
    ikDrivingJoints[3] = sim.getObject('./cartesianZ')
    ikDrivingJoints[4] = sim.getObject('./motor')
    
    -- create an IK enviorment
    ikEnv = simIK.createEnvironment()

    -- The main IK group (main task, the 3 arms and platform):
    
    -- get objects handles
    -- here the author had shooden to use the sim.getObject API function
    -- he could have used the simIK.getObjectHandle instead
    local base = sim.getObject('.')
    local ikTip = sim.getObject('./ikTip')
    local ikTarget = sim.getObject('./ikTarget')
    local loop1Tip = sim.getObject('./loopTip')
    local loop1Target=sim.getObject('./loopTarget')
    local loop2Tip=sim.getObject('./loopTip0')
    local loop2Target=sim.getObject('./loopTarget0')
    local loop3Tip=sim.getObject('./loopTip1')
    local loop3Target=sim.getObject('./loopTarget1')
    local loop4Tip=sim.getObject('./loopTip2')
    local loop4Target=sim.getObject('./loopTarget2')
    local loop5Tip=sim.getObject('./loopTip3')
    local loop5Target=sim.getObject('./loopTarget3')
    
    -- create the IK group
    -- see remarks at the CoppeliSim user manual: Funcionality->Kinematics->Solving IK and FK Tasks
    -- here, at the Delta robot, there are few kinematics chains attached together.
    -- therefore they all should be consists under the same kinematic group. 
    local ikGroup_main = simIK.createGroup(ikEnv)
    
    -- sets the calculation method 
    -- method options: simIK.method_pseudo_inverse (features a tiny bit of hard-coded damping), simIK.method_undamped_pseudo_inverse, 
    -- simIK.method_damped_least_squares and simIK.method_jacobian_transpose
    simIK.setGroupCalculation(ikEnv,ikGroup_main,simIK.method_pseudo_inverse,0,6)
    
    -- generate main IK group related IK elements
    -- simIK.addElementFromScene: is a convenient function to quickly generate an IK element from a kinematic chain in the scene
    -- at this Delta parallel robot model there are 6 kinemtics chain to be configured
    -- 1st chain: base -> tip, through right bar arm #1
    local ikElementTip = simIK.addElementFromScene(ikEnv,ikGroup_main,base,ikTip,ikTarget,simIK.constraint_position)
    -- 2nd chain: closed chain left bar arm #1
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_main,base,loop1Tip,loop1Target,simIK.constraint_pose)
    -- 3rd chain: closed chain left bar arm #2
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_main,base,loop2Tip,loop2Target,simIK.constraint_pose)
    -- 4th chain: closed chain right bar arm #2
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_main,base,loop3Tip,loop3Target,simIK.constraint_pose)
    -- 5th chain: closed chain left bar arm #3
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_main,base,loop4Tip,loop4Target,simIK.constraint_pose)
    -- 6th chain: closed chain left bar arm #3
    local ikElement, mapping = simIK.addElementFromScene(ikEnv,ikGroup_main,base,loop5Tip,loop5Target,simIK.constraint_pose)
    
    -- why/how does the last ikElement contains all DrivingJoints?
    local fkDrivingJoints_inIkEnv = {mapping[fkDrivingJoints[1]],mapping[fkDrivingJoints[2]],mapping[fkDrivingJoints[3]]}
    --local fkDrivingJoints_inIkEnv = {mapping[fkDrivingJoints[1]],mapping[fkDrivingJoints[2]],mapping[fkDrivingJoints[3]],mapping[fkDrivingJoints[4]]}
    local ikDrivingJoints_inIkEnv = {mapping[ikDrivingJoints[1]],mapping[ikDrivingJoints[2]],mapping[ikDrivingJoints[3]]}
    --local ikDrivingJoints_inIkEnv = {mapping[ikDrivingJoints[1]],mapping[ikDrivingJoints[2]],mapping[ikDrivingJoints[3]],mapping[ikDrivingJoints[4]]}
    local ikTarget_inIkEnv = mapping[ikTarget]
    local ikBase_inIkEnv = mapping[base]    
    
    -- The secondary IK group (handling the center axis):
    local axisBase = sim.getObject('./axisL')
    local axisTip = sim.getObject('./axisTip')
    local axisTarget = sim.getObject('./axisTarget')
    
    local ikGroup_axis = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv,ikGroup_axis,simIK.method_pseudo_inverse,0,6)
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_axis,axisBase,axisTip,axisTarget,simIK.constraint_pose)
    simIK.setElementPrecision(ikEnv,ikGroup_axis,ikElement,{0.0001,0.1})
    
    
    -- The IK groups handling the secondary arm bridges:
    local bridge1Base = sim.getObject('./j2')
    local bridge1Tip = sim.getObject('./bridgeLTip')
    local bridge1Target = sim.getObject('./bridgeLTarget')

    local ikGroup_bridge1 = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv,ikGroup_bridge1,simIK.method_damped_least_squares,0.01,3)
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_bridge1,bridge1Base,bridge1Tip,bridge1Target,simIK.constraint_position)
    
    -- why have the autor choosen 'j26' as base?
    local bridge2Base = sim.getObject('./j26')
    local bridge2Tip = sim.getObject('./bridgeRTip')
    local bridge2Target = sim.getObject('./bridgeRTarget')
    
    local ikGroup_bridge2 = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv,ikGroup_bridge2,simIK.method_damped_least_squares,0.01,3)
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_bridge2,bridge2Base,bridge2Tip,bridge2Target,simIK.constraint_position)
    
    -- why have the autor choosen 'j28' as base?
    local bridge3Base = sim.getObject('./j28')
    local bridge3Tip = sim.getObject('./bridgeLTip0')
    local bridge3Target = sim.getObject('./bridgeLTarget0')
    
    local ikGroup_bridge3 = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv,ikGroup_bridge3,simIK.method_damped_least_squares,0.01,3)
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_bridge3,bridge3Base,bridge3Tip,bridge3Target,simIK.constraint_position)
    
    -- why have the autor choosen 'j29' as base?
    local bridge4Base = sim.getObject('./j29')
    local bridge4Tip = sim.getObject('./bridgeRTip0')
    local bridge4Target = sim.getObject('./bridgeRTarget0')

    local ikGroup_bridge4 = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv,ikGroup_bridge4,simIK.method_damped_least_squares,0.01,3)
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_bridge4,bridge4Base,bridge4Tip,bridge4Target,simIK.constraint_position)
    
    -- why have the autor choosen 'j31' as base?
    local bridge5Base = sim.getObject('./j31')
    local bridge5Tip = sim.getObject('./bridgeLTip1')
    local bridge5Target = sim.getObject('./bridgeLTarget1')
    
    local ikGroup_bridge5 = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv,ikGroup_bridge5,simIK.method_damped_least_squares,0.01,3)
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_bridge5,bridge5Base,bridge5Tip,bridge5Target,simIK.constraint_position)

    -- why have the autor choosen 'j34' as base?
    local bridge6Base = sim.getObject('./j34')
    local bridge6Tip = sim.getObject('./bridgeRTip1')
    local bridge6Target = sim.getObject('./bridgeRTarget1')
    
    local ikGroup_bridge6 = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv,ikGroup_bridge6,simIK.method_damped_least_squares,0.01,3)
    local ikElement = simIK.addElementFromScene(ikEnv,ikGroup_bridge6,bridge6Base,bridge6Tip,bridge6Target,simIK.constraint_position)
    

    -- Movement profiles when in FK mode:
    local angVel = 180*math.pi/180
    local angAccel = 360*math.pi/180
    local angJerk = 3600*math.pi/180
    local maxAngVel = {angVel,angVel,angVel,angVel}
    local maxAngAccel = {angAccel,angAccel,angAccel,angAccel}
    local maxAngJerk = {angJerk,angJerk,angJerk,angJerk}

    -- Movement profiles when in IK mode:
    local linVel = 2
    local linAccel = 3
    local linJerk = 30
    local maxLinVel = {linVel,linVel,linVel,angVel}
    local maxLinAccel = {linAccel,linAccel,linAccel,angAccel}
    local maxLinJerk = {linJerk,linJerk,linJerk,angJerk}
    
    local structuralParameters = {}
    structuralParameters.robotHandle = base
    structuralParameters.tipHandle = ikTip
    structuralParameters.ikMode = false
    structuralParameters.ikEnv = ikEnv
    structuralParameters.mainIkGroup = ikGroup_main
    structuralParameters.platformIkElement = ikElementTip
    structuralParameters.axisIkGroup = ikGroup_axis
    structuralParameters.bridgeIkGroups = {ikGroup_bridge1,ikGroup_bridge2,ikGroup_bridge3,ikGroup_bridge4,ikGroup_bridge5,ikGroup_bridge6}
    structuralParameters.fkDrivingJoints = fkDrivingJoints
    structuralParameters.ikDrivingJoints = ikDrivingJoints
    structuralParameters.fkDrivingJoints_inIkEnv = fkDrivingJoints_inIkEnv
    structuralParameters.ikDrivingJoints_inIkEnv = ikDrivingJoints_inIkEnv
    structuralParameters.ikTarget_inIkEnv = ikTarget_inIkEnv
    structuralParameters.ikBase_inIkEnv = ikBase_inIkEnv
    
    sim.writeCustomTableData(base, "irb360StructuralParameters", structuralParameters)
    
    local kinematicParameters = {}
    kinematicParameters.jointMaxAngVel = maxAngVel
    kinematicParameters.jointMaxAngAccel = maxAngAccel
    kinematicParameters.jointMaxAngJerk = maxAngJerk
    kinematicParameters.toolMaxLinVel = maxLinVel
    kinematicParameters.toolMaxLinAccel = maxLinAccel
    kinematicParameters.toolMaxLinJerk = maxLinJerk
    
    sim.writeCustomTableData(base, "irb360KinematicParameters", kinematicParameters)
    
    -- Create robot collection:    
    robotCollection = sim.createCollection()
    sim.addItemToCollection(robotCollection,sim.handle_tree,base,0)
    
end

Code: Select all

function collisionValidation(config,info)
    -- check if a configuration is valid, i.e. doesn't collide  
    -- save current config:
    local tmp = getConfig(info)
    
    -- apply new config:
    setConfig(config,nil,nil,info)
    
    -- does new config collide?
    local res = sim.checkCollision(robotCollection,sim.handle_all)
    
    -- restore original config:
    setConfig(tmp,nil,nil,info)
    
    return res == 0
end

function moveToConfigOMPL(goalPose,structuralParameters,kinematicParameters)
    
    setIkMode(structuralParameters)
    
    local simJointHandles = structuralParameters.ikDrivingJoints
    local useForProjection = {}
    
    for i = 1,#simJointHandles,1 do
        useForProjection[i] = i<=3 and 1 or 0
    end
    
    local ikBase = structuralParameters.ikBase_inIkEnv
    local ikTarget = structuralParameters.ikTarget_inIkEnv
    
    local ikEnv = structuralParameters.ikEnv
    local ikGroup = structuralParameters.mainIkGroup
    local ikJointHandles = structuralParameters.ikDrivingJoints_inIkEnv
    
    --setConfig(goalPose,nil,nil,structuralParameters)
    simIK.setObjectPose(ikEnv,ikTarget,goalPose,ikBase)
    
    local config = simIK.findConfig(ikEnv,ikGroup,ikJointHandles,0.65,0.1,{1,1,1,0.1},collisionValidation,structuralParameters)
    print(config)
    
    if config then
        -- found a collision-free config that matches the desired pose!
        -- Now find a collision-free path (via path planning) that brings us from current config to the found config:
        local task = simOMPL.createTask('task')
        simOMPL.setAlgorithm(task,simOMPL.Algorithm.RRTConnect)
        simOMPL.setStateSpaceForJoints(task,simJointHandles,useForProjection)
        simOMPL.setCollisionPairs(task,{robotCollection,sim.handle_all})
        simOMPL.setStartState(task,getConfig())
        simOMPL.setGoalState(task,config)
            
        -- now we could add more goal states with simOMPL.addGoalState, to increase the chance to find a collision-free path. 
        -- But we won't do it for simplicity's sake
        simOMPL.setup(task)
        local res,path = simOMPL.compute(task,4,-1,300)
        
        print(res)
        print(path)
        
        if res and path then
            -- We found a collision-free path
            -- Now move along the path in a very simple way:
            local sw = sim.setStepping(true)
            for i=1,simOMPL.getPathStateCount(task,path) do
                local conf = simOMPL.getPathState(task,path,i)
                setConfig(conf,nil,nil,structuralParameters)
                
                local msg = string.format(" path state #%.0f",i)
                msg = msg..string.format(" (@tip configurationat: %.2f)",conf)
                sim.addLog(sim.verbosity_scriptinfos,msg)
                
                sim.step()
            end
            sim.setStepping(sw)
        end
        simOMPL.destroyTask(task)
    end
    
end

Code: Select all

%% System Initialization
disp('Program started');
% using zeroMQ remote API package
addpath ('C:\Program Files\CoppeliaRobotics\CoppeliaSimEdu\programming\zmqRemoteApi\clients\matlab')

client = RemoteAPIClient();
sim = client.require('sim');

sim.setStepping(true);

sim.startSimulation();
deltaRobot = sim.getObject("/irb360");
deltaRobotTip = sim.getObject("/ikTip");
irb360StructuralParameters = sim.readCustomTableData(deltaRobot, "irb360StructuralParameters");
irb360KinematicParameters = sim.readCustomTableData(deltaRobot, "irb360KinematicParameters");
sim.step()

while true
    
    cuboid = sim.getObject('/cuboidTarget');
    cuboidPosition = cell2mat( sim.getObjectPose(cuboid, deltaRobot) );
    sim.callScriptFunction("moveToConfigOMPL", deltaRobotScipt,...
        cuboidPosition,irb360StructuralParameters, irb360KinematicParameters)
    sim.step()
    pause(2)
        
    break

end

sim.stopSimulation();
disp('Program ended');