Help! Get the current TCP pose

davidina · July 2014

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 · July 2014

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 · July 2014

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 · July 2014

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 · July 2014

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.

Help! Get the current TCP pose

Welcome!

Comments

Welcome!

Welcome!

Quick Links

Categories

In this Discussion