如何获取xArm的关节力矩和关节电流

本文示例所适用的软件版本如下: UFACTORY xArm Studio version: ≥ 1.6.0 UFACTORY xArm Firmware version: ≥ 1.6.0 UFACTORY xArm Python-SDK: ≥ 1.6.0 UFACTORY xArm C++-SDK: ≥ 1.6.0 当机械臂执行以下Blockly示例时，请执行附录1 的Python示例或附录2的C ++示例，以获取机械臂的关节力矩数据/关节电流数据。 通过连接控制器的30003端口，可以以100Hz的频率获取该数据。 Blockly示例

关节力矩图

关节电流图

附录1 当获取关节扭矩数据时，请下载 get_joint_torque.py 当获取关节电流数据时，请下载 get_joint_current.py 您可以通过设置接口set_report_tau_or_i（）来设置获得的数据类型 要获取关节扭矩数据时，单位：N·m：

arm.set_report_tau_or_i(tau_or_i=0)

要获取关节扭矩数据时，单位：N·m：

arm.set_report_tau_or_i(tau_or_i=1)

以下示例是获取关节扭矩数据的示例。

import socket
import struct
import os
import sys
import time

sys.path.append(os.path.join(os.path.dirname(__file__), '../../..'))

from xarm.wrapper import XArmAPI
arm = XArmAPI('192.168.1.106', debug=True)

def bytes_to_fp32(bytes_data, is_big_endian=False):
    """
    bytes to float
    :param bytes_data: bytes
    :param is_big_endian: is big endian or not，default is False.
    :return: fp32
    """
    return struct.unpack('>f' if is_big_endian else '<f', bytes_data)[0]

def bytes_to_fp32_list(bytes_data, n=0, is_big_endian=False):
    """
    bytes to float list
    :param bytes_data: bytes
    :param n: quantity of parameters need to be converted，default is 0，all bytes converted.
    :param is_big_endian: is big endian or not，default is False.
    :return: float list
    """
    ret = []
    count = n if n > 0 else len(bytes_data) // 4
    for i in range(count):
        ret.append(bytes_to_fp32(bytes_data[i * 4: i * 4 + 4], is_big_endian))
    return ret

def bytes_to_u32(data):
    data_u32 = data[0] << 24 | data[1] << 16 | data[2] << 8 | data[3]
    return data_u32

robot_ip = '192.168.1.106' # IP of controller
robot_port = 30003 # Port of controller

# create socket to connect controller
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
sock.setblocking(True)
sock.settimeout(1)
sock.connect((robot_ip, robot_port))

arm.set_report_tau_or_i(tau_or_i=0)

while True:
    data = sock.recv(4)
    length = bytes_to_u32(data)
    data += sock.recv(length - 4)
    joint_data = bytes_to_fp32_list(data[59:87]) #The acquired data is the data from J1 to J7
    print(joint_data)

附录2

include <sys/types.h> 
include <sys/socket.h> 
include <arpa/inet.h> 
include "xarm/wrapper/xarm_api.h" 

int connect_remote_server(const char *server_ip, int port_num) 
{ 
  struct sockaddr_in server_addr; 
  int client_sockfd = socket(AF_INET, SOCK_STREAM, 0); 
  server_addr.sin_family = AF_INET; 
  server_addr.sin_addr.s_addr = inet_addr(server_ip); 
  server_addr.sin_port = htons(port_num); 
  if(connect(client_sockfd, (struct sockaddr *)&server_addr, sizeof(server_addr))) 
  { 
    fprintf(stderr, "Error connecting remote server!\n"); 
    close(client_sockfd); 
    return -1; 
  } 
  return client_sockfd; 
} 

// Run Example: $ ./xArm_report_30003 192.168.1.227 1 
// Use "Ctrl-C" to terminate 
int main(int argc, char** argv) 
{ 
    // PAY ATTENTION to this notice 
    fprintf(stderr, "\nMake sure robot is already ENABLED! Firmware version >= 1.5.2!\n\n"); 
    if(argc!=3) 
    { 
        fprintf(stderr, "[Usage: ] argument 1: robot ip address (e.g. 192.168.1.227), argument 2: 0 for estimated torque report, 1 for actual motor current report\n"); 
        return -1; 
    } 
    // connect to xArm control port 
    XArmAPI *arm = new XArmAPI(argv[1]); 
    sleep_milliseconds(500); 
    int ret = 0; 
    std::string report_str; 
    if (arm->is_connected()) { 
        switch(atoi(argv[2])) 
        { 
            case 0: 
                ret = arm->set_report_tau_or_i(0); 
                report_str = "estimated joint torques:\n"; 
                break; 
            case 1: 
                ret = arm->set_report_tau_or_i(1); 
                report_str = "feedback motor currents:\n"; 
                break; 
            default: 
                fprintf(stderr, "[Error: ] Invalid argument 2, 0 for estimated torque report, 1 for actual motor current report\n"); 
                arm->disconnect(); 
                return -1; 
        } 
        arm->disconnect(); 
        unsigned char recv_chars[128] = {0}; 
        float data[7] = {0}; 
        int client_sock = connect_remote_server(argv[1], 30003); 
        if(client_sock!=-1) 
        { 
            while(true) 
            { 
                ret = recv(client_sock, recv_chars, 87, 0); 
                if(ret<0) 
                    break; 
                hex_to_nfp32(&recv_chars[59], data, 7); 
                fprintf(stderr, "%s", report_str.c_str()); 
                for(int i=0; i<7; i++) 
                    fprintf(stderr, "%f\t", data[i]); 
                fprintf(stderr, "\n" ); 
                sleep_milliseconds(2); 
            } 
        } 
        else 
        { 
            fprintf(stderr, "Please check the robot connection!\n"); 
            return -1; 
        } 
    } 
    return 0; 
}