7. 机器人状态查询

7.1. 获取机器人安装角度

1/**
2* @brief  获取机器人安装角度
3* @param  [out] yangle 倾斜角
4* @param  [out] zangle 旋转角
5* @return  错误码
6*/
7errno_t  GetRobotInstallAngle(float *yangle, float *zangle);

7.2. 获取系统变量值

1/**
2* @brief  获取系统变量值
3* @param  [in] id 系统变量编号，范围[1~20]
4* @param  [out] value  系统变量值
5* @return  错误码
6*/
7errno_t  GetSysVarValue(int id, float *value);

7.3. 获取当前关节位置(角度)

1/**
2* @brief  获取当前关节位置(角度)
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] jPos 六个关节位置，单位deg
5* @return  错误码
6*/
7errno_t  GetActualJointPosDegree(uint8_t flag, JointPos *jPos);

7.4. 获取当前关节位置(弧度)

自 C++SDK-v2.1.1.0 版本弃用.

1/**
2* @brief  获取当前关节位置(弧度)
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] jPos 六个关节位置，单位rad
5* @return  错误码
6*/
7errno_t  GetActualJointPosRadian(uint8_t flag, JointPos *jPos);

7.5. 获取关节反馈速度

1/**
2 * @brief  获取关节反馈速度-deg/s
3 * @param  [in] flag 0-阻塞，1-非阻塞
4 * @param  [out] speed 六个关节速度
5 * @return  错误码
6 */
7errno_t  GetActualJointSpeedsDegree(uint8_t flag, float speed[6]);

7.6. 获取关节反馈加速度

1/**
2 * @brief  获取关节反馈加速度-deg/s^2
3 * @param  [in] flag 0-阻塞，1-非阻塞
4 * @param  [out] acc 六个关节加速度
5 * @return  错误码
6 */
7errno_t  GetActualJointAccDegree(uint8_t flag, float acc[6]);

7.7. 获取TCP指令速度

1/**
2 * @brief  获取TCP指令速度
3 * @param  [in] flag 0-阻塞，1-非阻塞
4 * @param  [out] tcp_speed 线性速度
5 * @param  [out] ori_speed 姿态速度
6 * @return  错误码
7 */
8errno_t  GetTargetTCPCompositeSpeed(uint8_t flag, float *tcp_speed, float *ori_speed);

7.8. 获取TCP反馈速度

1/**
2 * @brief  获取TCP反馈速度
3 * @param  [in] flag 0-阻塞，1-非阻塞
4 * @param  [out] tcp_speed 线性速度
5 * @param  [out] ori_speed 姿态速度
6 * @return  错误码
7 */
8errno_t  GetActualTCPCompositeSpeed(uint8_t flag, float *tcp_speed, float *ori_speed);

7.9. 获取TCP指令速度

1/**
2 * @brief  获取TCP指令速度
3 * @param  [in] flag 0-阻塞，1-非阻塞
4 * @param  [out] speed [x,y,z,rx,ry,rz]速度
5 * @return  错误码
6 */
7errno_t  GetTargetTCPSpeed(uint8_t flag, float speed[6]);

7.10. 获取TCP反馈速度

1/**
2 * @brief  获取TCP反馈速度
3 * @param  [in] flag 0-阻塞，1-非阻塞
4 * @param  [out] speed [x,y,z,rx,ry,rz]速度
5 * @return  错误码
6 */
7errno_t  GetActualTCPSpeed(uint8_t flag, float speed[6]);

7.11. 获取当前工具位姿

1/**
2* @brief  获取当前工具位姿
3* @param  [in] flag  0-阻塞，1-非阻塞
4* @param  [out] desc_pos  工具位姿
5* @return  错误码
6*/
7errno_t  GetActualTCPPose(uint8_t flag, DescPose *desc_pos);

7.12. 获取当前工具坐标系编号

1/**
2* @brief  获取当前工具坐标系编号
3* @param  [in] flag  0-阻塞，1-非阻塞
4* @param  [out] id  工具坐标系编号
5* @return  错误码
6*/
7errno_t  GetActualTCPNum(uint8_t flag, int *id);

7.13. 获取当前工件坐标系编号

