Universal Robots Forum

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.