【睿尔曼-RealMan】睿尔曼超轻量仿人机械臂--透传简介与使用

一、透传简介

       透传是指用户自己使用上位机进行轨迹规划，然后将各关节的角度直接下发给控制器，不经过控制器的处理，各关节直接运行。机械臂运行的效果直接依赖于用户轨迹规划的水平。
      透传的主要应用场景有验证用户算法或者结合视觉，在非结构环境下做动态轨迹规划实现抓取或者避障。
      本账号涉及的示例项目网盘链接如下：
      链接：https://pan.baidu.com/s/1B_NBbbry_N-xUK83_ij5OA?pwd=exbr
      提取码：exbr

二、提供哪些透传接口

 1、角度透传

       功能描述:movej_canfd：角度通过CANFD透传给机械臂，不需控制器规划。

 2、功能备注   

       透传周期越快，控制效果越好、越平顺。
       基础系列WIFI和网口模式透传周期最快20ms，USB和RS485模式透传周期最快10ms。高速网口的透传周期最快也可到10ms，不过在使用该高速网口前，需要使用指令打开配置。
       另外I系列有线网口周期最快可达2ms。

 3、JSON协议示例及说明

发送内容：{"command":"movej_canfd","joint":[1000,0,20000,30000,0,20000]}
说明：角度透传到CANFD，目标关节角度：[1°,0°,20°,30°,0,20°]
返回值格式：{s:s,s: [i,i,i,i,i,i],s:i}
返回值示例：{"state":"joint_state ","joint":[10,20,30,40,50,60],"arm_err":0}
arm_err：若为0，则代表系统正常，指令正常运行；若为其他错误，则反馈相应错误代码，指令不执行。

 4、API接口说明及应用实例

/// \brief Movej_CANFD 角度不经规划，直接通过CANFD透传给机械臂
/// \param ArmSocket socket句柄
/// \param joint 关节1~7目标角度数组
/// \return 0-成功，失败返回:错误码, rm_define.h查询.
/// 只要控制器运行正常并且目标角度在可达范围内，机械臂立即返回成功指令，此时机械臂可能仍在运行；
/// 若有错误，立即返回失败指令。
RM_BASESHARED_EXPORT int Movej_CANFD(SOCKHANDLE ArmSocket, const float *joint);

2、位姿透传

        1、功能描述

         目标位姿透传给机械臂，不需控制器规划

         2、功能备注

         目标位姿透传到机械臂，控制器进行逆解后，若逆解存在并且逆解出的各角度与当前角度未有较大差值，则直接下发给关节执行，不再进行轨迹规划。适用于用户需要对位姿进行周期性调整的场景，如视觉伺服等。

       透传周期越快，控制效果越好、越平顺。
       基础系列WIFI和网口模式透传周期最快20ms，USB和RS485模式透传周期最快10ms。高速网口的透传周期最快也可到10ms，不过在使用该高速网口前，需要使用指令打开配置。
       另外I系列有线网口周期最快可达2ms。

         3、JSON协议示例及说明

发送内容：{"command":"movep_canfd","pose":[100000,200000,30000,400,500,600]}
说明：pose：目标位姿，位置精度：0.001mm，姿态精度：0.001rad
目标位置：x：0.1m，y:0.2m，z：0.03m
目标姿态：rx:0.4rad，ry:0.5rad，rz:0.6rad
目标位姿为当前工具在当前工作坐标系下的数值。
返回值格式：{s:s,s: [i,i,i,i,i,i],s: [i,i,i,i,i,i],s:i}
返回值示例：{"state":"pose_state","pose":[10,20,30,40,50,60],"joint":[10,20,30,40,50,60], "arm_err":0}
pose：当前位姿，位置精度：0.001mm，姿态精度：0.001rad；joint：当前关节角度，关节精度：0.001°；arm_err：若为 0，则代表系统正常，指令正常运行；若为其他错误，则反馈相应错误代码，指令不执行。

         4、API接口说明及应用实例

