dual arm system

[jiye33]

Hi. Thank you for offering this great package! I'm trying to make dual arm system, but I don't know how could I do that. First, I tried to achieve it with launch file "panda_for_dual_test.launch", but it seems that mujoco_server only can publish joint_states of rightarm. I also tried to set group in launch file, but it doesn't work. I don't know how could I controller dual arm robot in this package. Can you recommend me some great way to achieve dual arm control in this package? If there is any source code or plugin do I need to modify, please let me know. I feel like somehow I need to modify franka_hw_mujoco.cpp but I'm not sure if it is necessary or not.

panda_for_dual_test.launch

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

  <arg name="urdf" default="$(find franka_description)/robots/panda/panda.urdf.xacro"/>
  <arg name="standalone"  default="true"  doc="Should a mujoco server instance be started?" />
  <arg name="sim_hw_file" default="$(find franka_mujoco)/config/franka_hw_sim.yaml"  doc="Path to sim hardware interface configuration" />
  <arg name="sim_controllers_file" default="$(find franka_mujoco)/config/sim_controllers.yaml"  doc="Path to sim controller definitions" />

  <!-- Robot Customization -->
  <arg name="rviz"        default="true"  doc="Should rviz be spawned?" />
  <arg name="arm_id"      default="panda" doc="Name of the panda robot to spawn" />
  <arg name="use_gripper" default="false"  doc="Should a franka hand be mounted on the flange?" />
  <arg name="controller"  default="position_joint_trajectory_controller"     doc="Which example controller should be started? (One of {cartesian_impedance,model,force,joint_position,joint_velocity}_example_controller)" />
  <arg name="xacro_args"  default=""      doc="Additional arguments to pass to panda.urdf.xacro" />

  <arg name="modelfile"            default="$(find franka_mujoco)/assets/dual_panda_notilt_nohuman.xml"                  doc="MuJoCo xml file to load. Should define robot model and world." />
  <arg name="interactive_marker"   default="$(eval arg('controller') == 'cartesian_impedance_example_controller')" doc="Should the interactive marker node be started?" />
  <arg name="initial_joint_states" default="$(find franka_mujoco)/config/initial_joint_states_dual.yaml"                doc="Location of param file containing the initial joint states" />
  <arg name="mujoco_plugin_config" default="$(find franka_mujoco)/config/default_mjplugin_config.yaml"             doc="Location of param file containing the mujoco plugin config" />
  <arg name="use_sim_time" doc="should mujoco publish its simulation time as ros time?"/>

  <!-- Needed for nesting this launchfile in test launchfiles -->
  <arg name="headless" default="false" />
  <arg name="unpause"  default="false" />
  <arg name="no_x"     default="false" />
  <arg name="verbose"  default="false" />

  <!-- Since robot position and orientation are defined by the xml file, these option currently do not change the robot position, but should match the position and orientation of the model file -->
  <arg name="x"           default="0"     doc="How far forward to place the base of the robot in [m]?" />
  <arg name="y"           default="0"     doc="How far leftwards to place the base of the robot in [m]?" />
  <arg name="z"           default="0"     doc="How far upwards to place the base of the robot in [m]?" />
  <arg name="roll"        default="0"     doc="How much to rotate the base of the robot around its X-axis in [rad]?" />
  <arg name="pitch"       default="0"     doc="How much to rotate the base of the robot around its Y-axis in [rad]?" />
  <arg name="yaw"         default="0"     doc="How much to rotate the base of the robot around its Z-axis in [rad]?" />

  <param name="/use_sim_time" value="$(arg use_sim_time)"/>

  <param name="robot_description"
         command="xacro $(arg urdf)
                  gazebo:=true
                  hand:=$(arg use_gripper)
                  arm_id:=$(arg arm_id)
                  xyz:='$(arg x) $(arg y) $(arg z)'
                  rpy:='$(arg roll) $(arg pitch) $(arg yaw)'
                  $(arg xacro_args)">
  </param>

  <rosparam file="$(arg sim_hw_file)" subst_value="true" />
  <rosparam file="$(arg sim_controllers_file)" subst_value="true" />

  <!-- <group if="$(arg standalone)">
    <rosparam file="$(arg mujoco_plugin_config)" subst_value="true" />
    <include file="$(find mujoco_ros)/launch/launch_server.launch" pass_all_args="true" >
      <arg name="console_config_file"  value="$(find franka_mujoco)/config/rosconsole.config" />
    </include>
  </group> -->

  <!-- spawns the controller after the robot was put into its initial joint pose -->
  <node pkg="controller_manager"
        type="spawner"
        name="$(arg arm_id)_controller_spawner"
        respawn="false"
        output="screen"
        args="--wait-for initialized franka_state_controller $(arg controller)"
  />

  <node name="$(arg arm_id)_robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  <node name="$(arg arm_id)_joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
    <rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
    <param name="rate" value="30"/>
  </node>

  <!-- Start only if cartesian_impedance_example_controller -->
  <node name="$(arg arm_id)_interactive_marker"
        pkg="franka_example_controllers"
        type="interactive_marker.py"
        if="$(arg interactive_marker)">
    <param name="link_name" value="$(arg arm_id)_link0" />
    <remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />
  </node>

  <node  pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz" if="$(arg rviz)"/>

