8. Zapytanie o stan robota

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

/**
* @brief  Pobiera bieżącą pozycję stawów (kąt)
* @param  [in] flag 0-blokujący, 1-nieblokujący
* @param  [out] jPos Pozycje sześciu stawów, jednostka deg
* @return  Kod błędu
*/
errno_t  GetActualJointPosDegree(uint8_t flag, JointPos *jPos);

8.2. Pobieranie prędkości sprzężenia zwrotnego stawów

/**
 * @brief  Pobiera prędkość sprzężenia zwrotnego stawów - deg/s
 * @param  [in] flag 0-blokujący, 1-nieblokujący
 * @param  [out] speed Prędkości sześciu stawów
 * @return  Kod błędu
 */
errno_t  GetActualJointSpeedsDegree(uint8_t flag, float speed[6]);

8.3. Pobieranie przyspieszenia sprzężenia zwrotnego stawów

/**
 * @brief  Pobiera przyspieszenie sprzężenia zwrotnego stawów - deg/s^2
 * @param  [in] flag 0-blokujący, 1-nieblokujący
 * @param  [out] acc Przyspieszenia sześciu stawów
 * @return  Kod błędu
 */
errno_t  GetActualJointAccDegree(uint8_t flag, float acc[6]);

8.4. Pobieranie prędkości wypadkowej zadanej TCP

/**
 * @brief  Pobiera prędkość wypadkową zadaną TCP
 * @param  [in] flag 0-blokujący, 1-nieblokujący
 * @param  [out] tcp_speed Prędkość liniowa
 * @param  [out] ori_speed Prędkość kątowa
 * @return  Kod błędu
 */
errno_t  GetTargetTCPCompositeSpeed(uint8_t flag, float *tcp_speed, float *ori_speed);

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

/**
 * @brief  Pobiera prędkość wypadkową sprzężenia zwrotnego TCP
 * @param  [in] flag 0-blokujący, 1-nieblokujący
 * @param  [out] tcp_speed Prędkość liniowa
 * @param  [out] ori_speed Prędkość kątowa
 * @return  Kod błędu
 */
errno_t  GetActualTCPCompositeSpeed(uint8_t flag, float *tcp_speed, float *ori_speed);

8.6. Pobieranie prędkości zadanej TCP

/**
 * @brief  Pobiera prędkość zadaną TCP
 * @param  [in] flag 0-blokujący, 1-nieblokujący
 * @param  [out] speed Prędkość [x, y, z, rx, ry, rz]
 * @return  Kod błędu
 */
errno_t  GetTargetTCPSpeed(uint8_t flag, float speed[6]);

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

/**
 * @brief  Pobiera prędkość sprzężenia zwrotnego TCP
 * @param  [in] flag 0-blokujący, 1-nieblokujący
 * @param  [out] speed Prędkość [x, y, z, rx, ry, rz]
 * @return  Kod błędu
 */
errno_t  GetActualTCPSpeed(uint8_t flag, float speed[6]);

8.8. Pobieranie bieżącej pozy i orientacji narzędzia

/**
* @brief  Pobiera bieżącą pozy i orientację narzędzia
* @param  [in] flag  0-blokujący, 1-nieblokujący
* @param  [out] desc_pos  Pozy i orientacja narzędzia
* @return  Kod błędu
*/
errno_t  GetActualTCPPose(uint8_t flag, DescPose *desc_pos);

8.9. Pobieranie numeru bieżącego układu współrzędnych narzędzia

/**
* @brief  Pobiera numer bieżącego układu współrzędnych narzędzia
* @param  [in] flag  0-blokujący, 1-nieblokujący
* @param  [out] id  Numer układu współrzędnych narzędzia
* @return  Kod błędu
*/
errno_t  GetActualTCPNum(uint8_t flag, int *id);

8.10. Pobieranie numeru bieżącego układu współrzędnych obiektu

/**
* @brief  Pobiera numer bieżącego układu współrzędnych obiektu
* @param  [in] flag  0-blokujący, 1-nieblokujący
* @param  [out] id  Numer układu współrzędnych obiektu
* @return  Kod błędu
*/
errno_t  GetActualWObjNum(uint8_t flag, int *id);

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