1.///
2./// \brief Movep_CANFD 位姿不经规划，直接通过CANFD透传给机械臂
3./// \param ArmSocket socket句柄
4./// \param pose 位姿
5./// \return 0-成功，失败返回:错误码, rm_define.h查询
6.///
7.RM_BASESHARED_EXPORT int Movep_CANFD(SOCKHANDLE ArmSocket, POSE pose);

 3、代码示例

#include "mainwindow.h"
#include "ui_mainwindow.h"
#include <chrono>
#include <thread>
#include <iostream>
#include <QDebug>

SOCKHANDLE m_sockhand;
MainWindow::MainWindow(QWidget *parent) :
    QMainWindow(parent),
    ui(new Ui::MainWindow)
{
    ui->setupUi(this);
}

MainWindow::~MainWindow()
{
    delete ui;
}

// 休眠(毫秒)
void sleep_cp(int milliseconds)
{
#ifdef _WIN32
    Sleep(milliseconds);
#endif

#ifdef __linux
    usleep(milliseconds * 1000);
#endif
}

// 透传接口回调函数
void MCallback(CallbackData data)
{
    qDebug() << "MCallback MCallback MCallback";
    // 判断接口类型
    switch(data.codeKey)
    {
        case MOVEJ_CANFD_CB: // 角度透传
            qDebug() << u8"透传结果:" << data.errCode;
            qDebug() << u8"当前角度:" << data.joint[0] << data.joint[1] << data.joint[2] << data.joint[3] << data.joint[4] << data.joint[5];
        break;
        case MOVEP_CANFD_CB: // 位姿透传
            qDebug() << u8"透传结果:" << data.errCode;
            qDebug() << u8"当前角度:" << data.joint[0] << data.joint[1] << data.joint[2] << data.joint[3] << data.joint[4] << data.joint[5];
            qDebug() << u8"当前位姿:" << data.pose.position.x << data.pose.position.y << data.pose.position.z << data.pose.euler.rx << data.pose.euler.ry << data.pose.euler.rz;
        break;
        case FORCE_POSITION_MOVE_CB: // 力位混合透传
            qDebug() << u8"透传结果:" << data.errCode << data.nforce << data.sys_err;
            qDebug() << u8"当前角度:" << data.joint[0] << data.joint[1] << data.joint[2] << data.joint[3] << data.joint[4] << data.joint[5];
            qDebug()<<u8"系统外部力"<< data.direction_force[0]<< data.direction_force[1] << data.direction_force[2] << data.direction_force[3] << data.direction_force[4] << data.direction_force[5];
        break;
    }

}

// 开始连接
void MainWindow::on_pushButton_Start_clicked()
{
    // 初始化API, 注册回调函数
    RM_API_Init(65, MCallback);

    // 连接服务器
    m_sockhand =  Arm_Socket_Start((char*)"192.168.1.20", 8080, 5000);
    qDebug() << "m_sockhand:" << m_sockhand;

    char* version;
    version = API_Version();
    qDebug() << "version:" << version;
}