</launch>

dual_launch_test.launch

<launch>

    <arg name="ns"                   default=""      doc="namespace" />
    <arg name="verbose"              default="false" doc="Whether more debug output should be printed." />
    <arg name="unpause"              default="false" doc="Whether the simulation should be unpaused on start." />
    <arg name="headless"             default="false" />
    <arg name="render_offscreen"     default="true"  doc="Whether offscreen rendering should be enabled." />
    <arg name="no_x"                 default="false" doc="Set to true to enable running on a server without X, disabling everything related to OpenGL rendering."/>
    <arg name="eval_mode"            default="false" doc="Whether to run mujoco_ros in evaluation mode." />
    <arg name="admin_hash"           default="''"    doc="Hash to verify critical operations in evaluation mode." />
    <arg name="debug"                default="false" doc="Whether to run with gdb." />
    <arg name="debug_server"         default="false" doc="Whether to run with gdbserver on port 1234" />
    <arg name="valgrind"             default="false" doc="Whether to run with valgrind" />
    <arg name="valgrind_args"        default=""      doc="arguments for valgrind" />
    <arg name="wait_for_xml"         default="false" doc="Whether mujoco_ros should wait for an xml in the parameter server." />
    <arg name="realtime"             default=""      doc="Fraction of desired realtime (0,1]. -1 to run as fast as possible." />
    <arg name="profile"              default="false" doc="Whether mujoco_ros should be profiled." />
    <arg name="num_sim_steps"        default="-1" />
    <arg name="mujoco_plugin_config" default="$(find franka_mujoco)/config/default_mjplugin_config.yaml"      doc="Optionally provide the path to a yaml with plugin configurations to load." />
    <arg name="mujoco_threads"       default="1"     doc="Number of threads to use in the MuJoCo simulation." />

    <arg name="modelfile"            default="$(find franka_mujoco)/assets/dual_panda_notilt_nohuman.xml"        doc="MuJoCo xml file to load. Should define robot model and world." />
    <arg name="initial_joint_states" default="$(find franka_mujoco)/config/initial_joints_dual.yaml" doc="Provide a filepath containing initial joint states to load." />
    <arg name="console_config_file"  default="$(find mujoco_ros)/config/rosconsole.config"         doc="Path to ROS console config used when verbose logging is active." />

    <arg name="use_sim_time" />

    <param name="/use_sim_time" value="$(arg use_sim_time)"/>

    <env name="ROSCONSOLE_FORMAT" value="[${severity}] [${time}] [${logger}] [${node}] [${function}]: ${message}"/>
    <env if="$(arg verbose)" name="ROSCONSOLE_CONFIG_FILE"
        value="$(arg console_config_file)"/>

    <group if="$(eval arg('mujoco_plugin_config') != '')">
        <rosparam file="$(arg mujoco_plugin_config)" subst_value="true" />
    </group>

    <node pkg="mujoco_ros" type="mujoco_node" name="mujoco_server" output="screen" args="--admin-hash $(arg admin_hash)" >
        <param name="ns"                   value="$(arg ns)" />
        <param name="unpause"              value="$(arg unpause)" />
        <param name="headless"             value="$(arg headless)" />
        <param name="render_offscreen"     value="$(arg render_offscreen)" />
        <param name="no_x"                 value="$(arg no_x)" />
        <param name="num_steps"            value="$(arg num_sim_steps)" />
        <param name="eval_mode"            value="$(arg eval_mode)" />
        <param name="modelfile"            value="$(arg modelfile)" />
        <param name="wait_for_xml"         value="$(arg wait_for_xml)" />
        <param name="realtime"             value="$(arg realtime)" />
        <param name="num_mj_threads"       value="$(arg mujoco_threads)" />
        <rosparam file="$(arg initial_joint_states)" subst_value="true" />
    </node>

    <include file="$(find franka_mujoco)/launch/panda_for_dual_test.launch" >
        <arg name="use_sim_time" value="$(arg use_sim_time)" />
        <arg name="arm_id" value="leftpanda" />
        <arg name="x" value="0.0" />
        <arg name="y" value="0.5" />
        <arg name="z" value="0.0" />
        <arg name="roll" value="0.0" />
        <arg name="pitch" value="0.0" />
        <arg name="yaw" value="0.0" />
        <arg name="rviz" value="false" />
    </include>

    <include file="$(find franka_mujoco)/launch/panda_for_dual_test.launch" >
        <arg name="use_sim_time" value="$(arg use_sim_time)" />
        <arg name="arm_id" value="rightpanda" />
        <arg name="x" value="0.0" />
        <arg name="y" value="-0.5" />
        <arg name="z" value="0.0" />
        <arg name="roll" value="0.0" />
        <arg name="pitch" value="0.0" />
        <arg name="yaw" value="0.0" />
        <arg name="rviz" value="false" />
    </include>

    <arg name="rviz" default="true" />
    <node  pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_mujoco)/launch/rviz/dualarm_test.rviz" if="$(arg rviz)"/>

