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/JointStateControllerpublish_rate: 50

controllers.yamlADDED TO REALIZE GAZEBO CONTROLLER CONFIGURATION

Copy from universal robot package and robotiq package

arm_controller:type: position_controllers/JointTrajectoryControllerjoints:- shoulder_pan_joint- shoulder_lift_joint- elbow_joint- wrist_1_joint- wrist_2_joint- wrist_3_jointconstraints:goal_time: 0.6stopped_velocity_tolerance: 0.05shoulder_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.5state_publish_rate:  25action_monitor_rate: 10
joint_group_position_controller:type: position_controllers/JointGroupPositionControllerjoints:- shoulder_pan_joint- shoulder_lift_joint- elbow_joint- wrist_1_joint- wrist_2_joint- wrist_3_joint
gripper:type: position_controllers/JointTrajectoryControllerjoints:- gripper_finger1_jointconstraints:goal_time: 0.6stopped_velocity_tolerance: 0.05gripper_finger1_joint: {trajectory: 0.1, goal: 0.1}stop_trajectory_duration: 0.5state_publish_rate:  25action_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: armjoint_model_group_pose: home
# Settings for ros_control_boilerplate control loop
generic_hw_control_loop:loop_hz: 300cycle_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_jointsim_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/JointStateControllerpublish_rate: 50
controller_list:- name: arm_controlleraction_ns: follow_joint_trajectorydefault: Truetype: FollowJointTrajectoryjoints:- shoulder_pan_joint- shoulder_lift_joint- elbow_joint- wrist_1_joint- wrist_2_joint- wrist_3_joint- name: gripperaction_ns: follow_joint_trajectorydefault: Truetype: FollowJointTrajectoryjoints: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

解决方法

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

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

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