Crash in while loop

[juyeon]

Code: Select all

function sysCall_init()
    corout=coroutine.create(coroutineMain)
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 sysCall_cleanup()
    -- do some clean-up here
end

function coroutineMain()
    -- Put some initialization code here
     model=sim.getObject('.')
    --fire=sim.getObject('/fire')
    leftJointHandle=sim.getObject("./leftJoint_")
    rightJointHandle=sim.getObject("./rightJoint_")
    goal = sim.getObject('/Cuboid1')
    
    cont=sim.addDrawingObject(sim.drawing_linestrip+sim.drawing_cyclic,2,0,-1,2000,{1,0,0})
    
    initial_model_pos=sim.getObjectPosition(model,-1)
    --recive_x = initial_model_pos[1]
    --recive_y = initial_model_pos[2]
    recive_x = 0
    recive_y = 0
    
    recive_ang =0
    angle=0

    
    arrive_sig=0

    -- Put your main loop here, e.g.:
    --
    while true do
    --     local p=sim.getObjectPosition(objHandle,-1)
    --     p[1]=p[1]+0.001
    --     sim.setObjectPosition(objHandle,-1,p)
    --     sim.switchThread() -- resume in next simulation step
    
    model_pos=sim.getObjectPosition(model,-1)
    
     sim.setObjectPosition(goal,-1,{recive_x,recive_y,0.050})
    
    rel_pos = sim.getObjectPosition(goal,model)
    rel_y = rel_pos[2]
    rel_z = rel_pos[3]
    
    theta = math.atan(rel_y/rel_z)

    
    if  recive_ang==0 then
    
        if rel_y>=0 and rel_z<=0 and theta<-0.02 then
            sim.setJointTargetVelocity(rightJointHandle,100*math.pi/180)        --turn left
            sim.setJointTargetVelocity(leftJointHandle,50*math.random()*math.pi/180)
        elseif rel_y>=0 and rel_z>=0 and theta>0.02 then
            sim.setJointTargetVelocity(rightJointHandle,100*math.pi/180)        --turn left
            sim.setJointTargetVelocity(leftJointHandle,50*math.random()*math.pi/180)
        elseif rel_y<=0 and rel_z<=0 and theta>0.02 then
            sim.setJointTargetVelocity(leftJointHandle,100*math.pi/180)           --turn right
            sim.setJointTargetVelocity(rightJointHandle,50*math.random()*math.pi/180)
        elseif rel_y<=0 and rel_z>=0 and theta<-0.02 then
            sim.setJointTargetVelocity(leftJointHandle,100*math.pi/180)           --turn right
            sim.setJointTargetVelocity(rightJointHandle,50*math.random()*math.pi/180)
        elseif math.abs(theta)<=0.02 then
            sim.setJointTargetVelocity(leftJointHandle,200*math.pi/180)         --go forward
            sim.setJointTargetVelocity(rightJointHandle,200*math.pi/180)
        end
    
        if math.abs(rel_y)<=0.02 and math.abs(rel_z)<=0.02 then
            sim.setJointTargetVelocity(leftJointHandle,0)         --stop
            sim.setJointTargetVelocity(rightJointHandle,0)
            
            if math.abs(model_pos[1]-initial_model_pos[1])>0.3 and math.abs(model_pos[2]-initial_model_pos[2])>0.3 then
                arrive_sig=1
            end
        else
            arrive_sig=0
        end
        
        
        
    elseif recive_ang ~=0  then
        --mar={9.4195546873621e-07, -0.99749498659008, -0.070737201858524, 1.0, 1.3282911062062e-05, 0.070737201864796, -0.99749498650164, 1.0, 0.99999999991134, -1.0319523013891e-13, 1.3316268485664e-05, 0.05}

        model_matrix = sim.getObjectMatrix(model,-1)
        --as,res = sim.getRotationAxis(model_matrix,mar)
        --print(as)
        --print(model_matrix)
        --print(res)
        --x = {model_matrix[1],model_matrix[5],model_matrix[9]}
        z = {0,0,1}
        pos = {model_matrix[4],model_matrix[8],0}
        --pos={0,0,0}
        goal_matrix = sim.rotateAroundAxis(model_matrix,z,pos,recive_ang)
        print('goal matrix:',goal_matrix)
        
        
        
        while recive_ang>0 do
        --for t=1,1000 do
            model_matrix = sim.getObjectMatrix(model,-1)
            print('model matrix:',model_matrix)

            sim.setJointTargetVelocity(rightJointHandle,100*math.pi/180)        --turn left
            sim.setJointTargetVelocity(leftJointHandle,50*math.random()*math.pi/180)
            
            
            res,rota = sim.getRotationAxis(model_matrix,goal_matrix)
            print('rota',rota)
            
            if rota <= 0.1 then
                print('t')
                recive_ang=0
                break
            end
            
            
        
        end
        
          
        
    end
    end
end

If I write it like this, it can rotate to the angle I need and perform other actions normally, but it can't stop, it will circle in place.
If the same code is written in the above sysCall_actuation(), the statement of rotating to the angle I need cannot be executed, but it can stop normally without turning in circles.

[fferri]

It would be simpler if you explain what you are trying to achieve.

If you are trying to get the robot's yaw, you can simply take the robot's front-facing axis (in case of DR12 that would be -z) project it onto the XY plane and use math.atan2(y,x) to get the angle, e.g.:

Code: Select all

function getYaw()
    local m=sim.getObjectMatrix(model,sim.handle_world)
    local dir={-m[3],-m[7],-m[11]}
    return math.atan2(dir[2],dir[1])
end

Then, if you are trying to implement a position controller, you have to understand the topology of SO(2). You can't simply take subtraction between current and target. The orientation error between 359º and 1º is not -358º! It is 2º.

You can do something like that:

Code: Select all

function getYawError(targetYaw)
    local e=getYaw()-targetYaw
    if math.abs(e-2*math.pi)<math.abs(e) then
        return e-2*math.pi
    elseif math.abs(e+2*math.pi)<math.abs(e) then
        return e+2*math.pi
    else
        return e
    end
end

Above code is able to track robot's yaw for any target angle (e.g. by adding yaw error to left wheel velocity, and subtracting it to right wheel velocity).

[juyeon]

It would be simpler if you explain what you are trying to achieve.

If you are trying to get the robot's yaw, you can simply take the robot's front-facing axis (in case of DR12 that would be -z) project it onto the XY plane and use math.atan2(y,x) to get the angle, e.g.:

Code: Select all

function getYaw()
    local m=sim.getObjectMatrix(model,sim.handle_world)
    local dir={-m[3],-m[7],-m[11]}
    return math.atan2(dir[2],dir[1])
end

Then, if you are trying to implement a position controller, you have to understand the topology of SO(2). You can't simply take subtraction between current and target. The orientation error between 359º and 1º is not -358º! It is 2º.

You can do something like that:

Code: Select all

function getYawError(targetYaw)
    local e=getYaw()-targetYaw
    if math.abs(e-2*math.pi)<math.abs(e) then
        return e-2*math.pi
    elseif math.abs(e+2*math.pi)<math.abs(e) then
        return e+2*math.pi
    else
        return e
    end
end

Above code is able to track robot's yaw for any target angle (e.g. by adding yaw error to left wheel velocity, and subtracting it to right wheel velocity).

Thanks a lot for your reply, I have a better understanding of rotation matrices.
I'm still relatively new to robotics, so set a target point as target tracking to control the yaw angle, and the target has been reached.
Thanks a lot for your patient reply.