/**
* @brief  Pobiera bieżącą pozy i orientację kołnierza końcowego
* @param  [in] flag  0-blokujący, 1-nieblokujący
* @param  [out] desc_pos  Pozy i orientacja kołnierza
* @return  Kod błędu
*/
errno_t  GetActualToolFlangePose(uint8_t flag, DescPose *desc_pos);

8.12. Pobieranie bieżącego momentu stawów

/**
* @brief Pobiera bieżący moment stawów
* @param  [in] flag 0-blokujący, 1-nieblokujący
* @param  [out] torques Momenty stawów
* @return  Kod błędu
*/
errno_t  GetJointTorques(uint8_t flag, float torques[6]);

8.13. Pobieranie czasu systemowego

/**
* @brief  Pobiera czas systemowy
* @param  [out] t_ms Jednostka ms
* @return  Kod błędu
*/
errno_t  GetSystemClock(float *t_ms);

8.14. Synchronizacja Czasu Systemowego z Robotem

/**
* @brief Pobiera bieżący czas systemowy komputera hosta i wysyła go do robota w celu synchronizacji czasu systemowego (ze względu na ograniczenia systemu QNX, dokładność synchronizacji jest na poziomie minut)
* @return Kod błędu
*/
errno_t SetRobotTime();

8.15. Przykład Kodu Ustawiania i Odczytu Czasu Systemowego Robota

void TestSetAndGetRobotTime()
{
ROBOT_STATE_PKG pkg = {};
FRRobot robot;
robot.LoggerInit();
robot.SetLoggerLevel(1);
robot.SetReConnectParam(true, 30000, 500);
int rtn = robot.RPC("192.168.58.2");
if (rtn != 0) {
    return;
}
float t_ms = 0.0f;
int ret = robot.GetSystemClock(&t_ms);
if (ret == 0) {
    constexpr int64_t OFFSET_8H_MS = 8LL * 3600LL * 1000LL;
    int64_t rawMs = static_cast<int64_t>(t_ms);
    int64_t fixedMs = rawMs - OFFSET_8H_MS;
    std::time_t sec = static_cast<std::time_t>(fixedMs / 1000LL);
    std::tm utcTm {};
    #if defined (_WIN32) || defined (_WIN64)
        localtime_s(&utcTm, &sec);
    #else
        localtime_r(&sec, &utcTm);
    #endif
        char utcBuf[64] = { 0 };
        strftime(utcBuf, sizeof(utcBuf), "%Y-%m-%d %H:%M:%S", &utcTm);
        printf("system clock : %.0f\n", t_ms);
        printf("BEFORE UTC Time : %s\n", utcBuf);
    }
    else {
        printf("GetSystemClock failed, ret:%d\n", ret);
    }

    robot.SetRobotTime();
    robot.Sleep(1000);
    float t_ms_after = 0.0f;
    ret = robot.GetSystemClock(&t_ms_after);
    if (ret == 0) {
        printf("system clock : %.0f\n", t_ms_after);

        constexpr int64_t OFFSET_8H_MS = 8LL * 3600LL * 1000LL;
        int64_t robotMs = static_cast<int64_t>(t_ms_after);
        int64_t robotMsFixed = robotMs - OFFSET_8H_MS;

        auto nowSys = std::chrono::system_clock::now();
        int64_t pcMs = std::chrono::duration_cast<std::chrono::milliseconds>(nowSys.time_since_epoch()).count();
        constexpr int64_t MS_PER_MINUTE = 60LL * 1000LL;
        bool isConsistent = ((robotMsFixed / MS_PER_MINUTE) == (pcMs / MS_PER_MINUTE));

        std::time_t robotSec = robotMsFixed / 1000LL;
        std::tm robotTm {};
    #if defined (_WIN32) || defined (_WIN64)
        localtime_s(&robotTm, &robotSec);
    #else
        localtime_r(&robotSec, &robotTm);
    #endif

        std::time_t pcSec = pcMs / 1000LL;
        std::tm pcTm {};
    #if defined (_WIN32) || defined (_WIN64)
        localtime_s(&pcTm, &pcSec);
    #else
        localtime_r(&pcSec, &pcTm);
    #endif

    char pcBuf[64] = { 0 };
    char robotBuf[64] = { 0 };
    strftime(pcBuf, sizeof(pcBuf), "%Y-%m-%d %H:%M", &pcTm);
    strftime(robotBuf, sizeof(robotBuf), "%Y-%m-%d %H:%M", &robotTm);
    if (isConsistent) {
    printf("Consistent | PC time: %s | Robot time: %s\n", pcBuf, robotBuf);
    }
    else {
    printf("[Inconsistent] | PC time: %s | Robot time: %s\n", pcBuf, robotBuf);
    }
}
else {
    printf("GetSystemClock failed, ret:%d\n", ret);
}
robot.Sleep(1000);
robot.CloseRPC();
robot.Sleep(1000);
}

