How to apply joint configuration at once

[Chiara]

Hi,

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

Once the target is reached, I need to repeat again the motion. Thus, I thought to loop the jointsRecordedPositions variable and apply the values to the joints.

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

My problem is that my robot can't repeat the motion (the result is always "configuration not feasible") but I can't understand the reason why. Do you have any idea?
Many thanks!

[Chiara]

I'm sorry, but I was wondering if someone can help me..is it possible to assign joints angular position at once?

[fferri]

You should first set all joints' angles, then call simIK.handleGroup.

E.g.:

Code: Select all

for i = 1, #simJoints do
    sim.setJointPosition(simJoints[i], jointPos[i])
end
local result, flags, prec = simIK.handleGroup(ikEnv, ikGroup_parallel, opts)

[Chiara]

Thanks for the answer!

I tried what you suggested me but I still don't see my robot moving. I wonder if it is related to visualization/script handling. I noticed that in the last software update there is a new way to handle the functions, i.e. via sim.step(). Could be this the reason why I don't see anything moving?

[coppelia]

Hello,

can you post a link to the scene?

Cheers

[Chiara]

Hi,

here there is the link https://drive.google.com/file/d/1Ba1Ig- ... sp=sharing

You should:
- (if you want) set a starting configuration for the robot
- press Continue
- press Repeat

You will find the code I posted at the function repeatClick. I have added some prints in order to check if the movement is performed succesfully: you will see them in the console.

Many thanks!

[coppelia]

Sorry for the late reply due to end-of-year holidays.

I think what is happening in your code is that you are calling sim.step (or sim.switchThread) while not in a switchable section: the mouse click on the button invokes a callback function that does not run in a coroutine. So all the movement is played back at once (i.e. sim.step has no effect in that case).

A button click should be memorized, and next time the coroutine resumes, you can check for that click and play back the movement from within the coroutine.

Cheers