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. 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