8.16. Sprawdzanie, czy ruch robota został zakończony

/**
* @brief  Sprawdza, czy ruch robota został zakończony
* @param  [out]  state  0-niezakończony, 1-zakończony
* @return  Kod błędu
*/
errno_t  GetRobotMotionDone(uint8_t *state);

8.17. Sprawdzanie długości bufora kolejki ruchu robota

/**
 * @brief  Sprawdza długość bufora kolejki ruchu robota
 * @param  [out]  len  Długość bufora
 * @return  Kod błędu
 */
errno_t  GetMotionQueueLength(int *len);

8.18. Pobieranie stanu awaryjnego zatrzymania robota

/**
* @brief Pobiera stan awaryjnego zatrzymania robota
* @param [out] state Stan awaryjnego zatrzymania, 0-brak, 1-awaryjne zatrzymanie
* @return Kod błędu
*/
errno_t GetRobotEmergencyStopState(uint8_t *state);

8.19. Pobieranie stanu komunikacji SDK z robotem

/**
* @brief Pobiera stan komunikacji SDK z robotem
* @param [out] state Stan komunikacji, 0-komunikacja normalna, 1-komunikacja nieprawidłowa
*/
errno_t GetSDKComState(int *state);

8.20. Pobieranie sygnału bezpiecznego zatrzymania

/**
* @brief Pobiera sygnał bezpiecznego zatrzymania
* @param [out] si0_state Sygnał bezpiecznego zatrzymania SI0, 0-nieaktywny, 1-aktywny
* @param [out] si1_state Sygnał bezpiecznego zatrzymania SI1, 0-nieaktywny, 1-aktywny
*/
errno_t GetSafetyStopState(uint8_t *si0_state, uint8_t *si1_state);

8.21. Pobieranie temperatury sterownika stawów robota (℃)

Dodane w wersji C++SDK-v2.1.5.0.

/**
* @brief Pobiera temperaturę sterownika stawów robota (℃)
* @return Kod błędu
*/
errno_t GetJointDriverTemperature(double temperature[]);

8.22. Pobieranie momentu sterownika stawów robota (Nm)

Dodane w wersji C++SDK-v2.1.5.0.

/**
* @brief Pobiera moment sterownika stawów robota (Nm)
* @return Kod błędu
*/
errno_t GetJointDriverTorque(double torque[]);

8.23. Pobieranie struktury stanu robota w czasie rzeczywistym

Dodane w wersji C++SDK-v2.1.3.0.

/**
* @brief Pobiera strukturę stanu robota w czasie rzeczywistym
* @param [out] pkg Struktura stanu robota w czasie rzeczywistym
* @return Kod błędu
*/
errno_t GetRobotRealTimeState(ROBOT_STATE_PKG *pkg);

8.24. Przykład kodu zapytania o stan robota