</launch>

Details

[ERROR] [1724685818.220501, 0.619000] [rosout] [/leftpanda_controller_spawner] [main]: Failed to load franka_state_controller [INFO] [1724685818.222018, 0.620000] [rosout] [/leftpanda_controller_spawner] [main]: Loading controller: joint_position_example_controller [ERROR] [1724685818.224359337, 0.622000000] [ros.controller_manager] [/mujoco_server] [ControllerManager::loadController]: A controller named 'joint_position_example_controller' was already loaded inside the controller manager [ WARN] [1724685818.243412442, 0.635000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link1 (parent rightpanda_link0) at time 0.634000 according to authority unknown_publisher [ WARN] [1724685818.243718358, 0.635000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link2 (parent rightpanda_link1) at time 0.634000 according to authority unknown_publisher [ WARN] [1724685818.243751393, 0.635000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link3 (parent rightpanda_link2) at time 0.634000 according to authority unknown_publisher [ WARN] [1724685818.243802707, 0.635000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link4 (parent rightpanda_link3) at time 0.634000 according to authority unknown_publisher [ WARN] [1724685818.243849556, 0.635000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link5 (parent rightpanda_link4) at time 0.634000 according to authority unknown_publisher [ WARN] [1724685818.243870511, 0.635000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link6 (parent rightpanda_link5) at time 0.634000 according to authority unknown_publisher [ WARN] [1724685818.243890314, 0.635000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link7 (parent rightpanda_link6) at time 0.634000 according to authority unknown_publisher [ WARN] [1724685818.301014127, 0.668000000] [ros.rosconsole_bridge.console_bridge] [/mujoco_server] []: TF_REPEATED_DATA ignoring data with redundant timestamp for frame rightpanda_link1 (parent rightpanda_link0) at time 0.667000 according to authority unknown_publisher

[DavidPL1]

With multiple robots you have to ensure each is in its own namespace. The arm_id only ensures the joints have unique identifiers, but for the controllers you still have name clashes. That's why in your log controller_manager complains that franka_state_controller and joint_position_example_controller are already started.

Try using <group ns="..."> around the panda_for_dual_test.launch includes. Though I'm not sure the control plugin supports this. Though I'd be interested in making it work. I hope I can make the time to look into this further before my vacation, otherwise I'll look into it into about 3 weeks.

Anyways, please report if my suggestion already works or if you encounter more problems.

[jiye33]

Thank you for your reply and suggestion!

I tried what you suggested. When I changed "panda_for_dual_test.launch" with group namespace like below, It is possible to control each dual arm. However the problem is that launch_server.launch called twice, so I got two mujoco window and each operates only for one robot.

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

  <arg name="urdf" default="$(find franka_description)/robots/panda/panda.urdf.xacro"/>
  <arg name="standalone"  default="false"  doc="Should a mujoco server instance be started?" />
  <arg name="sim_hw_file" default="$(find franka_mujoco)/config/franka_hw_sim.yaml"  doc="Path to sim hardware interface configuration" />
  <arg name="sim_controllers_file" default="$(find franka_mujoco)/config/sim_controllers.yaml"  doc="Path to sim controller definitions" />

  <!-- Robot Customization -->
  <arg name="rviz"        default="true"  doc="Should rviz be spawned?" />
  <arg name="arm_id"      default="panda" doc="Name of the panda robot to spawn" />
  <arg name="use_gripper" default="false"  doc="Should a franka hand be mounted on the flange?" />
  <arg name="controller"  default="joint_position_example_controller"     doc="Which example controller should be started? (One of {cartesian_impedance,model,force,joint_position,joint_velocity}_example_controller)" />
  <arg name="xacro_args"  default=""      doc="Additional arguments to pass to panda.urdf.xacro" />

  <arg name="modelfile"            default="$(find franka_mujoco)/assets/dual_panda_notilt_nohuman.xml"                  doc="MuJoCo xml file to load. Should define robot model and world." />
  <arg name="interactive_marker"   default="$(eval arg('controller') == 'cartesian_impedance_example_controller')" doc="Should the interactive marker node be started?" />
  <arg name="initial_joint_states" default="$(find franka_mujoco)/config/initial_joint_states.yaml"                doc="Location of param file containing the initial joint states" />
  <arg name="mujoco_plugin_config" default="$(find franka_mujoco)/config/default_mjplugin_config.yaml"             doc="Location of param file containing the mujoco plugin config" />
  <arg name="use_sim_time" doc="should mujoco publish its simulation time as ros time?"/>

  <!-- Needed for nesting this launchfile in test launchfiles -->
  <arg name="headless" default="false" />
  <arg name="unpause"  default="false" />
  <arg name="no_x"     default="false" />
  <arg name="verbose"  default="false" />

  <!-- Since robot position and orientation are defined by the xml file, these option currently do not change the robot position, but should match the position and orientation of the model file -->
  <arg name="x"           default="0"     doc="How far forward to place the base of the robot in [m]?" />
  <arg name="y"           default="0"     doc="How far leftwards to place the base of the robot in [m]?" />
  <arg name="z"           default="0"     doc="How far upwards to place the base of the robot in [m]?" />
  <arg name="roll"        default="0"     doc="How much to rotate the base of the robot around its X-axis in [rad]?" />
  <arg name="pitch"       default="0"     doc="How much to rotate the base of the robot around its Y-axis in [rad]?" />
  <arg name="yaw"         default="0"     doc="How much to rotate the base of the robot around its Z-axis in [rad]?" />

  <param name="/use_sim_time" value="$(arg use_sim_time)"/>

  <group ns="$(arg arm_id)">

    <param name="robot_description"
          command="xacro $(arg urdf)
                    gazebo:=true
                    hand:=$(arg use_gripper)
                    arm_id:=$(arg arm_id)
                    xyz:='$(arg x) $(arg y) $(arg z)'
                    rpy:='$(arg roll) $(arg pitch) $(arg yaw)'
                    $(arg xacro_args)">
    </param>

    <rosparam file="$(arg sim_hw_file)" subst_value="true" />
    <rosparam file="$(arg sim_controllers_file)" subst_value="true" />

    <!-- spawns the controller after the robot was put into its initial joint pose -->
      <node pkg="controller_manager"
            type="spawner"
            name="$(arg arm_id)_controller_spawner"
            respawn="false"
            output="screen"
            args="--wait-for initialized franka_state_controller $(arg controller)"
      />

      <node name="$(arg arm_id)_robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
      <node name="$(arg arm_id)_joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
        <rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
        <param name="rate" value="30"/>
      </node>

    <rosparam file="$(arg mujoco_plugin_config)" subst_value="true" />
    <include file="$(find mujoco_ros)/launch/launch_server.launch" pass_all_args="true" >
        <arg name="console_config_file"  value="$(find franka_mujoco)/config/rosconsole.config" />
    </include>

  </group>

</launch>

I tried to comment part to include "launch_server.launch" and include launch_server.launch only once in "dual_launch_test.launch". However It seems that mujoco can not load model. In addition, I tried to make urdf file of dual arm and tested with it, but also didn't work.

Is there perhaps another way? I definitely want to do this.