OK! Thank you.
Code: Select all
function sysCall_init()
h=sim.getObjectAssociatedWithScript(sim.handle_self)
hTip = sim.getObjectHandle("cable_tip")
hTarget = sim.getObjectHandle("cable_target")
child = sim.getObjectsInTree(h,sim.object_joint_type,0)
xml = readXML()
ui=simUI.create(xml)
simUI.setPosition(ui,1077,85,true)
child = sim.getObjectsInTree(h,sim.object_joint_type,0)
num = table.getn(child)*0.5
for i= 1,num,1 do
InitsetSliderValue(1000+i)
InitsetSliderValue(2000+i)
end
makeIK()
end
function InitsetSliderValue(id)
hJoint = HandleFromID(id)
val = sim.getJointPosition(hJoint)
simUI.setSliderValue(ui,id,val*180/math.pi,true)
end
sliderChange_callback=function(ui,id,value)
hJoint = HandleFromID(id)
sim.setJointPosition(hJoint,value*math.pi/180)
--set Tip and Target
child = sim.getObjectsInTree(sim_handle_scene,sim.handle_all,0)
for i=1,table.getn(child) do
-- hLastBody_bef = hLastBody
if string.match(sim.getObjectName(child[i]), "cable_body") == "cable_body" then
hLastBody = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_orientTarget") == "cable_orientTarget" then
hLastTarget = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_orientTip") == "cable_orientTip" then
hLastTip = child[i]
end
end
-- print(sim.getObjectName(hLastBody))
pBody = sim.getObjectPosition(hLastBody,-1)
oBody = sim.getObjectOrientation(hLastBody,-1)
sim.setObjectPosition(hTip,-1,pBody)
sim.setObjectOrientation(hTip,-1,oBody)
sim.setObjectPosition(hTarget,-1,pBody)
sim.setObjectOrientation(hTarget,-1,oBody)
end
function renewUI()
x,y=simUI.getPosition(ui)
simUI.destroy(ui)
xml = readXML()
ui=simUI.create(xml)
child = sim.getObjectsInTree(h,sim.object_joint_type,0)
num = table.getn(child)*0.5
for i= 1,num,1 do
InitsetSliderValue(1000+i)
InitsetSliderValue(2000+i)
end
simUI.setPosition(ui,x,y,true)
end
function readXML()
xml = [[
<ui closeable="true" on-close="closeEventHandler" resizable="true">
<tabs>
<tab title="CABLE" layout="grid">
<button text="Add cable" on-click="addCable" />
<button text="Delete cable" on-click="delCable" />
<button text="Renew UI" on-click="renewUI" />
<br />
'<label text="Joint Name" />'
'<label text="Joint A" />'
'<label text="Joint B" />'
<br />
]]
child = sim.getObjectsInTree(h,sim.object_joint_type,0)
num = table.getn(child)*0.5
for i= 1,num,1 do
if i == 1 then
xml = xml .. '<label text="Joint Ori" />'
else
xml = xml .. '<label text="Joint '..-2 + i..'" />'
end
xml = xml .. '<hslider minimum="-180" maximum="180" on-change="sliderChange_callback" id="'..1000+i..'"/>'
xml = xml .. '<hslider minimum="-180" maximum="180" on-change="sliderChange_callback" id="'..2000+i..'"/>'
xml = xml .. '<br />'
end
xml = xml .. '</tab></tabs></ui>'
return xml
end
function HandleFromID(id)
if id == 1001 then
hJoint = sim.getObjectHandle("cable_jointA")
elseif id == 2001 then
hJoint = sim.getObjectHandle("cable_jointB")
elseif id > 2000 then
num = id - 2002
hJoint = sim.getObjectHandle("cable_jointB" .. num)
elseif id > 1000 then
num = id - 1002
hJoint = sim.getObjectHandle("cable_jointA" .. num)
end
return hJoint
end
function closeEventHandler(h)
sim.addStatusbarMessage('Window '..h..' is closing...')
simUI.hide(h)
end
function makeIK()
if sim.getIkGroupHandle("cable_straight@silentError") ~= -1 then
hIKgroup = sim.getIkGroupHandle("cable_straight")
sim.removeIkGroup(hIKgroup)
end
if sim.getIkGroupHandle("IK_Group@silentError") ~= -1 then
hIKgroup = sim.getIkGroupHandle("IK_Group")
sim.removeIkGroup(hIKgroup)
end
hIKgroup = sim.createIkGroup(0,{sim.ik_damped_least_squares_method,3})
hBody = sim.getObjectHandle("cable_body")
hOrientTip = sim.getObjectHandle("cable_orientTip")
sim.createIkElement(hIKgroup,0,{hOrientTip,hBody,-1,sim.ik_alpha_beta_constraint})
child = sim.getObjectsInTree(h,sim.object_joint_type,0)
num = table.getn(child)*0.5
for i=0,num-2 do
hBody = sim.getObjectHandle("cable_body"..i)
hOrientTip = sim.getObjectHandle("cable_orientTip"..i)
sim.createIkElement(hIKgroup,0,{hOrientTip,hBody,-1,sim.ik_alpha_beta_constraint})
end
end
function addCable(ui, id)
child = sim.getObjectsInTree(sim_handle_scene,sim.handle_all,0)
for i=1,table.getn(child) do
-- hLastBody_bef = hLastBody
if string.match(sim.getObjectName(child[i]), "cable_body") == "cable_body" then
hLastBody = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_jointA") == "cable_jointA" then
hLastJointA = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_jointB") == "cable_jointB" then
hLastJointB = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_orientTarget") == "cable_orientTarget" then
hLastTarget = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_orientTip") == "cable_orientTip" then
hLastTip = child[i]
end
end
hNew = sim.copyPasteObjects({hLastTarget,hLastJointA,hLastJointB,hLastBody,hLastTip},0)
sim.setObjectPosition(hNew[2],hNew[4],{0,0,0.01})
sim.setObjectParent(hNew[1],hLastBody,true)
sim.setObjectParent(hNew[2],hLastBody,true)
pLastTip = sim.getObjectPosition(hLastTip,-1)
oLastTip = sim.getObjectOrientation(hLastTip,-1)
sim.setObjectPosition(hNew[1],-1,pLastTip)
sim.setObjectOrientation(hNew[1],-1,oLastTip)
-- sim.createIkElement(hIKgroup,0,{hLastTip,hLastBody,-1,sim.ik_alpha_beta_constraint})
sim.setObjectParent(hTip,hNew[4],true)
pNewTip = sim.getObjectPosition(hNew[5],-1)
oNewTip = sim.getObjectOrientation(hNew[5],-1)
sim.setObjectPosition(hTip,-1,pNewTip)
sim.setObjectOrientation(hTip,-1,oNewTip)
sim.setObjectPosition(hTarget,-1,pNewTip)
sim.setObjectOrientation(hTarget,-1,oNewTip)
makeIK()
end
function delCable(ui,id)
child = sim.getObjectsInTree(sim_handle_scene,sim.handle_all,0)
for i=1,table.getn(child) do
-- hLastBody_bef = hLastBody
if string.match(sim.getObjectName(child[i]), "cable_body") == "cable_body" then
hLastBody = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_jointA") == "cable_jointA" then
hLastJointA = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_jointB") == "cable_jointB" then
hLastJointB = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_orientTarget") == "cable_orientTarget" then
hLastTarget = child[i]
end
if string.match(sim.getObjectName(child[i]), "cable_orientTip") == "cable_orientTip" then
hLastTip = child[i]
end
end
pOldTarget = sim.getObjectPosition(hLastTarget,-1)
oOldTarget = sim.getObjectOrientation(hLastTarget,-1)
sim.removeObject(hLastBody)
sim.removeObject(hLastJointA)
sim.removeObject(hLastJointB)
sim.removeObject(hLastTarget)
sim.removeObject(hLastTip)
sim.setObjectPosition(hTip,-1,pOldTarget)
sim.setObjectOrientation(hTip,-1,oOldTarget)
sim.setObjectPosition(hTarget,-1,pOldTarget)
sim.setObjectOrientation(hTarget,-1,oOldTarget)
makeIK()
end
function sysCall_nonSimulation()
-- local selectedObjects=sim.getObjectSelection()
-- if selectedObjects and (#selectedObjects==1) and (selectedObjects[1]==h) then
-- if simUI.isVisible(ui) == false then
-- simUI.show(ui)
-- end
-- else
-- simUI.destroy(ui)
-- print(ui)
-- end
end
function sysCall_cleanup()
simUI.destroy(ui)
print("CLEAN UP")
end