Robotics StackExchange | Archived questions

how to build a dual_arm system in Gazebo and MoveIt?

enter code here I tried to build a dual-arm system by iiwa. I downloaded code on Github, the name of package is: "iiwa_stack". Then, I wrote a xacro file to include the model of iiwa7 twice. And named them as "iiwa_Left" and "iiwa_Right" There is my xacro file:

<?xml version="1.0"?>
<robot name="dual_iiwa" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- Import Rviz colors -->
  <xacro:include filename="$(find iiwa_description)/urdf/materials.xacro" />
  <!--Import the lbr iiwa macro -->
  <xacro:include filename="$(find iiwa_description)/urdf/iiwa7.xacro"/>

  <!--<xacro:include filename="$(find iiwa_description)/urdf/iiwa.gazebo.xacro"/>-->

  <xacro:arg name="hardware_interface" default="PositionJointInterface"/>
  <!--Right arm-->
  <xacro:arg name="robotL_name" default="iiwaLeft"/>
  <xacro:arg name="originL_xyz" default="0 0.5 0.5"/>
  <xacro:arg name="originL_rpy" default="-1.57 0 0"/>
  <!--Left arm-->
  <xacro:arg name="robotR_name" default="iiwaRight"/>
  <xacro:arg name="originR_xyz" default="0 -0.5 0.5"/>
  <xacro:arg name="originR_rpy" default="1.57 0 0"/>

  <!-- Fix to world just for testing -->
  <link name="world"/>

  <!--Robot Body Link-->
  <link name="body_link">
    <inertial>
      <origin xyz="0 0 0.5" rpy="0 0 0" />
      <mass value="1" />
      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.03" />
    </inertial>
    <visual>
      <origin xyz="0 0 0.5" rpy="0 0 0" />
      <geometry>
        <box size="1 1 1" />
      </geometry>
      <material name="white">
        <color rgba="1 1 1 1"/>
      </material>
    </visual>
    <collision>
      <origin xyz="0 0 0.5" rpy="0 0 0" />
      <geometry>
        <box size="1 1 1" />
      </geometry>
    </collision>
  </link>

  <gazebo reference="body_link">
            <material>Gazebo/White</material>
  </gazebo>

  <joint name="body_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <parent link="world"/>
      <child link="body_link"/>
  </joint>

  <!--Left_iiwa-->
  <xacro:iiwa7 hardware_interface="$(arg hardware_interface)" robot_name="$(arg robotL_name)" parent="body_link">
    <origin xyz="$(arg originL_xyz)" rpy="$(arg originL_rpy)" />
  </xacro:iiwa7>

  <!--Right_iiwa-->
  <xacro:iiwa7 hardware_interface="$(arg hardware_interface)" robot_name="$(arg robotR_name)" parent="body_link">
    <origin xyz="$(arg originR_xyz)" rpy="$(arg originR_rpy)" />
  </xacro:iiwa7>

</robot>

Then, I wrote a new gazebo.launch file. You can see that, left and right arm use different control.launch file in different namespace. I think two arms should in different namespace. The file dual_iiwa_world.launch is same to the origin file "iiwa_world.launch", I just changed the model file. There is my gazebo.launch file.

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

    <arg name="hardware_interface" default="PositionJointInterface" />
    <arg name="robotL_name" default="iiwaLeft" />
    <arg name="robotR_name" default="iiwaRight" />
    <arg name="model" default="dual_iiwa"/>
    <arg name="trajectory" default="true"/>

    <!-- Loads the Gazebo world. -->
    <include file="$(find iiwa_gazebo)/launch/dual_iiwa_world.launch">
        <arg name="hardware_interface" value="$(arg hardware_interface)" />
        <arg name="model" value="$(arg model)" />
    </include>

    <!-- Spawn LEFT controllers - it uses a JointTrajectoryController -->
    <group  ns="$(arg robotL_name)" if="$(arg trajectory)">

        <include file="$(find iiwa_control)/launch/left_iiwa_control.launch">
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
            <arg name="controllers" value="joint_state_controller $(arg hardware_interface)_trajectory_controller" />
            <arg name="robot_name" value="$(arg robotL_name)" />
        </include>

    </group>

    <!-- Spawn LEFT controllers - it uses an Effort Controller for each joint -->
    <group ns="$(arg robotL_name)" unless="$(arg trajectory)">

        <include file="$(find iiwa_control)/launch/left_iiwa_control.launch">
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
            <arg name="controllers" value="joint_state_controller
                 $(arg hardware_interface)_J1_controller
                 $(arg hardware_interface)_J2_controller
                 $(arg hardware_interface)_J3_controller
                 $(arg hardware_interface)_J4_controller
                 $(arg hardware_interface)_J5_controller
                 $(arg hardware_interface)_J6_controller
                 $(arg hardware_interface)_J7_controller"/>
            <arg name="robot_name" value="$(arg robotL_name)" />
        </include>

    </group>

    <!-- Spawn RIGHT controllers - it uses a JointTrajectoryController -->
    <group  ns="$(arg robotR_name)" if="$(arg trajectory)">

        <include file="$(find iiwa_control)/launch/right_iiwa_control.launch">
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
            <arg name="controllers" value="joint_state_controller $(arg hardware_interface)_trajectory_controller" />
            <arg name="robot_name" value="$(arg robotR_name)" />
        </include>

    </group>

    <!-- Spawn RIGHT controllers - it uses an Effort Controller for each joint -->
    <group ns="$(arg robotR_name)" unless="$(arg trajectory)">

        <include file="$(find iiwa_control)/launch/right_iiwa_control.launch">
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
            <arg name="controllers" value="joint_state_controller
                 $(arg hardware_interface)_J1_controller
                 $(arg hardware_interface)_J2_controller
                 $(arg hardware_interface)_J3_controller
                 $(arg hardware_interface)_J4_controller
                 $(arg hardware_interface)_J5_controller
                 $(arg hardware_interface)_J6_controller
                 $(arg hardware_interface)_J7_controller"/>
            <arg name="robot_name" value="$(arg robotR_name)" />
        </include>

    </group>