// 常用接口使用示例
void MainWindow::on_pushButton_Test_clicked()
{

    int ret = -1;
    // 网络连接状态
    ret = Arm_Socket_State(m_sockhand);
    ui->textEdit->append(QString(u8"网络连接状态 Arm_Socket_State: [%1]").arg(ret));

    //清除关节错误代码
    ret = Set_Joint_Err_Clear( m_sockhand, 1, RM_BLOCK);
    ui->textEdit->append(QString(u8"清除关节错误代码 Set_Joint_Err_Clear: [%1]").arg(ret));

    //获取关节最大速度
    float fSpeeds[6] = {0,1,2,3,4,5};
    ret = Get_Joint_Speed(m_sockhand,fSpeeds);
    ui->textEdit->append(QString(u8"获取关节最大速度 Get_Joint_Speed: [%1] - Speed[%2, %3, %4, %5, %6, %7]")
                         .arg(ret).arg(fSpeeds[0]).arg(fSpeeds[1]).arg(fSpeeds[2]).arg(fSpeeds[3]).arg(fSpeeds[4]).arg(fSpeeds[5]));

    //获取关节最大加速度
    float fAccs[6] = {0,1,2,3,4,5};
    ret = Get_Joint_Acc(m_sockhand,fAccs);
    ui->textEdit->append(QString(u8"获取关节最大加速度 Get_Joint_Acc: [%1] - Acc[%2, %3, %4, %5, %6, %7]")
                         .arg(ret).arg(fAccs[0]).arg(fAccs[1]).arg(fAccs[2]).arg(fAccs[3]).arg(fAccs[4]).arg(fAccs[5]) );

    //获取关节最小限位
    float fMinJoint[7] = {0,1,2,3,4,5,6};
    ret = Get_Joint_Min_Pos(m_sockhand,fMinJoint);
    ui->textEdit->append(QString(u8"获取关节最小限位 Get_Joint_Min_Pos: [%1] - MinJoint[%2, %3, %4, %5, %6, %7]")
                         .arg(ret).arg(fMinJoint[0]).arg(fMinJoint[1]).arg(fMinJoint[2]).arg(fMinJoint[3]).arg(fMinJoint[4]).arg(fMinJoint[5]));

    //获取关节最大限位
    float fMaxJoint[7] = {0,1,2,3,4,5,6};
    ret = Get_Joint_Max_Pos(m_sockhand,fMaxJoint);
    ui->textEdit->append(QString(u8"获取关节最大限位 Get_Joint_Max_Pos: [%1] - MaxJoint[%2, %3, %4, %5, %6, %7]")
                         .arg(ret).arg(fMaxJoint[0]).arg(fMaxJoint[1]).arg(fMaxJoint[2]).arg(fMaxJoint[3]).arg(fMaxJoint[4]).arg(fMaxJoint[5]));

    //获取机械臂末端最大线速度
    float fLSpeed = 0;
    ret = Get_Arm_Line_Speed(m_sockhand,&fLSpeed);
    ui->textEdit->append(QString(u8"获取机械臂末端最大线速度 Get_Arm_Line_Speed: [%1] - LineSpeed:[%2]").arg(ret).arg(fLSpeed));

    //获取机械臂末端最大线加速度
    float fLAcc = 0;
    ret = Get_Arm_Line_Acc(m_sockhand,&fLAcc);
     ui->textEdit->append(QString(u8"获取机械臂末端最大线加速度 Get_Arm_Line_Acc: [%1] - fLAcc:[%2]").arg(ret).arg(fLAcc));
}

// 关闭连接
void MainWindow::on_pushButton_Close_clicked()
{
    // 关闭连接
    Arm_Socket_Close(m_sockhand);
    m_sockhand = -1;
}

// 画8字运动例程
void MainWindow::on_pushButton_move_clicked()
{
    // 回零位
    float joint[6] = {0,0,0,0,0,0};
    int ret = Movej_Cmd(m_sockhand,joint,20,0,0,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString("Movej_Cmd 1:[%1]").arg(ret));
        return;
    }
    // 移动到初始点位
    float joint1[6] = {18.44,-10.677,-124.158,-15,-45.131,-71.445};
    ret = Movej_Cmd(m_sockhand,joint1,20,0,0,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString("Movej_Cmd 2:[%1]").arg(ret));
        return;
    }

    // 画八字
    for(int i=0;i<3;i++)
    {
        Pose po1,po2,po3;
        po1.position.x = 0.186350; po1.position.y = 0.062099; po1.position.z = 0.2; po1.euler.rx = 3.141; po1.euler.ry = 0; po1.euler.rz = 1.569;
        po2.position.x = 0.21674; po2.position.y = 0.0925; po2.position.z = 0.2; po2.euler.rx = 3.141; po2.euler.ry = 0; po2.euler.rz = 1.569;
        po3.position.x = 0.20785; po3.position.y = 0.114; po3.position.z = 0.2; po3.euler.rx = 3.141; po3.euler.ry = 0; po3.euler.rz = 1.569;
        ret = Movel_Cmd(m_sockhand, po1, 20, 0,0, RM_BLOCK);
        if(ret != 0)
        {
            ui->textEdit->append(QString("Movel_Cmd 1:[%1]").arg(ret));
            return;
        }

        ret = Movec_Cmd(m_sockhand,po2,po3,20,0,0,0,RM_BLOCK);
        if(ret != 0)
        {
            ui->textEdit->append(QString("Movec_Cmd 1:[%1]").arg(ret));
            return;
        }

        Pose po4 ,po5,po6;
        po4.position.x = 0.164850;po4.position.y = 0.157;po4.position.z = 0.2;po4.euler.rx = 3.141; po4.euler.ry = 0;po4.euler.rz = 1.569;
        po5.position.x = 0.186350;po5.position.y = 0.208889;po5.position.z = 0.2; po5.euler.rx = 3.141;po5.euler.ry = 0; po5.euler.rz = 1.569;
        po6.position.x = 0.20785;po6.position.y = 0.157;po6.position.z = 0.2; po6.euler.rx = 3.141;po6.euler.ry = 0; po6.euler.rz = 1.569;
        ret = Movel_Cmd(m_sockhand,po4,20,0,0,1);
        if(ret != 0)
        {
            ui->textEdit->append(QString("Movel_Cmd 2:[%1]").arg(ret));
            return;
        }

        ret = Movec_Cmd(m_sockhand,po5,po6,20,0,0,0,RM_BLOCK);
        if(ret != 0)
        {
            ui->textEdit->append(QString("Movec_Cmd 2:[%1]").arg(ret));
            return;
        }
    }
    ui->textEdit->append(QString("cycle draw 8 deno:[%1]").arg(ret));
}

