How to connect to two iiwa14 arms on the hardware from a single remote PC

[victoralad]

My experiment involves dual-arm manipulation; does your framework allow for simultaneously communicating with two Kuka robots from a single remote PC? Will the iiwa_driver package for connecting to the hardware support that? I am considering wrapping the two robots in different namespace, but I worry that will only work in simulation. My understanding is that in simulation, IP addresses for the robots are not considered, but on the hardware, both robots have the same IP address. Hence, wrapping with namespace would result in conflict.

[victoralad]

Is it as simple changing the IP address in the java application for each robot, then also changing the IP address in the iiwa.cpp based on some namespace assignment?

[costashatz]

Hello @victoralad ,

I would not advise you to use two robots connected on the same PC. I never tried, but keeping up communication to >=200Hz is difficult for one robot. Our classical configuration was: one PC per robot with just the iiwa_driver and a different PC where all the processing was happening, all connected to the same ROS master (usually the PC that did the processing).

Wrapping each robot into a namespace works (it needs a bit of caress for the control namespace). See the namespaces branch where we did it hackily for a paper, but you can understand what happens.

Is it as simple changing the IP address in the java application for each robot, then also changing the IP address in the iiwa.cpp based on some namespace assignment?

I would say this is enough, but I never tried and I am not an expert in the Java part of iiwa/KUKA. :) As I said before though, I do not recommend it. We had many issues with just one PC (it is also advised to use the real-time kernel).

[matthias-mayr]

We have two iiwa7 and I ran both of them from a single machine at the same time with one running 500Hz and the other at 200Hz. I can not make a statement about long-term stability, but at least a couple of minutes for testing worked without problems.

We are using from i5 11th-gen intel CPU with a realtime kernel and two ethernet ports.

Both our robot configurations are in different namespace. Furthermore, their IP address and port need to be different. For example for the left robot (bh) we have:

  bh_driver:
    fri:
      port: 30200
      robot_ip: 192.170.10.2
      robot_description: /bh/robot_description

    hardware_interface:
      control_freq: 500 # in Hz
      joints:
        - bh_joint_1
        - bh_joint_2
        - bh_joint_3
        - bh_joint_4
        - bh_joint_5
        - bh_joint_6
        - bh_joint_7

and for the right robot we then use

  hh_driver:
    fri:
      port: 30201
      robot_ip: 192.170.10.3
      robot_description: /hh/robot_description

    hardware_interface:
      control_freq: 200 # in Hz
      joints:
        - hh_joint_1
        - hh_joint_2
        - hh_joint_3
        - hh_joint_4
        - hh_joint_5
        - hh_joint_6
        - hh_joint_7

There is no need to change the cpp file, because the values in the YAML file would overwrite these defaults.

[victoralad]

Thanks! I will try these suggestions and let you know how it goes.

[matthias-mayr]

On your comment in the other issue:

I was hoping I could load just one robot_description to the parameter server, and spawn two separate robots using the same robot_description, but it appears that is not possible.

We actually have one common robot_description in /robot_description and then separate ones in the individual namespaces. Some components like the driver need an individual one. Depending on your configuration, MoveIt might be another example.

[victoralad]

I was able to successfully spawn and control two robots using namespaces in simulation. However, when trying to replicate this on the hardware by calling the iiwa_bringup.launch file, I get the following error:

Exception thrown: Could not find resource 'iiwa_joint_1' in 'hardware_interface::EffortJointInterface'.

I followed the suggestion given by @matthias-mayr where I did not modify any .cpp file, but changed my iiwa.yaml file to look like this

iiwa14_A:
  fri:
    port: 30200
    robot_ip: 192.170.10.2
    robot_description: /iiwa14_A/robot_description

  hardware_interface:
    control_freq: 200 # in Hz
    joints:
      - iiwa14_A_joint_1
      - iiwa14_A_joint_2
      - iiwa14_A_joint_3
      - iiwa14_A_joint_4
      - iiwa14_A_joint_5
      - iiwa14_A_joint_6
      - iiwa14_A_joint_7

iiwa14_B:
  fri:
    port: 30201
    robot_ip: 192.170.10.3
    robot_description: /iiwa14_B/robot_description

  hardware_interface:
    control_freq: 200 # in Hz
    joints:
      - iiwa14_B_joint_1
      - iiwa14_B_joint_2
      - iiwa14_B_joint_3
      - iiwa14_B_joint_4
      - iiwa14_B_joint_5
      - iiwa14_B_joint_6
      - iiwa14_B_joint_7

I also added group_ns to my iiwa_bringup.launch file, which looks like this:

<launch>
  <!-- Select the robot -->
  <arg name="robot_name" default="iiwa"/>
  <arg name="model" default="14" />

  <!-- Select the controller -->
  <arg name="controller" default="CustomControllers"/>

  <!-- Setup iiwa -->
  <include file="$(find iiwa_driver)/launch/iiwa_setup.launch">
      <arg name="robot_name" value="$(arg robot_name)"/>
      <arg name="model" value="$(arg model)"/>
      <arg name="controller" value="$(arg controller)"/>
  </include>

  <!-- Spawn iiwa FRI driver -->
  <group ns="iiwa14_A">
    <node pkg="iiwa_driver" type="iiwa_driver" name="iiwa_driver" respawn="false" output="screen">
      <remap from="/joint_states" to="/iiwa14_A/joint_states"/>
      <remap from="/controller_manager" to="/iiwa14_A/controller_manager"/>
      <remap from="/commanding_status" to="/iiwa14_A/commanding_status"/>
      <!-- Load configurations from YAML file to parameter server -->
      <rosparam file="$(find iiwa_driver)/config/iiwa.yaml" command="load"/>
    </node>

    <!-- Spawn controller -->
    <include file="$(find iiwa_control)/launch/iiwa_control.launch">
      <arg name="controller" value="$(arg controller)"/>
      <arg name="ns" value="iiwa14_A"/>
    </include>
  </group>
  <group ns="iiwa14_B">
    <node pkg="iiwa_driver" type="iiwa_driver" name="iiwa_driver" respawn="false" output="screen">
      <remap from="/joint_states" to="/iiwa14_B/joint_states"/>
      <remap from="/controller_manager" to="/iiwa14_B/controller_manager"/>
      <remap from="/commanding_status" to="/iiwa14_B/commanding_status"/>
      <!-- Load configurations from YAML file to parameter server -->
      <rosparam file="$(find iiwa_driver)/config/iiwa.yaml" command="load"/>
    </node>

    <!-- Spawn controller -->
    <include file="$(find iiwa_control)/launch/iiwa_control.launch">
      <arg name="controller" value="$(arg controller)"/>
      <arg name="ns" value="iiwa14_B"/>
    </include>
  </group>

