Get_actual_joint_positions() but for a pose

Technical Questions URScript
2

This is a part of my other topic (Linear move while one joint rotating - #15 by michael.andresen), but this topic is dedicated to getting the joint positions for a pose.

I got it all working, when I move the robot to the start and stop positions, and then use the get_actual_joint_positions function, to store the start and stop angles for the move.

And it looks like this

(the small deviation is just from the spring loaded finger not being perfectly straight)

To make it easier to work with, I started putting it into scripts, still using a start and stop pose into a IF with a FALSE, so it will never run, and then use the two poses to define start and stop.

Udklip
Udklip802×630 45.2 KB

But putting it into a script and getting the positions without actually moving to them, is giving me some trouble. So far this is what i got

def slibSetup(pose_start, pose_stop):
    global slib_start = get_inverse_kin(pose_start)
    global slib_stop = get_inverse_kin(pose_stop)
    global slibDist = pose_dist(pose_start, pose_stop)
    global wrist3_step_deg = norm(pose_start[5]-pose_stop[5])/10.0
end

def slibRutine(time, steps):
    if steps < 2:
        while 1:
            popup("Steps cannot be less than 2", warning=True, blocking=True)
        end
   end

    global stepCurrent = 0
    while stepCurrent < steps:
        goto = interpolate_pose(slib_start, slib_stop, stepCurrent/(steps*1.0))

        if slib_start[5] < slib_stop[5]:
            gotoJ = [goto[0],goto[1],goto[2],goto[3],goto[4],(slib_start[5]+(wrist3_step_deg*stepCurrent))]
        else:
            gotoJ = [goto[0],goto[1],goto[2],goto[3],goto[4],(slib_start[5]-(wrist3_step_deg*stepCurrent))]
        end

        if stepCurrent == 0 or stepCurrent == steps-1:
            movej(gotoJ,15,1,time,slibDist/10)
        else:
            movej(gotoJ,15,1,time,0)
        end

        stepCurrent = stepCurrent + 1
    end
end

When moving to the positions and storing the angles seems to work fine, but using the get_inverse_kin function isn’t giving me the values needed.

2 Likes