Kinematics Simulation for two arms.

Recently there was a task assigned by my boss.
First, He want me to know how to load two arms in Rviz together. One is psm another is mtm. Obviously there is a launch file under file named drvk_model. It seems like this function is deprecated.

<!-- one mtmr + one psm1  -->

<launch>
  <group ns="/dvrk_mtm">
    <arg name="model" default="$(find dvrk_model)/model/mtm_right.urdf.xacro" />
    <arg name="gui" default="true" />
    <arg name="rate" default="200" />
    
    <param name="robot_description"
           command="rosrun xacro xacro $(arg model)"/>
    <param name="use_gui" value="$(arg gui)"/>
    <param name="rate" value="$(arg rate)"/>

    <rosparam>
      source_list: [/dvrk_mtm/joint_states_command]
    </rosparam>
    
    <node name="joint_state_publisher"
          pkg="joint_state_publisher"
          type="joint_state_publisher">
      <remap from="/joint_state_publisher/enable_slider"
             to="/dvrk_mtm/joint_state_publisher/enable_slider"/>
    </node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  </group>

  <group ns="/dvrk_psm">
    <arg name="model" default="$(find dvrk_model)/model/psm_one.urdf.xacro" />
    <arg name="gui" default="true" />
    <arg name="rate" default="200" />

    <param name="robot_description" command="rosrun xacro xacro $(arg model)"/>
    <param name="use_gui" value="$(arg gui)"/>
    <param name="rate" value="$(arg rate)"/>

    <rosparam>
      source_list: [/dvrk_psm/joint_states_command]
    </rosparam>

    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <remap from="/joint_state_publisher/enable_slider"
             to="/dvrk_psm/joint_state_publisher/enable_slider"/>
    </node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  </group>
  
  <node name="rviz" pkg="rviz" type="rviz"
    args="-d $(find dvrk_model)/rviz_config/mtm_psm.rviz" />
  
</launch>

It seems like I have to change launch file to load two arms together based on the reference below.

<launch>

  <arg name="arm" />   
  <arg name="config" />   

  <arg name="model" default="$(find dvrk_model)/model/$(arg arm).urdf.xacro" />
  
  <node name="dvrk_$(arg arm)_node"
        pkg="dvrk_robot"
        type="dvrk_console_json"        
        args="-j $(arg config)"                     
        output="screen"/>

  <group ns="/dvrk/$(arg arm)">
      <arg name="gui" default="false" />
      <arg name="rate" default="20" />
      
      <param name="robot_description" command="rosrun xacro xacro $(arg model) prefix:=$(arg arm)_"/>
      <param name="use_gui" value="$(arg gui)"/>
      <param name="rate" value="$(arg rate)"/>
      
      <rosparam param="source_list" subst_value="True">
          [/dvrk/$(arg arm)/state_joint_current,
           /dvrk/$(arg arm)/state_jaw_current,
           /dvrk/$(arg arm)/state_gripper_current]
      </rosparam>

      <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
      <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  </group>

  <node name="rviz" pkg="rviz" type="rviz"
     args="-d $(find dvrk_model)/rviz_config/$(arg arm).rviz" />

</launch>

<!-- one mtmr + one psm1  -->

<launch>
  <group ns="/dvrk/MTML">
    <arg name="model" default="$(find dvrk_model)/model/MTML.urdf.xacro" />
    <arg name="gui" default="false" />
    <arg name="rate" default="20" />// pay full attention on this parameters cause the robot arm will not move with high value when we run test progream.
    
    <param name="robot_description"
           command="rosrun xacro xacro $(arg model)"/>
    <param name="use_gui" value="$(arg gui)"/>
    <param name="rate" value="$(arg rate)"/>

    <rosparam>
      source_list: [/dvrk_psm/joint_states_command]
    </rosparam>
    
    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <remap from="/joint_state_publisher/enable_slider"
             to="/dvrk_mtm/joint_state_publisher/enable_slider"/>
    </node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  </group>

  <group ns="/dvrk/PSM2">
    <arg name="model" default="$(find dvrk_model)/model/PSM2.urdf.xacro" />
    <arg name="gui" default="false" />
    <arg name="rate" default="20" />

    <param name="robot_description" command="rosrun xacro xacro $(arg model)"/>
    <param name="use_gui" value="$(arg gui)"/>
    <param name="rate" value="$(arg rate)"/>

    <rosparam>
      source_list: [/dvrk_psm/joint_states_command]
    </rosparam>

    <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
      <remap from="/joint_state_publisher/enable_slider"
             to="/dvrk_psm/joint_state_publisher/enable_slider"/>
    </node>
    <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
  </group>
  
  <node name="rviz" pkg="rviz" type="rviz"
    args="-d $(find dvrk_model)/rviz_config/MTML-PSM2.rviz" />
  
</launch>

the command of loading two arms together is follows.

roslaunch dvrk_robot mtm_psm_rviz.launch

or

roslaunch dvrk_model mtm_psm_rviz.launch

I can load two arms together. The next step is let these two arms move together. Mainly through operating MTM to let PSM move.

 <node name="dvrk_$(arg arm)_node"
        pkg="dvrk_robot"
        type="dvrk_console_json"
        args="-j $(arg config)"
        output="screen"/>

This part patched above is responsible for having robot arm to communicate with ROS topics.
In order to have two arms loaded together, the json file should be changed. The snippet patched below is updated version.

{
    "arms":
     [
        {
            "name": "PSM2", //This parameter is important. Pay attention to PSM1 or PSM2 or PSM3
            "type": "PSM",
            "simulation": "KINEMATIC",
            "pid": "sawControllersPID-PSM.xml",
            "kinematic": "psm-large-needle-driver.json"
        }
        ,
        {
            "name": "MTML",
            "type": "MTM",
            "simulation": "KINEMATIC",
            "pid": "sawControllersPID-MTMR.xml",
            "kinematic": "mtm.json"
        }
     ]
    
}

After change the json file, we can change parameters after the config. As long as we change the json file, we can have two consoles parameters for two arms.

roslaunch dvrk_robot mtm_psm_rviz.launch arm:=MTML config:=/home/haotian/catkin_ws/src/cisst-saw/sawIntuitiveResearchKit/share/console-MTMR-PSM1-KIN_SIMULATED.json