ROS 2 servoj not moving the robot

[KrzysztofDubel]

I have got a Doosan E0509.

I managed to have the robot move with our whole stack including ROS2 Control, Moveit2 and more, recently.

Last week I was not able to move the robot with the servoj function that previously worked perfectly for our use case. I've tried a few ways of doing it and I am not sure what the problem is.

The issue was precisely that whenever the subsequent calls to servoj were performed, nothing happened. I updated the robot firmware to version v2.11.2.3, the servoj function causes the robot to violently stutter with the movement and eventually do an emergency stop. I will record some videos this week to help you understand and advise on the issue. The movej function works without problems but isn't suitable for working with ROS 2 Control and Moveit planners.

Here is an extract from the code I am running which outlines all the Drfl functions I invoke and in which order they happen. (it is not the whole thing, just the doosan API functions to isolate the issue):

CDRFLEx Drfl;

//--- doosan API's call-back fuctions : Only work within 50msec in call-back functions
Drfl.set_on_tp_initializing_completed(DSRInterface::OnTpInitializingCompletedCB);
Drfl.set_on_homming_completed(DSRInterface::OnHommingCompletedCB);
Drfl.set_on_program_stopped(DSRInterface::OnProgramStoppedCB);
Drfl.set_on_monitoring_modbus(DSRInterface::OnMonitoringModbusCB);
Drfl.set_on_monitoring_data(DSRInterface::OnMonitoringDataCB);      // Callback function in M2.4 and earlier
Drfl.set_on_monitoring_ctrl_io(DSRInterface::OnMonitoringCtrlIOCB); // Callback function in M2.4 and earlier
Drfl.set_on_monitoring_state(DSRInterface::OnMonitoringStateCB);
Drfl.set_on_monitoring_access_control(DSRInterface::OnMonitoringAccessControlCB);
Drfl.set_on_log_alarm(DSRInterface::OnLogAlarm);

if (Drfl.open_connection(m_host, m_port))
{
    while ((Drfl.GetRobotState() != STATE_STANDBY)){
            sleep(1.0);
        }

        //--- Set Robot mode : MANUAL or AUTO
        assert(Drfl.SetRobotMode(ROBOT_MODE_AUTONOMOUS));

        //--- Set Robot mode : virual or real 
        ROBOT_SYSTEM eTargetSystem = ROBOT_SYSTEM_REAL;
        assert(Drfl.SetRobotSystem(eTargetSystem));

        LPROBOT_POSE pose = Drfl.GetCurrentPose();

        Drfl.set_safety_mode(SAFETY_MODE_AUTONOMOUS, SAFETY_MODE_EVENT_STOP);
}

while(true)
{
    Drfl.set_safety_mode(SAFETY_MODE_AUTONOMOUS, SAFETY_MODE_EVENT_MOVE);
    std::array<float, NUM_JOINT> fTargetPos;
    std::array<float, NUM_JOINT> fTargetVel;
    std::array<float, NUM_JOINT> fTargetAcc;
    fTargetPos = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
    fTargetVel = {120.0f, 120.0f, 149.0f, 225.0f, 225.0f, 225.0f};
    fTargetAcc = {42.0f, 38.0f, 42.0f, 80.0f, 80.0f, 120.0f};
    Drfl.servoj(fTargetPos.data(), fTargetVel.data(), fTargetAcc.data(), period.seconds());

    Drfl.set_safety_mode(SAFETY_MODE_AUTONOMOUS, SAFETY_MODE_EVENT_STOP);

    sleep(period.seconds());
}

LPROBOT_POSE pose = Drfl.get_current_posj();
LPROBOT_VEL vel = Drfl.get_current_velj();

Drfl.close_connection();

void DSRInterface::OnTpInitializingCompletedCB()
{
    // request control authority after TP initialized
    cout << "[callback OnTpInitializingCompletedCB] tp initializing completed" << endl;
    g_bTpInitailizingComplted = TRUE;
    //Drfl.manage_access_control(MANAGE_ACCESS_CONTROL_REQUEST);
    Drfl.manage_access_control(MANAGE_ACCESS_CONTROL_FORCE_REQUEST);

    g_stDrState.bTpInitialized = TRUE;
}

Am I doing anything wrong in this extract? How could I debug why servoj doesn't work while movej does?