Moveit控制真实机械臂

使用机械臂刚开始的时候都是先在仿真环境下进行我们的实验，后来由于工作的需要，需要我们去用代码利用Moveit去控制我们的真实机械臂。最近，作者就刚好遇到了这类问题，所以想记录一下，方便自己以后查阅也给后人铺路。
关于如何配置我们的文件。下面的链接有着详细的说明。这里就不多说废话了。
链接1: link
链接2: link
链接3: link
链接4: link

我想讲的是如何通过代码的形式去控制我们的机械臂，而不是通过Rviz界面的Motionplanning去移动机械臂然后Plan & Execute形式去规划控制我们的机械臂。因为实际操作中还是要通过代码的形式给定一个点让机械臂去完成运动规划。

机械臂代码控制的主程序

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

int main(int argc, char** argv)
{
  ros::init(argc, argv, "tzademo1");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  //机械臂的名称
  static const std::string PLANNING_GROUP = "gluon";

  
  moveit::planning_interface::MoveGroupInterface move_group_interface(PLANNING_GROUP);

  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  
  const moveit::core::JointModelGroup* joint_model_group =
      move_group_interface.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("dummy");
  visual_tools.deleteAllMarkers();

 
  visual_tools.loadRemoteControl();
  
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
  text_pose.translation().z() = 1.0;
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  visual_tools.trigger();

  
  ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group_interface.getPlanningFrame().c_str());

  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group_interface.getEndEffectorLink().c_str());

  ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
  std::copy(move_group_interface.getJointModelGroupNames().begin(),
            move_group_interface.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));

  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

  //设定一个目标点
  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.2;
  target_pose1.position.y = -0.2;
  target_pose1.position.z = 0.2;
  move_group_interface.setPoseTarget(target_pose1);

  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  //规划机械臂的轨迹
  bool success = (move_group_interface.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

  //执行机械臂轨迹运动
  move_group_interface.execute(my_plan.trajectory_);

  ros::shutdown();
  return 0;
}

主程序的launch文件

<launch>

  <node name="tzademo1" pkg="MyDemo" type="tzademo1" respawn="false" output="screen">
    <rosparam command="load" file="$(find gluon_moveit_config)/config/kinematics.yaml"/>
    <param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/>
    <rosparam command="load" file="$(find gluon_moveit_config)/config/ompl_planning.yaml"/>
  </node>

</launch>

下面是终端爆出来的一些信息，使用了OMPL算法库里的RRT算法来找寻我们的路径。