Bug: Multi-Panda Control

[ndehio]

Hi, I am trying to control two panda-robots via libfranka (without ros). The robots are directly connected to my network card. I set IP addresses "172.16.0.2" and "172.16.1.2" and can ping them both. I can control them individually without any problem. Unfortunately, I did not find an example for multi-panda control, so I wrote my own one, see below. The following test program is able to control a single robot, but fails when controlling two robots. The control_command_success_rate received is always zero for the first robot and drops to zero for the second robot after approximately 40-60 iterations. Note that the motion_id received is identical for both pandas. Is this a problem? Could you please provide a running example? kind regards Niels

#include <franka/robot.h>
#include <franka/robot_state.h>
#include <motion_generator_traits.h>
#include <network.h>
#include <robot_impl.h>

#include <iostream>

int main(int argc, char * argv[])
{
  try{
    const int numPandas = 2;
    std::string ip1 = "172.16.0.2";
    std::string ip2 = "172.16.1.2";
    std::unique_ptr<franka::Robot::Impl> franka_control1;
    std::unique_ptr<franka::Robot::Impl> franka_control2;
    uint32_t motion_id1 = 0;
    uint32_t motion_id2 = 0;
    franka::RobotState franka_state1;
    franka::RobotState franka_state2;
    static constexpr research_interface::robot::Move::Deviation kDefaultDeviation {10.0, 3.12, 2 * M_PI};
    research_interface::robot::MotionGeneratorCommand motion_command;
    motion_command.q_c.fill(0);
    motion_command.dq_c.fill(0);
    motion_command.O_T_EE_c.fill(0);
    motion_command.O_dP_EE_c.fill(0);
    motion_command.elbow_c.fill(0);

    franka_control1 = std::make_unique<franka::Robot::Impl>(std::make_unique<franka::Network>(ip1, research_interface::robot::kCommandPort), 1, franka::RealtimeConfig::kEnforce);
    if(franka_control1 == NULL){
      std::cout << "franka_control1 is null " << std::endl;
      throw;
    }

    motion_id1 = franka_control1->startMotion(research_interface::robot::Move::ControllerMode::kJointImpedance, 
                                            research_interface::robot::Move::MotionGeneratorMode::kJointVelocity,
                                            kDefaultDeviation,
                                            kDefaultDeviation);
    std::cout << "motion_id1 = " << motion_id1 << std::endl;

    if(numPandas==2){
      franka_control2 = std::make_unique<franka::Robot::Impl>(std::make_unique<franka::Network>(ip2, research_interface::robot::kCommandPort), 1, franka::RealtimeConfig::kEnforce);
      if(franka_control2 == NULL){
        std::cout << "franka_control2 is null " << std::endl;
        throw;
      }
    }

    if(numPandas==2){
      motion_id2 = franka_control2->startMotion(research_interface::robot::Move::ControllerMode::kJointImpedance, 
                                              research_interface::robot::Move::MotionGeneratorMode::kJointVelocity,
                                              kDefaultDeviation,
                                              kDefaultDeviation);
      std::cout << "motion_id2 = " << motion_id2 << std::endl;
    }

    bool increase = true;
    for(int i=0; i<1000*5; i++){ //execute main loop for 5 seconds
      if(i==800){
        increase = false;
      }
      if(increase){
        motion_command.dq_c[0] += 0.001; //slowly increase velocity for first joint
      }
      else if(motion_command.dq_c[0]>0){
        motion_command.dq_c[0] -= 0.001; //slowly decrease velocity for first joint
      }      

      franka_state1 = franka_control1->update(&motion_command, nullptr);
      if(numPandas==2){
        franka_state2 = franka_control2->update(&motion_command, nullptr);
      }

      if(i % 500 == 0 || i < 70){ //print status every 0.5 second
        if(numPandas==1){
          // std::cout << "iteration " << i << " -> 1. panda q[0] = " << franka_state1.q[0] << "\n";
          std::cout << "iteration " << i << " -> 1. panda rate = " << franka_state1.control_command_success_rate << "\n";
        }
        else if(numPandas==2){
          // std::cout << "iteration " << i << " -> 1. panda q[0] = " << franka_state1.q[0] << " and 2. panda q[0] " << franka_state2.q[0] << "\n";
          std::cout << "iteration " << i << " -> 1. panda rate = " << franka_state1.control_command_success_rate << " and 2. panda rate " << franka_state2.control_command_success_rate << "\n";
        }
      }
    }
    std::cout << "main loop ok" << std::endl;

    franka_control1->finishMotion(motion_id1, &motion_command, nullptr);
    if(numPandas==2){
      franka_control2->finishMotion(motion_id2, &motion_command, nullptr);
    }
    std::cout << "done" << std::endl;
  }
  catch(const franka::Exception & e)
  {
    std::cerr << "franka::Exception " << e.what() << "\n";
    return 1;
  }
  return 0;
}

[sgabl]

As already state by Christoph in the Franka Community, the problem here is that you control everything in one thread, so I would not consider that as a bug of libfranka.

To elaborate on this a bit more: The FCI control loop operates at 1kHz, so we need every millisecond a control command. The time you have on the libfranka side is even far lower, as the network delay, processing and checking of your commands, etc. takes time. Late control commands are considered as lost, we check that through the sequence number which is sent out in the robot state and received by a control command. There is no buffering of your commands happening, we will just interpolate depending on your previous commands or stop the robot if the control_command_success_rate is too low.

The problem now of having two robots controlled in one thread is that the internal FCI clock is not synchronized between the robots (how should they). So the cycles between robot 1 and robot 2 are shifted by a (random) delta (with 0 <= delta < 1ms). In your code you wait for the robot states after each other, what only works when delta is near 0. If delta is nearer to 1ms all packets for one robot arrive late at the FCI and are dropped.

You can use our ROS implementation or I would be happy reviewing a PR for a libfranka implementation (example).