400f79ff-8a2f-4584-b099-ffe7dbc31462.jpeg
机械臂，遥操作+加力反馈，如何实现？
遥操作：通过手柄，手机，或者任何形式的“遥控器”遥控机械臂的运动。最简单的形式就是以一个机械臂（主臂）遥控另一个机械臂（从臂）。
力反馈：当从臂碰到障碍物时，主臂可以以力的形式感受到这个障碍物，所谓“感受”就是操作者感受到主臂的力。
代码：https://gitee.com/qingqing-gaq/feel_force.git
视频：https://www.bilibili.com/video/BV1KoEqzyEFR/?vd_source=36451ca805358187c4a0efa1c5694b35
我的实现思路：
软件：rso2
硬件：双通道usb转can模块，六个达妙4310电机。
六个电机，组成两个三自由度机械臂。一个can通道控制一个机械臂。
大体逻辑：
主臂时重力补偿模式，因为只有重力补偿才能任意拖动。主臂把实时的关节位置，发送给从臂，因此从臂必然是位置控制模式，这样才能跟踪主臂的位置。
那么主臂如何感受从臂的力呢？这里就需要电机有力矩接口了（达妙电机的mit模式），当从臂电机受到阻力时，pid控制力就会变大，电机反馈会的力也会变大，这个变大的值就是阻力了。把这个力反馈给主臂即可。
程序运行逻辑：
23467808-ec76-44af-b4b5-f339ae8f85d4.png
操作逻辑的代码：
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