int TestGetStatus(void)
{
  ROBOT_STATE_PKG pkg = {};
  FRRobot robot;
  robot.LoggerInit();
  robot.SetLoggerLevel(1);
  int rtn = robot.RPC("192.168.58.2");
  if (rtn != 0)
  {
    return -1;
  }
  robot.SetReConnectParam(true, 30000, 500);
  float yangle, zangle;
  robot.GetRobotInstallAngle(&yangle, &zangle);
  printf("yangle:%f,zangle:%f\n", yangle, zangle);
  JointPos j_deg = {};
  robot.GetActualJointPosDegree(0, &j_deg);
  printf("joint pos deg:%f,%f,%f,%f,%f,%f\n", j_deg.jPos[0], j_deg.jPos[1], j_deg.jPos[2], j_deg.jPos[3], j_deg.jPos[4], j_deg.jPos[5]);
  float jointSpeed[6] = { 0.0 };
  robot.GetActualJointSpeedsDegree(0, jointSpeed);
  printf("joint speeds deg:%f,%f,%f,%f,%f,%f\n", jointSpeed[0], jointSpeed[1], jointSpeed[2], jointSpeed[3], jointSpeed[4], jointSpeed[5]);
  float jointAcc[6] = { 0.0 };
  robot.GetActualJointAccDegree(0, jointAcc);
  printf("joint acc deg:%f,%f,%f,%f,%f,%f\n", jointAcc[0], jointAcc[1], jointAcc[2], jointAcc[3], jointAcc[4], jointAcc[5]);
  float tcp_speed = 0.0;
  float ori_speed = 0.0;
  robot.GetTargetTCPCompositeSpeed(0, &tcp_speed, &ori_speed);
  printf("GetTargetTCPCompositeSpeed tcp %f; ori %f\n", tcp_speed, ori_speed);
  robot.GetActualTCPCompositeSpeed(0, &tcp_speed, &ori_speed);
  printf("GetActualTCPCompositeSpeed tcp %f; ori %f\n", tcp_speed, ori_speed);
  float targetSpeed[6] = { 0.0 };
  robot.GetTargetTCPSpeed(0, targetSpeed);
  printf("GetTargetTCPSpeed %f,%f,%f,%f,%f,%f\n", targetSpeed[0], targetSpeed[1], targetSpeed[2], targetSpeed[3], targetSpeed[4], targetSpeed[5]);
  float actualSpeed[6] = { 0.0 };
  robot.GetActualTCPSpeed(0, actualSpeed);
  printf("GetTargetTCPSpeed %f,%f,%f,%f,%f,%f\n", actualSpeed[0], actualSpeed[1], actualSpeed[2], actualSpeed[3], actualSpeed[4], actualSpeed[5]);
  DescPose tcp = {};
  robot.GetActualTCPPose(0, &tcp);
  printf("tcp pose:%f,%f,%f,%f,%f,%f\n", tcp.tran.x, tcp.tran.y, tcp.tran.z, tcp.rpy.rx, tcp.rpy.ry, tcp.rpy.rz);
  DescPose flange = {};
  robot.GetActualToolFlangePose(0, &flange);
  printf("flange pose:%f,%f,%f,%f,%f,%f\n", flange.tran.x, flange.tran.y, flange.tran.z, flange.rpy.rx, flange.rpy.ry, flange.rpy.rz);
  int id = 0;
  robot.GetActualTCPNum(0, &id);
  printf("tcp num:%d\n", id);
  robot.GetActualWObjNum(0, &id);
  printf("wobj num:%d\n", id);
  float jtorque[6] = { 0.0 };
  robot.GetJointTorques(0, jtorque);
  printf("torques:%f,%f,%f,%f,%f,%f\n", jtorque[0], jtorque[1], jtorque[2], jtorque[3], jtorque[4], jtorque[5]);
  float t_ms = 0.0;
  robot.GetSystemClock(&t_ms);
  printf("system clock:%f\n", t_ms);
  int config = 0;
  robot.GetRobotCurJointsConfig(&config);
  printf("joint config:%d\n", config);
  uint8_t motionDone = 0;
  robot.GetRobotMotionDone(&motionDone);
  printf("GetRobotMotionDone :%d\n", motionDone);
  int len = 0;
  robot.GetMotionQueueLength(&len);
  printf("GetMotionQueueLength :%d\n", len);
  uint8_t emergState = 0;
  robot.GetRobotEmergencyStopState(&emergState);
  printf("GetRobotEmergencyStopState :%d\n", emergState);
  int comstate = 0;
  robot.GetSDKComState(&comstate);
  printf("GetSDKComState :%d\n", comstate);
  uint8_t si0_state, si1_state;
  robot.GetSafetyStopState(&si0_state, &si1_state);
  printf("GetSafetyStopState :%d %d\n", si0_state, si1_state);
  double temp[6] = { 0.0 };
  robot.GetJointDriverTemperature(temp);
  printf("Temperature:%f,%f,%f,%f,%f,%f\n", temp[0], temp[1], temp[2], temp[3], temp[4], temp[5]);
  double torque[6] = { 0.0 };
  robot.GetJointDriverTorque(torque);
  printf("torque:%f,%f,%f,%f,%f,%f\n", torque[0], torque[1], torque[2], torque[3], torque[4], torque[5]);
  robot.GetRobotRealTimeState(&pkg);
  robot.CloseRPC();
  return 0;
}

