Capturing pose with a button, questions and troubles

Dear community,

  • In this .urp code, the first block is entered using GUI in the normal way, second is captured and created by
    pressing a tool switch:

    • What is the exact meaning and differences between: referencedName=“Joint_0_name” and reference=“…/…/Move/feature” ?

    • What is the exact meaning and differences between: kinematicsFlags=“4” and kinematicsFlags="-1"

      <Move motionType="MoveL" speed="0.25" acceleration="1.2" useActiveTCP="true">
        <feature class="GeomFeatureReference" referencedName="Joint_0_name"/>
        <children>
      	<Waypoint type="Fixed" name="Punto_de_paso_1" kinematicsFlags="4">
      	  <motionParameters/>
      	  <position joints="-1.0197895208941858, -1.7194765249835413, -2.219410244618551, -0.7816556135760706, 1.605222225189209, 0.550082802772522" pose="0.18000004637817335, -0.600000069577526, 0.22999994588635275, -0.0012210266636962872, 3.116276477498168, 0.038891810931774094"/>
      	  <poseInFeatureCoordinates pose="0.18000004637817335, -0.600000069577526, 0.22999994588635275, -0.0012210266636960864, 3.116276477498177, 0.03889181093177421"/>
      	  <outputFlangePose pose="0.18000004637817335, -0.600000069577526, 0.22999994588635275, -0.0012210266636960864, 3.116276477498177, 0.03889181093177421"/>
      	</Waypoint>
        </children>
      </Move>
      
      <Move motionType="MoveL" speed="0.25" acceleration="1.2" useActiveTCP="true">
        <feature class="GeomFeatureReference" reference="../../Move/feature"/>
        <children>
      	<Waypoint type="Fixed" name="Punto_de_paso_2" kinematicsFlags="-1">
      	  <motionParameters/>
      	  <position joints="-1.0197895208941858, -1.7194765249835413, -2.219410244618551, -0.7816556135760706, 1.605222225189209, 0.550082802772522" pose="0.18000004637817335, -0.600000069577526, 0.22999994588635275, -0.0012210266636962872, 3.116276477498168, 0.038891810931774094"/>
      	  <poseInFeatureCoordinates pose="0.18000004637817335, -0.600000069577526, 0.22999994588635275, -0.0012210266636960864, 3.116276477498177, 0.03889181093177421"/>
      	  <outputFlangePose pose="0.18000004637817335, -0.600000069577526, 0.22999994588635275, -0.0012210266636960864, 3.116276477498177, 0.03889181093177421"/>
      	</Waypoint>
        </children>
      </Move>
      
  • I use the following code to capture a pose and jointPositions at the press of a tool switch:

    • Why are the last two terminal prints showing the real angles (in rad) + 2PI? There are two println because I tried two different ways just to compare.

      void addWaypointNode() {
      	ProgramAPI programAPI = apiProvider.getProgramAPI();
      	ProgramModel programModel = programAPI.getProgramModel();
      	ProgramNodeFactory nf = programModel.getProgramNodeFactory();
      	ValueFactoryProvider valueFactoryProvider = apiProvider.getProgramAPI().getValueFactoryProvider();
      
      	realtimeReader.readNow();
      	double[] tcp_pose = realtimeReader.getActualTcpPose(); // Read the actual TCP pose
      	double[] tcp_jointPositions = realtimeReader.getActualJointPose(); // Read the actual Joint pose
      	final Pose[] pose = new Pose[1];
      	pose[0] = valueFactoryProvider.getPoseFactory().createPose(tcp_pose[0], tcp_pose[1], tcp_pose[2], tcp_pose[3], tcp_pose[4], tcp_pose[5], Length.Unit.M, Angle.Unit.RAD);
      	final JointPositions[] jointPositions = new JointPositions[1];
      	jointPositions[0] = valueFactoryProvider.getJointPositionFactory().createJointPositions(tcp_jointPositions[0], tcp_jointPositions[1], tcp_jointPositions[2], tcp_jointPositions[3], tcp_jointPositions[4], tcp_jointPositions[5], Angle.Unit.RAD);
      
      	JointPosition[] jointPosition = jointPositions[0].getAllJointPositions();
      
      	double[] jointPositionsArray1 = new double[6];
      	double[] jointPositionsArray2 = new double[6];
      	for (int i = 0; i < 6; i++) {
      		jointPositionsArray1[i] = jointPosition[i].getAngle(Angle.Unit.RAD);
      		jointPositionsArray2[i] = jointPositions[0].getAllJointPositions()[i].getAngle(Angle.Unit.RAD);
      	}
      
      	System.out.println("p" + dobleArrayToString(tcp_pose) + " [m, rad]");
      	System.out.println("j" + dobleArrayToString(tcp_jointPositions) + " [rad]");
      	System.out.println("p" + dobleArrayToString(pose[0].toArray(Length.Unit.M, Angle.Unit.RAD)) + " [m, rad] PoseFactory");
      	System.out.println("j" + dobleArrayToString(jointPositionsArray1) + " [rad] JointPositionFactory");
      	System.out.println("j" + dobleArrayToString(jointPositionsArray2) + " [rad] JointPositionFactory");
      
      	final TreeNode[] root = new TreeNode[1];
      	root[0] = programModel.getRootTreeNode(this);
      
      	final MoveNode[] moveNode = new MoveNode[1];
      	moveNode[0] = null;
      
      	final TreeNode[] moveTreeNode = new TreeNode[1];
      	moveTreeNode[0] = null;
      
      	final WaypointNode[] waypointNode = new WaypointNode[1];
      	waypointNode[0] = null;
      
      	if (root[0].getChildren().isEmpty()) {
      	
      		moveNode[0] = nf.createMoveNode(); //It creates a Waypoint inside it too
      	
      		undoRedoManager.recordChanges( new UndoableChanges() {
      			@Override
      			public void executeChanges() {
      		
      				try {
      					moveTreeNode[0] = root[0].addChild(moveNode[0]);
      					root[0].setChildSequenceLocked(true);
      					waypointNode[0] = (WaypointNode) moveTreeNode[0].getChildren().get(0).getProgramNode();
      					waypointNodeConfigFactory = waypointNode[0].getConfigFactory();
      					BlendParameters blendParameters = waypointNodeConfigFactory.createNoBlendParameters();
      					WaypointMotionParameters motionParameters = waypointNodeConfigFactory.createSharedMotionParameters();
      					WaypointNodeConfig newWaypointNodeConfig = waypointNodeConfigFactory.createFixedPositionConfig(pose[0], jointPositions[0], blendParameters, motionParameters);
      					waypointNode[0].setConfig(newWaypointNodeConfig);
      				} catch (TreeStructureException e) {
      					e.printStackTrace();
      				}
                  }
              });
      	} else {
      		moveTreeNode[0] = root[0].getChildren().get(0);
      
      		//Add Waypoint inside			
      		waypointNode[0] = nf.createWaypointNode();
      	
      		undoRedoManager.recordChanges( new UndoableChanges() {
      			@Override
      			public void executeChanges() {
      		
      				try {
      					moveTreeNode[0].addChild(waypointNode[0]);
      					waypointNodeConfigFactory = waypointNode[0].getConfigFactory();
      					BlendParameters blendParameters = waypointNodeConfigFactory.createNoBlendParameters();
      					WaypointMotionParameters motionParameters = waypointNodeConfigFactory.createSharedMotionParameters();
      					WaypointNodeConfig newWaypointNodeConfig = waypointNodeConfigFactory.createFixedPositionConfig(pose[0], jointPositions[0], blendParameters, motionParameters);
      					waypointNode[0].setConfig(newWaypointNodeConfig);
      				} catch (TreeStructureException e) {
      					e.printStackTrace();
      				}
                  }
              });
      	}
      }
      

    TERMINAL OUT:

      	p[-0,179926,-0,60687, 0,230789,-0,001221, 3,116276, 0,038892] [m, rad]
      	j[-1,6007,-1,7271,-2,203,-0,808, 1,5951,-0,031] [rad]
      	p[-0,179926,-0,60687, 0,230789,-0,001221, 3,116276, 0,038892] [m, rad] PoseFactory
      	j[ 4,682485, 4,556085, 4,080185, 5,475185, 1,5951, 6,252185] [rad] JointPositionFactory
      	j[ 4,682485, 4,556085, 4,080185, 5,475185, 1,5951, 6,252185] [rad] JointPositionFactory
    

