# 开始控制真实世界的机械臂 #######   我们的目的是使用moveit控制真实的机械臂，而我们真正需要的action是FollowJointTrajectoryAction，这个action是moveit留出来专门控制真实机器人的 。启动刚开始生成的moveit！配置文件中的demo.launch文件后，就能看见FollowJointTrajectoryAction，可以看一下control_msgs/FollowJointTrajectory Action的具体定义，它的内部有一个重要成员 trajectory，当你使用moveit规划了一个机械臂运动路径时，它记录了机械臂运动的各个轨迹点，每个轨迹点以机械臂关节角的形式表述，这个数据也是我们最终控制机械臂所想要的最终数据要想使用moveit来控制真实机械臂，我们需要修改配置文件夹下的几个文件，因为默认生成的moveit配置文件中，所使用的部分参数是针对虚拟机械臂的，可以在rviz环境下观察模型的运动，但真正的控制信号并不会发出来！但真正的控制信号并不会发出来！但真正的控制信号并不会发出来！重要的事情讲三遍   按照上一篇博客的说明,如下操作和修改，让move_group将控制信号发出来。   具体要修改以下几个地方：   ##1、demo.launch文件中参数fake_execution的值改为false  

  <arg name="fake_execution" value="false"/>

  2、修改moveit_controller_manager参数   move_group.launch文件中，moveit_controller_manager在选择参数值时，“unless”前面那个velue值要修改，写一个自己机器人名称作为前缀  

  <arg name="moveit_controller_manager" value="redwall_arm" unless="$(arg fake_execution)"/>

  moveit_controller_manager的参数是有选择的，要么等于“redwall_arm”，要么等于“fake”，这要取决于后面的参数fake_execution, 而这个参数我们上一步已经改为了false，即当前moveit_controller_manager应该等于“redwall_arm”接下来，这个参数会传递给trajectory_execution.launch.xml文件，该文件中有下面这句话,按照原先的设置，本应该启动fake_moveit_controller_manager.launch.xml，而现在redwall_arm_moveit_controller_manager.launch.xml将会被启动  

   <include file="$(find moveit)/launch/$(arg moveit_controller_manager)_moveit_controller_manager.launch.xml" />

  ##3、修改redwall_arm_moveit_controller_manager.launch.xml文件   双击打开这个文件，发现里面只写了一对launch标签，基本算是一个空文件，说明moveit早就帮你留好了位置，等着填充,之前已经填写完成了，如下修改  

<launch>
 
    <!-- Set the param that trajectory_execution_manager needs to find the controller plugin -->
    <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
    <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
 
    <arg name="use_controller_manager" default="true" />
    <param name="use_controller_manager" value="$(arg use_controller_manager)" />
 
    <!-- load controller_list -->
    <rosparam file="$(find redwall_arm_moveit_config)/config/controllers.yaml"/>
 
</launch>

4、创建controllers.yaml配置文件

  上一步的代码最后一句指向了config目录下一个叫做controller.yaml的文件，决定了你所使用的moveit控制器的基本参数。  

controller_list:
  - name: redwall_arm
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
    - Link1
    - Link2
    - Link3
    - Link4
    - Link5

5、解决真实机械臂与joint_states_publisher消息冲突的问题

  当控制真实机械臂时，moveit的配置文件中，demo.launch文件中关于joint_states_publisher的一段代码需要注释掉， 因为该程序也在发布机械臂关节状态，而且是一个模拟的状态，与真实机械臂状态是不相符的，若不注释掉该代码，rviz下显示的机械臂状态总是在不停的跳变,将原本的注释，使用真实的传感器发布的/joint_states来控制。  

  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="$(arg use_gui)"/>
    <!--rosparam param="/source_list">[/move_group/fake_controller_joint_states]</rosparam -->
    <rosparam param="/source_list">[/joint_states]</rosparam >
  </node>

  ##6、解决真实机械臂轨迹执行时间超时的问题。   moveit配置文件中有几个参数还需要添加上，否则后续真实机械臂执行轨迹时会报超时的错误。需要在move_group.launch文件中增加几个参数，来延长允许执行轨迹的时间。 找到<node name=”move_group”> 所对应的标签组，在其中加入以下参数  

<param name="trajectory_execution/allowed_execution_duration_scaling" value="6"/>      //允许轨迹执行时间的一个放大倍数，可以根据实际情况自行修改
    <param name="trajectory_execution/allowed_goal_duration_margin" value="0.5"/>            //超时的一个百分比范围
    或者加入下面的参数，直接关掉监视轨迹执行状况的一个monitoring
    <param name="trajectory_execution/execution_duration_monitoring" value="false"/>

  至此，moveit！已经修改完成，先启动试试看！！！  

$ roslaunch redwall_arm_moveit_config demo.launch 

  出现如下报错信息：  

[ INFO] [1564730909.628248268]: Constructing new MoveGroup connection for group 'arm_group' in namespace ''
[ INFO] [1564730910.984667942]: MoveItSimpleControllerManager: Waiting for redwall_arm/follow_joint_trajectory to come up
[ERROR] [1564730915.984849668]: MoveItSimpleControllerManager: Action client not connected: redwall_arm/follow_joint_trajectory
[ INFO] [1564730916.087709422]: Returned 0 controllers in list
[ INFO] [1564730916.107154984]: Trajectory execution is managing controllers

  奇怪，明明fake_controllers调用好好的，到真实的controllers咋就不行了，为啥没有action client连接呢。使用rostopic list 查看发布的消息     发现竟然没有找到follow_joint_trajectory消息，难道move_group没有发布吗？ 纠结了一天，终于找到原因了，之前虚拟机械臂有gazebo的ros_control插件自动帮我们获取了follow_joint_trajectory的动作action信息，现在到真实机器人了，需要我们自己写程序了。问题找到了，最重要的是要理解一点，move_group此时是follow_joint_trajectory的client端，动作消息是client端的move_group发布的，听起来有点绕，但是千万不要把报错信息的action client not connect搞混淆了，我们现在client是存在且被感知的(就是move_group)，而之所以报错是因为我缺少了action server端，我们需要添加一个server订阅这个action。   如下添加一个server节点启动即可  