8.25. Rozwiązanie odwrotnej kinematyki

/**
* @brief  Rozwiązanie odwrotnej kinematyki
* @param  [in] type 0-pozycja absolutna (układ bazowy), 1-pozycja przyrostowa (układ bazowy), 2-pozycja przyrostowa (układ narzędzia)
* @param  [in] desc_pos Pozy i orientacja kartezjańska
* @param  [in] config Konfiguracja przestrzeni stawów, [-1]-obliczenia w oparciu o bieżącą pozycję stawów, [0~7]-obliczenia zgodnie z określoną konfiguracją przestrzeni stawów
* @param  [out] joint_pos Pozycja stawów
* @return  Kod błędu
*/
errno_t  GetInverseKin(int type, DescPose *desc_pos, int config, JointPos *joint_pos);

8.26. Rozwiązanie odwrotnej kinematyki (z pozycją odniesienia)

/**
* @brief  Rozwiązanie odwrotnej kinematyki, z uwzględnieniem zadanej pozycji stawów
* @param  [in] type 0-pozycja absolutna (układ bazowy), 1-pozycja przyrostowa (układ bazowy), 2-pozycja przyrostowa (układ narzędzia)
* @param  [in] desc_pos Pozy i orientacja kartezjańska
* @param  [in] joint_pos_ref Referencyjna pozycja stawów
* @param  [out] joint_pos Pozycja stawów
* @return  Kod błędu
*/
errno_t  GetInverseKinRef(int type, DescPose *desc_pos, JointPos *joint_pos_ref, JointPos *joint_pos);

8.27. Rozwiązanie odwrotnej kinematyki z pozycją osi rozszerzonej w przestrzeni kartezjańskiej

/**
* @brief Rozwiązanie odwrotnej kinematyki z pozycją osi rozszerzonej w przestrzeni kartezjańskiej
* @param [in] type 0-pozycja absolutna (układ bazowy), 1-pozycja przyrostowa (układ bazowy), 2-pozycja przyrostowa (układ narzędzia)
* @param [in] desc_pos Pozy i orientacja kartezjańska
* @param [in] exaxis Pozycja osi rozszerzonej
* @param [in] tool Numer narzędzia
* @param [in] workPiece Numer obiektu
* @param [out] joint_pos Pozycja stawów
* @param [in] config Konfiguracja przestrzeni stawów, [-1] - rozwiązanie w oparciu o bieżącą pozycję stawów jako odniesienie, [0~7] - rozwiązanie zgodnie z określoną konfiguracją przestrzeni stawów
* @return Kod błędu
*/
errno_t GetInverseKinExaxis(int type, DescPose desc_pos, ExaxisPos exaxis, int tool, int workPiece, JointPos& joint_pos, int config = -1);

8.28. Przykład kodu rozwiązania odwrotnej kinematyki z pozycją osi rozszerzonej

