Get_inverse_kin "close to qnear"

Technical Questions URScript
1

I refer to topic

where this question was asked already, but not completely answered:

In short, it is the sum of squared error. But it is a bit more complicated than that.

I made some tests with an UR10e in a simulation and got that result:

=== Compare results for QNear1 and QNear2 ===
Result1 for QNear1: [ -108.729756° /-167.715858° /53.28137° /23.502614° /87.616465° /-63.232788° ]
Result2 for QNear2: [ 43.902276° /-87.599227° /83.131068° /-83.608423° /91.688389° /144.144138° ]
Pose for Result1: [ -399.8mm /-621.7mm /949.8mm /-2.1° /-2° /98° ]
Pose for Result2: [ -399.8mm /-621.7mm /949.8mm /-2.1° /-2° /98° ]
Distance btw. poses: [ -0mm /-0mm /-0mm /-0° /-0° /0° ]
===both joint lists (Result1 and Result2) are valid results!===
Distance btw. Result1 and QNear1: [ 0° /-57.715858° /-3.41863° /68.502614° /-2.383535° /0° ]
Distance btw. Result2 and QNear1: [ 0° /22.400773° /26.431068° /-38.608423° /1.688389° /0° ]
Shoulder: Result2 is 35.315085° closer
Ellbow:   Result1 is 23.012438° closer
Wrist1:   Result2 is 29.894191° closer
Wrist2:   Result2 is 0.695146° closer
If Result2 is closer to QNear1, why was it not returned to get_inverse_kin( TargetPos, QNear1)?

from that script (I omit some functions):

def QNearTestShort():
  local TargetPose = p[ -0.3998, -0.6217, 0.9498, d2r(-2.1), d2r(-2.0), d2r(98) ]
  local QNear1 = [ d2r(0), d2r(-110), d2r(56.7), d2r(-45), d2r(90), d2r(0) ]
  local QNear2 = [ d2r(45), d2r(-110), d2r(56.7), d2r(-45), d2r(90), d2r(0) ]

  local Result1 = get_inverse_kin( TargetPose, QNear1 )
  local Result2 = get_inverse_kin( TargetPose, QNear2 )

  local Pose1 = get_forward_kin( Result1 )
  local Pose2 = get_forward_kin( Result2 )

  Debug( "=== Compare results for QNear1 and QNear2 === " )
  Debug( "Result1 for QNear1: " + to_joints_str( Result1 ) )
  Debug( "Result2 for QNear2: " + to_joints_str( Result2 ) )
  Debug( "Pose for Result1: " + to_pose_str( Pose1 ) )
  Debug( "Pose for Result2: " + to_pose_str( Pose2 ) )
  Debug( "Distance btw. poses: " + to_pose_str( SubtractPose(Pose1,Pose2) ) )
  Debug( "===both joint lists (Result1 and Result2) are valid results!===" )

  local Distance1 = SubtractJoint(Result1,QNear1)
  local Distance2 = SubtractJoint(Result2,QNear1)

  # mask the first and last coordinate away (I was told, if QNear[i]==0 this coordinate will not be used)
  Distance1[0] = 0
  Distance2[0] = 0
  Distance1[5] = 0
  Distance2[5] = 0
  # check, if Result1 or Result2 is "nearer" to QNear1:
  Debug( "Distance btw. Result1 and QNear1: " + to_joints_str( Distance1 ) )
  Debug( "Distance btw. Result2 and QNear1: " + to_joints_str( Distance2 ) )
  Judge( "Shoulder: ", Distance1[1], Distance2[1] )
  Judge( "Ellbow:   ", Distance1[2], Distance2[2] )
  Judge( "Wrist1:   ", Distance1[3], Distance2[3] )
  Judge( "Wrist2:   ", Distance1[4], Distance2[4] )
  Debug( "If Result2 is closer to QNear1, why was it not returned to get_inverse_kin( TargetPos, QNear1)?" )
end

QNearTestShort()
halt

