in my script I collect the joints' position while the robot is reaching a target via IK.
Code: Select all
function getTableJointsPos()
local jointsPos = {}
for i=1,#simJoints,1 do
jPos = radToDeg(sim.getJointPosition(simJoints[i]))
if i == 5 then
jPos = -1 * jPos
end
table.insert(jointsPos, jPos)
end
return jointsPos
end
function sysCall_sensing()
if startIK == 1 then
local pos=sim.getObjectPosition(simTip,simBase)
if positionReached == 0 then
tipRecordedPositions[indexTable] = pos
jointsRecordedPositions[indexTable] = getTableJointsPos()
indexTable=indexTable+1
end
end
-- Update chart
sim.setGraphStreamValue(posGraph,j0PosStream,radToDeg(sim.getJointPosition(simJoints[1])))
sim.setGraphStreamValue(posGraph,j1PosStream,radToDeg(sim.getJointPosition(simJoints[2])))
sim.setGraphStreamValue(posGraph,j2PosStream,radToDeg(sim.getJointPosition(simJoints[3])))
sim.setGraphStreamValue(posGraph,j3PosStream,radToDeg(sim.getJointPosition(simJoints[4])))
sim.setGraphStreamValue(posGraph,j4PosStream,-radToDeg(sim.getJointPosition(simJoints[5])))
end
Code: Select all
function repeatClick(ui,id,v)
for k, v in pairs(jointsRecordedPositions) do
for i=1,#v,1 do
local jointPos = v[i]
if i == 5 then
jointPos = -1 * jointPos
end
setJointAnglePosition(simJoints[i],jointPos)
end
end
end
function setJointAnglePosition(jointH,pos)
sim.setJointPosition(jointH,pos)
local result,flags,prec=simIK.handleGroup(ikEnv,ikGroup_parallel,{syncWorlds=true,allowError=false})
if result~=simIK.result_success then
print ("Configuration not feasible")
end
end
Many thanks!