// 夹爪使用例程
void MainWindow::on_pushButton_gripper_clicked()
{
    float joint2[6]={0,0,0,0,0,0};
    float joint3[6]= {4.526,93.549,84,-8.894,-85.343,57.208};
    float joint4[6]= {4.61,93.551,75.276,-10.098,-76.508,57.224};
    float joint5[6]= {4.61,67.175,96.152,-10.385,-71.095,58.244};
    float joint6[6]= {-106.244,67.172,96.15,-10.385,-71.097,58.243};
    Movej_Cmd(m_sockhand,joint2,20,0,0,1);
    int ret = Movej_Cmd(m_sockhand,joint3,20,0,0,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString("gripper pick fault1:[%1]").arg(ret));
        return;
    }

    Set_Gripper_Release(m_sockhand,500,1);
    Movej_Cmd(m_sockhand,joint4,20,0,0,1);
    ret = Set_Gripper_Pick_On(m_sockhand,500,500,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString("gripper pick fault2:[%1]").arg(ret));
        return;
    }

    Movej_Cmd(m_sockhand,joint5,20,0,0,1);
    ret = Movej_Cmd(m_sockhand,joint6,20,0,0,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString("gripper pick fault3:[%1]").arg(ret));
        return;
    }
    ret = Set_Gripper_Release(m_sockhand,500,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString("gripper pick fault4:[%1]").arg(ret));
        return;
    }
    sleep_cp(200);
    ret = Movej_Cmd(m_sockhand,joint5,20,0,0,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString("gripper pick fault5:[%1]").arg(ret));
        return;
    }
    ui->textEdit->append(QString("gripper pick success:[%1]").arg(ret));
}