Related threads:

Thank you all for your help!

Hi there!

I’ve found out createJointPositions acquires well the joint angles you pass, at least in the range -2Pi to +2PI.

jointPositions[0] = valueFactoryProvider.getJointPositionFactory().createJointPositions(tcp_jointPositions[0], tcp_jointPositions[1], tcp_jointPositions[2], tcp_jointPositions[3], tcp_jointPositions[4], tcp_jointPositions[5], Angle.Unit.RAD);

But when you read back the angles from it, if they where negative, you get them with 2PI added. So it only gives you back angles in the range 0 to +2PI, which can lead (and leads) to errors.

It gives the same result no matter if you use these two ways to get the angles back.

jointPositionsArray1[i] = jointPosition[i].getAngle(Angle.Unit.RAD);
jointPositionsArray2[i] = jointPositions[0].getAllJointPositions()[i].getAngle(Angle.Unit.RAD);

I think of this as a bug, can you confirm that or put some light in the matter, UR team?

1 Like

this is a bug and i think it’s related to this

1 Like

Thank you @m.birkholz!!

As you said in the other post, “Thank God i’m not the only one facing this odd Bug”.
Believe me, I read nearly a hundred posts these week just trying to find something on this, but I wouldn’t found this post without your help.

That said, at this point I’ve been able to separate two different issues:

  1. The related in this very post. My best explanation is this. @jbm Please, UR team, fix this for us!

  2. The one in the post you pointed out (Problem with getUserDefinedRobotPosition / joint positions).

The first difficulty for me was to sepparate them, as they appeared together.

Hi there!

Thanks to our nice UR technical support in Spain Jordi Saboya, I have the solution for the JointPosition getAngle() method, that returns only in the range [0:2pi] (RAD) or [0:360] (DEG).

  • In the API reference explains that getAngle() gets only the angle offset. That was it.

  • There is a complementary method getRevolutions() to know the number of full revolutions.

  • And the method I wanted in the first place is getPosition(). This gives the actual angle for the joint, in the range [-2pi:2pi] (RAD) or [-360:360] (DEG).

3 Likes