8. Zapytanie o stan robota

8.1. Pobieranie bieżącej pozycji przegubów (kąt)

1/**
2* @brief  Pobieranie bieżącej pozycji przegubów (kąt)
3* @param  [out] jPos Pobrane pozycje sześciu przegubów, jednostka deg
4* @return  Kod błędu
5*/
6int GetActualJointPosDegree(JointPos jPos);

8.2. Pobieranie prędkości sprzężenia zwrotnego przegubów - deg/s

1/**
2* @brief Pobieranie prędkości sprzężenia zwrotnego przegubów - deg/s
3* @param [out] speed Prędkości sześciu przegubów
4* @return Kod błędu
5*/
6int GetActualJointSpeedsDegree(Object[] speed);

8.3. Pobieranie przyspieszenia sprzężenia zwrotnego przegubów

1/**
2* @brief  Pobieranie przyspieszenia sprzężenia zwrotnego przegubów - deg/s^2
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] acc Przyspieszenia sześciu przegubów
5* @return  Kod błędu
6*/
7public int GetActualJointAccDegree(int flag, Object[] acc)

8.4. Pobieranie prędkości wypadkowej zadanej TCP

1/**
2* @brief  Pobieranie prędkości zadanej TCP
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] tcp_speed Prędkość liniowa
5* @param  [out] ori_speed Prędkość pozy
6* @return  Kod błędu
7*/
8public int GetTargetTCPCompositeSpeed(int flag, double tcp_speed, double ori_speed)

8.5. Pobieranie prędkości wypadkowej sprzężenia zwrotnego TCP

1/**
2* @brief  Pobieranie prędkości wypadkowej sprzężenia zwrotnego TCP
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] tcp_speed Prędkość liniowa
5* @param  [out] ori_speed Prędkość pozy
6* @return  Kod błędu
7*/
8public int GetActualTCPCompositeSpeed(int flag, double tcp_speed, double ori_speed)

8.6. Pobieranie prędkości zadanej TCP

1/**
2* @brief  Pobieranie prędkości zadanej TCP
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] speed Prędkości [x,y,z,rx,ry,rz]
5* @return  Kod błędu
6*/
7public int GetTargetTCPSpeed(int flag, Object[] speed)

8.7. Pobieranie prędkości sprzężenia zwrotnego TCP

1/**
2* @brief  Pobieranie prędkości sprzężenia zwrotnego TCP
3* @param  [in] flag 0-blokujący, 1-nieblokujący
4* @param  [out] speed Prędkości [x,y,z,rx,ry,rz]
5* @return  Kod błędu
6*/
7public int GetActualTCPSpeed(int flag, Object[] speed)

8.8. Pobieranie bieżącej pozy narzędzia

1/**
2* @brief  Pobieranie bieżącej pozy narzędzia
3* @param  [out] desc_pos  Pozycja narzędzia
4* @return  Kod błędu
5*/
6int GetActualTCPPose(DescPose desc_pos);

8.9. Pobieranie bieżącego numeru układu narzędzia

1/**
2* @brief  Pobieranie bieżącego numeru układu narzędzia
3* @param  [in] flag  0-blokujący, 1-nieblokujący
4* @param  [out] id  Numer układu narzędzia
5* @return  Kod błędu
6*/
7int GetActualTCPNum(int flag, int[] id)

8.10. Pobieranie bieżącego numeru układu przedmiotu

1/**
2* @brief  Pobieranie bieżącego numeru układu przedmiotu
3* @param  [in] flag  0-blokujący, 1-nieblokujący
4* @param  [out] id  Numer układu przedmiotu
5* @return  Kod błędu
6*/
7public int GetActualWObjNum(int flag, int[] id)

8.11. Pobieranie bieżącej pozy kołnierza końcowego

1/**
2* @brief  Pobieranie bieżącej pozy kołnierza końcowego
3* @param  [in] flag  0-blokujący, 1-nieblokujący
4* @param  [out] desc_pos  Pozycja kołnierza
5* @return  Kod błędu
6*/
7public int GetActualToolFlangePose(int flag, DescPose desc_pos)

8.12. Pobieranie bieżącego momentu obrotowego przegubów

