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