How to connect the real robot's joint states to the current state of MoveIt motion planning?

asked 2021-11-30 03:13:41 -0600

mug gravatar image

updated 2021-11-30 20:03:50 -0600

After completing the MoveIt configuration of my custom robot with moveit_setup_assistant, I modified the source_list of joint_state_publisher in demo.launch to synchronize the joint state of the actual robot to joint_state_publisher.

<launch>

  <!-- specify the planning pipeline -->
  <arg name="pipeline" default="ompl" />

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find th6_moveit_config)/default_warehouse_mongo_db" />

  <!-- By default, we are not in debug mode -->
  <arg name="debug" default="false" />

  <!-- By default, we will load or override the robot_description -->
  <arg name="load_robot_description" default="true"/>

  <!-- Set execution mode for fake execution controllers -->
  <arg name="execution_type" default="interpolate" />

  <!--
  By default, hide joint_state_publisher's GUI

  MoveIt's "demo" mode replaces the real robot driver with the joint_state_publisher.
  The latter one maintains and publishes the current joint configuration of the simulated robot.
  It also provides a GUI to move the simulated robot around "manually".
  This corresponds to moving around the real robot without the use of MoveIt.
  -->
  <arg name="use_gui" default="false" />
  <arg name="use_rviz" default="true" />

  <!-- If needed, broadcast static tf for robot root -->


  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
    <rosparam param="source_list">[th6_joint_state]</rosparam>
  </node>

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />

  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find th6_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="execution_type" value="$(arg 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>

  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find th6_moveit_config)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
    <arg name="rviz_config" value="$(find th6_moveit_config)/launch/moveit.rviz"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

  <!-- If database loading was enabled, start mongodb as well -->
  <include file="$(find th6_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)">
    <arg name="moveit_warehouse_database_path" value="$(arg db_path)"/>
  </include>

</launch>

image description

The picture above is the screen after demo.launch is launched. The model marked with 1 shows the pose of the actual robot read by joint_state_pubsliher from my custom location update node. But when I use the MotionPlanning plugin to initiate a motion planning request, the current state of MoveIt has been maintained at the origin (model marked with 2), not the pose of the actual robot. How can I synchronize the pose of the actual robot with ... (more)

edit retag flag offensive close merge delete