D-H注释

具有n个关节的串联机器人具有n+1个连杆。连杆的数字从相对于不可移动的基体(0)(通常被称作base_link)开始，依次增加到末端执行器连杆(n)。对于将第一个可移动的连杆连接到基体连杆上的关节来说，关节命名数字是从1开始的，并依次增加到n。因此，连杆(i)通过关节i连接到位于其近端的下一个连杆上，然后通过关节i+1连接到位于其远端的一个连杆上。

假设有一连杆(i)，那么与他相连的joint(i)和joint(i+1)分别对应的关节轴为Zi-1和Zi。假设有一连杆(i-1)，那么与他相连的joint(i-1)和joint(i)分别对应的关节轴为Zi-2和Zi-1。接下来我们基于标准的D-H标准方法，刚性地在位于关节i+1的每个连杆i上建立一个局部坐标系Bi。

1.zi轴与i+1关节轴对齐

毫无例外，所有的关节都可由一个z轴表述，表述总是从辨识Zi轴开始，Zi轴的正方向是任意的。对于旋转关节，便是其关节轴是很容易的，然而棱柱关节(即平动关节)可以选择任何轴平行于平动方向。也可以通过对齐Zi轴，在每个关节的任一边上的连杆对，以及每个连杆任一边上的两个关节来辨别。

尽管位于连杆末端的两个关节轴相对于连杆可以是倾斜的，但两个关节轴之间可以存在平行、垂直或正交的关系。两个关节轴线垂直交叉，那么两个关节轴是正交的。如果两个关节轴相对于其共法线是直角，那么这两个关节是垂直的。

2.Xi轴沿着Zi-1轴和Zi轴之间的公共法线所定义，从Zi-1轴指向Zi轴

一般来说，z轴可以是条斜线，然而总有一条直线于其他任何两条斜线相互垂直，我们称之为公共法线。公共法线长度是两条斜线之间最短的距离。

当两个z轴平行时，他们之间有无数个公共法线。在这种情况下，我们选择与前面关节的公共法线共线的直线作为公共法线。

当两个z轴相交时，他们之间时没有公共法线的。在这种情况下，在Zi-1×Zi所确定的方向上由这两轴所形成的平面，建立与该平面相垂直的Xi轴。

这里不对两个z轴共线做讨论。

3.用右手定则确定y轴，Yi=Zi×Xi

一般来说，我们可以将参考坐标系分配到每根连杆上，一边3个坐标轴中的一个坐标轴与远端关节轴对齐。

D-H坐标系可由ai、αi、θi、di四个参数来识别

1）连杆长度ai是指沿着Xi轴的Zi-1轴和Zi轴之间的距离。ai是连杆（i）的运动长度。

2）连杆扭(旋)转角αi是Zi-1轴绕着Xi轴所转动的角度，以使Zi-1轴平行于Zi轴。

3）关节距离di是指沿着Zi-1轴的Xi-1轴和Xi轴之间的距离。关节距离也成为连杆偏置值。

4）关节角θi是Xi-1轴绕着Zi-1轴所转动的角度，以使Xi-1轴平行于Xi轴。

参数θi和di被称为关节参数，因为他们确定了由关节i所连接的两相邻连杆的相对位置。在机器人设计中，每个关节都是旋转的或平移的。因此对于每个关节，参数θi和di是固定的，其他参数是可变的。

Moveit配置控制实际机械臂：

主要分为三步：

1. 使用solidworks的插件sw2urdf将模型转换为标准的功能包

2. 在本地ros环境安装moveit相关包，并使用 下面的指令进行配置，关于如何配置这里不做过多的描述。配置完成后，将会生成一个moveit相关的功能包

roslaunch moveit_setup_assistant setup_assistant.launch

3. 修改生成的launch文件,config文件，编写外部控制器与moveit交互的相关脚本。

这里讲述一下第三项。

主要修改这四个launch文件：

demo.launch 将<arg name="fake_execution_type" value="$(arg fake_execution_type)"/>注释，并使用<arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />

<include file="$(dirname)/move_group.launch">
        <arg name="allow_trajectory_execution" value="true" />
        <arg name="moveit_controller_manager" value="$(arg moveit_controller_manager)" />
        <!-- <arg name="fake_execution_type" value="$(arg fake_execution_type)"/> -->
        <arg name="info" value="true" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="pipeline" value="$(arg pipeline)" />
        <arg name="load_robot_description" value="$(arg load_robot_description)" />
</include>

move_group.launch

<!-- move_group settings -->下的 <!-- <arg name="fake_execution_type" default="interpolate"/> -->注释掉

ros_control_moveit_controller_manager.launch.xml 这里我们需要导入两个参数文件

<rosparam file="$(find xxx)/config/simple_moveit_controllers.yaml"/>

