在上一篇完成场景搭建后觉得自己对Moveit！还不是很熟悉，找了b站上古月居的机械臂开发原理的视频进行学习。教程分为三部分：
首先是写自己的urdf机械臂模型文件，主要包括<-link>连杆和连接两个连杆的<-joint>关节（有6中连接类型）。通常还是xacro格式，因为xacro中可以有一些编程语句，变量运算等。如果用ur等机械臂的话就可以使用其现成的模型文件。
有了模型文件后，可以通过rviz来查看该模型——将.xacro文件加载到robot_description的变量中（因为rviz中RobotModel插件订阅了robot_description），启动joint_state_publisher和robot_state_publisher节点（ROS提供）。
<launch>
<arg name="gui" default="true" />
<arg name="robot_desciption" default="$(find xacro)/xacro --inoder '$(find ur5_single_arm_turfs)/urdf/ur5_single_arm.urdf.xacro'"/>
<arg name="rvizconfig" default="$(find ur5_single_arm_turfs)/launch/ur5_single_arm_rviz.rviz" />
<param name="use_gui" value="$(arg gui)"/>
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>
joint_state_publisher：关节状态发布器。如六轴机械臂，每一个轴有自己的一个位置角度，这六个角度就是关节状态。即将关节状态通过Topic发布出来。
robot_state_publisher：订阅发布的关节状态Topic，将六个角度通过TF发布出来。可以理解为正向运动学（已知六个轴的角度参数，求六个轴的空间位置）
其中rvizconfig中.rviz文件是rviz中保存的参数文件，可在第一次设置机械臂各种参数（如选择fix_frame、添加RobotModel插件等）
同样，ur等机械臂已经完成了配置，在moveit_config文件夹中。如果是自己做的机械臂，通过以下步骤：
roslaunch moveit_setup_assistant setup_assistant.launch
主要步骤：
可以运行文件夹中demo.launch尝试是否配置成功
将创建的六轴机械臂放在gazebo环境中，通过Moveit！来控制机械臂做移动。我目前认知，只有在gazebo中仿真才需要完善模型等操作，而真实机械臂是不需要的。
同样，ur等机械臂已经完成了该步骤，如果需要自己设置，则参考以下步骤：
<gazebo reference="base_link">
<material>Gazebo/White</material>
</gazebo>
<xacro:include filename="$(find ur_description)/urdf/ur.transmission.xacro" />
该部分涉及ros_control的知识，可以参考https://www.guyuehome.com/890。如要自己写该部分，以elbow_joint为例，其他joint几乎一样：
<transmission name="${prefix}elbow_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="${prefix}elbow_joint">
<hardware_interface>hardware_interface/PositionJointInterface</hardware_interface>
</joint>
<actuator name="${prefix}elbow_motor">
<machanicalReduction>1</machanicalReduction>
</actuator>
</transmission>
<gazebo>
<plugin name="ros_control" filename="libgazebo_ros_control.so">
</plugin>
</gazebo>
<?xml version="1.0"?>
<launch>
<arg name="paused" default="true"/>
<arg name="gui" default="true"/>
<arg name="sim" default="true" />
<arg name="debug" default="false"/>
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<!--arg name="world_name" default="worlds/empty.world"/-->
<arg name="world_name" default="$(find ur5_single_arm_tufts)/worlds/ur5_cubes.world"/>
<arg name="paused" value="$(arg paused)"/>
<arg name="gui" value="$(arg gui)"/>
</include>
<!-- send robot urdf to param server -->
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur5_single_arm_tufts)/urdf/ur5_single_arm.urdf.xacro'"/>
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model robot -z 0.594
-J shoulder_lift_joint -2.0
-J elbow_joint 1.0"
output="screen" />
<!-- 同时设置了初始位姿 -->
</launch>
到这一步为止，运行上面代码的launch文件，可以在gazebo中看到机械臂模型，但是无法通过GUI rviz、C++、python等接口去发布信息控制机械臂运动。要控制机械臂运动还需要完善ros_control控制器。
参考https://www.guyuehome.com/890
在ur中，ur_gazebo/controller/arm_controller_ur5.yaml中有进行配置。
注：此时从步骤上分析/arm_controller_ur5.yaml中仅有joint_group_position_controller，并无arm_controller（其中的JointTrajectoryController为Moveit！和机械臂的接口，参考下面重要的图）
arm_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
goal_time: 0.6
stopped_velocity_tolerance: 0.05
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 10
joint_group_position_controller:
type: position_controllers/JointGroupPositionController
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint`在这里插入代码片`
然后在launch文件中：
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
<rosparam file="$(find robotiq_85_gazebo)/controller/gripper_controller_robotiq.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller gripper" respawn="false" output="screen"/>
（此时还没有joint_state_controller[等价于joint_state_publisher]和robot_state_publisher），所以在最终的launch文件中有：
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<?xml version="1.0"?>
<launch>
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" type="string" value="" />
</node>
<!-- Fake Calibration -->
<node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
args="pub /calibrated std_msgs/Bool true" />
<!-- joint_state_controller -->
<rosparam file="$(find ur_gazebo)/controller/joint_state_controller.yaml" command="load"/>
<node name="joint_state_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn joint_state_controller" respawn="false" output="screen"/>
</launch>
joint_state_controller.yaml文件中为：
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
此时相当于有三个控制器——joint_state_controller、joint_group_position_controller:（包含六个关节的position）、gripper，还未包含arm_controller中的JointTrajectoryController。/joint_states话题可以查看机械臂状态。
以上虽然让机械臂在gazebo中动起来了，但是到刚才的运动为止，没有任何Moveit！的操作。那Moveit！给我们的究竟是什么呢？——关节轨迹（JointTrajectory）
刚才配置的PositionController为针对每一个关节的输入（应该是位置？），该输入并不是轨迹。
如何将机器人控制Moveit！连接在一起，即Moveit！的输出和机器人的输入之间接口是什么。通过Action通信机制——FollowJointTrajectory传递Trajectory数据P[] V[] A[] T（位置、速度、角度、时间）组合起来描述一系列轨迹点。follow_joint_trajectory是MoveIt!最终运动规划发布的action消息，由机器人控制器端接收该消息后控制机器人完成运动。在rostopic list中，可以找到follow_joint_trajectory，由仿真机器人的控制器插件订阅。
下图十分重要，解释了Moveit！最终输出的为轨迹点，以及我们使用Moveit！需要配置的controller
此时从步骤上来说，即完成了ur_gazebo/controller/arm_controller_ur5.yaml中有关arm_controller的配置。
总的来说，此时完成了上图中插座端的JointTrajectoryController和JointStateController。现在gazebo可以接收轨迹，反馈状态，接下来则需要完成插头端Moveit！的Controller配置FollowJointTrajectory（ros中的Action），以便将轨迹发布出去。
在ur中，ur5_moveit_config/config/controllers.yaml中即完成了该配置，同时在ur5_moveit_config/launch/ur5_moveit_controller_manager.launch.xml中加载了该参数包。
controller_list:
- name: ""
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
接下来即可定义最上层的launch文件，将各类参数加载进来：
这一部分的套路就是先写controller的yaml参数文件，再在launch文件中rosparam load，最后通过controller_manager pkg载入。
通过编程控制机械臂做运动。
实际上通过编程接口控制机械臂运动经过了以下四步：
Moveit！中编程控制机械臂的主要步骤：
主要通过学习Moveit！官方教程来学习相应的API功能。
设置关节角度：主要API：
设置终端位姿：主要API：
笛卡尔路径运动
plan：规划出来的运动轨迹
fraction：描述规划成功的路径在给定路点列表中的覆盖率，如果fraction小于1，说明给定的路点列表没办法完整规划路径。
给轨迹加约束，如夹杯子，希望杯子保持朝上，则终端需要保持朝上。
Moveit！中运动规划的流程如下：
从左到右：
修改已经plan好的轨迹，也可以理解为自己加一些“约束”，通过修改结构体（轨迹）中的数据。需要自己事先实现相应的API。
以上即是博主结合课程和ur5机械臂的理解笔记，如有不当之处还望各位同行大佬多多指教。单Moveit！这一部分感觉还有运动学插件、规划场景没有讲到，接下来应该就是运动学插件、物体抓取与加入视觉了。
ps： 写了两天，第二天写完的时候都发布了，发现发布的只有第一天的内容，又要重新写，气死了