int TestInverseKinExaxis()
{
    ROBOT_STATE_PKG pkg = {};
    FRRobot robot;
    robot.LoggerInit();
    robot.SetLoggerLevel(1);
    int rtn = robot.RPC("192.168.58.2");
    if (rtn != 0)
    {
        return 0;
    }
    robot.SetReConnectParam(true, 30000, 500);
    DescPose desc(99.957, -0.002, 29.994, -176.569, -6.757, -167.462);
    ExaxisPos exaxis(100.0, 0.0, 0.0, 0.0);
    JointPos jointPos = {};
    DescPose offsetPos = {};
    robot.GetRobotRealTimeState(&pkg);
    int toolnum = pkg.tool;
    int workPcsNum = pkg.user;
    robot.GetInverseKinExaxis(0, desc, exaxis, toolnum, workPcsNum, jointPos);
    printf("GetInverseKinExaxis joint is %f, %f, %f, %f, %f, %f\n", jointPos.jPos[0], jointPos.jPos[1], jointPos.jPos[2], jointPos.jPos[3], jointPos.jPos[4], jointPos.jPos[5]);
    robot.ExtAxisMove(exaxis, 100, -1);
    robot.MoveJ(&jointPos, &desc, toolnum, workPcsNum, 100.0, 100.0, 100.0, &exaxis, -1, 0, &offsetPos);
    robot.CloseRPC();
    robot.Sleep(9999999);
    return 0;
}

8.29. Sprawdzanie, czy odwrotna kinematyka ma rozwiązanie

/**
* @brief  Sprawdza, czy odwrotna kinematyka ma rozwiązanie, z uwzględnieniem zadanej pozycji stawów
* @param  [in] type 0-pozycja absolutna (układ bazowy), 1-pozycja przyrostowa (układ bazowy), 2-pozycja przyrostowa (układ narzędzia)
* @param  [in] desc_pos Pozy i orientacja kartezjańska
* @param  [in] joint_pos_ref Referencyjna pozycja stawów
* @param  [out] result 0-brak rozwiązania, 1-jest rozwiązanie
* @return  Kod błędu
*/
errno_t  GetInverseKinHasSolution(int type, DescPose *desc_pos, JointPos *joint_pos_ref, uint8_t *result);

8.30. Pobieranie 8 zestawów rozwiązań odwrotnych dla robota

/**
* @brief Pobierz 8 zestawów rozwiązań odwrotnych dla robota
* @param [in] tcfPose Pozycja kartezjańska
* @param [in] tool Układ współrzędnych narzędzia
* @param [in] workpiece Układ współrzędnych przedmiotu obrabianego
* @param [in] exPos Pozycja osi rozszerzonej
* @param [out] jointPos Wyjście 8 zestawów kątów przegubów
* @return Kod błędu
*/
errno_t TCFToAllJoint(DescPose tcfPose, int tool, int workpiece, ExaxisPos exPos, std::vector<JointPos>& jointPos);

8.31. Przykład kodu pobierania 8 zestawów rozwiązań odwrotnych dla robota

int TestTCFToAllJoint()
{
    ROBOT_STATE_PKG pkg = {};
    FRRobot robot;
    robot.LoggerInit();
    robot.SetLoggerLevel(1);
    robot.SetReConnectParam(true, 30000, 500);
    int rtn = robot.RPC("192.168.58.2");
    if (rtn != 0)
    {
        return -1;
    }
    DescPose curTcp(0, 0, 0, 0, 0, 0);
    rtn = robot.GetActualTCPPose(1, &curTcp);
    if (rtn != 0)
    {
        return rtn;
    }
    std::cout << std::fixed << std::setprecision(3);
    std::cout << "cur TCF: x=" << curTcp.tran.x << ", y=" << curTcp.tran.y << ", z=" << curTcp.tran.z << ", "
        << "a=" << curTcp.rpy.rx << ", b=" << curTcp.rpy.ry << ", c=" << curTcp.rpy.rz << std::endl;
    ExaxisPos exPos(0, 0, 0, 0);
    std::vector<JointPos> allJoints(8, JointPos(0, 0, 0, 0, 0, 0));
    rtn = robot.TCFToAllJoint(curTcp, 0, 0, exPos, allJoints);
    if (rtn != 0)
    {
        return rtn;
    }
    for (int i = 0; i < 8; i++)
    {
        JointPos joint = allJoints[i];
        std::cout << "JOINT" << (i + 1) << ": "
            << "j1=" << joint.jPos[0] << ", j2=" << joint.jPos[1] << ", j3=" << joint.jPos[2] << ", "
            << "j4=" << joint.jPos[3] << ", j5=" << joint.jPos[4] << ", j6=" << joint.jPos[5] << std::endl;
        DescPose pos(0, 0, 0, 0, 0, 0);
        rtn = robot.GetForwardKin(&joint, &pos);
        if (rtn != 0)
        {
            continue;
        }
        std::cout << "POS" << (i + 1) << ": "
            << "x=" << pos.tran.x << ", y=" << pos.tran.y << ", z=" << pos.tran.z << ", "
            << "a=" << pos.rpy.rx << ", b=" << pos.rpy.ry << ", c=" << pos.rpy.rz << std::endl;
    }
    return 0;
}

