NiryoOne0_Script.lua

-- Importing required libraries 
sim=require('sim') 
simIK = require('simIK')
simROS2 = require('simROS2')

-- HELPER FUNCTIONS --

-- The function below moves the robot to a certain configuration  
function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf)
    local params = {
        joints = handles,
        targetPos = targetConf,
        maxVel = maxVel,
        maxAccel = maxAccel,
        maxJerk = maxJerk,
    }
    sim.moveToConfig(params)
end

-- The function below helps to solve inverse kinematics
function solveIK()

    ikEnv = simIK.createEnvironment()
    
    ikGroup_undamped = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv, ikGroup_undamped, simIK.method_pseudo_inverse, 0, 6)
    simIK.addElementFromScene(ikEnv, ikGroup_undamped, simBase, simTip, simTarget, simIK.constraint_position)
    
    ikGroup_damped = simIK.createGroup(ikEnv)
    simIK.setGroupCalculation(ikEnv, ikGroup_damped, simIK.method_damped_least_squares, 0.01, 99)
    simIK.addElementFromScene(ikEnv, ikGroup_damped, simBase, simTip, simTarget, simIK.constraint_position) 
    
    local result, jointPositions = simIK.handleGroup(ikEnv, ikGroup_undamped, {syncWorlds = true})
    
    -- Check the result status
    if result == simIK.result_success then
        print('Niryo0: ik successful')
    elseif result ~= simIK.result_success then
        local result, jointPositions = simIK.handleGroup(ikEnv, ikGroup_damped, {syncWorlds = true})
        if result == simIK.result_success then
            print('Niryo0: ik successful')
        else
            print('Niryo0: ik not successful')
        end
    end
end

-- ROS2 CALLBACKS --

function onRodReadyForPickupFromNy1(msg)
    if msg.data == true then 
        _G.rodReadyForPickupFromNy1 = true
    end
end

-- This function initilizes the needed variables
function sysCall_init()
    
    -- Load the robot's base, tip (end-effector), and target
    _G.simTip = sim.getObject('/NiryoOne0/NiryoLGripper/tip')
    _G.simTarget = sim.getObject('/fuelRod/target2')
    _G.simBase = sim.getObject('/NiryoOne0')
    
    -- Initial robot configuration
    _G.initConfig = {math.pi/2, 0, 0, 0, -math.pi*1/12, -math.pi/2}
        
    -- Initialize the gripper object
    local connection=sim.getObject('../connection')
    local gripper=sim.getObjectChild(connection,0)
    _G.gripperName="NiryoNoGripper"
    if gripper~=-1 then
        _G.gripperName=sim.getObjectAlias(gripper,4)
    end
    
    -- Close the gripper object initially
    sim.setInt32Signal(_G.gripperName..'_close', 1)
    
    -- Set the joint target forces
    leftJoint = sim.getObject('../leftJoint1')
    rightJoint = sim.getObject('../rightJoint1')
    sim.setJointTargetForce(leftJoint, 80.0)
    sim.setJointTargetForce(rightJoint, 80.0)
    
     -- Get all robot joint handles
    _G.jointHandles={
        sim.getObject('../NiryoOne_joint1'),
        sim.getObject('../NiryoOne_joint2'),
        sim.getObject('../NiryoOne_joint3'),
        sim.getObject('../NiryoOne_joint4'),
        sim.getObject('../NiryoOne_joint5'),
        sim.getObject('../NiryoOne_joint6')
    }
    
    -- Initialize publishers and subscribers
    rodReadyForPickupFromNy1Topic = '/niryo1/rod_ready_for_pickup'
    rodPickedUpByNy0Topic = '/niryo0/rod_picked_up'
    rodDroppedIntoReceptacleTopic = '/niryo0/rod_dropped_into_receptacle'
    
    rodReadyForPickupFromNy1Subscriber = simROS2.createSubscription(rodReadyForPickupFromNy1Topic, 'std_msgs/msg/Bool', onRodReadyForPickupFromNy1)
    sim.addLog(sim.verbosity_scriptinfos, 'Niryo0 rod pickup ready from Niryo1 subscriber initialized.')
        
    rodPickedUpByNy0Publisher = simROS2.createPublisher(rodPickedUpByNy0Topic, 'std_msgs/msg/Bool')
    sim.addLog(sim.verbosity_scriptinfos, 'Niryo0 rod pickup from Niryo1 publisher initialized.')
        
    rodDroppedIntoReceptaclePublisher = simROS2.createPublisher(rodDroppedIntoReceptacleTopic, 'std_msgs/msg/Bool')
    sim.addLog(sim.verbosity_scriptinfos, 'Niryo0 rod dropped into receptacle publisher initialized.')
end

-- Threaded function for movement
function sysCall_thread()
    while sim.getSimulationState() ~= sim.simulation_advancing_abouttostop do
    
        -- If the rod is ready for pickup from Niryo1
        if _G.rodReadyForPickupFromNy1 then
            _G.rodReadyForPickupFromNy1 = false
            
            -- Open the gripper
            sim.clearInt32Signal(_G.gripperName..'_close')
            sim.wait(4)
            
            -- Define dynamic trajectory constraints
            local vel = 50
            local accel = 100
            local jerk = 120
            local maxVel = {vel*math.pi/180, (vel)*math.pi/180, 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, 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, jerk*math.pi/180, jerk*math.pi/180}

            -- Solve the inverse kinematics
            solveIK()
            sim.wait(4)
            
            -- Close the gripper
            sim.setInt32Signal(_G.gripperName..'_close', 1)
            sim.wait(4)
            
            -- Publish rod pickup
            simROS2.publish(rodPickedUpByNy0Publisher, {data=true})
            sim.wait(4)
            
            -- Move robot to target location 1
            local targetPos1 = {-math.pi/5, 0, 0, 0, 0, -math.pi/2}
            moveToConfig(jointHandles, maxVel, maxAccel, maxJerk, targetPos1)
            sim.wait(2)
            
            -- Move robot to target location 2
            local targetPos2 = {-math.pi/5, 0, 0, 0, 0, 0}
            moveToConfig(jointHandles, maxVel, maxAccel, maxJerk, targetPos2)
            sim.wait(2)
            
            -- Open the gripper to release the object
            sim.clearInt32Signal(_G.gripperName..'_close')
            
            -- Publish rod dropped into receptacle
            simROS2.publish(rodDroppedIntoReceptaclePublisher, {data=true})
            
            moveToConfig(jointHandles, maxVel, maxAccel, maxJerk, _G.initConfig)
        end
        
        sim.wait(0.1) -- short delay to yield simulation time
        
    end
end

function sysCall_cleanup()
    simROS2.shutdownSubscription(rodReadyForPickupFromNy1Subscriber)
    simROS2.shutdownPublisher(rodPickedUpByNy0Publisher)
end