</launch>

Next, I use moveitsetupassistance to set the dualarm model. I put two arms in different planning group. Then I put both of groups in one planning group as two subgroups.Like this. image description Finally, I imitate the origin moveitplanning_execution.launch, I wrote a new one.

<?xml version="1.0"?>
<launch>
    <!-- The planning and execution components of MoveIt! configured to run -->
    <!-- using the ROS-Industrial interface. -->

    <!-- |      PARAMETERS          | -->
    <!-- the "sim" argument controls whether we connect to a Simulated or Real robot. -->
    <arg name="sim" default="true" doc="If true, the robot will be simulated in Gazebo" />
    <!-- hardware_interface to use : PositionJointInterface, EffortJointInterface, VelocityJointInterface. -->
    <arg name="hardware_interface" default="PositionJointInterface"/>
    <!-- The is gonna have its nodes/topics under a namespace with the same name. -->
    <arg name="robot_name" default="dual_iiwa"/>
    <arg name="robotL_name" default="iiwaLeft"/>
    <arg name="robotR_name" default="iiwaRight"/>
    <!-- Model of the iiwa to use : iiwa7, iiwa14 -->
    <arg name="model" default="dual_iiwa"/>
    <arg name="rviz" default="true" />

    <!-- 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 dual_iiwa_moveit)/default_warehouse_mongo_db" />

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

     <arg name="use_gui" default="false" />

    <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
    <include file="$(find dual_iiwa_moveit)/launch/planning_context.launch">
        <arg name="load_robot_description" value="true"/>
    </include>

    <!-- Run the robot within a Gazebo simulation. -->
    <group if="$(arg sim)">

        <!-- Load Gazebo with given values -->
        <include file="$(find iiwa_gazebo)/launch/dual_iiwa_gazebo.launch">
            <arg name="hardware_interface" value="$(arg hardware_interface)"/>
            <arg name="robotL_name" default="iiwaLeft"/>
            <arg name="robotR_name" default="iiwaRight"/>
            <arg name="model" value="$(arg model)"/>
        </include>
    </group>

    <!-- Working with the real robot.  -->
    <group ns="$(arg robotL_name)" unless="$(arg sim)">

        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
            <param name="use_gui" value="$(arg use_gui)"/>
            <rosparam param="source_list">[$(arg robotL_name)/joint_states]</rosparam>
        </node>

        <!-- Load controllers accordingly to parameters -->
        <include file="$(find iiwa_control)/launch/left_iiwa_control.launch">
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
            <arg name="controllers" value="joint_state_controller $(arg hardware_interface)_trajectory_controller" />
            <arg name="robot_name" value="$(arg robotL_name)" />
        </include>

        <!-- Robot interface -->
        <include file="$(find iiwa_hw)/launch/left_iiwa_hw.launch" >
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
        </include>

    </group>

       <group ns="$(arg robotR_name)" unless="$(arg sim)">

        <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
            <param name="use_gui" value="$(arg use_gui)"/>
            <rosparam param="source_list">[$(arg robotR_name)/joint_states]</rosparam>
        </node>

        <!-- Load controllers accordingly to parameters -->
        <include file="$(find iiwa_control)/launch/right_iiwa_control.launch">
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
            <arg name="controllers" value="joint_state_controller $(arg hardware_interface)_trajectory_controller" />
            <arg name="robot_name" value="$(arg robotR_name)" />
        </include>

        <!-- Robot interface -->
        <include file="$(find iiwa_hw)/launch/right_iiwa_hw.launch" >
            <arg name="hardware_interface" value="$(arg hardware_interface)" />
        </include>

    </group>

    <group ns="dual_iiwa">

        <include file="$(find dual_iiwa_moveit)/launch/move_group.launch">
            <arg name="allow_trajectory_execution" value="true"/>
            <arg name="fake_execution" value="true"/>
            <arg name="info" value="true"/>
            <arg name="debug" value="$(arg debug)"/>
        </include>

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

</launch>

I had tried build two name space iiwaLeft and iiwaRight to include movegroup.launch, but it had seemed doesn't work. So i tried build one name space `dualiiwa`. But i didn't work all the same. The problem is that, I can plan and execute two arms in Rviz, but in Gazebo, dualarm didn't move. I think there may be something wrong with my namespace or I use the movegroup.launch in a wrong way.

Asked by FORGRESS on 2019-07-04 22:16:21 UTC

Comments

Did you solve this in the meantime? I would suggest using a single move_group with the two robots, and starting up only the robots in their own namespaces.

Asked by fvd on 2020-03-03 00:42:47 UTC

Answers