// 获取机械臂状态
void MainWindow::on_pushButton_state_clicked()
{
    // 获取工具坐标系
    FRAME tool;
    int ret = Get_Current_Tool_Frame(m_sockhand,&tool);
    if(ret != 0)
    {
        ui->textEdit->append(QString("Get_Current_Tool_Frame fault:[%1]").arg(ret));
        return;
    }
    ui->textEdit->append(QString(u8"工具坐标系名称:[%1]; ").arg(tool.frame_name.name) +
                         QString(u8"工具姿态:[%1,%2,%3,%4,%5,%6]; ").arg(tool.pose.position.x).arg(tool.pose.position.y).
                         arg(tool.pose.position.z).arg(tool.pose.euler.rx).arg(tool.pose.euler.ry).arg(tool.pose.euler.rz)+
                         QString(u8"重量:[%1]; ").arg(tool.payload)+ QString("质心:[%1,%2,%3]; ").arg(tool.x).arg(tool.y).arg(tool.z));

    // 角度角度运动
    float fl[6] = {0,0,0,0,90,0};
    ret = Movej_Cmd(m_sockhand,fl,20,0,0,RM_BLOCK);

    // 获取机械臂当前状态
    float joint[6] ; Pose po1; uint16_t Arm_Err; uint16_t Sys_Err;
    ret = Get_Current_Arm_State(m_sockhand,joint,&po1,&Arm_Err,&Sys_Err);
    if(ret != 0)
    {
        ui->textEdit->append(QString("Get_Current_Arm_State fault:[%1]").arg(ret));
        return;
    }
    ui->textEdit->append(QString(u8"关节角度:[%1,%2,%3,%4,%5,%6]; ").arg(joint[0]).arg(joint[1]).arg(joint[2]).arg(joint[3]).arg(joint[4]).arg(joint[5]) +
                         QString(u8"末端位姿:[%1,%2,%3,%4,%5,%6]; ").arg(po1.position.x).arg(po1.position.y).arg(po1.position.z).arg(po1.euler.rx).arg(po1.euler.ry).arg(po1.euler.rz)+
                         QString(u8"机械臂运行错误代码:[%1]; ").arg(Arm_Err)+ QString("控制器错误代码:[%1]; ").arg(Sys_Err));

    // 关节姿态运动
    ret = Movej_P_Cmd(m_sockhand,po1,20,0,0,RM_BLOCK);
    ret = Get_Current_Arm_State(m_sockhand,joint,&po1,&Arm_Err,&Sys_Err);
    if(ret != 0)
    {
        ui->textEdit->append(QString(u8"获取机械臂状态 Get_Current_Arm_State :[%1]").arg(ret));
        return;
    }
    ui->textEdit->append(QString(u8"关节角度:[%1,%2,%3,%4,%5,%6]; ").arg(joint[0]).arg(joint[1]).arg(joint[2]).arg(joint[3]).arg(joint[4]).arg(joint[5]) +
                         QString(u8"末端位姿:[%1,%2,%3,%4,%5,%6]; ").arg(po1.position.x).arg(po1.position.y).arg(po1.position.z).arg(po1.euler.rx).arg(po1.euler.ry).arg(po1.euler.rz)+
                         QString(u8"机械臂运行错误代码:[%1]; ").arg(Arm_Err)+ QString(u8"控制器错误代码:[%1]; ").arg(Sys_Err));
}

// 关节示教
void MainWindow::on_pushButton_Teach_clicked()
{
    // 示教
    byte num = 4; byte direction = 1; byte v = 20;
    int ret = Joint_Teach_Cmd(m_sockhand,num ,direction ,v,RM_BLOCK);
    sleep_cp(2000);
    ret = Teach_Stop_Cmd(m_sockhand,RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString(u8"关节示教 fault:[%1]").arg(ret));
        return;
    }
}

//透传力位混合控制补偿
void MainWindow::on_pushButton_clicked()
{
    int ret = -1;
    Stop_Force_Position_Move(m_sockhand,1);
    //初始位姿
    float joint[7] = {-61.449,36.747,64.543,0.37,79.383,-0.029,0};
    Movej_Cmd(m_sockhand, joint, 20, 0,0, RM_BLOCK);

    ret = Start_Force_Position_Move(m_sockhand, RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString( u8"开启透传力位混合补偿模式失败 :[%1]" ).arg(ret));
        return;
    }
    sleep_cp(1000);

    Pose nowpo;
    uint16_t Arm_Err,Sys_Err;
    Get_Current_Arm_State(m_sockhand, joint, &nowpo, &Arm_Err, &Sys_Err);

    //角度
//    for(int i=0;i<3;i++)
//    {
//        for (int j =0;j<500;j++)
//        {
//            joint[0]+=0.1;
//            ret = Force_Position_Move_Joint (m_sockhand,joint,1,0,2,-2);
//            sleep_cp(20);