</launch>

I also slightly modified my iiwa_control.launch file to look like this:

<launch>
  <!-- Select the controller -->
  <arg name="controller" default="TorqueController"/>
  <arg name="ns" default="iiwa"/>

  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find iiwa_control)/config/iiwa_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="iiwa_controller" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/$(arg ns)" args="/$(arg ns)/$(arg controller) /$(arg ns)/joint_state_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/$(arg ns)/joint_states" />
  </node>

</launch>

I tried debugging this for a while, but still could not get why the hardware_interface is looking for iiwa_joint_1 instead of iiwa14_A_joint_1 or iiwa14_B_joint_2. I did not run into this issue in simulation.

[matthias-mayr]

Most likely it's a namespace issue. Giving it a quick look it seems that

 <group ns="iiwa14_A">
...
      <!-- Load configurations from YAML file to parameter server -->
      <rosparam file="$(find iiwa_driver)/config/iiwa.yaml" command="load"/>

would place this lines in the yaml file:

iiwa14_A:
  hardware_interface:
    joints:
      - iiwa14_A_joint_1

in iiwa14_A/iiwa14_A/hardware_interface/joints/iiwa14_A_joint_1, because it is loading inside the namespace already.

What you can do is to check your parameter server with rosparam list. Another debugging step is to output the namespace in which the driver is launched with something like

ROS_INFO("Initializing in namespace: %s", node_handle.getNamespace().c_str());

in the code of the driver.

[costashatz]

@victoralad did you solve your problem?

[kowac]

Sorry for hijacking this thread:
Does anyone know why changing any of the rates, e.g.:

  hardware_interface:
    control_freq: 300 # in Hz

or the default value in:
_p.param("hardware_interface/control_freq", _control_freq, 300.);
has no impact on the actual rate of the "/iiwa/joint_states" topic? It always stays at 200 Hz despite these changes. Am I missing something?

[matthias-mayr]

has no impact on the actual rate of the "/iiwa/joint_states" topic? It always stays at 200 Hz despite these changes. Am I missing something?

This observation is correct. We did some changes to the code to use the FRI library correctly. As of now the control_freq parameter is only needed for ros_control and iirc the velocity filter.

The actual rate is dictated by the FRI connection, so it only changes if the multiplier in the KUKA Java program is changed.
Since this is a "multiplier" that divides 1000 Hz this also means that 300 Hz is not a viable choice. 333 Hz is then though.

[kowac]

Okay, thanks for this clarification.
One follow-up question: Changing the control_freq to 500 in the iiwa.yaml and looking at the /iiwa/PositionTorqueController/command topic via rostopic hz can lead to higher rates when I intentionally send example messages with e.g. a rate of 600 Hz. Or am I misunderstanding the concept of the control_freq?

The reason for my question is that when I currently move the robot via the PositionTorqueController I get really strange high pitch noises I don't usually get with the PositionController. I was thinking that my actual control frequency was the root of the problem.
Currently I just don't know what to check anymore to have a smoothly moving and not strangely sounding iiwa robot when using the PositionTorqueController.

[matthias-mayr]

looking at the /iiwa/PositionTorqueController/command topic via rostopic hz can lead to higher rates when I intentionally send example messages with e.g. a rate of 600 Hz.

This is because this is an input topic and what you're measuring is how fast you input new commands:

So this rate has nothing to do with the control rate.

The reason for my question is that when I currently move the robot via the PositionTorqueController I get really strange high pitch noises I don't usually get with the PositionController. I was thinking that my actual control frequency was the root of the problem.
Currently I just don't know what to check anymore to have a smoothly moving and not strangely sounding iiwa robot when using the PositionTorqueController.

I am not super surprised. What happens here eventually is that the position commands are converted into torques with the PID gains. You might need to tune the gains. Most likely it also helps to increase the control frequency to 1 kHz.
I haven't tried using this controller combination, so I don't have own experiences. We either used pure position control or pure torque control.

[kowac]

The actual rate is dictated by the FRI connection, so it only changes if the multiplier in the KUKA Java program is changed. Since this is a "multiplier" that divides 1000 Hz this also means that 300 Hz is not a viable choice. 333 Hz is then though.

A late reply: Can you please guide me to the exact location? I wasn't able to find the frequency in the "FRIOverlay.java". The only location I was able to find was in the "iiwa.cpp" which has a "_control_freq". This again is not a decimal value but set to 200. Thanks in advance!

[matthias-mayr]

It's controlled on the Java side here: https://github.com/epfl-lasa/iiwa_ros/blob/master/iiwa_driver/java/FRIOverlay.java#L47
The frequency on the ROS side is only used for the joint velocity filter.