moveit和gazebo的联合抓取仿真

moveit和gazebo的联合抓取仿真

部分代码来源: UR5+robotiq_85_gripper GAZEBO模拟视觉抓取平台仿真
我的代码仓库

GAZEBO

new ur5_gazebo.launch

<?xml version="1.0"?>
<launch>
    <arg name="limited" default="false"  doc="If true, limits joint range [-PI, PI] on all joints." />
    <arg name="paused" default="true" doc="Starts gazebo in paused mode" />
    <arg name="gui" default="true" doc="Starts gazebo gui" />

    <!-- startup simulated world -->
    <!-- ！！！！ only paused==true  "spawn_model -J  shoulder_lift_joint -2.0" valid ！！！！ -->
    <!-- should begin gazebo simulation firstly and roslaunch moveit_rviz, or rviz will be blocked -->
    <include file="$(find gazebo_ros)/launch/empty_world.launch">
        <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'"/>

    <!-- spawn gazebo model -->
    <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" />

    <include file="$(find ur5_single_arm_moveit_config)/launch/controller_utils.launch"/>

    <!-- start this controller -->
    <!-- Corresponding to the controllers.yaml -->
    <rosparam file="$(find ur5_single_arm_moveit_config)/config/controllers.yaml" command="load"/>
    <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller gripper" respawn="false" output="screen"/>

    <!-- load other controllers -->
    <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load joint_group_position_controller" />

</launch>

controller_utils.launch FIXED

Include joint_state_controller to publish /joint_states in gazebo and robot_state_publisher to subscribe /joint_states and publish /tf topic.

<?xml version="1.0"?>
<launch>

  <!-- Robot state publisher -->
  <!-- subscribe /joint_states and then publish /tf -->
  <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 robotiq_85_gazebo)/controller/joint_state_controller.yaml" command="load"/>
  <node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller" />
  
</launch>

joint_state_controller.yamlFIXED

joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50  

controllers.yamlADDED TO REALIZE GAZEBO CONTROLLER CONFIGURATION

Copy from universal robot package and robotiq package

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
gripper:
  type: position_controllers/JointTrajectoryController
  joints:
    - gripper_finger1_joint
  constraints:
    goal_time: 0.6
    stopped_velocity_tolerance: 0.05
    gripper_finger1_joint: {trajectory: 0.1, goal: 0.1}
  stop_trajectory_duration: 0.5
  state_publish_rate:  25
  action_monitor_rate: 10

roslaunch ur5_single_arm_moveit_config ur5_gazebo.launch # launch gazebo simulation

MOVEIT

ur5_moveit_planning_execution.launchADDED TO LAUNCH MOVEIT

<launch>
  <arg name="sim" default="true" />
  <arg name="debug" default="false" />
    
  <!-- ！！！！ Remap follow_joint_trajectory ！！！！ -->
  <remap if="$(arg sim)" from="/follow_joint_trajectory" to="/arm_controller/follow_joint_trajectory"/>
  
  <!-- Launch moveit -->
  <include file="$(find ur5_single_arm_moveit_config)/launch/move_group.launch">
    <arg name="debug" default="$(arg debug)" />
  </include>

  <include file="$(find ur5_single_arm_moveit_config)/launch/moveit_rviz.launch">
    <arg name="debug" value="false" />
    <arg name="config" value="true" />
  </include>
</launch>

ur5_moveit_controller_manager.launch.xmlMODIFIED TO SET CONTROLLER_LIST FOR 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)"/>

    <!--load controller_list-->
    <rosparam file="$(find ur5_single_arm_moveit_config)/config/ros_controllers.yaml"/>

</launch>

ros_controllers.yaml

# Simulation settings for using moveit_sim_controllers
moveit_sim_hw_interface:
  joint_model_group: arm
  joint_model_group_pose: home
# Settings for ros_control_boilerplate control loop
generic_hw_control_loop:
  loop_hz: 300
  cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
  joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
    - gripper_finger1_joint
  sim_control_mode: 1  # 0: position, 1: velocity
# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50
controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
  - name: gripper
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      gripper_finger1_joint

If the YAML file is missing, “No controller_list specified” will occur and MOVEIT cannot execute planned path.

# launch moveit
# note: should start the gazebo simulation!!!
roslaunch ur5_single_arm_moveit_config ur5_moveit_planning_execution.launch 

NOTE

• Gazebo implements the robot simulation function
  Publish the state of robot and Subscribe the instruction from planner move_group
• Controller name must be consistent in ros_controllers.yaml and controllers.yaml, or ERROR INFO: “Unable to identify any set of controllers that can actuate the specified joints”
• ERROR INFO: “**Could not load controller ‘arm_controller’ because controller type ‘position_controllers/JointTrajectoryController’ does not exist.
  **”
  Solution
  sudo apt-get install ros-melodic-joint-trajectory-controller
• ERROR INFO: “[gazebo-1] process has died”
  Solution
  echo " export SVGA_VGPU10=0" >> ~/.bashrc
  该错误主要是由于电脑性能较差

由于在安装anaconda3的时候，默认anaconda修改路径，会导致在编译某些cmake包的时候出现错误，如：

[100%] Linking CXX executable /home/zhao/catkin_ws2/devel/lib/find_object_2d/find_object_2d
/usr/lib/x86_64-linux-gnu/libopencv_imgcodecs.so.3.2.0: undefined reference to `TIFFReadDirectory@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFLastDirectory@LIBTIFF_4.0'
/usr/lib/x86_64-linux-gnu/libopencv_imgcodecs.so.3.2.0: undefined reference to `TIFFWriteEncodedStrip@LIBTIFF_4.0'
/usr/lib/x86_64-linux-gnu/libopencv_imgcodecs.so.3.2.0: undefined reference to `TIFFIsTiled@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFSwabArrayOfShort@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFIsByteSwapped@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFFlushData@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFFreeDirectory@LIBTIFF_4.0'
/usr/lib/x86_64-linux-gnu/libopencv_imgcodecs.so.3.2.0: undefined reference to `TIFFScanlineSize@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFWriteEncodedTile@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFWriteBufferSetup@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFTileSize@LIBTIFF_4.0'
/usr/lib/x86_64-linux-gnu/libopencv_imgcodecs.so.3.2.0: undefined reference to `TIFFRGBAImageOK@LIBTIFF_4.0'
/usr/lib/x86_64-linux-gnu/libopencv_imgcodecs.so.3.2.0: undefined reference to `TIFFClose@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFWriteRawStrip@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFSetTagExtender@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFGetFieldDefaulted@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFSwabArrayOfLong@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFTileSize64@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFReadRGBATileExt@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFStripSize@LIBTIFF_4.0'
//usr/lib/libgdal.so.20: undefined reference to `TIFFMergeFieldInfo@LIBTIFF_4.0'
/usr/lib/x86_64-linux-gnu/libopencv_imgcodecs.so.3.2.0: undefined reference to `TIFFSetWarningHandler@LIBTIFF_4.0'

以上是用catkin_make编译结果
或在CLION CMAKE过程中提示

runtime library [libQt5Network.so.5] in /usr/lib/x86_64-linux-gnu may be hidden by files

解决方法

1. 在安装conda的过程中，手动添加路径而不是自动添加

export PATH=/home/zhao/anaconda3/bin${PATH:+:${PATH}}

2. 注释conda路径（尝试不行），直接将conda库位置更名
   需要conda时再修改过来