//        }
//        for (int k =0;k<500;k++)
//        {
//            joint[0]-=0.1;
//            ret = Force_Position_Move_Joint (m_sockhand,joint,1,0,2,-2);
//            sleep_cp(20);
//        }
//    }

    //位姿
    for(int i=0;i<3;i++)
    {
        for (int j =0;j<100;j++)
        {
            nowpo.position.x+=0.0001;
            ret = Force_Position_Move_Pose(m_sockhand,nowpo,1,0,2,-5,true);
            sleep_cp(30);
        }
        for (int k =0;k<100;k++)
        {
            nowpo.position.x-=0.0001;
            ret = Force_Position_Move_Pose(m_sockhand,nowpo,1,0,2,-2,false);
            sleep_cp(30);
        }
    }

    ret = Stop_Force_Position_Move(m_sockhand, RM_BLOCK);
    if(ret != 0)
    {
        ui->textEdit->append(QString( u8"停止透传力位混合补偿模式失败 :[%1]" ).arg(ret));
        return;
    }

    ui->textEdit->append(QString( u8"透传力位混合补偿模式结束 :[%1]" ).arg(ret));
}

//透传
void MainWindow::on_pushButton_2_clicked()
{
    int ret = -1;

    float mJoint[7];
    Pose mPose;
    uint16_t mArmErr[7];
    uint16_t mSysErr[7];
    Get_Current_Arm_State(m_sockhand,mJoint, &mPose, mArmErr, mSysErr);
    ui->textEdit->append(QString("POSE: px:[%1] py:[%2] pz:[%3] rx[%4] ry:[%5] rz:[%6]")
                          .arg(mPose.position.x).arg(mPose.position.y).arg(mPose.position.z)
                          .arg(mPose.euler.rx).arg(mPose.euler.ry).arg(mPose.euler.rz));

    for(int i = 1;i<=100;i++)
    {
        mJoint[4]+=1;
        ret = Movej_CANFD(m_sockhand,mJoint,false, 0);
        sleep_cp(20);
    }
    ui->textEdit->append(QString(" Movej_CANFD: [%1]").arg(ret));

//     for(int i = 1;i<=100;i++)
//     {
//         mPose.position.z+=0.001;
//         ret = Movep_CANFD(m_sockhand,mPose,true);
//         sleep_cp(20);
//     }
//     ui->textEdit->append(QString("POSE: px:[%1] py:[%2] pz:[%3] rx[%4] ry:[%5] rz:[%6]")
//                           .arg(mPose.position.x).arg(mPose.position.y).arg(mPose.position.z)
//                          .arg(mPose.euler.rx).arg(mPose.euler.ry).arg(mPose.euler.rz));
}

//正逆解算法
void MainWindow::on_pushButton_ik_clicked()
{
    RobotType dmode = RM65;
    SensorType rbt_type = B;
    Algo_Init_Sys_Data(dmode,rbt_type);
    float joint[7] = {0, 20, 0, 0, 0, 0, 0};
//    setLwt(2);
    Pose now = Algo_Forward_Kinematics(joint);
    qDebug()<<"Now Flange: "<<now.position.x<<","<<now.position.y<<","<<now.position.z<<","<<now.euler.rx<<","<<now.euler.ry<<","<<now.euler.rz;
    Pose tar;
    float j[7]={0};
    int ret = Algo_Inverse_Kinematics(joint, &now, j,1);
    if (ret == 0)
    {
         qDebug()<<"Target Joint:"<<j[0]<<","<<j[1]<<","<<j[2]<<","<<j[3]<<","<<j[4]<<","<<j[5]<<","<<j[6];
        tar = Algo_Forward_Kinematics(j);
         qDebug() <<"Target Flange: "<<tar.position.x<<","<<tar.position.y<<","<<tar.position.z<<","<<tar.euler.rx<<","<<tar.euler.ry<<","<<tar.euler.rz;
    }
}

3、透传力位混合补偿

        1、功能描述

Force_Position_Move：用户周期性下发目标角度或者目标位姿，使用机械臂底层力位混合控制模块通过一维力传感器或者六维力传感器实现力位补偿。

         2、功能备注

备注 1：该功能只适用于一维力传感器和六维力传感器机械臂版本

备注 2：透传效果和周期、轨迹是否平滑有关，周期要求稳定，防止出现较大波动，用户使用该指令时请做好轨迹规划，轨迹规划的平滑程度决定了机械臂的运行状态。

