Help! Get the current TCP pose

davidina

Hello everyone,
I was following tutorial in the PC SDK Manual and I want to read the current TCP pose in C# with the following:

public void Read_Current_Pose()
        {
           NetworkScanner scanner = new NetworkScanner(); scanner.Scan();
            Controller acontroller = new Controller(scanner.Controllers[0].SystemId);
            acontroller.Logon(UserInfo.DefaultUser);

            MotionSystem aMotionSystem = acontroller.MotionSystem;
            MechanicalUnitCollection aMechCol = acontroller.MotionSystem.MechanicalUnits;
            MechanicalUnit aMechUnit = acontroller.MotionSystem.ActiveMechanicalUnit;


            try
            {
                            
                if (acontroller.OperatingMode == ControllerOperatingMode.Auto)
                {
                   using (Mastership m = Mastership.Request(acontroller.Rapid))

                    {
                                  
                         RobTarget CurrPos = acontroller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);  
                         RapidData rd = acontroller.Rapid.GetRapidData("T_ROB1", "Module1", "rbt")
                         CurrPos = (RobTarget)rd.Value;

                         Console.WriteLine(CurrPos);   
                     }
               }
         }
     }

I wrote rbt:= CRobT(\Tool:=MyTool \WObj:=wobj0); from RAPID but I am not sure if that is the way to write it to get pose. I want to get the current TCP matrix, but when I printed the output it was not was I expected, as I got this output : [[1587.428550103,-522.655533591,0],[0,0,1,0],[-1,1,-2,0],[9E9,9E9,9E9,9E9,9E9,9E9]];. Please what am I doing wrong?

Any help will be appreciated. Thank you.

David

SjoLi

the pose consists of 2 parts a pos [x,y,z] and a rotation [q1,q2,q3,q4]

the layout you'r gettin is the complete robtarget which consists of a [pos][rotation][config][external axis]

The values u are getting are related to wobj0 which is usualy equal to the robot base frame

davidina

Thank you very much Sjoli. I understand better now. Since I am getting myself familiar with PC SDK with Robostudio my knowledge is still limited. Since the matrix will consist of both rotation and translation, please is there a way to read that into C#? What I actually want to do is to obtain matrix by:

Move robot to a position --> Read the the current position and rotation that will result in matrix --> Move robot to next position --> Read again --> until all positions reached and read.

I need a way to approach this sequence. Can you please show some light on how to proceed?? Thanks a lot!!

SjoLi

First get the CRobT

Then you can save the pos and orient(q1-q4) data from the target to there own datatype

MODULE Module1

    

    pers robtarget p1:=[[100,200,300],[1,0,0,0],[0,0,0,0],[9E9,9E9,9E9,9E9,9E9,9E9]];

    VAR pos pos1:=[0,0,0];

    VAR orient orient1:=[1,0,0,0]

    

    PROC Example()

        p1:=Crobt(\Tool:=tool0\WObj:=wobj0);

        pos1:=p1.trans;

        orient1:=p1.rot;

    ENDPROC

ENDMODULE

davidina

Following your code and with your help, I am now able to save the pos and orient. Thank you very much. I can write the code to convert them to form the matrix from the C#.... Or is there any better way to covert the output to rotation matrix instead of me writing that code?  ( I need it with the transformation matrix from the camera to make the calibration)

Once again, thank you very much.