Vrep map generator

동영상

Using devices

Schematic diagram

About the project

Algorithm

알고리즘

-- DO NOT WRITE CODE OUTSIDE OF THE if-then-end SECTIONS BELOW!! (unless the code is a function definition)
function rotate_point(pt,angle)
    return({math.cos(angle)*pt[1]+math.sin(angle)*pt[2],-math.sin(angle)*pt[1]+math.cos(angle)*pt[2]})
end

function mat_sum(pt1,pt2)
    return {pt1[1]+pt2[1],pt1[2]+pt2[2]}
end

function check_ori(pt1,ref)
    if math.abs(ref[2]-pt1[2])>w/4 then
        return {0,0,90/180*math.pi}
    else
        return {0,0,180/180*math.pi}
    end
end

function cppon(posi,point,name)
    ori=check_ori(posi,point)
    size_bar={0.1,w,1}
    obj = simCreatePureShape(0,10110,size_bar,1,nil)
    --simSetObjectParent(obj,nil,-1)
    simSetObjectPosition(obj,-1,posi)
    simSetObjectOrientation(obj,-1,ori)
    simSetObjectName(obj,name)
    simSetObjectSpecialProperty(obj,sim_objectspecialproperty_detectable_all)
end

function cal_block_point(point,LSR,angle)
    if LSR==-1 then
        block_angle={angle+math.pi,angle+math.pi/2}
    elseif LSR==0 then
        block_angle={angle-math.pi/2,angle+math.pi/2}
    elseif LSR==1 then
        block_angle={angle+math.pi,angle-math.pi/2}
    end
    
    pt_b1 = mat_sum(point,rotate_point({0,w/2},block_angle[1]))
    table.insert(pt_b1,0.5)
    pt_b2 = mat_sum(point,rotate_point({0,w/2},block_angle[2]))
    table.insert(pt_b2,0.5)    

    return pt_b1,pt_b2
end


function make_block(point,LSR,dir,num)
    cur_dir=(dir+LSR)%4
    angle=(cur_dir+1)*math.pi/2
    point_next=mat_sum(point,rotate_point({0,w},angle))
    table.insert(point_next,0.5)    
    
    pt_b1,pt_b2=cal_block_point(point,LSR,angle)
    
    cppon(pt_b1,point,'Block_'..num..'_1')
    --simAddStatusbarMessage(pt_b1)
    --simAddStatusbarMessage(pt_b1)
    cppon(pt_b2,point,'Block_'..num..'_2')
    return point_next,cur_dir
end

function mapUpdate()
    point,dir=make_block(posi0,0,ini_dir,1)
    for i=2,#map do
        point,dir=make_block(point,map[i],dir,i)
    end
end

if (sim_call_type==sim_childscriptcall_initialization) then
    w=1.2
    --{-1:left 0:straight 1:right}
    map={0,0,1,-1,1,-1,0,1,1,0,-1,-1,0,1}
    start=simGetObjectAssociatedWithScript(sim_handle_self)
    posi0=simGetObjectPosition(start,-1)
    posi0[3]=0.5
    --{N:0 E:1 S:2 W:3}
    ini_dir=2
    simSetObjectOrientation(start,start,{0,0,math.pi/2*ini_dir})
    mapUpdate()
end


if (sim_call_type==sim_childscriptcall_actuation) then

    -- Put your main ACTUATION code here

    -- For example:
    --
    -- local position=simGetObjectPosition(handle,-1)
    -- position[1]=position[1]+0.001
    -- simSetObjectPosition(handle,-1,position)

end


if (sim_call_type==sim_childscriptcall_sensing) then

    -- Put your main SENSING code here

end


if (sim_call_type==sim_childscriptcall_cleanup) then

    -- Put some restoration code here

end

Thanks to

Reference