8.32. Rozwiązanie prostej kinematyki

/**
* @brief  Rozwiązanie prostej kinematyki
* @param  [in] joint_pos Pozycja stawów
* @param  [out] desc_pos Pozy i orientacja kartezjańska
* @return  Kod błędu
*/
errno_t  GetForwardKin(JointPos *joint_pos, DescPose *desc_pos);

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

int TestInverseKin(void)
{
  ROBOT_STATE_PKG pkg = {};
  FRRobot robot;
  robot.LoggerInit();
  robot.SetLoggerLevel(1);
  int rtn = robot.RPC("192.168.58.2");
  if (rtn != 0)
  {
    return -1;
  }
  robot.SetReConnectParam(true, 30000, 500);
  JointPos j1(-11.904, -99.669, 117.473, -108.616, -91.726, 74.256);
  DescPose desc_pos1(-419.524, -13.000, 351.569, -178.118, 0.314, 3.833);
  JointPos inverseRtn = {};
  robot.GetInverseKin(0, &desc_pos1, -1, &inverseRtn);
  printf("dcs1 GetInverseKin rtn is %f %f %f %f %f %f \n", inverseRtn.jPos[0], inverseRtn.jPos[1], inverseRtn.jPos[2], inverseRtn.jPos[3], inverseRtn.jPos[4], inverseRtn.jPos[5]);
  robot.GetInverseKinRef(0, &desc_pos1, &j1, &inverseRtn);
  printf("dcs1 GetInverseKinRef rtn is %f %f %f %f %f %f \n", inverseRtn.jPos[0], inverseRtn.jPos[1], inverseRtn.jPos[2], inverseRtn.jPos[3], inverseRtn.jPos[4], inverseRtn.jPos[5]);
  uint8_t hasResut = 0;
  robot.GetInverseKinHasSolution(0, &desc_pos1, &j1, &hasResut);
  printf("dcs1 GetInverseKinRef result %d\n", hasResut);
  DescPose forwordResult = {};
  robot.GetForwardKin(&j1, &forwordResult);
  printf("jpos1 forwordResult rtn is %f %f %f %f %f %f \n", forwordResult.tran.x, forwordResult.tran.y, forwordResult.tran.z, forwordResult.rpy.rx, forwordResult.rpy.ry, forwordResult.rpy.rz);
  robot.CloseRPC();
  return 0;
}

8.34. Zapytanie o dane punktu nauczania w zarządzaniu nauczaniem robota

/**
 * @brief  Zapytanie o dane punktu nauczania w zarządzaniu nauczaniem robota
 * @param  [in]  name  Nazwa punktu
 * @param  [out]  data   Dane punktu
 * @return  Kod błędu
 */
errno_t  GetRobotTeachingPoint(char name[64], float data[20]);

8.35. Pobieranie wartości kompensacji parametrów DH robota

Dodane w wersji C++SDK-v2.1.1.0.

