Franka Emika Panda连接真实机械臂（二）

虚拟环境下已经可以进行机械臂的拖动了，下一步就是PC连接机械臂，并通过plan控制机械臂运动。
前文【Franka Emika Panda连接真实机械臂（一）】已经提到如何配置机械臂环境、如何在rviz仿真环境下拖动机械臂。下面记录的是最近一周学习panda机器人的一些心得，记录下来以便于以后“温故而知新”。

PC与机械臂相连的准备

1.机械臂底座有灯，灯的颜色表示不同模式。
蓝色：准备好了并准备移动
白色：停止（此时可以拖动示教）
黄色：错误（刚刚给机器人上电是底座灯显示黄色）
2.PC与机械臂通过网线相连，此时应该注意的是，网线一端连PC,一端连接机械臂控制柜（而不是机械臂底座）。
3.PC端已经设置好静态ip,打开浏览器输入网址172.16.0.2，可以看到以下界面。
打开Joints下面的关节锁，franka底座灯变蓝。手持外部激活设备，开始franka panda的探索啦！

demo.launch控制franka机械臂的探索

首先，现在panda_moveit_config 的launch文件夹里创建一个demo_real.launch的文件，先把demo.launch文件的全部内容复制到demo_real.launch。我们就通过修改这个文件来尝试控制机械臂。
打开demo_real.launch文件，我们可以看到：

<include file="$(find panda_moveit_config)/launch/move_group.launch">
    <arg name="planner" value="$(arg planner)" />
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

此时fake_execution的value="true"，我们了解到此时进行的虚拟执行，也就是仿真，所以先把true改成false。
尝试运行了：

roslaunch panda_moveit_config demo_real.launch robot_ip:=172.16.0.2

结果显示：
Action client not connected。 这个问题困扰了我很久，也阅读了很多博客。比如：
【使用moveit！控制真实机械臂（3）——修改moveit配置文件来控制真实机械臂（9月27日更新）】
这里面提到了怎么修改moveit生成的功能包来控制真实机器人。所以我开始查看是否需要修改yaml文件等。上文所git 的功能包里包括了panda_controller.yaml等文件。
打开move_group.launch，

<include ns="move_group" file="$(find panda_moveit_config)/launch/trajectory_execution.launch.xml" if="$(arg allow_trajectory_execution)">
    <arg name="moveit_manage_controllers" value="true" />
    <arg name="moveit_controller_manager" value="panda" if="$(eval not arg('fake_execution') and not arg('load_gripper'))"/>
    <arg name="moveit_controller_manager" value="panda_gripper" if="$(eval not arg('fake_execution') and arg('load_gripper'))"/>
    <arg name="moveit_controller_manager" value="fake" if="$(arg fake_execution)"/>
  </include>

从这一段可以看出，fake_execution值为false时，moveit_controller_manager的值为panda，即真实机器人，没有错误。
那为什么控制还是不行呢？
【通过ROS控制真实机械臂(5)—Moveit!真实机械臂(move_group和demo.launch修改)】
原来是moveit作为client端，并没有接收到server端的两个节点的消息导致的。
打开panda_controllers.yaml，

 controller_list:
  - name: position_joint_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - panda_joint1
      - panda_joint2
      - panda_joint3
      - panda_joint4
      - panda_joint5
      - panda_joint6
      - panda_joint7

name：这里你可以写一个与你机器人相关的名称，方便你使用

action_ns:follow_joint_trajectory ，follow_joint_trajectory 是后续action的名称的一部分，你需要填一个action的名称，这个名称就在这个配置文件里确定了，就是name/action_ns所代表的对应字符串的组合。

type: FollowJointTrajectory ，这个类型是ros下的自带action类型，是一种控制机械臂运动轨迹的数据结构，请原样填写，将来你可能还想控制手抓之类的结构，所填写的类型都是不一样的。

joints：这里是你机器人（机械臂）的关节名称，这些名称源自你的机器人模型文件，我们的controllers.yaml文件复制于fake_controllers.yaml文件，这部分应该是自动生成的。
原文链接：https://blog.csdn.net/nmssg1/article/details/100665543
这里大家可以看这篇博客，对于action讲解的比较清楚：
【机器人学习必看系列：如何使用moveit控制真实机械臂？】
简单来说，通信就像接插头，moveit就像插头，已经准备好了，但是插板（action server）一直不在，这样当然不能通电了。

问题是想明白了，但是接下来该怎么解决呢?这又让人犯难了。
一些博主提出，可以自己编写一个节点文件，在运行launch文件之前，先运行这个节点，使position_joint_trajectory_controller处于畅通状态。但是本人也是刚刚接触机器人，franka又是七自由度机器人，花了几天时间也没有写出有用的节点文件。
这里先把之前找到的有用文件统计下来，以备后续学习。
【ROS中Moveit生成轨迹如何作用于实际的机械臂(一)】
【ROS中Moveit生成的轨迹如何作用与实际的机械臂(二)】
【ROS中Moveit生成的轨迹如何作用于实际的机器人(三)】
【使用moveit！控制真实机械臂（5）——编写真实机械臂节点中的action服务程序】
【Moveit实际的机械臂控制（5）moveit，编写action server节点和Arudion端的节点】
【ROS学习笔记16 —— 控制真实机械臂的action（实例大集合）】
【通过ROS控制真实机械臂(5)—Moveit!真实机械臂(move_group和demo.launch修改)】
【使用moveit！控制真实机械臂（4）——了解moveit！所使用的action】
这条路暂时是被封死了，找遍网络也没有几个使用ROS控制franka panda机械臂的教程，挠头皮（发际线后移buff）。

控制机械臂

1.测试机械臂联通情况

ifconfig

查看ip是否已经修改。

uname -r

查看linux内核是否为实时内核。

ping 172.16.0.2

查看PC是否能与机械臂通信。

cd ~/catkin_franka/libfranka/build/examples
./echo_robot_state 172.16.0.2
./communication_test 172.16.0.2

如果此通信测试未通过，则表示通信网络不足以控制机器人。
2.机械臂实时控制

roslaunch franka_example_controllers cartesian_impedance_example_controller.launch robot_ip:=172.16.0.2 load_gripper:=true

此时拖动rviz中的机械臂，真实机械臂会实时运动，所以一定要确保周边并无障碍物。

cd ~/catkin_franka/libfranka/build/examples
./communication_test 172.16.0.2

使用此命令，使机械臂恢复初始位置。
3.机械臂的plan与execution

roslaunch franka_control franka_control.launch robot_ip:=172.16.0.2
roslaunch panda_moveit_config panda_moveit.launch
roslaunch panda_moveit_config demo_real.launch

想了很久才想到这个方法，虽然感觉属于旁门左道了，但是经实测，确实可以实现机械臂的plan与execution。