1/**
2* @brief Pobieranie bieżącego momentu obrotowego przegubów
3* @param  [in]  flag 0-blokujący, 1-nieblokujący
4* @param  [out]  torques Moment obrotowy przegubów
5* @return  Kod błędu
6*/
7int GetJointTorques(int flag, Object[] torques);

8.13. Pobieranie czasu systemowego

1/**
2* @brief  Pobieranie czasu systemowego
3* @return  List[0]:int Kod błędu; List[1]:double t_ms jednostka ms
4*/
5List<Number> GetSystemClock();

8.14. Sprawdzanie, czy ruch robota został zakończony

1/**
2* @brief  Sprawdzanie, czy ruch robota został zakończony
3* @param   [out] state  0-niezakończony, 1-zakończony
4* @return  Kod błędu
5*/
6public int GetRobotMotionDone(int[] state)

8.15. Sprawdzanie długości bufora kolejki ruchu robota

1/**
2* @brief  Sprawdzanie długości bufora kolejki ruchu robota
3* @param   [out] len  Długość bufora
4* @return  Kod błędu
5*/
6public int GetMotionQueueLength(int[] len)

8.16. Pobieranie stanu zatrzymania awaryjnego robota

1/**
2* @brief Pobieranie stanu zatrzymania awaryjnego robota
3* @param [out] state Stan zatrzymania awaryjnego, 0-brak zatrzymania awaryjnego, 1-zatrzymanie awaryjne
4* @return Kod błędu
5*/
6public int GetRobotEmergencyStopState(int[] state)

8.17. Pobieranie stanu komunikacji SDK z robotem

1/**
2* @brief Pobieranie stanu komunikacji SDK z robotem
3* @return state Stan komunikacji, 0-komunikacja normalna, 1-komunikacja nieprawidłowa
4*/
5public int GetSDKComState()

8.18. Pobieranie sygnału bezpiecznego zatrzymania

1/**
2* @brief Pobieranie sygnału bezpiecznego zatrzymania
3* @param  [out] si0_state Sygnał bezpiecznego zatrzymania SI0, 0-nieaktywny, 1-aktywny
4* @param  [out] si1_state Sygnał bezpiecznego zatrzymania SI1, 0-nieaktywny, 1-aktywny
5* @return Kod błędu
6*/
7public int GetSafetyStopState(int[] si0_state, int[] si1_state)

8.19. Pobieranie temperatury sterownika przegubu robota (℃)

1/**
2* @brief Pobieranie temperatury sterownika przegubu robota (℃)
3* @param  [out] temperature Temperatura
4* @return Kod błędu
5*/
6public int GetJointDriverTemperature(double[] temperature)

8.20. Pobieranie momentu obrotowego sterownika przegubu robota (Nm)

1/**
2* @brief Pobieranie momentu obrotowego sterownika przegubu robota (Nm)
3* @param  [out] torque Moment obrotowy
4* @return Kod błędu
5*/
6public int GetJointDriverTorque(double[] torque)

8.21. Pobieranie struktury stanu robota w czasie rzeczywistym

1/**
2* @brief Pobieranie struktury stanu robota w czasie rzeczywistym
3* @return Struktura stanu w czasie rzeczywistym
4*/
5public ROBOT_STATE_PKG GetRobotRealTimeState()

