Real-time Logging at 1000 Hz

[ruturajsambhusvt]

Hello, is there a minimal example showing the real-time logging to CSV at 1000 Hz? I have been struggling to do that for some time. I was thinking of using threads similar to the example https://github.com/frankaemika/libfranka/blob/master/examples/joint_impedance_control.cpp . I could not find any example demonstrating real-time logging (not error/exception logging). I tried a dummy example of generating random numbers and storing them with 1000 Hz and I think it is slow. I am not an expert in C++ so any help would be appreciated.

[marcbone]

So the logToCSV function is usually used for error cases. This is why it is a bit harder to use for normal logging. I took the generate_joint_positions_example and modified it to log to a csv file:

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#include <cmath>
#include <iostream>

#include <Poco/DateTimeFormatter.h>
#include <Poco/File.h>
#include <Poco/Path.h>
#include <franka/exception.h>
#include <franka/robot.h>
#include <fstream>

#include "examples_common.h"

/**
 * @example generate_joint_position_motion.cpp
 * An example showing how to generate a joint position motion.
 *
 * @warning Before executing this example, make sure there is enough space in front of the robot.
 */

void writeLogToFile(const std::vector<franka::Record>& log) {
  if (log.empty()) {
    return;
  }
  try {
    Poco::Path temp_dir_path(Poco::Path::temp());
    temp_dir_path.pushDirectory("libfranka-logs");

    Poco::File temp_dir(temp_dir_path);
    temp_dir.createDirectories();

    std::string now_string =
        Poco::DateTimeFormatter::format(Poco::Timestamp{}, "%Y-%m-%d-%h-%m-%S-%i");
    std::string filename = std::string{"log-" + now_string + ".csv"};
    Poco::File log_file(Poco::Path(temp_dir_path, filename));
    if (!log_file.createFile()) {
      std::cout << "Failed to write log file." << std::endl;
      return;
    }
    std::ofstream log_stream(log_file.path().c_str());
    log_stream << franka::logToCSV(log);

    std::cout << "Log file written to: " << log_file.path() << std::endl;
  } catch (...) {
    std::cout << "Failed to write log file." << std::endl;
  }
}

int main(int argc, char** argv) {
  if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl;
    return -1;
  }
  try {
    std::vector<franka::Record> records;
    franka::Robot robot(argv[1]);
    setDefaultBehavior(robot);
    // First move the robot to a suitable joint configuration
    std::array<double, 7> q_goal = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}};
    MotionGenerator motion_generator(0.5, q_goal);
    std::cout << "WARNING: This example will move the robot! "
              << "Please make sure to have the user stop button at hand!" << std::endl
              << "Press Enter to continue..." << std::endl;
    std::cin.ignore();
    robot.control(motion_generator);
    std::cout << "Finished moving to initial joint configuration." << std::endl;

    // Set additional parameters always before the control loop, NEVER in the control loop!
    // Set collision behavior.
    robot.setCollisionBehavior(
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}},
        {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}});

    std::array<double, 7> initial_position;
    double time = 0.0;
    robot.control([&initial_position, &time,&records](const franka::RobotState& robot_state,
                                             franka::Duration period) -> franka::JointPositions {
      franka::RobotCommand last_command;
      last_command.joint_positions = robot_state.q_d;
      franka::Record record{robot_state,last_command};
      records.push_back(record);
      time += period.toSec();

      if (time == 0.0) {
        initial_position = robot_state.q_d;
      }

      double delta_angle = M_PI / 8.0 * (1 - std::cos(M_PI / 2.5 * time));
      franka::JointPositions output = {{initial_position[0], initial_position[1],
                                        initial_position[2], initial_position[3] + delta_angle,
                                        initial_position[4] + delta_angle, initial_position[5],
                                        initial_position[6] + delta_angle}};

      if (time >= 5.0) {
        std::cout << std::endl << "Finished motion, shutting down example" << std::endl;
        return franka::MotionFinished(output);
      }
      return output;
    });

    writeLogToFile(records);

  } catch (const franka::Exception& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }
  return 0;
}

you also have to link against Poco::Foundation by adding:

target_link_libraries(generate_joint_position_motion Poco::Foundation)

into the CMakeLists.txt of the examples

I hope this helps

[ruturajsambhusvt]

Thank you very much! It helps a lot!

[ruturajsambhusvt]

Hello, I wanted to store logs for a longer duration. The above method works fine for 10-15 seconds and then throws a communication constraint violation error consistently across different programs. It would be great if you can suggest a way to circumvent this.

[marcbone]

the hacky way would be to reserve as much data for the vector as you need beforehand using records.reserve(number_of_elements); The proper solution would probably be to write your own logging using a logging library like spdlog

[ruturajsambhusvt]

Hello, thank you very much! The hacky way works good for 10 minutes which is sufficient at this point. I will look into spdlog!