鱼香ROS社区
    • 版块
    • 最新
    • 未解决
    • 已解决
    • 群组
    • 注册
    • 登录
    紧急通知:禁止一切关于政治&VPN翻墙等话题,发现相关帖子会立马删除封号
    提问前必看的发帖注意事项: 社区问答规则(小鱼个人)更新 | 高质量帖子发布指南

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

    已定时 已固定 已锁定 已移动
    机械臂力控板块
    机械臂力控 遥操作 数据采集
    1
    1
    69
    正在加载更多帖子
    • 从旧到新
    • 从新到旧
    • 最多赞同
    回复
    • 在新帖中回复
    登录后回复
    此主题已被删除。只有拥有主题管理权限的用户可以查看。
    • 24310882892
      晴
      最后由 编辑

      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

      b站“晴晴的机械臂”

      1 条回复 最后回复 回复 引用 0
      • 第一个帖子
        最后一个帖子
      皖ICP备16016415号-7
      Powered by NodeBB | 鱼香ROS