机械臂力反馈遥操作的简易实现

晴

机械臂，遥操作+加力反馈，如何实现？

遥操作：通过手柄，手机，或者任何形式的“遥控器”遥控机械臂的运动。最简单的形式就是以一个机械臂（主臂）遥控另一个机械臂（从臂）。

力反馈：当从臂碰到障碍物时，主臂可以以力的形式感受到这个障碍物，所谓“感受”就是操作者感受到主臂的力。

代码：https://gitee.com/qingqing-gaq/feel_force.git

视频：https://www.bilibili.com/video/BV1KoEqzyEFR/?vd_source=36451ca805358187c4a0efa1c5694b35

我的实现思路：
软件：rso2
硬件：双通道usb转can模块，六个达妙4310电机。

六个电机，组成两个三自由度机械臂。一个can通道控制一个机械臂。

大体逻辑：
主臂时重力补偿模式，因为只有重力补偿才能任意拖动。主臂把实时的关节位置，发送给从臂，因此从臂必然是位置控制模式，这样才能跟踪主臂的位置。
那么主臂如何感受从臂的力呢？这里就需要电机有力矩接口了（达妙电机的mit模式），当从臂电机受到阻力时，pid控制力就会变大，电机反馈会的力也会变大，这个变大的值就是阻力了。把这个力反馈给主臂即可。
程序运行逻辑：

操作逻辑的代码：

        std::cout  << "JointPositionControl::execute..............." << std::endl;
        //从对应的can接收线程中读取电机状态，并更新到机械臂类储存关节状态的变量（下面会用到）中
        arm->update_current_joint_states();//一定要在get_current_joint_states()之前执行
        arm2->update_current_joint_states();//一定要在get_current_joint_states()之前执行
        //从机械臂类里返回储存关节状态的变量
        const auto &curret_joint_states = arm->get_current_joint_states();
        const auto &curret_joint_states2 = arm2->get_current_joint_states();
        //根据当前的关节状态，计算逆动力学，进行重力补偿。
        arm->computeInverseDynamics(arm->current_joint_positions, arm->current_joint_velocities, arm->current_joint_acceleration, arm->gravity_joint_tauqes);
        arm2->computeInverseDynamics(arm2->current_joint_positions, arm2->current_joint_velocities, arm2->current_joint_acceleration, arm2->gravity_joint_tauqes);

        std::vector<float> pos1(arm->getDof(), 0.0f);
        std::vector<float> pos2(arm->getDof(), 0.0f);
        std::vector<float> vel(arm->getDof(), 0.0f);
        std::vector<float> vel2(arm->getDof(), 0.0f);
        std::vector<float> tor(arm->getDof(), 0.0f);
        std::vector<float> tor2(arm2->getDof(), 0.0f);
        for (size_t i = 0; i < arm->getDof(); i++)
        {
            if(i==2)
            {//三号电机转向和模型相反，所以要单独加一个负号
                tor[i] = -arm->gravity_joint_tauqes(i);//发送给主臂的重力补偿力矩
                tor2[i] = -arm2->gravity_joint_tauqes(i);//发送给从臂的重力补偿力矩
            }
            else
            {
                tor[i] = arm->gravity_joint_tauqes(i);//发送给主臂的重力补偿力矩
                tor2[i] = arm2->gravity_joint_tauqes(i);//发送给主臂的重力补偿力矩
            }
            //主臂的关节速度，发送给从臂
            vel2[i] = 0.10 * curret_joint_states.at(i).velocity; // 系数越大手感越硬。
            //主臂的关节位置，发送给从臂
            pos2[i] = curret_joint_states.at(i).position;
        }

        float torque_threshold = 0.04f; // 设置阈值
        //从臂受到的阻力，返回给主臂
        for (size_t i = 0; i < arm->getDof(); i++)
        {
            if (std::abs(curret_joint_states2.at(i).torque) > torque_threshold)
            {
                tor[i] += -0.5 * (curret_joint_states2.at(i).torque - tor2[i]); // 如果超过阈值，按原逻辑计算
            }
            else
            {
                tor[i] += 0.0f; // 否则赋值为 0
            }
        }
        
 
        // pos2[0] = 0; tor2[0] = 0;

        arm->send_joint_states(pos1, vel, tor);//向主臂发送关节的状态
        arm2->send_joint_states(pos2, vel2, tor2);//向从臂发送关节的状态

其中：主臂位置发送给从臂的逻辑好理解。

            pos2[i] = curret_joint_states.at(i).position;
            //curret_joint_states.at(i).position：主臂的实时关节位置
            //pos2[i]：发送给从臂的关节位置

这一句是从臂的力矩反馈给主臂。有一步操作是从臂反馈的力矩减去了重力补偿力矩，这是因为，反馈的电机力矩是pid产生的。这个pid产生的控制力矩就包含重力力矩，如果把这个力也算上发送给主臂，那主臂就会变的很称，因为从臂把重力也传过来了。

tor[i] += -0.5 * (curret_joint_states2.at(i).torque - tor2[i]); 
//tor[i]： 发送给主臂的关节力矩
//curret_joint_states2.at(i).torque：从臂电机反馈会来的力矩
//tor2[i]：发送给从臂的重力补偿力矩。
//-0.5：缩放系数

这个if判断的作用是，避免从臂传来的力一直在小幅度波动，只有从臂反馈的力大于一个值，这个力才会被加载到主臂上。

        float torque_threshold = 0.04f; // 设置阈值
        //从臂受到的阻力，返回给主臂
        for (size_t i = 0; i < arm->getDof(); i++)
        {
            if (std::abs(curret_joint_states2.at(i).torque) > torque_threshold)
            {
                tor[i] += -0.5 * (curret_joint_states2.at(i).torque - tor2[i]); // 如果超过阈值，按原逻辑计算
            }
            else
            {
                tor[i] += 0.0f; // 否则赋值为 0
            }
        }

这又是一个神奇的操作。就是让从臂有期望速度。作用就是会影响操作手感。

vel2[i] = 0.10 * curret_joint_states.at(i).velocity; // 系数越大手感越硬。

ok这就是整个的实现逻辑，非常简陋。

程序框架简介：https://zwf9l1z0bm3.feishu.cn/docx/Ee3EdW8meoy79uxzBQVcXe11nfh?from=from_copylink
qq交流群：523255719