1/**
2* @brief  获取当前工件坐标系编号
3* @param  [in] flag  0-阻塞，1-非阻塞
4* @param  [out] id  工件坐标系编号
5* @return  错误码
6*/
7errno_t  GetActualWObjNum(uint8_t flag, int *id);

7.14. 获取当前末端法兰位姿

1/**
2* @brief  获取当前末端法兰位姿
3* @param  [in] flag  0-阻塞，1-非阻塞
4* @param  [out] desc_pos  法兰位姿
5* @return  错误码
6*/
7errno_t  GetActualToolFlangePose(uint8_t flag, DescPose *desc_pos);

7.15. 逆运动学求解

1/**
2* @brief  逆运动学求解
3* @param  [in] type 0-绝对位姿(基坐标系)，1-增量位姿(基坐标系)，2-增量位姿(工具坐标系)
4* @param  [in] desc_pos 笛卡尔位姿
5* @param  [in] config 关节空间配置，[-1]-参考当前关节位置解算，[0~7]-依据特定关节空间配置求解
6* @param  [out] joint_pos 关节位置
7* @return  错误码
8*/
9errno_t  GetInverseKin(int type, DescPose *desc_pos, int config, JointPos *joint_pos);

7.16. 逆运动学求解

1/**
2* @brief  逆运动学求解，参考指定关节位置求解
3* @param  [in] type 0-绝对位姿(基坐标系)，1-增量位姿(基坐标系)，2-增量位姿(工具坐标系)
4* @param  [in] desc_pos 笛卡尔位姿
5* @param  [in] joint_pos_ref 参考关节位置
6* @param  [out] joint_pos 关节位置
7* @return  错误码
8*/
9errno_t  GetInverseKinRef(int type, DescPose *desc_pos, JointPos *joint_pos_ref, JointPos *joint_pos);

7.17. 逆运动学求解

1/**
2* @brief  逆运动学求解，参考指定关节位置判断是否有解
3* @param  [in] type 0-绝对位姿(基坐标系)，1-增量位姿(基坐标系)，2-增量位姿(工具坐标系)
4* @param  [in] desc_pos 笛卡尔位姿
5* @param  [in] joint_pos_ref 参考关节位置
6* @param  [out] result 0-无解，1-有解
7* @return  错误码
8*/
9errno_t  GetInverseKinHasSolution(int type, DescPose *desc_pos, JointPos *joint_pos_ref, uint8_t *result);

7.18. 正运动学求解

1/**
2* @brief  正运动学求解
3* @param  [in] joint_pos 关节位置
4* @param  [out] desc_pos 笛卡尔位姿
5* @return  错误码
6*/
7errno_t  GetForwardKin(JointPos *joint_pos, DescPose *desc_pos);

7.19. 获取当前关节转矩

1/**
2* @brief 获取当前关节转矩
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] torques 关节转矩
5* @return  错误码
6*/
7errno_t  GetJointTorques(uint8_t flag, float torques[6]);

7.20. 获取当前负载的重量

1/**
2* @brief  获取当前负载的重量
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] weight 负载重量，单位kg
5* @return  错误码
6*/
7errno_t  GetTargetPayload(uint8_t flag, float *weight);

7.21. 获取当前负载的质心

1/**
2* @brief  获取当前负载的质心
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] cog 负载质心，单位mm
5* @return  错误码
6*/
7errno_t  GetTargetPayloadCog(uint8_t flag, DescTran *cog);

7.22. 获取当前工具坐标系

1/**
2* @brief  获取当前工具坐标系
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] desc_pos 工具坐标系位姿
5* @return  错误码
6*/
7errno_t  GetTCPOffset(uint8_t flag, DescPose *desc_pos);

7.23. 获取当前工件坐标系

1/**
2* @brief  获取当前工件坐标系
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] desc_pos 工件坐标系位姿
5* @return  错误码
6*/
7errno_t  GetWObjOffset(uint8_t flag, DescPose *desc_pos);

7.24. 获取关节软限位角度

1/**
2* @brief  获取关节软限位角度
3* @param  [in] flag 0-阻塞，1-非阻塞
4* @param  [out] negative  负限位角度，单位deg
5* @param  [out] positive  正限位角度，单位deg
6* @return  错误码
7*/
8errno_t  GetJointSoftLimitDeg(uint8_t flag, float negative[6], float positive[6]);

7.25. 获取系统时间

