Problem in robot control with UI

Typically: "How do I... ", "How can I... " questions
Post Reply
MaJiamu
Posts: 14
Joined: 19 Jan 2021, 03:52

Problem in robot control with UI

Post by MaJiamu »

Hello, I want to control a hexapod robot with a simple UI, but when the button was trigger and the function related to the button will give a 'index' value to decide which function the coroutineMain() should actuate. However when the 'index' value is obtain and the corresponding function didn't actuate. I analysed that the problem is related to the mechanism of coroutine, but have no idea about how to solve it.
My code is :

Code: Select all

function sysCall_init()
    -- do some initialization here
    corout=coroutine.create(coroutineMain)
    jointHandle={}
    for i=1,18,1 do
    jointHandle[i]=sim.getObjectHandle('hexa_joint'..i)
    end
    antBase=sim.getObjectHandle('hexa_base')
    legBase=sim.getObjectHandle('hexa_legBase')
    legBase1=sim.getObjectHandle('hexa_legBase1')
    sizeFactor=sim.getObjectSizeFactor(antBase)
    sizeFactor=sizeFactor*1
    Floor=sim.getObjectHandle('Floor')
    body=sim.getObjectHandle('hexapod')
    simTip=sim.getObjectHandle('hexa_footTip2')
    R={}
    P={}
    for i=1,6 do    
        R[i]=sim.getObjectHandle('hexa_link4Respondable_'..i-1)
        P[i]=sim.getObjectHandle('hexa_Proximity_'..i-1) 
    end 
    --print(force)
    graph=sim.getObjectHandle('Graph')
    hexa_bodypos_x=sim.addGraphStream(graph,'x pos','m',0,{1,0,0})
    hexa_bodypos_y=sim.addGraphStream(graph,'y pos','m',0,{0,1,0})
    hexa_bodypos_z=sim.addGraphStream(graph,'z pos','m',0,{0,1,1})
    
    sim.addGraphCurve(graph,'hexa_body pos x/y/z',3,{hexa_bodypos_x,hexa_bodypos_y,hexa_bodypos_z},{0,0,0},'m by m by m',0,{0.1,0.6,0},4)

xml = [[
<ui title="GSY Robot UI" closeable="true" on-close="closeEventHandler" resizable="false" activate="false">
    <tabs>    
        <tab title="Button ">
        <group layout="form" flat="true">
        <label text="Forward" id="31"/>
        <button text="Forward" on-click="ForwardFun" checkable="true" id="32"/>
        <label text="turnRight" id="33"/>
        <button text="turnRight" on-click="turnRightFun" checkable="true" id="34"/>
        <label text="turnLeft" id="35"/>
        <button text="turnLeft" on-click="turnLeftFun" checkable="true" id="36"/>
        </group>
        </tab>
    </tabs>
<label text="" style="* {margin-left: 400px}"/>    
</ui>
]]
    ui=simUI.create(xml)
    vel=0.5
    accel=0.5
    initialP={0,0,0}
    initialP1={0,0,0}
    initialO={0,0,0}
    initialO1={0,0,0}
    waittime=0.1
    index=3
end



function callback(m,vel,accel,auxData)
    sim.setObjectMatrix(auxData[1],auxData[2],m)
end

function moveToPose(obj,relObj,pos,euler,vel,accel)
    local auxData={obj,relObj}
    local mStart=sim.getObjectMatrix(obj,relObj)
    local mGoal=sim.buildMatrix(pos,euler)
    sim.moveToPose(-1,mStart,{vel},{accel},{0.1},mGoal,callback,auxData,{1,1,1,0.1})
    --Generates object movement data, by performing interpolations between two poses.
end



function Forward()
    initialP[3]=initialP[3]+0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)
    initialP[2]=initialP[2]+0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)
    initialP1[2]=initialP1[2]-0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)
    initialP[3]=initialP[3]-0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)
    for i=1,2 do
    initialP1[3]=initialP1[3]+0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)    
    initialP1[2]=initialP1[2]+0.06*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)
    initialP[2]=initialP[2]-0.06*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)
    initialP1[3]=initialP1[3]-0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)
    
    initialP[3]=initialP[3]+0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)    
    initialP[2]=initialP[2]+0.06*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)
    initialP1[2]=initialP1[2]-0.06*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)
    initialP[3]=initialP[3]-0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)
    end

    initialP1[3]=initialP1[3]+0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)    
    initialP1[2]=initialP1[2]+0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)
    initialP[2]=initialP[2]-0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(waittime)
    initialP1[3]=initialP1[3]-0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(waittime)
end

function turnRight()
    initialP[3]=initialP[3]+0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(0.1)    
    initialO[3]=initialO[3]-22.5*math.pi/180
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(0.1)
    initialP[3]=initialP[3]-0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(0.1)
    initialP1[3]=initialP1[3]+0.03*sizeFactor
    initialO[3]=initialO[3]+22.5*math.pi/180
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(0.1)
    initialP1[3]=initialP1[3]-0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(0.1)
end

function turnLeft()
    initialP1[3]=initialP1[3]+0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(0.5)    
    initialO1[3]=initialO1[3]+22.5*math.pi/180
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(0.5)
    initialP1[3]=initialP1[3]-0.03*sizeFactor
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(0.5)
    initialP[3]=initialP[3]+0.03*sizeFactor
    initialO1[3]=initialO1[3]-22.5*math.pi/180
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    moveToPose(legBase,antBase,initialP1,initialO1,vel,accel)
    sim.wait(0.5)
    initialP[3]=initialP[3]-0.03*sizeFactor
    moveToPose(legBase1,antBase,initialP,initialO,vel,accel)
    sim.wait(0.5)
    
end



function ForwardFun()
        index=1
end
function turnRightFun()
        index=2
end
function turnLeftFun()
        index=3
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
        end
end

function coroutineMain()
        if index==1 then
        Forward()
        end
        if index==2 then
        turnRight()
        end
        if index==3 then
        turnLeft()
        end
end

function sysCall_sensing()
    sim.setGraphStreamValue(graph,hexa_bodypos_x,sim.getObjectPosition(simTip,-1)[1])
    sim.setGraphStreamValue(graph,hexa_bodypos_y,sim.getObjectPosition(simTip,-1)[2])
    sim.setGraphStreamValue(graph,hexa_bodypos_z,sim.getObjectPosition(simTip,-1)[3])
end

function sysCall_cleanup()
end

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: Problem in robot control with UI

Post by fferri »

Try to add some print(...) here and there to understand what's not being called.

coppelia
Site Admin
Posts: 10336
Joined: 14 Dec 2012, 00:25

Re: Problem in robot control with UI

Post by coppelia »

Hello,

your coroutine will execute once then end. You need a loop, try with something like:

Code: Select all

function coroutineMain()
    while true do
        if index then
            if index==1 then
                index=nil
                Forward()
            end
            if index==2 then
                index=nil
                turnRight()
            end
            if index==3 then
                index=nil
                turnLeft()
            end
        else
            sim.switchThread()
        end
    end
end
Cheers

MaJiamu
Posts: 14
Joined: 19 Jan 2021, 03:52

Re: Problem in robot control with UI

Post by MaJiamu »

Thanks! It works! And I found that most of .ttt documents I made before were corrected by the coroutine mode automatically . Were those phenomena normal?

MaJiamu
Posts: 14
Joined: 19 Jan 2021, 03:52

Re: Problem in robot control with UI

Post by MaJiamu »

fferri wrote: 09 Nov 2021, 11:04 Try to add some print(...) here and there to understand what's not being called.
Thanks for ur advices!

Post Reply