After all it seems to me, that QNear in get_inverse_kin is only usable in special cases, but not in the general case.
Can you give me some more explanation about that? Especially how to use it in a general case reliably?

-Ludwig

2

Hi Ludvig,

I doubt this functionality:

And I think that is the explanation to your question.

3

Hello ebof!

Not really. Because after I found out, that this function is not working as they told me, I come back to the original question:

"What is the criteria for the closest solution return by get_inverse_kin ? "

How is the result chosen (if there are ambiguities) depending on qnear. Or even more difficult: How do I have to choose my qnear in order to get the desired answers from get_inverse_kin?

-Ludwig

4

As JBM wrote, the LMS is the short explanation to how the solutions is chosen. Do you have an example where you doubt the output?

If you want a short movement time from the robots current position, then I will suggest you to use “get_actual_joint_positions()” as q_near.

5

I want to be sure to understand, what you mean with LMS. I think, you mean:

The resulting joint_position j for a pose p and joint positions q:
j = get_inverse_kin( p, q)
is such that (for e[i] = j[i]-q[i], i=0…5)
S = e[0]*e[0] + e[1]*e[1] + e[2]*e[2] + e[3]*e[3] + e[4]*e[4] + e[5]*e[5]
is minimal?

This is how I have understood the get_inverse_kin right from the beginning. I used this function to keep the robot in a certain “arrangement” in the ellbow: (u is the base mounted at the ceiling)

   u        instead of      u_
    \_                        \
      '                        '

But I remember, that I got some solutions, that surprised me.

Sorry, I cannot deliver any examples at the moment, since my project is finished (for now) and I have no time or occasion to work on the robot (for now).

6

Yes exactly. Let me know next time you get into troubles!

7

Hello,
I’m not 100% sure that my issue is related to this topic, but it seems like it to me.

I’m currently struggling to understand how get_inverse_kin and qnear work.
in a script I’ve got this:
textmsg("Drop_Pre_Pt1: ",Drop_Pre_Pt1)
Drop_Pre_Pt1J=get_inverse_kin(pose_trans(Uframe,Drop_Pre_Pt1))
textmsg("Drop_Pre_Pt1J[0]: ",r2d(Drop_Pre_Pt1J[0]))
textmsg("Drop_Pre_Pt1J[1]: ",r2d(Drop_Pre_Pt1J[1]))
textmsg("Drop_Pre_Pt1J[2]: ",r2d(Drop_Pre_Pt1J[2]))
textmsg("Drop_Pre_Pt1J[3]: ",r2d(Drop_Pre_Pt1J[3]))
textmsg("Drop_Pre_Pt1J[4]: ",r2d(Drop_Pre_Pt1J[4]))
textmsg("Drop_Pre_Pt1J[5]: ",r2d(Drop_Pre_Pt1J[5]))
textmsg("Drop_Pre_Pt1J: ",Drop_Pre_Pt1J)
Drop_Pre_Pt1J=get_inverse_kin(pose_trans(Uframe,Drop_Pre_Pt1),[d2r(-100),d2r(-80),d2r(-120),d2r(-70),d2r(90),d2r(-90)])
textmsg("Drop_Pre_Pt1J[0]: ",r2d(Drop_Pre_Pt1J[0]))
textmsg("Drop_Pre_Pt1J[1]: ",r2d(Drop_Pre_Pt1J[1]))
textmsg("Drop_Pre_Pt1J[2]: ",r2d(Drop_Pre_Pt1J[2]))
textmsg("Drop_Pre_Pt1J[3]: ",r2d(Drop_Pre_Pt1J[3]))
textmsg("Drop_Pre_Pt1J[4]: ",r2d(Drop_Pre_Pt1J[4]))
textmsg("Drop_Pre_Pt1J[5]: ",r2d(Drop_Pre_Pt1J[5]))
textmsg("Drop_Pre_Pt1J: ",Drop_Pre_Pt1J)