1/**
2* @brief  获取系统时间
3* @param  [out] t_ms 单位ms
4* @return  错误码
5*/
6errno_t  GetSystemClock(float *t_ms);

7.26. 获取机器人当前关节配置

1/**
2* @brief  获取机器人当前关节位置
3* @param  [out]  config  关节空间配置，范围[0~7]
4* @return  错误码
5*/
6errno_t  GetRobotCurJointsConfig(int *config);

7.27. 获取当前速度

1/**
2* @brief  获取机器人当前速度
3* @param  [out]  vel  速度，单位mm/s
4* @return  错误码
5*/
6errno_t  GetDefaultTransVel(float *vel);

7.28. 查询机器人运动是否完成

1/**
2* @brief  查询机器人运动是否完成
3* @param  [out]  state  0-未完成，1-完成
4* @return  错误码
5*/
6errno_t  GetRobotMotionDone(uint8_t *state);

7.29. 代码示例

 1#include <cstdlib>
 2#include <iostream>
 3#include <stdio.h>
 4#include <cstring>
 5#include <unistd.h>
 6#include "FRRobot.h"
 7#include "RobotTypes.h"
 8
 9using namespace std;
10
11int main(void)
12{
13    FRRobot robot;                 //实例化机器人对象
14    robot.RPC("192.168.58.2");     //与机器人控制器建立通信连接
15
16    float yangle, zangle;
17    int flag = 0;
18    JointPos j_deg, j_rad;
19    DescPose tcp, flange, tcp_offset, wobj_offset;
20    DescTran cog;
21    int id;
22    float torques[6] = {0.0};
23    float weight;
24    float neg_deg[6]={0.0},pos_deg[6]={0.0};
25    float t_ms;
26    int config;
27    float vel;
28
29    memset(&j_deg, 0, sizeof(JointPos));
30    memset(&j_rad, 0, sizeof(JointPos));
31    memset(&tcp, 0, sizeof(DescPose));
32    memset(&flange, 0, sizeof(DescPose));
33    memset(&tcp_offset, 0, sizeof(DescPose));
34    memset(&wobj_offset, 0, sizeof(DescPose));
35    memset(&cog, 0, sizeof(DescTran));
36
37    robot.GetRobotInstallAngle(&yangle, &zangle);
38    printf("yangle:%f,zangle:%f\n", yangle, zangle);
39
40    robot.GetActualJointPosDegree(flag, &j_deg);
41    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]);
42
43    robot.GetActualJointPosRadian(flag, &j_rad);
44    printf("joint pos rad:%f,%f,%f,%f,%f,%f\n", j_rad.jPos[0],j_rad.jPos[1],j_rad.jPos[2],j_rad.jPos[3],j_rad.jPos[4],j_rad.jPos[5]);
45
46    robot.GetActualTCPPose(flag, &tcp);
47    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);
48
49    robot.GetActualToolFlangePose(flag, &flange);
50    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);
51
52    robot.GetActualTCPNum(flag, &id);
53    printf("tcp num:%d\n", id);
54
55    robot.GetActualWObjNum(flag, &id);
56    printf("wobj num:%d\n", id);
57
58    robot.GetJointTorques(flag, torques);
59    printf("torques:%f,%f,%f,%f,%f,%f\n", torques[0],torques[1],torques[2],torques[3],torques[4],torques[5]);
60
61    robot.GetTargetPayload(flag, &weight);
62    printf("payload weight:%f\n", weight);
63
64    robot.GetTargetPayloadCog(flag, &cog);
65    printf("payload cog:%f,%f,%f\n",cog.x, cog.y, cog.z);
66
67    robot.GetTCPOffset(flag, &tcp_offset);
68    printf("tcp offset:%f,%f,%f,%f,%f,%f\n", tcp_offset.tran.x,tcp_offset.tran.y,tcp_offset.tran.z,tcp_offset.rpy.rx,tcp_offset.rpy.ry,tcp_offset.rpy.rz);
69
70    robot.GetWObjOffset(flag, &wobj_offset);
71    printf("wobj offset:%f,%f,%f,%f,%f,%f\n", wobj_offset.tran.x,wobj_offset.tran.y,wobj_offset.tran.z,wobj_offset.rpy.rx,wobj_offset.rpy.ry,wobj_offset.rpy.rz);
72
73    robot.GetJointSoftLimitDeg(flag, neg_deg, pos_deg);
74    printf("neg limit deg:%f,%f,%f,%f,%f,%f\n",neg_deg[0],neg_deg[1],neg_deg[2],neg_deg[3],neg_deg[4],neg_deg[5]);
75    printf("pos limit deg:%f,%f,%f,%f,%f,%f\n",pos_deg[0],pos_deg[1],pos_deg[2],pos_deg[3],pos_deg[4],pos_deg[5]);
76
77    robot.GetSystemClock(&t_ms);
78    printf("system clock:%f\n", t_ms);
79
80    robot.GetRobotCurJointsConfig(&config);
81    printf("joint config:%d\n", config);
82
83    robot.GetDefaultTransVel(&vel);
84    printf("trans vel:%f\n", vel);
85
86    return 0;
87}