<rosparam file="$(find xxx)/config/ros_controllers.yaml"/>

trajectory_execution.launch.xml

<arg name="fake_execution_type" default="interpolate" />注释掉

config下的文件可以修改可以不修改，主要是controller相关的规划器的选择，格式如下：

controller_list:
  - name: mid_term_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: True
    joints:
      - joint_1
      - joint_2
      - joint_3
      - joint_4
      - joint_5
      - joint_6

ros_control_moveit_controller_manager.launch.xml中rosparam引入了什么controllers.yaml,就往里面填充controller_list。

这些参数我们主要关注于type，我们选择的FollowJointTrajectory类型，将会在求解出路径后，moveit将会发布出关节的trajectory,。rajectory包括从起点到终点，每一个采样点中的每一个关节的角度。

添加一个脚本，用于处理moveit规划器生成的trajectory。主要思路时创建一个server，规划器作为客户端

• 引入头文件，较为重要的有"actionlib/server/simple_action_server.h"，"control_msgs/FollowJointTrajectoryAction.h"，"moveit_msgs/RobotTrajectory.h"，我们要启动一个action_server，使用的消息是FollowJointTrajectoryAction。actionserver和普通server区别在于actionserver可以知道当前客户端发起请求完成的进度。

#include "ros/ros.h"
#include "actionlib/server/simple_action_server.h"
#include "control_msgs/FollowJointTrajectoryAction.h"
#include "std_msgs/Float32MultiArray.h"
#include <iostream>
#include "moveit_msgs/RobotTrajectory.h"
#include <sensor_msgs/JointState.h>
#include <thread>
using namespace std;
// 重命名类型为 Server
typedef actionlib::SimpleActionServer<control_msgs::FollowJointTrajectoryAction> Server;

创建一个类，声明两个函数对象，其中execute_callback函数是用于解析规划出来的路径点的服务回调函数，param()是用于更新机械臂的jointstate。

class action_ser
{
public:
    void execute_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goalPtr, Server *moveit_server);
    void param();
    ros::NodeHandle nh;
    moveit_msgs::RobotTrajectory moveit_tra;
    ros::Publisher pub,pub2;
    Server moveit_server;
    sensor_msgs::JointState joint_state;
    float joint_1, joint_2, joint_3, joint_4, joint_5, joint_6;
    action_ser(std::string name) : moveit_server(nh, name, boost::bind(&action_ser::execute_callback, this, _1, &moveit_server), false)
    {
        pub = nh.adve rtise<moveit_msgs::RobotTrajectory>("chatter", 10);
        pub2 = nh.advertise<sensor_msgs::JointState>("joint_states", 10);
        moveit_server.start();
    }
    ~action_ser(void)
    {
    }
};

void action_ser::execute_callback(const control_msgs::FollowJointTrajectoryGoalConstPtr &goalPtr, Server *moveit_server)
{
    // 1、解析提交的目标值
    int n_joints = goalPtr->trajectory.joint_names.size();
    int n_tra_Points = goalPtr->trajectory.points.size();

    moveit_tra.joint_trajectory.header.frame_id = goalPtr->trajectory.header.frame_id;
    moveit_tra.joint_trajectory.joint_names = goalPtr->trajectory.joint_names;
    moveit_tra.joint_trajectory.points.resize(n_tra_Points);

    for (int i = 0; i < n_tra_Points; i++) // 遍历每组路点
    {
        moveit_tra.joint_trajectory.points[i].positions.resize(n_joints);
        moveit_tra.joint_trajectory.points[i].velocities.resize(n_joints);
        moveit_tra.joint_trajectory.points[i].accelerations.resize(n_joints);

        moveit_tra.joint_trajectory.points[i].time_from_start = goalPtr->trajectory.points[i].time_from_start;
        for (int j = 0; j < n_joints; j++) // 遍历每组路点中的每个关节数据
        {
            moveit_tra.joint_trajectory.points[i].positions[j] = goalPtr->trajectory.points[i].positions[j];
            moveit_tra.joint_trajectory.points[i].velocities[j] = goalPtr->trajectory.points[i].velocities[j];
            moveit_tra.joint_trajectory.points[i].accelerations[j] = goalPtr->trajectory.points[i].accelerations[j];
        }
    }
    pub.publish(moveit_tra);
    moveit_server->setSucceeded();
}
void action_ser::param()
{
    ros::Rate r(10);
    while (ros::ok())
    {
        pub2.publish(joint_state);
        r.sleep();
    }
}
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "moveit_action_server");
    // 手动启动服务器
    action_ser my_ser("mid_term_controller/follow_joint_trajectory");
    std::thread recv(&action_ser::param, &my_ser);
    ros::spin();
    return 0;
}