OVERVIEW
We have an issue in the programatic initialization of a waypoint from a custom urcap, we program the insertion of the waypoint as follows:
FixedPositionWaypointNodeConfig cfg = (FixedPositionWaypointNodeConfig)waypoint_config;
Pose posa = ((FixedPositionDefinedWaypointNodeConfig)cfg).getPose();
BlendParameters blendParameters = programNode.getConfigFactory().createSharedBlendParameters();
WaypointMotionParameters motionParameters = programNode.getConfigFactory().createSharedMotionParameters();
JointPositions j = ((FixedPositionDefinedWaypointNodeConfig)cfg).getJointPositions().get();
double[] tcp = installation.getLastActiveTCP();
Pose tcp_offset = factory_provider.getPoseFactory().createPose(tcp[0], tcp[1], tcp[2], tcp[3], tcp[4], tcp[5], Length.Unit.M, Angle.Unit.RAD);
WaypointNodeConfig config = programNode.getConfigFactory().createFixedPositionConfig(posa, j, tcp_offset, blendParameters, motionParameters);
programNode.setConfig(config);
We want to execute a program with multiple waypoints built as above, each containing different TCP target poses.
Despite the current Tool pose (i.e. the pose before the program starts running) is relatively close to the target Tool pose, the robot does not reach the target Tool pose as expected, our intuition is that it interprets the provided target Tool pose as a target Flange pose, and since it tries to reach the target Tool pose with the Flange, it may consider the pose as not feasible for the Flange to reach and therefore it executes the movement towards the waypoint with a joint configuration different from the one expected if it had consider the target as a Tool pose (as intended). It is important to note that the reached Tool pose is correct, but the corresponding joint configuration is wrong with the provided qnear.
We think that the issue concerns how the robot computes the inverse kinematics towards the first Waypoint within a MoveJ or MoveL block in the following cases:
- By selecting one of the waypoints in the program and click “Move here”;
- When running the whole program and the robot has to reach the first waypoint of the program which leverages the same functionalities embedded in the “Move here” button;
This happens only in some regions of the workspace where the above feasibility motion of the flange occurs.
Steps to reproduce:
Description of how to reproduce. Attached programs, script, configuration are all helpful
Current pose of the tool on the robot (before program starts running):
-
tool [mm] BASE
x 217.94
y 832.90
z 244.44
rot vec [rad]
rx 2.683
ry -2.380
rz 1.357 -
joints [degrees]
92.80
-105.29
-89.56
-76.91
78.13
266.03
Desired pose of the tool on the robot (first waypoint of the program)
-
tool [mm] BASE
238.79
870.42
205.16
rot vec [rad]
2.682
-2.322
1.580 -
joints [degrees]
94.08
-134.71
-50.91
91.23
-71.68
87.71
Figure showing approximately the pose programmatically picked as qnear and the target pose proposed by “Move here”.
TCP coordinates for the experiment
Position [mm]
x -0.57
y 2.56
z 271.04
Orientation (Rotation vector [rad])
rx 0.0111
ry -1.1897
rz -2.8898
Expected Behavior:
The robot should interpret a target Tool pose defined as follows from a custom urcap
FixedPositionWaypointNodeConfig cfg = (FixedPositionWaypointNodeConfig)waypoint_config;
Pose posa = ((FixedPositionDefinedWaypointNodeConfig)cfg).getPose();
BlendParameters blendParameters = programNode.getConfigFactory().createSharedBlendParameters();
WaypointMotionParameters motionParameters = programNode.getConfigFactory().createSharedMotionParameters();
JointPositions j = ((FixedPositionDefinedWaypointNodeConfig)cfg).getJointPositions().get();
double[] tcp = installation.getLastActiveTCP();
Pose tcp_offset = factory_provider.getPoseFactory().createPose(tcp[0], tcp[1], tcp[2], tcp[3], tcp[4], tcp[5], Length.Unit.M, Angle.Unit.RAD);
WaypointNodeConfig config = programNode.getConfigFactory().createFixedPositionConfig(posa, j, tcp_offset, blendParameters, motionParameters);
programNode.setConfig(config);
as a taget Tool pose. Therefore, if the starting tool pose is close to the target tool pose, the robot should not try to reach strange joints configuration.
Actual Behavior:
Even, if the starting tool pose is close to the target tool pose, the robot tries to reach a strange joint configuration which is further a way in the joint space with respect to the expected target joint configuration.
Workaround Suggestion:
Use as qnear the current joint configuration which also embeds the tool pose.
Robot Serial Number:
20225201172
Affected Version(s):
Software version :
- Polyscope 5.21.3
- SDK 1.14.0
Robot generation :
- UR10e