8.22. Przykład kodu zapytania o stan robota

 1public static int TestGetStatus(Robot robot)
 2{
 3    List<Number> angle=new ArrayList<>();
 4    angle=robot.GetRobotInstallAngle();
 5    System.out.println("yangle:"+angle.get(1)+",zangle:"+angle.get(2));
 6
 7    JointPos j_deg =new JointPos(){};
 8    robot.GetActualJointPosDegree( j_deg);
 9
10    Object[] jointSpeed =new Object[] { 0,0,0,0,0,0 };
11    robot.GetActualJointSpeedsDegree(jointSpeed);
12
13    Object[] jointAcc = new Object[]{0,0,0,0,0,0 };
14    robot.GetActualJointAccDegree(0, jointAcc);
15
16    double tcp_speed = 0.0;
17    double ori_speed = 0.0;
18    robot.GetTargetTCPCompositeSpeed(0, tcp_speed, ori_speed);
19
20    robot.GetActualTCPCompositeSpeed(0, tcp_speed, ori_speed);
21
22    Object[] targetSpeed =new Object[] { 0,0,0,0,0,0 };
23    robot.GetTargetTCPSpeed(0, targetSpeed);
24
25    Object[] actualSpeed =new Object[] {0,0,0,0,0,0 };
26    robot.GetActualTCPSpeed(0, actualSpeed);
27
28    DescPose tcp = new DescPose(){};
29    robot.GetActualTCPPose(tcp);
30
31    DescPose flange = new DescPose(){};
32    robot.GetActualToolFlangePose(0, flange);
33
34    int[] id = {};
35    robot.GetActualTCPNum(0, id);
36
37    robot.GetActualWObjNum(0, id);
38
39    List<Number> jtorque=new ArrayList<>();
40    jtorque=robot.GetJointTorques(0);
41
42    List<Number> t_ms = new ArrayList<>();
43    t_ms=robot.GetSystemClock();
44
45    List<Integer> config = new ArrayList<>();
46    config=robot.GetRobotCurJointsConfig();
47
48    int motionDone = 0;
49    robot.GetRobotMotionDone(motionDone);
50
51    int[] len ={0 };
52    robot.GetMotionQueueLength(len);
53
54    int[] emergState = {0};
55    robot.GetRobotEmergencyStopState(emergState);
56
57    int comstate = 0;
58    comstate=robot.GetSDKComState();
59
60    int[] si0_state=new int[]{0}, si1_state=new int[]{0};
61    robot.GetSafetyStopState(si0_state, si1_state);
62
63    double[] temp =new double[] { 0,0,0,0,0,0 };
64    robot.GetJointDriverTemperature(temp);
65
66    double[] torque = new double[]{ 0,0,0,0,0,0 };
67    robot.GetJointDriverTorque(torque);
68
69    ROBOT_STATE_PKG pkg=new ROBOT_STATE_PKG();
70    pkg=robot.GetRobotRealTimeState();
71
72    return 0;
73}

8.23. Rozwiązanie kinematyki odwrotnej

1/**
2* @brief  Rozwiązanie kinematyki odwrotnej
3* @param  [in] type 0-pozycja absolutna (układ bazowy), 1-pozycja przyrostowa (układ bazowy), 2-pozycja przyrostowa (układ narzędzia)
4* @param  [in] desc_pos Pozycja kartezjańska
5* @param  [in] config Konfiguracja przestrzeni przegubów, [-1]-obliczenia względem bieżącej pozycji przegubów, [0~7]-obliczenia według określonej konfiguracji przestrzeni przegubów
6* @param  [out] joint_pos Pozycja przegubów
7* @return  Kod błędu
8*/
9int GetInverseKin(int type, DescPose desc_pos, int config, JointPos joint_pos);

8.24. Rozwiązanie kinematyki odwrotnej (z pozycją referencyjną)

1/**
2* @brief  Rozwiązanie kinematyki odwrotnej, obliczenia względem podanej pozycji referencyjnej przegubów
3* @param  [in] posMode 0-pozycja absolutna, 1-pozycja względna - układ bazowy, 2-pozycja względna - układ narzędzia
4* @param  [in] desc_pos Pozycja kartezjańska
5* @param  [in] joint_pos_ref Referencyjna pozycja przegubów
6* @param  [out] joint_pos Pozycja przegubów
7* @return  Kod błędu
8*/
9int GetInverseKinRef(int posMode, DescPose desc_pos, JointPos joint_pos_ref, JointPos joint_pos);

8.25. Rozwiązanie kinematyki odwrotnej, przestrzeń kartezjańska z uwzględnieniem pozycji osi rozszerzenia

 1/**
 2* @brief Rozwiązanie kinematyki odwrotnej, przestrzeń kartezjańska z uwzględnieniem pozycji osi rozszerzenia
 3* @param  type 0-pozycja absolutna (układ bazowy), 1-pozycja przyrostowa (układ bazowy), 2-pozycja przyrostowa (układ narzędzia)
 4* @param  desc_pos Pozycja kartezjańska
 5* @param  exaxis Pozycja osi rozszerzenia
 6* @param  tool Numer narzędzia
 7* @param  workPiece Numer przedmiotu
 8* @param  joint_pos Pozycja przegubów
 9* @return Kod błędu
10*/
11public int GetInverseKinExaxis(int type, DescPose desc_pos, ExaxisPos exaxis, int tool, int workPiece, JointPos joint_pos)