/**
* @brief Pobiera wartości kompensacji parametrów DH robota
* @param [out] dhCompensation Wartości kompensacji parametrów DH robota (mm) [cmpstD1, cmpstA2, cmpstA3, cmpstD4, cmpstD5, cmpstD6]
* @return Kod błędu
*/
errno_t GetDHCompensation(double dhCompensation[6]);

8.36. Pobieranie numeru seryjnego szafy sterowniczej

Dodane w wersji C++SDK-v2.2.1-3.8.1.

/**
* @brief Pobiera numer seryjny szafy sterowniczej
* @param [out] SNCode Numer seryjny szafy sterowniczej
* @return Kod błędu
*/
errno_t GetRobotSN(std::string& SNCode);

8.37. Przykład kodu zapytania o dane punktu nauczania w zarządzaniu nauczaniem robota

int TestGetTeachPoint(void)
{
  ROBOT_STATE_PKG pkg = {};
  FRRobot robot;
  robot.LoggerInit();
  robot.SetLoggerLevel(1);
  int rtn = robot.RPC("192.168.58.2");
  if (rtn != 0)
  {
    return -1;
  }
  robot.SetReConnectParam(true, 30000, 500);
  char name[64] = "P1";
  float data[20] = { 0 };
  rtn = robot.GetRobotTeachingPoint(name, data);
  printf(" %d name is: %s \n", rtn, name);
  for (int i = 0; i < 20; i++)
  {
    printf("data is: %f \n", data[i]);
  }
  int que_len = 0;
  rtn = robot.GetMotionQueueLength(&que_len);
  printf("GetMotionQueueLength rtn is: %d, queue length is: %d \n", rtn, que_len);
  double dh[6] = { 0 };
  int retval = 0;
  retval = robot.GetDHCompensation(dh);
  cout << "retval is: " << retval << endl;
  cout << "dh is: " << dh[0] << " " << dh[1] << " " << dh[2] << " " << dh[3] << " " << dh[4] << " " << dh[5] << endl;
  string SN = "";
  robot.GetRobotSN(SN);
  cout << "robot SN is " << SN << endl;
  robot.CloseRPC();
  return 0;
}

8.38. Pobieranie Układu Współrzędnych Narzędzia według ID

Zmienione w wersji C++SDK-v3.9.8.

/**
* @brief Pobiera układ współrzędnych narzędzia według ID
* @param [in] id Numer układu współrzędnych narzędzia
* @param [out] coord Wartości układu współrzędnych
* @param [out] type Typ narzędzia: 0-narzędzie; 1-czujnik
* @param [out] install Pozycja instalacji: 0-koniec robota; 1-na zewnątrz robota
* @param [out] toolID ID narzędzia
* @param [out] loadNo Numer obciążenia
* @return Kod błędu
*/
errno_t GetToolCoordWithID(int id, DescPose& coord, int& type, int& install, int& toolID, int& loadNo);

8.39. Pobieranie Układu Współrzędnych Przedmiotu według ID

Zmienione w wersji C++SDK-v3.9.8.

/**
* @brief Pobiera układ współrzędnych przedmiotu według ID
* @param [in] id Numer układu współrzędnych przedmiotu
* @param [out] coord Wartości układu współrzędnych
* @param [out] refFrame Referencyjny układ współrzędnych
* @return Kod błędu
*/
errno_t GetWObjCoordWithID(int id, DescPose& coord, int& refFrame);

8.40. Pobieranie Zewnętrznego Układu Współrzędnych Narzędzia według ID

Zmienione w wersji C++SDK-v3.9.8.

/**
* @brief Pobiera zewnętrzny układ współrzędnych narzędzia według ID
* @param [in] id Numer zewnętrznego układu współrzędnych narzędzia, 20-39 odpowiadają zewnętrznym układom współrzędnych narzędzi 0-19
* @param [out] coord Pozycja TCP stałego narzędzia zewnętrznego na robocie
* @param [out] tcoord Pozycja układu współrzędnych przedmiotu zamontowanego na końcówce robota
* @return Kod błędu
*/
errno_t GetExToolCoordWithID(int id, DescPose& coord, DescPose& tcoord);