Profile the RobotInterface::YarpRobotControl::setReferences method

[GiulioRomualdi]

I noticed that the current implementation of the RobotInterface::YarpRobotControl::setReferences is particularly slow. I wrote this simple main where I used the std::chrono::steady_clock::now() to profile RobotInterface::YarpRobotControl::setReferences(). blf is compiled in release.

#include <chrono>

#include <BipedalLocomotion/TextLogging/Logger.h>
#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
#include <BipedalLocomotion/ParametersHandler/StdImplementation.h>
#include <BipedalLocomotion/RobotInterface/YarpHelper.h>
#include <BipedalLocomotion/RobotInterface/YarpRobotControl.h>
using namespace BipedalLocomotion;

int main(int argc, char **argv) {
    // initialize the polydriver 
    auto handler = std::make_shared<ParametersHandler::StdImplementation>();
    handler->setParameter("joints_list", std::vector<std::string>({"torso_pitch", "torso_roll", "torso_yaw",
                         "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow",
                         "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow",
                         "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll",
                         "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll"}));
    handler->setParameter("remote_control_boards", std::vector<std::string>({"torso", "left_arm", "right_arm", "left_leg", "right_leg"}));
    handler->setParameter("local_prefix", "blf-time-profiling");
    handler->setParameter("robot_name", "icubSim");
    handler->setParameter("positioning_duration", 3.0);
    handler->setParameter("positioning_tolerance", 0.05);
    handler->setParameter("position_direct_max_admissible_error", 0.1);
    auto controlBoard = RobotInterface::constructRemoteControlBoardRemapper(handler);

    // initialize the robot control class
    RobotInterface::YarpRobotControl robotControl;
    robotControl.initialize(handler);
    robotControl.setDriver(controlBoard.poly);

    // Measure the elapsed time of PositionControl
    const auto start = std::chrono::steady_clock::now();
    robotControl.setReferences(Eigen::VectorXd::Zero(23),
                               RobotInterface::IRobotControl::ControlMode::Position);
    const auto end = std::chrono::steady_clock::now();

    const std::chrono::duration<double, std::milli> elapsed =  end - start;
    BipedalLocomotion::log()->info("Time elapsed {} ms.", elapsed.count());
    return 0;
};

The application prints

[2021-04-03 19:33:22.296] [thread: 131892] [blf] [info] Time elapsed 26.028044 ms.

This is unacceptable in our use-case. I also ran callgrind. In particular I edited the previous main with

    CALLGRIND_START_INSTRUMENTATION;
    robotControl.setReferences(Eigen::VectorXd::Zero(23),
                               RobotInterface::IRobotControl::ControlMode::Position);
    CALLGRIND_STOP_INSTRUMENTATION;
    CALLGRIND_DUMP_STATS;

and I call callgrind (valgrind --tool=callgrind --instr-atstart=no ./blf-time-profiling).
Here I noticed that setRefSpeed is called 23 times.
.
This is actually a bug

https://github.com/dic-iit/bipedal-locomotion-framework/blob/d6d20969ce617af8510e8d1abb1985a6048107c0/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L455-L468

Ineed this->positionInterface->setRefSpeeds(); should be outside the for loop

Click me if you want to check the full valgrind tree

callgrind_slow.out.zip

I fixed the bug and I ran again the test. Now the output of the application is

[2021-04-03 19:34:49.869] [thread: 133760] [blf] [info] Time elapsed 1.235745 ms.

Click me if you want to check the full valgrind tree

callgrind_fast.out.zip

[traversaro]

Ok, but just to understand why you profiled Position instead of PositionDirect? I guess that for the actual walking you want to use PositionDirect, right?

[GiulioRomualdi]

Ok, but just to understand why you profiled Position instead of PositionDirect? I guess that for the actual walking you want to use PositionDirect, right?

You're right, in the walking we use position direct. However, I started to profiling with the position control.

When the position direct is used I have the following results

[2021-04-05 18:45:09.207] [thread: 307676] [blf] [info] 1. Time elapsed 3.557195 ms.
[2021-04-05 18:45:10.208] [thread: 307676] [blf] [info] 2. Time elapsed 0.546802 ms.

In the first Time elapsed the control mode changes from position to position direct. While in the second Time elapsed the control mode is not changed.

If I comment

https://github.com/dic-iit/bipedal-locomotion-framework/blob/d6d20969ce617af8510e8d1abb1985a6048107c0/src/RobotInterface/YarpImplementation/src/YarpRobotControl.cpp#L412-L428

I got the following performances

[2021-04-05 18:49:34.164] [thread: 309858] [blf] [info] 2. Time elapsed 0.097226 ms.

Notice that when position direct is used the joints' positions are retrieved only for a safety check. So if we want to run the controller at 1khz we may think to remove the check. I don't know if it can be moved in the firmware.