8.26. Przykład kodu rozwiązania kinematyki odwrotnej z uwzględnieniem pozycji osi rozszerzenia

 1public static void  TestInverseKinExaxis(Robot robot)
 2{
 3    DescPose desc=new DescPose(99.957, -0.002, 29.994, -176.569, -6.757, -167.462);
 4    ExaxisPos exaxis=new ExaxisPos(100.0, 0.0, 0.0, 0.0);
 5    JointPos jointPos =new JointPos();
 6    DescPose offsetPos =new DescPose();
 7    ROBOT_STATE_PKG pkg=robot.GetRobotRealTimeState();
 8    int toolnum = pkg.tool;
 9    int workPcsNum = pkg.user;
10    robot.GetInverseKinExaxis(0, desc, exaxis, toolnum, workPcsNum, jointPos);
11    System.out.printf("GetInverseKinExaxis joint is %f, %f, %f, %f, %f, %f\n", jointPos.J1, jointPos.J2, jointPos.J3, jointPos.J4, jointPos.J5, jointPos.J6);
12    robot.ExtAxisMove(exaxis, 100, -1);
13    robot.MoveJ(jointPos, desc, toolnum, workPcsNum, 100.0, 100.0, 100.0, exaxis, -1, 0, offsetPos);
14    robot.CloseRPC();
15    robot.Sleep(9999999);
16}

8.27. Sprawdzanie, czy kinematyka odwrotna ma rozwiązanie

1/**
2* @brief  Sprawdzanie, czy kinematyka odwrotna ma rozwiązanie, odniesienie do podanej pozycji referencyjnej przegubów
3* @param  [in] posMode 0-pozycja absolutna, 1-pozycja względna - układ bazowy, 2-pozycja względna - układ narzędzia
4* @param  [in] desc_pos Pozycja kartezjańska
5* @param  [in] joint_pos_ref Referencyjna pozycja przegubów
6* @return  Kod błędu  List[0]:Kod błędu; List[1]: int hasResult 0-brak rozwiązania, 1-jest rozwiązanie
7*/
8List<Integer> GetInverseKinHasSolution(int posMode, DescPose desc_pos, JointPos joint_pos_ref);

8.28. Rozwiązanie kinematyki prostej

1/**
2* @brief  Rozwiązanie kinematyki prostej
3* @param  [in] joint_pos Pozycja przegubów
4* @param  [out] desc_pos Pozycja kartezjańska
5* @return  Kod błędu
6*/
7int GetForwardKin(JointPos joint_pos, DescPose desc_pos);