透传周期越快，控制效果越好、越平顺。
基础系列WIFI和网口模式透传周期最快20ms，USB和RS485模式透传周期最快10ms。高速网口的透传周期最快也可到10ms，不过在使用该高速网口前，需要使用指令打开配置。
另外I系列有线网口周期最快可达2ms。

         3、JSON协议示例及说明

Force_Position_Move：透传力位混合补偿

pose：当前坐标系下目标位姿，位置精度：0.001mm，欧拉角表示姿态，姿态精度：0.001rad

pose_quat：表示当前坐标系下的目标位姿，位置精度：0.001mm，采用四元数方式表示姿态，姿态精度：0.000001

joint：目标关节角度，精度 0.001°

sensor：所使用传感器类型，0-一维力，1-六维力

mode：模式，0-沿工作坐标系，1-沿工具端坐标系

dir：力控方向，0~5 分别代表 X/Y/Z/Rx/Ry/Rz，其中一维力类型时默认方向为 Z 方向

force：力的大小，精度 0.1N 或者 0.1Nm

follow：表示驱动器的运动跟随效果，true 为高跟随，false 为低跟随。

六自由度机械臂关节角度方式或位姿（姿态欧拉角）方式{s:s,s:[i,i,

i,i,i,i],s:i,s:i,s:i,s:i,s:b}

七自由度机械臂关节角度方式或位姿（姿态四元数）方式{s:s,s:[i,i,

i,i,i,i,i],s:i,s:i,s:i,s:i,s:b}

示例：

位姿（姿态欧拉角）方式

{"command":"Force_Position_Move","pose":[100000,200000,30000,400,500,600],"sensor":0,"mode":0,"dir":0, "force":15,"follow":true}

位姿（姿态四元数）方式

{"command":"Force_Position_Move","pose_quat":[100000,200000,30000,400000,500000,600000,700000],"sensor":0,"mode":0,"dir":0, "force":15,"follow":true}

说明：透传目标位姿进行力位混合控制补偿

目标位置（欧拉角方式）：x：0.1m，y:0.2m，z：0.03m，Rx：0.4rad， Ry：0.5rad，Rz：0.6rad

目标位置（四元数方式）：x：0.1m，y:0.2m，z：0.03m，w：0.4， x：0.5，y：0.6，z：0.7

使用一维力沿工作坐标系进行 Z 方向补偿，力控为 Fz：1.5N

{"command":"Force_Position_Move","joint":[1000,2000,3000,4000,5000,6000],"sensor":0,"mode":0, "dir":0, "force":15}

说明：透传目标角度进行力位混合控制补偿

关节 1~6 目标角度：1°，2°，3°，4°，5°，6°

使用一维力沿工作坐标系进行 Z 方向补偿，力控为 Fz：1.5N

返回值：

规划成功——返回当前各关节角度和所使用力控方式的力或力矩，如果使用的是六维力，则也会返回全部方向上的力和力矩

{s:s,s:[i,i,i,i,i,i],s:i, s:i}或{s:s, s:[i,i,i,i,i,i], s:i, s:[i,i,i,i,i,i], s:i}

一维力：

{"state":"Force_Position_State","joint":[10,20,30,40,50,60],"force":-15, "arm_err":0}

当前关节 1~6 角度为 0.01°~0.06°，所受到的力或力矩为-1.5

六维力：

{"state":"Force_Position_State","joint":[10,20,30,40,50,60],"force":-15, "all_direction_force":[11,21,-15,41,51,61],"arm_err":0}

当前关节 1~6 角度为 0.01°~0.06°，力控方向所受到的力或力矩为-1.5N，所有方向的力或力矩为 X：1.1N，Y：2.1N，Z：-1.5N，Rx：4.1Nm，Ry：5.1Nm，Rz：6.1Nm

需要注意的是 I 系列机械臂不再提供返回值，可通过 UDP 状态主动上报接口采集机械臂实时状态。

规划失败——返回错误提示, {s:s,s:b}

{"command":"Force_Position_Move", "set_state":false}

备注：

透传开始的起点务必为机械臂当前位姿，否则可能会力控补偿失败

或者机械臂无法运动！

由于机械臂相关软件版本不定期更新，如果你使用的机械臂软件接口或协议与本文有出入，请联系官方技术人员及时更新。