Get_actual_joint_positions() but for a pose

Technical Questions URScript
1

Is there a function that will return the same values as get_actual_joint_positions(), but also takes a pose?

The closest thing I found is get_inverse_kin, but it gives the positions, as if it was an L movement moving to it. I am looking to get the values, as if it was a J move.

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
3

@jbm Do you have any inputs to this?

4

What you are looking for is forward kinematics.
It is currently not supported, but is also covered in this feature request.

5

Isn’t that doing the opposite?

I am looking at getting a list of joint positions from a pose, and not a pose from a list of joint positions.

Instead of moving the robot to a position, and using get_actual_joint_positions() I am trying to have a pose, and then use that pose to return the joint positions, like if it was possible to do get_actual_joint_positions(pose)

6

Then you should use the get_inverse_kin() script function.

7

I already wrote about trying that too, it isn’t giving me the right values. It seems to be giving me the values for the shortest move from where the robot is, so where the pose is, and it does not end in the pose I need it to end in then.

8

It looks like you may have a couple issues with mixing up joint vectors and poses in your script @michael.andresen .

First
wrist3_step_deg = norm(pose_start[5]-pose_stop[5])/10.0

You are taking the absolute distance between one component of a rotation vector at two different poses. I’m pretty sure what you want is the difference in the wrist 3 joint angles.
wrist3_step_deg = norm(slib_start[5]-slib_stop[5])/10.0

Next
goto = interpolate_pose(pose_start, pose_stop, stepCurrent/(steps*1.0))

However, interpolate_pose takes in two poses and returns an interpolated pose between them. You are passing in two joint vectors and expecting a joint vector back. If you read the doc on interpolate_pose you can see that it doesn’t treat rotations and translations in the same way so I would expect passing in joint vectors to produce weird results.

You might try something like:
gotop = interpolate_pose(pose_start, pose_stop, stepCurrent/(steps1.0))
gotoj = get_inverse_kin(gotop)
gotoj[5] = slib_start[5]+(wrist3_step_degstepCurrent)

Also, note that you will likely need to pass good guesses for qnear everywhere you call get_inverse_kin() in order to ensure you get the solution you actually want.

1 Like
9

Looks like I made some mistakes when converting it from the robot program, into a script. Got most of it working, but as expected, the wrist 3 joint does not rotate the full distance.

I am not sure how I should be able to do that, it assume it would depend on the poses the user program the robot to move between, and that could be anything.

10

I cooked it down to a single function, like that the robot can be moved into a position with a normal viapoint, and it will then grab the joint positions, the problem is still getting the end point of the move, but I am now considering simply giving the user an option to put in how many degrees they want wrist 3 to rotate. That way I can use the get_inverse_kin to calculate all joints, except wrist 3, and the script then calculate wrist 3 position.