8.29. Przykład kodu obliczeń kinematyki prostej i odwrotnej robota

 1public static int TestInverseKin(Robot robot)
 2{
 3    JointPos j1=new JointPos(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
 4    DescPose desc_pos1=new DescPose(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
 5
 6    JointPos inverseRtn = new JointPos(){};
 7
 8    robot.GetInverseKin(0, desc_pos1, -1, inverseRtn);
 9    robot.GetInverseKinRef(0, desc_pos1, j1, inverseRtn);
10
11    int hasResut = 0;
12    robot.GetInverseKinHasSolution(0, desc_pos1, j1);
13
14    DescPose forwordResult = new DescPose(){};
15    robot.GetForwardKin(j1, forwordResult);
16
17    return 0;
18}

8.30. Zapytanie o dane punktu zarządzania nauczaniem robota

1/**
2* @brief Zapytanie o dane punktu zarządzania nauczaniem robota
3* @param [in]  name  Nazwa punktu
4* @return  List[0]:Kod błędu; List[1] - List[20] : Dane punktu double[20]{x,y,z,rx,ry,rz,j1,j2,j3,j4,j5,j6,tool,wobj,speed,acc,e1,e2,e3,e4}
5*/
6List<Number> GetRobotTeachingPoint(String name);

8.31. Pobieranie wartości kompensacji parametrów DH robota

1/**
2* @brief Pobieranie wartości kompensacji parametrów DH robota
3* @param dhCompensation Wartości kompensacji parametrów DH robota (mm) [cmpstD1,cmpstA2,cmpstA3,cmpstD4,cmpstD5,cmpstD6]
4* @return Kod błędu
5*/
6public int GetDHCompensation(Object[] dhCompensation)

8.32. Pobieranie numeru SN skrzynki kontrolnej

Nowe w wersji Java: SDK-v1.0.4-3.8.1

1/**
2* @brief Pobieranie numeru SN skrzynki kontrolnej
3* @param [out] SNCode Numer SN skrzynki kontrolnej
4* @return Kod błędu
5*/
6int GetRobotSN(String[] SNCode);

8.33. Przykład kodu zapytania o dane punktu zarządzania nauczaniem robota

 1public static int TestGetTeachPoint(Robot robot)
 2{
 3    String name = "P1";
 4    List<Number> data=new ArrayList<>();
 5    data = robot.GetRobotTeachingPoint(name);
 6    System.out.println(name+" name is: "+data.get(0));
 7    for (int i = 0; i < 20; i++)
 8    {
 9        System.out.println("data is: "+ data.get(i+1));
10    }
11
12    int[] que_len = {0};
13    int rtn = robot.GetMotionQueueLength(que_len);
14    System.out.println("GetMotionQueueLength rtn is:"+rtn+", queue length is:"+ que_len[0]);
15
16    Object[] dh = new Object[]{ 0,0,0,0,0,0 };
17    int retval = 0;
18    retval = robot.GetDHCompensation(dh);
19    System.out.println("retval is: "+retval);
20
21    String[] SN = new String[]{""};
22    robot.GetRobotSN(SN);
23    System.out.println("robot SN is "+SN[0]);
24    return 0;
25}

8.34. Pobieranie układu narzędzia według numeru

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2* @brief Pobieranie układu narzędzia według numeru
3* @param [in] id Numer układu narzędzia
4* @param [out] coord Wartości układu współrzędnych
5* @return Kod błędu
6*/
7int GetToolCoordWithID(int id, DescPose coord)

8.35. Pobieranie układu przedmiotu według numeru

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2* @brief Pobieranie układu przedmiotu według numeru
3* @param [in]  id Numer układu przedmiotu
4* @param [out] coord Wartości układu współrzędnych
5* @return Kod błędu
6*/
7public int GetWObjCoordWithID(int id, DescPose coord)

8.36. Pobieranie zewnętrznego układu narzędzia według numeru

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2* @brief Pobieranie zewnętrznego układu narzędzia według numeru
3* @param [in]  id Numer zewnętrznego układu narzędzia
4* @param [out] coord Wartości układu współrzędnych
5* @return Kod błędu
6*/
7public int GetExToolCoordWithID(int id, DescPose coord)

8.37. Pobieranie układu osi rozszerzenia według numeru

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2* @brief Pobieranie układu osi rozszerzenia według numeru
3* @param [in]  id Numer zewnętrznego układu narzędzia
4* @param [out] coord Wartości układu współrzędnych
5* @return Kod błędu
6*/
7public int GetExAxisCoordWithID(int id, DescPose coord)

8.38. Pobieranie bieżącego układu narzędzia

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2 * @brief Pobieranie bieżącego układu narzędzia
3 * @param [out] coord Wartości układu współrzędnych
4 * @return Kod błędu
5 */
6public int GetCurToolCoord(DescPose coord)

8.39. Pobieranie bieżącego układu przedmiotu

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2 * @brief Pobieranie bieżącego układu przedmiotu
3 * @param [out] coord Wartości układu współrzędnych
4 * @return Kod błędu
5 */
6public int GetCurWObjCoord(DescPose coord)

8.40. Pobieranie bieżącego zewnętrznego układu narzędzia

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2 * @brief Pobieranie bieżącego zewnętrznego układu narzędzia
3 * @param  [out] coord Wartości układu współrzędnych
4 * @return Kod błędu
5 */
6public int GetCurExToolCoord(DescPose coord)

8.41. Pobieranie bieżącego układu osi rozszerzenia

Nowe w wersji Java: SDK-v1.0.9-3.8.6

1/**
2 * @brief Pobieranie bieżącego układu osi rozszerzenia
3 * @param [out] coord Wartości układu współrzędnych
4 * @return Kod błędu
5 */
6public int GetCurExAxisCoord(DescPose coord)

8.42. Przykład kodu układów współrzędnych robota i obciążenia

 1public static void TestCoord(Robot robot)
 2{
 3    int id = 1;
 4    int rtn = 0;
 5    DescPose toolCoord = new DescPose();
 6    DescPose extoolCoord = new DescPose();
 7    DescPose wobjCoord = new DescPose();
 8    DescPose exAxisCoord = new DescPose();
 9
10
11    robot.GetCurToolCoord(toolCoord);//narzędzie
12    System.out.println("GetToolCoord:"+id+","+
13            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
14            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);
15
16
17    robot.GetCurWObjCoord(toolCoord);//przedmiot
18    System.out.println("GetCurWObjCoord:"+id+","+
19            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
20            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);
21
22    robot.GetCurExToolCoord(toolCoord);//zewnętrzne narzędzie
23    System.out.println("GetCurExToolCoord:"+id+","+
24            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
25            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);
26
27
28    robot.GetCurExAxisCoord(toolCoord);//oś rozszerzenia
29    System.out.println("GetCurExToolCoord:"+id+","+
30            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
31            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);
32
33
34    List<Number> weightT = new ArrayList<>();//środek ciężkości
35    DescTran cogT=new DescTran();
36    weightT=robot.GetTargetPayload(0);
37    robot.GetTargetPayloadCog(0,cogT);
38    System.out.println("GetTargetPayload :"+weightT.get(1).doubleValue()+", "+
39            cogT.x+", "+cogT.y+", "+cogT.z);
40
41
42    robot.GetToolCoordWithID(id, toolCoord);
43    System.out.println("GetToolCoordWithID:"+id+","+
44            toolCoord.tran.x+","+ toolCoord.tran.y+","+ toolCoord.tran.z+","+
45            toolCoord.rpy.rx+","+ toolCoord.rpy.ry+","+ toolCoord.rpy.rz);
46
47    robot.GetWObjCoordWithID(id, wobjCoord);
48    System.out.println("GetWObjCoordWithID "+id+", "+
49            wobjCoord.tran.x+","+ wobjCoord.tran.y+","+ wobjCoord.tran.z+","+
50            wobjCoord.rpy.rx+","+ wobjCoord.rpy.ry+","+ wobjCoord.rpy.rz);
51
52
53    robot.GetExToolCoordWithID(id, extoolCoord);//zewnętrzne narzędzie
54    System.out.println("GetExToolCoordWithID :"+ id+","+
55            extoolCoord.tran.x+","+ extoolCoord.tran.y+","+ extoolCoord.tran.z+","+
56            extoolCoord.rpy.rx+","+ extoolCoord.rpy.ry+","+ extoolCoord.rpy.rz);
57
58    robot.GetExAxisCoordWithID(id, exAxisCoord);//oś rozszerzenia
59    System.out.println("GetExAxisCoordWithID "+id+","+
60            exAxisCoord.tran.x+","+ exAxisCoord.tran.y+","+ exAxisCoord.tran.z+","+
61            exAxisCoord.rpy.rx+","+ exAxisCoord.rpy.ry+","+ exAxisCoord.rpy.rz);
62
63
64    double[] weight = new double[1];//środek ciężkości ładunku
65    DescTran getCog = new DescTran();
66    robot.GetTargetPayloadWithID(id, weight, getCog);
67    System.out.println("GetTargetPayloadWithID :"+ id+","+ weight[0]+","+
68            getCog.x+","+ getCog.y+","+ getCog.z);
69
70    DescPose coordSet0 = new DescPose(0, 0, 0, 0, 0, 0);
71    DescPose coordSet = new DescPose(1, 2, 3, 4, 5, 6);
72    DescPose etcp = new DescPose(10, 20, 30, 40, 50, 60);
73    DescPose etool = new DescPose(0.1, 0.2, 0.3, 0.4, 0.5, 0.6);
74    DescTran cog = new DescTran(1, 2, 3);
75
76    robot.SetToolCoord(id, coordSet, 0, 0, 1, 0);
77    robot.Sleep(100);
78    robot.SetWObjCoord(id, coordSet, 0);
79    robot.Sleep(100);
80    robot.ExtAxisActiveECoordSys(id, 1, coordSet, 1); //Zastosowanie wyniku kalibracji do układu współrzędnych osi rozszerzenia
81    robot.Sleep(100);
82    rtn = robot.SetExToolCoord(id, etcp, etool);
83    robot.Sleep(100);
84    rtn = robot.SetLoadWeight(id, 1.5);
85    robot.Sleep(500);
86    rtn = robot.SetLoadCoord(id, cog);
87    robot.Sleep(100);
88}