Moveit-轨迹优化

mvoeit轨迹周期不固定的问题，以及我们希望对moveit规划出来的轨迹进一步优化的项目需求

首先看一个关节角运动的python函数，通过这个函数我们可以实现机械臂从当前位姿运动到设定位姿的功能

#用于控制机械臂移动到目标位置，接受三个参数
#target_place：目标位置的关节角度列表，单位为度。这里将角度转换为弧度，并设置为机械臂的目标关节角度
#v：机械臂的速度缩放因子，用于设置最大速度的比例。
#a：机械臂的加速度缩放因子，用于设置最大加速度的比例。
    def arm_go(self, target_place, v, a):
        self.arm_group.set_joint_value_target(
            [target_place[0]*math.pi/180,
             target_place[1]*math.pi/180,
             target_place[2]*math.pi/180,
             target_place[3]*math.pi/180,
             target_place[4]*math.pi/180,
             target_place[5]*math.pi/180])
        self.arm_group.set_max_acceleration_scaling_factor(a)
        self.arm_group.set_max_velocity_scaling_factor(v)
        #实际使机械臂执行移动操作
        self.arm_group.go(wait=True)

我们通过上一篇中的方法来捕捉一下mvoeit到底做了什么，让这个函数最终实现到robot上，我们通过代码让robot从零位（对于UR5，就是平躺的那个姿态），到达给定的pose1

pose1 = [-9.38, -56.31, 94.33, -125.08, 276.39, 84.7]
self.arm_go(pose1,0.25,0.25)

关节速度以及关节加速度图上可以看到，由于我的joint_limit.yaml中关节最大速度是6.28，程序中的因子设定为0.25，途中紫色的线就可以看到最大速度被限制在了1.57。

我们看到mvoeit给这个目标一共设定了18个轨迹点，还是主要关注两个问题，一个是周期不定，一个是加速度存在突变，但是这个问题倒不是很严重。

还是参考上一篇文章那位大佬的帖子

使用 MoveIt 控制自己的真实机械臂【3】——优化轨迹发给真实机械臂执行_ros moveit 控制实际机械臂-CSDN博客

首先是下载对应的工业包

GitHub - ros-industrial/industrial_core at kinetic-devel

下载后修改ompl_planning_pipeline.launch.xml文件，主要是增加了

industrial_trajectory_filters/UniformSampleFilter

这一行代码，以及

sample_duration (float) — 两个路径点间的时间步进值（单位为秒）

<param name="sample_duration" value="0.1"/>

这里的sample_duration参数来适配合适的控制周期，这里的0.1对应了100ms的控制周期

<launch>

  <!-- OMPL Plugin for MoveIt! -->
  <arg name="planning_plugin" value="ompl_interface/OMPLPlanner" />

  <!-- The request adapters (plugins) used when planning with OMPL. 
       ORDER MATTERS -->
       <!--这里加了一个ind包-->
  <arg name="planning_adapters" value="
  industrial_trajectory_filters/UniformSampleFilter
               default_planner_request_adapters/AddTimeParameterization
				       default_planner_request_adapters/FixWorkspaceBounds
				       default_planner_request_adapters/FixStartStateBounds
				       default_planner_request_adapters/FixStartStateCollision
				       default_planner_request_adapters/FixStartStatePathConstraints" />

  <arg name="start_state_max_bounds_error" value="0.1" />

    <!-- 机械臂的控制周期是 100ms -->
  <param name="sample_duration" value="0.1"/>

  <param name="planning_plugin" value="$(arg planning_plugin)" />
  <param name="request_adapters" value="$(arg planning_adapters)" />
  <param name="start_state_max_bounds_error" value="$(arg start_state_max_bounds_error)" />

  <rosparam command="load" file="$(find ur5_moveit_config)/config/ompl_planning.yaml"/>

</launch>

这样就可以再次运行机器人运动的程序