Wrong inverse calculation when adding a waypoint to a movej node

Hello Everyone,
i currently programming urcaps and adding a movej node and a single waypoint including the joint position and the cartiesian position.

see example pseudo code

MoveNode node = programFactory.createMoveNode();
node.setConfig(makeMoveJConfig(<params>);
TreeNode t = tree.add(node);
WaypointNode waypoint = waypointFactory.createWaypoint(jointPosition, pose, tcp);
// the pose position is the corresponding position for the joint position input
t.add(waypoint);


Now comes the problem. After adding the waypoint to the movenode the urcaps framework solves the inverse again based on the given pose position BUT ignoring the joint position hint. The result is a CRITICAL reorientation of the kinematic because a differnent joint configuration is choosen by the internal ur inverse to reach the same pose.
Here are the test values I used on an UR3e Polyscope 5.3 - 5.9

//input tcp offset
p[0.0,0.0,0.2635,0.0,0.0,3.141592653589793]
//input pose
p[0.29083346645790226,-0.186185858548883,0.3440003552674519,-2.7679726234945243,-0.6691066833975665,0.22304531836940067] 
//input joints
[2.210392713546753, -1.6298962831497192, 1.2838162183761597, -0.9011446833610535, -1.6013234853744507, 0.17021575570106506]
// calculated joint position of the waypoint addter adding to the program tree
[2.5411662609620347, -1.3793784772170028, 1.981965391556674, -2.3026003476199053, -1.64590041629791, 2.7160270138941875]

Can somebody help me to solve this issue. How can i force the urcaps waypoint node to use my joint position for a movej command.

Thank you for your help.

Hi @jan.falkenberg,

I do not see exactly how you create the “createFixedPositionConfig” configuration, since you state that you are testing on Polyscope version 5.3 I am assuming that you are not using the “createFixedPositionConfig(Pose poseIncludingTcp, JointPositions qNear, Pose tcpOffset, BlendParameters blendParameters, WaypointMotionParameters waypointMotionParameters)” method that was added in 5.9.

Best Regards
Ebbe

Hey @Ebbe here the code to see how the waypoint node is created

Pose poseIncludingTCP = simpleForwardKinematic(joints, tcp);
WaypointNode waypoint = programFactory.createWaypointNode();
FixedPositionDefinedWaypointNodeConfig config = wayPointFactory.createFixedPositionConfig(poseIncludingTCP, joints, wayPointFactory.createNoBlendParameters(),
				wayPointFactory.createSharedMotionParameters());
waypoint.setConfig(config);

Note that i have already the matching joint position to the corresponding pose with the tcp because i calculated the pose by using the forward kinematic.
Im am currently using URCaps Version 1.6.

Hi @jan.falkenberg,

The method you are using is deprecated, you need to use the method where you explicit define the TCP.

Let me hear your findings with the latest functionality.

Ebbe

Hi @Ebbe
can you please send me the class and function what has to be use in order to create the
FixedPositionDefinedWaypointNodeConfig in URCap 1.6. I am definitly unable to Update the UrCaps API version.
The function is also not marked as deprecared, which is a little bit confusing
Thank you.

You need to update the API version to at least 1.11. Hopefully this link can help you.

If you cannot update, then we are challenges on fixing your issue.

Thank you, but i am really unable to update the URCaps API because my company has to support Polyscope 5.3.
Is there a way (e.g. by try and error) to get the correct answer form the inverse function?

You need to configure the Move node before the Waypoint is inserted as a child. I guess you have to select the specific TCP you are using, and use the same TCP for your forward kinematic.

I hope that makes sense?

Yes, it does make sense but when you look into my code example in my initial post, that is exactly what i am doing
the params containing all information like tcp, speed… for the MoveNode. This is added before the waypoint is added onto the movement node.

Hi @Ebbe thank you for your fast feedback.
After testing with a lot of qNear positions as hints, i am now definitly sure, that this is a bug. Is there any way that this can be fixed? That the controller calculated the inverse again confuses me a lot. This does not make sense at all and is not required when defining a joint movements…Is there any way to disable this? How can i support fo fix this issue an the URCaps version 1.6?

Hi @jan.falkenberg,
From your initial post I have a hard time to see the TCP part of the Movej configuration. When looking at the values you shared, I do not get result that matches yours.

get_forward_kin(input_joints, input_tcp_offset)
p[0.188405,  -0.0518845, 0.103168 ,-2.7679726234945243,-0.6691066833975665,0.22304531836940067]

that matches your orientation, but is with a distance to you pose longer than the TCP.

The reason for the need is due to the calibration, so if the program is moved to another robot, the TCP will end up in the same location. For a MoveJ movement you you can enable the selection “Use joint angles” to avoid this.

It will be a challenge to fix it in Polyscope if you can not update.

He @Ebbe ,
sorry for my late answer, i was on vacation. Now i can reproduce this behavior with a UR3 with a lot of polyscope versions. I tested with 3.11 up to 3.15 and got the same results.

Defined TCP:
[-77.09, -45.11,220.73,0,0,0] (mm,rad)
Caresian Pose:
p[181.53, -218.95, 333.87, 126.56, 128.00, 0.02] (mm, deg)
Correct joints
[-27.09, -81.55, -92.24, -96.36, 90.06, 152.26] (deg)
Internal Calculated
[-27.07, -81.48, -92.35, -96.16, 90.01, 152.28] (deg)

(i hope i make no copy paste error)
The deviation is definitly more that differnet correction parameter would cause, because this the deviation for this pose is present on multiple robots.

I also have to add the “Use joint angles” selection is no option since we ONLY talk about UrCaps available functionality and there is no API to set this selection in URCaps.

My current fallback to resolve this issue is generating a urscript command using the corect joints. But this destroys entirely the usability of robot programming.

Bes Regards
Jan

Hi @jan.falkenberg,

Just to be sure. Is your current issue the difference between “Correct joints” and “Internal calculated”?

What is your method for obtaining these two values?

The method createFixedPositionConfig(Pose poseIncludingTcp, JointPositions qNear, Pose tcpOffset, BlendParameters blendParameters, WaypointMotionParameters waypointMotionParameters) was added in API version 1.11 that was introduced with Polyscope version 3.14. So I do not understand how you evaluate that method in version 3.11 - 3.13.

Best Regards
Ebbe

Hi @Ebbe,
i use the UrCap API 1.6 because we have to support a lot for Polyscope versions so i use the method.

createFixedPositionConfig(Pose poseIncludingTcp, JointPositions qNear, Pose tcpOffset, BlendParameters blendParameters, WaypointMotionParameters waypointMotionParameters)
After creating the waypoint i simple move the robot to this point and see the joint deviation. This is hard to reproduce because it seems th be robot dependent and depending on the joint positions choosen.
Currently we really have issues with our customers because a position deviating robot is not acceptable. The only fallback what i can use is a urscript movej command with the correct joint positions. Buy using this fallback the customer looses all usability of programming a UR.
Best
Jan

Hi @jan.falkenberg,

Every robot is calibrate to compensate for the tolerances in parts and the assembly process. That leads to unique kinematic DH parameters for each robot. The target for Polyscope (unless use joint angles is selected) is to move the joints into a position where target TCP matches the actual active TCP. With the difference in the DH parameters, the joint angles will differ slightly from robot to robot.
With the the minor difference in your last values, it seems plausible that is the explanation for the difference.

I hope that calibration explanation makes sense.

Best regards
Ebbe