#include <ros/ros.h>
#include <iostream>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <std_msgs/Float32MultiArray.h>
 
#include <sstream>
#include <stdlib.h>
#include <sys/types.h>
#include <stdio.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <signal.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <errno.h>
#include <pthread.h>
 
using namespace std;
 
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;
 
std::basic_string<char> Link1;
std::basic_string<char> Link2;
std::basic_string<char> Link3;
std::basic_string<char> Link4;
std::basic_string<char> Link5;
std::basic_string<char> Link6;
 
/* 目标位置 */
double cp1;
double cp2;
double cp3;
double cp4;
double cp5;
double cp6;
 
/* 目标位置 */
double tp1;
double tp2;
double tp3;
double tp4;
double tp5;
double tp6;
 
/* 收到action的goal后调用的回调函数 */
void execute(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal, Server* as)
{
	//ros::Rate rate(1);
 
    Link1 = goal->trajectory.joint_names[0];
    Link2 = goal->trajectory.joint_names[1];
    Link3 = goal->trajectory.joint_names[2];
    Link4 = goal->trajectory.joint_names[3];
    Link5 = goal->trajectory.joint_names[4];
    Link6 = goal->trajectory.joint_names[5];
 
    cp1 = goal->trajectory.points[0].positions[0];
    cp2 = goal->trajectory.points[0].positions[1];
    cp3 = goal->trajectory.points[0].positions[2];
    cp4 = goal->trajectory.points[0].positions[3];
    cp5 = goal->trajectory.points[0].positions[4];
    cp6 = goal->trajectory.points[0].positions[5];
 
    tp1 = goal->trajectory.points[1].positions[0];
    tp2 = goal->trajectory.points[1].positions[1];
    tp3 = goal->trajectory.points[1].positions[2];
    tp4 = goal->trajectory.points[1].positions[3];
    tp5 = goal->trajectory.points[1].positions[4];
    tp6 = goal->trajectory.points[1].positions[5];
 
	// 并且按照1hz的频率发布进度feedback
	// control_msgs::FollowJointTrajectoryFeedback feedback;
    //feedback = NULL;
    //as->publishFeedback(feedback);
 
    // 当action完成后，向客户端返回结果
    //printf("goal=[%f,%f,%f,%f,%f,%f]\n",tp1,tp2,tp3,tp4,tp5,tp6);
    ROS_INFO("Recieve action successful!");
    as->setSucceeded();
}
 
 
/* 主函数主要用于动作订阅和套接字通信 */
int main(int argc, char** argv)
{
	ros::init(argc, argv, "trajectory_server");
	ros::NodeHandle nh;
	// 定义一个服务器
	Server server(nh, "redwall_arm/follow_joint_trajectory", boost::bind(&execute, _1, &server), false);
	// 服务器开始运行
	server.start();
	ros::spin();
 
}
 

再次启动demo.launch和server节点：

    点击 plan and execute之后查看上述action内容：  

 $ rostopic echo /redwall_arm/follow_joint_trajectory/goal

##last补充、kinematic.yaml

  最近完成了伺服电机控制的机器人的程序调试,发现了之前缺少的一些步骤,比如刚开始生产的moveit_config包下的kinematic.yaml文件是空的,然后每次启动demo程序之后拖动marker小球之后,机器人无法选取目标位置和姿态,找了很久发现是这个问题.如下所示,其中arm_group是通过建模时设定的规划组.  

arm_group:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005
  kinematics_solver_attempts: 3

涉及到底层的控制：

  1、机械臂底层驱动已经完成。 预先完成好底层驱动控制部分，如之前的文章通过手柄控制的机械臂。   2、机械臂能够按照路点（轨迹）运动 这应该是一般机械臂的一项基本功能。这里的轨迹由一个个路点组成，路点可以理解为机械臂在空间中运动时必须通过的点，一个个路  点串起来就形成了轨迹。路点的数据结构一般是一组关节角度，比如5自由度机械臂，有5个独立关节，其确定了机械臂在空间的唯一位置，那么，就可以用 [ joint1，joint2，joint3，joint4，joint5] 来表示一个路点。因为moveit最终发出来的控制指令的实质就是一连串的路点，只要机械臂能够执行这一串路点 就达到了机械臂按照规划的轨迹运动的目的。   3、ros_control除了作为action的服务端，还需要通过joint_states话题发布自身的关节角move_group订阅了该话题，就能够获取机械臂当前的状态了，joint_states话题存储了机械臂的当前真实的关节角。因此，构造该话题时，要通过机械臂开放给用户的编程接口，一般机械臂都允许用户获取关节角的~   4、 关于action服务端写在哪的问题，action服务端的编写需要结合你自己的机械臂来进行，因为它要将moveit的控制指令翻译给机械臂的驱动,action的一个优点就是避免了繁杂的话题处理，使用action处理两个节点的连接时客户端向服务端发送请求，服务端会在相应的回调函数里通过参数直接得到这个请求数据，数据的传递过程被action这种机制隐藏了，虽然底层也是话题传递，但没必要再找话题订阅者或发布者了   5、 moveit配置的时候movegroup注意名称,因为trajectory_gen中使用的是arm_group，名称要统一   6、出现如下报错，将URDF中base_link的inertia属性去除即可  

The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF