How to switch from one RT mode to another RT mode?

[acf986]

@pzhu-flexiv From the example, a RT mode is always paired with a periodicTask that is scheduled by a scheduler. If the robot is at RT_CARTESIAN_MOTION_FORCE_BASE mode, we need to call streamCartesianMotionForce() inside the periodicTask. If the robot is at RT_JOINT_POSITION mode, we need to call streamJointPosition() inside the periodicTask. Could you kindly show the suggested way to organize the scheduler and periodicTask, when switching from one RT mode to a different RT mode (say RT_CARTESIAN_MOTION_FORCE_BASE to RT_JOINT_POSITION).

• Shall we have two schedulers paired with two periodicTasks, one pair for RT_CARTESIAN_MOTION_FORCE_BASE and the other pair for RT_JOINT_POSITION?
• Or two periodicTasks added to the same scheduler?
• Or only one periodicTask with a switch case to check robot's current working mode, and then call the corresponding stream function?
• Do we need to stop the scheduler every time we change the mode of the robot?

[pzhu-flexiv]

@acf986

std::atomic<bool> g_stop = {false};

/** @brief Callback function for realtime periodic task 1 */
void periodicTask1(flexiv::Robot& robot, flexiv::Scheduler& scheduler)
{
    robot.streamCartesianMotionForce();

    if (g_stop == true) {
        robot.stop();
        scheduler.stop();
    }
}

/** @brief Callback function for realtime periodic task 2 */
void periodicTask2(flexiv::Robot& robot, flexiv::Scheduler& scheduler)
{
    robot.streamJointTorque();

    if (g_stop == true) {
        robot.stop();
        scheduler.stop();
    }
}

int main(int argc, char* argv[])
{
    flexiv::Robot robot("IP", "IP");

    // Create some other thread that will set g_stop = true
    // ...

    {
        robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE);
        // Create real-time scheduler to run periodicTask1
        flexiv::Scheduler scheduler;

        // Run periodicTask1 with blocking
        scheduler.addTask(std::bind(periodicTask1, std::ref(robot), std::ref(scheduler)),
            "HP periodic", 1, scheduler.maxPriority());
        scheduler.start(true);
    } // scheduler instance goes out of scope, destroyed

    {
        robot.setMode(flexiv::Mode::RT_JOINT_TORQUE);
        // Create another real-time scheduler to run periodicTask2
        flexiv::Scheduler scheduler;

        // Run periodicTask1 with blocking
        scheduler.addTask(std::bind(periodicTask2, std::ref(robot), std::ref(scheduler)),
            "HP periodic", 1, scheduler.maxPriority());
        scheduler.start(true);
    } // scheduler instance goes out of scope, destroyed

    return 0;
}

[acf986]

@pzhu-flexiv Thanks very much for your fast response and detailed example code. Following your example, may I ask:

1. Is atomic reliable enough for RT loop? since c++ atomic just guarantee lock_free, but not wait_free?
2. Can I call scheduler.start(true) from a thread that is not main thread?
3. Is there any way to check whether the scheduler has fully finished (thread joined), if the scheduler.start(false) is used?

[pzhu-flexiv]

@acf986 That's totally fine, you can manipulate the resources from wherever you want, just make sure to design proper protection for multi-thread resource access.

[munseng-flexiv]

@acf986

You could also implement a state machine to perform the mode switching, the architecture design could be like this, but it depends on your use cases and you could add/delete more states.

void stateMachine(flexiv::Robot& robot)
{
  switch (g_state) {
    case SM_IDLE: {
      // Move to another state depends on the condition
      g_state = ...
      break;
    }

    case SM_ENTER_RT_JOINT_POSITION: {
      // Set operation mode to joint position control mode
      robot->setMode(flexiv::Mode::RT_JOINT_POSITION);

      g_state = SM_STREAM_JOINT_POSITION;

      break;
    }

    case SM_STREAM_JOINT_POSITION: {
      // execute joint trajectory

      // exit this state if finish
      g_state = SM_EXIT_RT_JOINT_POSITION;

      break;
    }

    case SM_EXIT_RT_JOINT_POSITION: {
      // Set operation mode to idle mode
      robot->setMode(flexiv::Mode::IDLE);
      g_state = SM_IDLE;
      break;
    }

    case SM_ENTER_RT_CARTESIAN_MOTION_FORCE: {
      // Set operation mode to Cartesian motion force control mode
      robot->setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE);

      g_state = SM_STREAM_CARTESIAN_MOTION_FORCE;

      break;
    }

    case SM_STREAM_CARTESIAN_MOTION_FORCE: {
      // execute Cartesian motion force

      // exit this state if finish
      g_state = SM_EXIT_RT_CARTESIAN_MOTION_FORCE;

      break;
    }

    case SM_EXIT_RT_CARTESIAN_MOTION_FORCE: {
      // Set operation mode to idle mode
      robot->setMode(flexiv::Mode::IDLE);
      g_state = SM_IDLE;
      break;
    }
  }
}

[acf986]

@munseng-flexiv , Hi, I have tried to use your approach, however, by putting the controller switch inside the 1000Hz realtime loop, it somes times gives me the following warning:

[2023-10-10 15:25:52.650] [info] Stop from here.
[2023-10-10 15:25:52.650] [warning] [Timeliness Monitor] 20 missed messages in 5s, RDK connection is about to close
[2023-10-10 15:25:53.650] [info] [flexiv::Robot] Operation mode transited to [IDLE]

The switching code looks like this

      // Stop the robot immediately from the RT loop
      // if an IDLE state has been required
      // if this does not prevent the RT monitor throw error, nothing user can do?
      if (signal->payload_ == IDLE)
      {
        log_->info("Stop from here.");
        robot_->stop();
      }

Notice, the code has reached "Stop from here", then it gives the Timeliness monitor warning, then the stop has been finished. There is nothing I could do, because the stop function is blocking, and itself takes around 1s to finish.

The problem in this case is that the timeliness monitor would terminate the entire RDK connection and kill the program effectively.

[munseng-flexiv]

@acf986 May I know in what mode you call the robot->stop()? And I did not see any mode-switching API in your code.

[munseng-flexiv]

@acf986 I did a quick test using the state machine to do mode switching between the two modes, and I did not encounter the timeliness monitor error. The example switches the RT control mode, and streams the same motion commands to hold the current robot positions. You can try running this example.

#include <flexiv/Robot.hpp>
#include <flexiv/Exception.hpp>
#include <flexiv/Log.hpp>
#include <flexiv/Scheduler.hpp>
#include <flexiv/Utility.hpp>

#include <iostream>
#include <string>
#include <thread>

namespace {

/** State machine for mode switching between Joint Position and Cartesian Motion Force */
enum StateMachine
{
    SM_IDLE = 0,
    SM_ENTER_RT_JOINT_POSITION,
    SM_STREAM_JOINT_POSITION,
    SM_EXIT_RT_JOINT_POSITION,
    SM_ENTER_RT_CARTESIAN_MOTION_FORCE,
    SM_STREAM_CARTESIAN_MOTION_FORCE,
    SM_EXIT_RT_CARTESIAN_MOTION_FORCE
};

/** Commands */
enum CmdType
{
    CMD_RUN_JOINTTRAJECTORY = 0,
    CMD_RUN_CARTESIANMOTIONFORCE,
    CMD_STOP
};

/** Global variables */
StateMachine g_state = SM_IDLE;
CmdType m_cmd = CMD_STOP;

}

/** @brief Print program usage help */
void printHelp()
{
    // clang-format off
    std::cout << "Required arguments: [robot IP] [local IP]" << std::endl;
    std::cout << "    robot IP: address of the robot server" << std::endl;
    std::cout << "    local IP: address of this PC" << std::endl;
    std::cout << "Optional arguments: None" << std::endl;
    std::cout << std::endl;
    // clang-format on
}

void stateMachine(flexiv::Robot& robot, flexiv::RobotStates& robotStates, flexiv::Log& log)
{
    switch (g_state) {
        case SM_IDLE: {
            // Move to another state depends on the commands
            if (m_cmd == CMD_RUN_JOINTTRAJECTORY) {
                log.info("Moving to SM_ENTER_RT_JOINT_POSITION state");
                g_state = SM_ENTER_RT_JOINT_POSITION;
            } else if (m_cmd == CMD_RUN_CARTESIANMOTIONFORCE) {
                log.info("Moving to SM_ENTER_RT_CARTESIAN_MOTION_FORCE state");
                g_state = SM_ENTER_RT_CARTESIAN_MOTION_FORCE;
            }
            break;
        }

        case SM_ENTER_RT_JOINT_POSITION: {
            // Set operation mode to joint position control mode
            robot.setMode(flexiv::Mode::RT_JOINT_POSITION);
            log.info("Moving to SM_STREAM_JOINT_POSITION state");
            g_state = SM_STREAM_JOINT_POSITION;
            break;
        }

        case SM_STREAM_JOINT_POSITION: {
            // Read robot states
            robot.getRobotStates(robotStates);

            // Get current position as initial position
            auto initPos = robotStates.q;

            // Robot degrees of freedom
            size_t robotDOF = robotStates.q.size();

            // Initialize target vectors to hold position
            std::vector<double> targetPos(robotDOF, 0);
            std::vector<double> targetVel(robotDOF, 0);
            std::vector<double> targetAcc(robotDOF, 0);

            targetPos = initPos;

            // Stream smooth and continuous commands
            // In this example, we set to stream the same commands to hold the initial position
            robot.streamJointPosition(targetPos, targetVel, targetAcc);

            // Exit this state if get stop command
            // Or indicator condition that the joint position is reached
            if (m_cmd == CMD_STOP) {
                log.info("Moving to SM_EXIT_RT_JOINT_POSITION state");
                g_state = SM_EXIT_RT_JOINT_POSITION;
            }

            break;
        }

        case SM_EXIT_RT_JOINT_POSITION: {
            // Set operation mode to idle mode or stop the robot
            // robot.setMode(flexiv::Mode::IDLE);
            robot.stop();
            log.info("Moving to SM_IDLE state");
            g_state = SM_IDLE;
            break;
        }

        case SM_ENTER_RT_CARTESIAN_MOTION_FORCE: {
            // Set operation mode to Cartesian motion force control mode
            robot.setMode(flexiv::Mode::RT_CARTESIAN_MOTION_FORCE_BASE);
            log.info("Moving to SM_STREAM_CARTESIAN_MOTION_FORCE state");
            g_state = SM_STREAM_CARTESIAN_MOTION_FORCE;

            break;
        }

        case SM_STREAM_CARTESIAN_MOTION_FORCE: {
            // Read robot states
            robot.getRobotStates(robotStates);

            // Get current position as initial position
            auto initTcpPose = robotStates.tcpPose;

            // Hold the TCP position
            auto targetTcpPose = initTcpPose;

            // Stream smooth and continuous commands
            // In this example, we set to stream the same commands to hold the initial position
            robot.streamCartesianMotionForce(targetTcpPose);

            // exit this state if get stop command
            // Or indicator condition that the joint position is reached
            if (m_cmd == CMD_STOP) {
                log.info("Moving to SM_EXIT_RT_CARTESIAN_MOTION_FORCE state");
                g_state = SM_EXIT_RT_CARTESIAN_MOTION_FORCE;
            }
            break;
        }

        case SM_EXIT_RT_CARTESIAN_MOTION_FORCE: {
            // Set operation mode to idle mode or stop the robot
            // robot.setMode(flexiv::Mode::IDLE);
            robot.stop();
            log.info("Moving to SM_IDLE state");
            g_state = SM_IDLE;
            break;
        }
    }
}

int main(int argc, char* argv[])
{
    // Program Setup
    // =============================================================================================
    // Logger for printing message with timestamp and coloring
    flexiv::Log log;

    // Parse parameters
    if (argc < 3 || flexiv::utility::programArgsExistAny(argc, argv, {"-h", "--help"})) {
        printHelp();
        return 1;
    }
    // IP of the robot server
    std::string robotIP = argv[1];
    // IP of the workstation PC running this program
    std::string localIP = argv[2];

    try {
        // RDK Initialization
        // =========================================================================================
        // Instantiate robot interface
        flexiv::Robot robot(robotIP, localIP);

        // Create data struct for storing robot states
        flexiv::RobotStates robotStates;

        // Clear fault on robot server if any
        if (robot.isFault()) {
            log.warn("Fault occurred on robot server, trying to clear ...");
            // Try to clear the fault
            robot.clearFault();
            std::this_thread::sleep_for(std::chrono::seconds(2));
            // Check again
            if (robot.isFault()) {
                log.error("Fault cannot be cleared, exiting ...");
                return 1;
            }
            log.info("Fault on robot server is cleared");
        }

        // Enable the robot, make sure the E-stop is released before enabling
        log.info("Enabling robot ...");
        robot.enable();

        // Wait for the robot to become operational
        int secondsWaited = 0;
        while (!robot.isOperational()) {
            std::this_thread::sleep_for(std::chrono::seconds(1));
            if (++secondsWaited == 10) {
                log.warn(
                    "Still waiting for robot to become operational, please check that the robot 1) "
                    "has no fault, 2) is in [Auto (remote)] mode");
            }
        }
        log.info("Robot is now operational");

        // Move robot to home pose
        log.info("Moving to home pose");
        robot.setMode(flexiv::Mode::NRT_PRIMITIVE_EXECUTION);
        robot.executePrimitive("Home()");

        // Wait for the primitive to finish
        while (robot.isBusy()) {
            std::this_thread::sleep_for(std::chrono::seconds(1));
        }

        // Real-time Mode Switching
        // =========================================================================================

        // Create real-time scheduler to run periodic tasks
        flexiv::Scheduler scheduler;
        // Add periodic task with 1ms interval and highest applicable priority
        scheduler.addTask(
            std::bind(stateMachine, std::ref(robot), std::ref(robotStates), std::ref(log)),
            "HP periodic", 1, scheduler.maxPriority());
        // Start all added tasks, this is by default a blocking method
        scheduler.start(false);

        // TODO: Better to set the commands in a separate thread and left the main thread to start
        // TODO: the scheduler only
        // Wait for 2s
        std::this_thread::sleep_for(std::chrono::seconds(2));

        // Change to Joint Position Control Mode and
        // Holds for 5s
        m_cmd = CMD_RUN_JOINTTRAJECTORY;
        std::this_thread::sleep_for(std::chrono::seconds(5));

        // Stop the robot and wait for 1s
        m_cmd = CMD_STOP;
        std::this_thread::sleep_for(std::chrono::seconds(1));

        // Change to CartesianMotionForce mode and
        // Holds for 5s
        m_cmd = CMD_RUN_CARTESIANMOTIONFORCE;
        std::this_thread::sleep_for(std::chrono::seconds(5));

        // Stop the robot command
        m_cmd = CMD_STOP;
        std::this_thread::sleep_for(std::chrono::seconds(1));

    } catch (const flexiv::Exception& e) {
        log.error(e.what());
        return 1;
    }

    return 0;
}

[acf986]

Thanks very much, so it is fine for a cycle in the 1000hz realtime loop to exceed 1ms? Previously was told it will trigger an error.

[munseng-flexiv]

Thanks very much, so it is fine for a cycle in the 1000hz realtime loop to exceed 1ms? Previously was told it will trigger an error.

setMode() is fine, but not for the RT motion commands e.g. streamJointPosition(). It will trigger the timeliness monitor error if the robot controller does not receive motion commands in 1ms in RT mode.

[acf986]

@munseng-flexiv , after some testing, by modifying the state switching part on the main thread to sth like:

    for (int i=0; i< 20; i++)
    {
      m_cmd = CMD_RUN_JOINTTRAJECTORY;
      std::this_thread::sleep_for(std::chrono::milliseconds(1));

      // Stop the robot and wait for 1s
      m_cmd = CMD_STOP;
      std::this_thread::sleep_for(std::chrono::seconds(1));
    }

could easily trigger this phenomenon:

[2023-10-10 18:52:36.583] [info] ================================================
[2023-10-10 18:52:36.583] [info] ===              Flexiv RDK 0.8              ===
[2023-10-10 18:52:36.583] [info] ================================================
[2023-10-10 18:52:36.583] [info] [flexiv::Robot] Network service started [RT:99]
[2023-10-10 18:52:37.83] [info] [flexiv::Robot] Connection established
[2023-10-10 18:52:37.86] [info] [flexiv::Robot] Server information:
[2023-10-10 18:52:37.86] [info] Serial number: Rizon 4s-062060
[2023-10-10 18:52:37.86] [info] Degrees of freedom: 7
[2023-10-10 18:52:37.86] [info] Software version: v2.10.5
[2023-10-10 18:52:37.89] [info] [flexiv::Robot] Initialization complete
[2023-10-10 18:52:37.89] [info] Enabling robot ...
[2023-10-10 18:52:37.114] [info] Robot is now operational
[2023-10-10 18:52:37.114] [info] Moving to home pose
[2023-10-10 18:52:38.114] [info] [flexiv::Robot] Operation mode transited to [NRT_PRIMITIVE_EXECUTION]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Initialization complete
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Maximum available priority for the user task is [97]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Added task [HP periodic]: interval [1 ms], priority [97], CPU affinity [-1]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Starting all tasks
[2023-10-10 18:52:42.181] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
[2023-10-10 18:52:44.182] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:44.182] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:44.182] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:45.182] [info] [flexiv::Robot] Operation mode transited to [IDLE]
[2023-10-10 18:52:45.182] [info] Moving to SM_IDLE state
[2023-10-10 18:52:45.185] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
[2023-10-10 18:52:46.186] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:46.186] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:46.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:47.187] [info] [flexiv::Robot] Operation mode transited to [IDLE]
[2023-10-10 18:52:47.187] [info] Moving to SM_IDLE state
[2023-10-10 18:52:47.187] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
//------------------------------For-the-ease-of-your-view-----------------------------------------------
[2023-10-10 18:52:47.260] [warning] [Timeliness Monitor] 10 missed messages in 5s
[2023-10-10 18:52:47.269] [warning] [Timeliness Monitor] 20 missed messages in 5s, RDK connection is about to close
//------------------------------For-the-ease-of-your-view-----------------------------------------------
[2023-10-10 18:52:48.187] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:48.187] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:48.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:50.83] [info] [flexiv::Robot] Network service closed
^C[moveit_cpp_tutorial-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

In my original program, because I set to switch the mode after a certain trajectory has been finished. Unfortunately, during testing, the trajectory I passed to it is just a single point (of current pose), so it always finishes instantly. Therefore, resulting in this issue. It seems as long as there are some delay, the issue will not appear.

However, it also suggest that the RDK probably should handle this formally through a logic synchronization? Relying on a user introduced delay probably isn't so reliable in the long term.

[munseng-flexiv]

@acf986

in what use case do you have to send the motion command for only 1 ms and then stop the robot?

In a normal use case, you should stream continuous joint motion commands streamJointPosition() until it reaches the joint target (check if the joint position is reached in the SM_STREAM_JOINT_POSITION state) and then only stop the robot.

In RT mode (or SM_STREAM_JOINT_POSITION in the state machine example), the joint motion commands / joint trajectory should be interpolated with your controller and sent continuously. If not, you could use the NRT sendJointPosition() which uses the internal trajectory generator that will interpolate between two set points and make the motion smooth.

[acf986]

@pzhu-flexiv @munseng-flexiv, Hi, the cause for "send the motion command for only 1ms" is due to:

1. The trajectory planner generated a extremely short trajectory.
2. once we start to execute it, the trajectory is finished instantly (because the final point is already within the reaching radius).

3. After this, we switch the mode, which is by putting the robot to stop first. These 3 steps together can be simulated by the code:

   {
    ...
     // Switch to joint trajectory mode, execute some trajectory 
     m_cmd = CMD_RUN_JOINTTRAJECTORY;
   
     // The trajectory is finished instantly
     std::this_thread::sleep_for(std::chrono::milliseconds(1));
   
     // Stop the robot and prepare to switch mode
     m_cmd = CMD_STOP;
     ...
   }

   We will add more check on the trajectory length and rules out those will be finished instantly. And if possible, could you kindly provide some guidelines on the minimum allowed execution time of the trajectory in this case.

[pzhu-flexiv]

@munseng-flexiv , after some testing, by modifying the state switching part on the main thread to sth like:

    for (int i=0; i< 20; i++)
    {
      m_cmd = CMD_RUN_JOINTTRAJECTORY;
      std::this_thread::sleep_for(std::chrono::milliseconds(1));

      // Stop the robot and wait for 1s
      m_cmd = CMD_STOP;
      std::this_thread::sleep_for(std::chrono::seconds(1));
    }

could easily trigger this phenomenon:

[2023-10-10 18:52:36.583] [info] ================================================
[2023-10-10 18:52:36.583] [info] ===              Flexiv RDK 0.8              ===
[2023-10-10 18:52:36.583] [info] ================================================
[2023-10-10 18:52:36.583] [info] [flexiv::Robot] Network service started [RT:99]
[2023-10-10 18:52:37.83] [info] [flexiv::Robot] Connection established
[2023-10-10 18:52:37.86] [info] [flexiv::Robot] Server information:
[2023-10-10 18:52:37.86] [info] Serial number: Rizon 4s-062060
[2023-10-10 18:52:37.86] [info] Degrees of freedom: 7
[2023-10-10 18:52:37.86] [info] Software version: v2.10.5
[2023-10-10 18:52:37.89] [info] [flexiv::Robot] Initialization complete
[2023-10-10 18:52:37.89] [info] Enabling robot ...
[2023-10-10 18:52:37.114] [info] Robot is now operational
[2023-10-10 18:52:37.114] [info] Moving to home pose
[2023-10-10 18:52:38.114] [info] [flexiv::Robot] Operation mode transited to [NRT_PRIMITIVE_EXECUTION]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Initialization complete
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Maximum available priority for the user task is [97]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Added task [HP periodic]: interval [1 ms], priority [97], CPU affinity [-1]
[2023-10-10 18:52:39.177] [info] [flexiv::Scheduler] Starting all tasks
[2023-10-10 18:52:42.181] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
[2023-10-10 18:52:44.182] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:44.182] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:44.182] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:45.182] [info] [flexiv::Robot] Operation mode transited to [IDLE]
[2023-10-10 18:52:45.182] [info] Moving to SM_IDLE state
[2023-10-10 18:52:45.185] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
[2023-10-10 18:52:46.186] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:46.186] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:46.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:47.187] [info] [flexiv::Robot] Operation mode transited to [IDLE]
[2023-10-10 18:52:47.187] [info] Moving to SM_IDLE state
[2023-10-10 18:52:47.187] [info] Moving to SM_ENTER_RT_JOINT_POSITION state
//------------------------------For-the-ease-of-your-view-----------------------------------------------
[2023-10-10 18:52:47.260] [warning] [Timeliness Monitor] 10 missed messages in 5s
[2023-10-10 18:52:47.269] [warning] [Timeliness Monitor] 20 missed messages in 5s, RDK connection is about to close
//------------------------------For-the-ease-of-your-view-----------------------------------------------
[2023-10-10 18:52:48.187] [info] [flexiv::Robot] Operation mode transited to [RT_JOINT_POSITION]
[2023-10-10 18:52:48.187] [info] Moving to SM_STREAM_JOINT_POSITION state
[2023-10-10 18:52:48.187] [info] Moving to SM_EXIT_RT_JOINT_POSITION state
[2023-10-10 18:52:50.83] [info] [flexiv::Robot] Network service closed
^C[moveit_cpp_tutorial-2] killing on exit
[rosout-1] killing on exit
[master] killing on exit
shutting down processing monitor...
... shutting down processing monitor complete
done

In my original program, because I set to switch the mode after a certain trajectory has been finished. Unfortunately, during testing, the trajectory I passed to it is just a single point (of current pose), so it always finishes instantly. Therefore, resulting in this issue. It seems as long as there are some delay, the issue will not appear.

However, it also suggest that the RDK probably should handle this formally through a logic synchronization? Relying on a user introduced delay probably isn't so reliable in the long term.

@acf986 What do you mean by user-introduced delay? The only requirement for RT mode to work properly on the robot server side is that the commands arrive at 1kHz, not faster, not slower. flexiv::Scheduler is meant for this purpose.

After transited into an RT mode, the timeliness monitor is activated at the time point where you call the corresponding streamXXX() function for the first time, after which the timeliness monitor expect you to call streamXXX() at exactly 1kHz, the command cannot arrive sooner or later, which is why strict scheduling is required. Note that std::this_thread::sleep_for(std::chrono::milliseconds(1)) won't be able to do strict 1kHz scheduling as this will result in a loop time of (1ms + computation_time).

The timeliness monitor is deactivated when the robot server receives the signal to transit to IDLE or any NRT mode (calling Robot::stop() sends a signal to the server to transit to IDLE mode). With a wired connection, the time taken for the server to receive the transition signal should be no longer than 1/10 of the time threshold required to trigger timeliness monitor violation, so you should be able to deactivate the timeliness monitor without violation by simply calling

robot.stop();
scheduler.stop();

Then you setMode() for the second RT mode, and re-initialize the scheduler to run another periodic task for that second RT mode. Just like mentioned in the first example I provided. The above mentioned RT mode transition is part of our unit test.

[pzhu-flexiv]

@acf986 I have fixed the memory leak issue https://github.com/flexivrobotics/flexiv_rdk/issues/37 in v0.9 so it'll be safe to use the first example I provided for switching RT modes.

[acf986]

Thanks very much for all your help. For the timeliness monitor related topic, I think I would stated it in a different thread.