Capturing pose with a button, questions and troubles

[dmartin]

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:

Create Waypoints in MoveNodes with external Button

Hello, I’m facing big trouble on how to create waypoints in MoveNodes. A short explanation of what has to be done by my URCap: I need to create MoveP and MoveJ Nodes with some of the digital inputs of the controller. For example, pushbutton 1 creates a MovePNode with 1 Waypoint. If pushbutton 2 is pressed, another waypoint should be created. The URCap Node is inserted into the node tree by the operator. [image] As long as this view is open, the URCap will recognize changes to the digital in…

Thank you all for your help!

[dmartin]

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?

[m.birkholz]

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

[dmartin]

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.

[dmartin]

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).