Drop_Pre_Pt1 is calculated and relative to Uframe.
I log inverse kinematic because time to time I’ve got some “weird” axes configuration, which led to collision.

On the last collision the log return this:
Drop_Pre_Pt1: p[0.308678,0.0377616,0.3,2.16857,2.273,-9.39359e-05]

#get_inverse_kin solution with no qnear parameter
Drop_Pre_Pt1J[0]: 25.3461
Drop_Pre_Pt1J[1]: 13.3889
Drop_Pre_Pt1J[2]: -99.7442
Drop_Pre_Pt1J[3]: -184.133
Drop_Pre_Pt1J[4]: 90.5411
Drop_Pre_Pt1J[5]: 23.6616
Drop_Pre_Pt1J: [0.442372,0.233681,-1.74087,-3.21372,1.58024,0.412974]

#get_inverse_kin solution with qnear parameter
Drop_Pre_Pt1J[0]: 25.3461
Drop_Pre_Pt1J[1]: 13.3889
Drop_Pre_Pt1J[2]: -99.7442
Drop_Pre_Pt1J[3]: -184.133
Drop_Pre_Pt1J[4]: 90.5411
Drop_Pre_Pt1J[5]: 23.6616
Drop_Pre_Pt1J: [0.442372,0.233681,-1.74087,-3.21372,1.58024,0.412974]

How come get_inverse_kin with qnear gives such results?
Drop_Pre_Pt1J[0]: 25.3461 qnear[0]=d2r(-100) -> more than 90° difference
Drop_Pre_Pt1J[1]: 13.3889 qnear[1]=d2r(-80) -> more than 90° difference
Drop_Pre_Pt1J[2]: -99.7442 qnear[2]=d2r(-120) -> acceptable
Drop_Pre_Pt1J[3]: -184.133 qnear[3]=d2r(-70) -> more than 90° difference
Drop_Pre_Pt1J[4]: 90.5411 qnear[4]=d2r(90) -> fine
Drop_Pre_Pt1J[5]: 23.6616 qnear[5]d2r(-90) -> more than 90° difference

Especially when a solution exist with the following axes angle really close to qnear.
Drop_Pre_Pt1J[0]: -117.25
Drop_Pre_Pt1J[1]: -75.15
Drop_Pre_Pt1J[2]: -121.52
Drop_Pre_Pt1J[3]: -73.19
Drop_Pre_Pt1J[4]: 89.15
Drop_Pre_Pt1J[5]: -119.1

Something doesn’t seem to work properly or I’m miss something?

8

I’m not a 100% sure, but I’m guessing it has something to do with singularities. The UR has 3: tool flange close to the base, arm fully stretched out and wrist joint 2 being close to 0 or 180 degrees. You are either to close to these singularities or the qnear forces the robot to move through one of them (although I’m not sure the robot controller is that intelligent… :slight_smile: )

It would explain why Joint 0 & 5 are not considered (they do not impact singularities)

9

I have a related question, I’d like to “predict” what joints the robot will assume after a movel call. For this purpose I call get_inverse_kinematics() for the target cartesian position (with qnear as the current joint angles). Usually the predicted and the assumed joint angles are the same, but in some cases they differ. Is there a way to consistently predict the outcome of a movel (Given that it is a valid, reachable position)?

10

Hello,
I don’t think singularity is the problem.
Tool flange is fairly away from the base at least 350mm, the arm is not stretched at all and wrist joint 2 is around 90°.
All points are reachable it’s just that 5% of the time the axes configurations change from one point to another which lead to collision.
And I was surprise that get_inverse_kinematics associated with qnear parameter close to the desire axes configuration would give me a result that different from qnear.

11

Yes, no direct singularity, but may it has to pass through one in order to get to the desired poont. This passing through may not be physically, but by means of the internal workings of the solver. I don’t know what they use, but the effects are typical for a Jacobian based ik solver…

12

Both script snippets that are shared here is affected by the issue described here:

Please apply the workaround and give us the feedback if you are still having issues!