7.30. 查询机器人错误码

1/**
2 * @brief  查询机器人错误码
3 * @param  [out]  maincode  主错误码
4 * @param  [out]  subcode   子错误码
5 * @return  错误码
6 */
7errno_t  GetRobotErrorCode(int *maincode, int *subcode);

7.31. 查询机器人示教管理点位数据

1/**
2 * @brief  查询机器人示教管理点位数据
3 * @param  [in]  name  点位名
4 * @param  [out]  data   点位数据
5 * @return  错误码
6 */
7errno_t  GetRobotTeachingPoint(char name[64], float data[20]);

7.32. 查询机器人运动队列缓存长度

1/**
2 * @brief  查询机器人运动队列缓存长度
3 * @param  [out]  len  缓存长度
4 * @return  错误码
5 */
6errno_t  GetMotionQueueLength(int *len);

7.33. 代码示例

在 C++SDK-v2.1.2.0 版本加入.

  1#include "libfairino/robot.h"
  2
  3//如果使用Windows，包含下面的头文件
  4#include <string.h>
  5#include <windows.h>
  6//如果使用linux，包含下面的头文件
  7/*
  8#include <cstdlib>
  9#include <iostream>
 10#include <stdio.h>
 11#include <cstring>
 12#include <unistd.h>
 13*/
 14
 15#include <chrono>
 16#include <thread>
 17
 18using namespace std;
 19
 20int main(void)
 21{
 22    FRRobot robot;
 23    robot.RPC("192.168.58.2");
 24
 25    uint8_t result = 0;
 26    int retval = 0;
 27    float joint_speed_deg[6] = {0};
 28
 29    retval = robot.GetActualJointSpeedsDegree(1, joint_speed_deg);
 30    printf("GetActualJointSpeedsDegree retval is: %d \n", retval);
 31    printf("joint degree speed is: %f, %f, %f, %f, %f, %f \n", joint_speed_deg[0], joint_speed_deg[1], joint_speed_deg[2], joint_speed_deg[3], joint_speed_deg[4], joint_speed_deg[5]);
 32
 33    float joint_acc_deg[6] = {0};
 34    retval = robot.GetActualJointAccDegree(1, joint_acc_deg);
 35    printf("GetActualJointAccDegree retval is: %d \n", retval);
 36    printf("joint degree acc is: %f, %f, %f, %f, %f, %f \n", joint_acc_deg[0], joint_acc_deg[1], joint_acc_deg[2], joint_acc_deg[3], joint_acc_deg[4], joint_acc_deg[5]);
 37
 38    float tcp_speed = 0;
 39    float ori_speed = 0;
 40    retval = robot.GetTargetTCPCompositeSpeed(1, &tcp_speed, &ori_speed);
 41    printf("GetTargetTCPCompositeSpeed retval is: %d \n", retval);
 42    printf("tcp_speed is:%f, ori_speed is: %f \n", tcp_speed, ori_speed);
 43
 44    retval = robot.GetActualTCPCompositeSpeed(1, &tcp_speed, &ori_speed);
 45    printf("GetActualTCPCompositeSpeed retval is: %d \n", retval);
 46    printf("tcp_speed is:%f, ori_speed is: %f \n", tcp_speed, ori_speed);
 47
 48    float targer_tcp_speed[6] = {0};
 49    retval = robot.GetTargetTCPSpeed(1, targer_tcp_speed);
 50    printf("GetTargetTCPSpeed retval is: %d \n", retval);
 51    printf("xyz is: %f, %f, %f; rpy is: %f, %f, %f\n", targer_tcp_speed[0], targer_tcp_speed[1], targer_tcp_speed[2], targer_tcp_speed[3], targer_tcp_speed[4], targer_tcp_speed[5]);
 52
 53    float actual_tcp_speed[6] = {0};
 54    robot.GetActualTCPSpeed(1, actual_tcp_speed);
 55    printf("GetActualTCPSpeed retval is: %d \n", retval);
 56    printf("xyz is: %f, %f, %f; rpy is: %f, %f, %f\n", actual_tcp_speed[0], actual_tcp_speed[1], actual_tcp_speed[2], actual_tcp_speed[3], actual_tcp_speed[4], actual_tcp_speed[5]);
 57
 58    JointPos j;
 59    DescPose desc_pos, offset_pos1, offset_pos2;
 60
 61    memset(&j, 0, sizeof(JointPos));
 62    memset(&desc_pos, 0, sizeof(DescPose));
 63    memset(&offset_pos1, 0, sizeof(DescPose));
 64    memset(&offset_pos2, 0, sizeof(DescPose));
 65
 66    j = {{-39.666, -96.491, -79.531, -94.251, 90.961, -58.714}};
 67    offset_pos1.tran.x = 10.0;
 68    offset_pos1.rpy.rx = -10.0;
 69    offset_pos2.tran.x = 30.0;
 70    offset_pos2.rpy.rx = -5.0;
 71
 72    retval = 0;
 73    retval = robot.GetForwardKin(&j, &desc_pos); // 只有关节位置的情况下，可用正运动学接口求解笛卡尔空间坐标
 74    printf("GetForwardKin ret is: %d \n", retval);
 75    printf("GetForwardKin result:%f,%f,%f,%f,%f,%f\n", desc_pos.tran.x, desc_pos.tran.y, desc_pos.tran.z, desc_pos.rpy.rx, desc_pos.rpy.ry, desc_pos.rpy.rz);
 76
 77    retval = 0;
 78    JointPos start_joint_pose;
 79    memset(&start_joint_pose, 0, sizeof(JointPos));
 80    retval = robot.GetInverseKinRef(0, &desc_pos, &j, &start_joint_pose);
 81    printf("GetInverseKinRef retval is: %d \n", retval);
 82    printf("joint is: %f, %f, %f,%f, %f, %f\n", start_joint_pose.jPos[0], start_joint_pose.jPos[1], start_joint_pose.jPos[2], start_joint_pose.jPos[3], start_joint_pose.jPos[4], start_joint_pose.jPos[5]);
 83
 84    retval = 0;
 85    retval = robot.GetInverseKinHasSolution(1, &offset_pos1, &j, &result); // 根据参考关节坐标，判断目标位姿是否有解
 86    printf("GetInverseKinHasSolution ret: %d\n", result);
 87    if (0 == result)
 88    {
 89        printf("pose1 can not be solved\n");
 90    }
 91
 92    retval = 0;
 93    retval = robot.GetInverseKin(1, &offset_pos1, -1, &start_joint_pose);
 94    printf("GetInverseKin retval is: %d \n", retval);
 95    printf("GetInverseKin result is: %f, %f, %f, %f, %f, %f\n", start_joint_pose.jPos[0], start_joint_pose.jPos[1], start_joint_pose.jPos[2], start_joint_pose.jPos[3], start_joint_pose.jPos[4], start_joint_pose.jPos[5]);
 96
 97    int main_code = 0;
 98    int sub_code = 0;
 99    retval = 0;
100    retval = robot.GetRobotErrorCode(&main_code, &sub_code);
101    printf("GetRobotMotionDone retval is: %d , amin code is:%d, sub code is: %d\n", retval, main_code, sub_code);
102
103    char name[64] = "F1";
104    float data[20] = {0};
105    int ret = robot.GetRobotTeachingPoint(name, data);
106    printf(" %d name is: %s \n", ret, name);
107    for (int i = 0; i < 20; i++)
108    {
109        printf("data is: %f \n", data[i]);
110    }
111
112    int que_len = 0;
113    retval = 0;
114    retval = robot.GetMotionQueueLength(&que_len);
115    printf("GetMotionQueueLength retval is: %d, queue length is: %d \n", retval, que_len);
116}