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 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.