Franka libfranka実行性能

コールバック関数の時間的正確性

generate_cartesian_pose_motion.cpp にコードを追加して測定
https://frankaemika.github.io/libfranka/generate_cartesian_pose_motion_8cpp-example.html
結果は「count 9957 Zero 1 P001 9924 P002 20 P003 11」となる

int count = 0;
int tosecZero = 0;
int tosecP001 = 0;
int tosecP002 = 0;
int tosecP003 = 0;
robot.control([&time, &count, &tosecZero, &tosecP001, &tosecP002, &tosecP003, &initial_pose](const franka::RobotState& robot_state, franka::Duration period) -> franka::CartesianPose {
double tosec = period.toSec();
time += tosec;
if (time == 0.0) {
  initial_pose = robot_state.O_T_EE_c;
}
count++;
if(tosec == 0.0){
  tosecZero++;
} else if(tosec == 0.001){
  tosecP001++;
} else if(tosec == 0.002){
  tosecP002++;
} else if(tosec == 0.003){
  tosecP003++;
}

double tosec = period.toSec();
time += tosec;
struct timespec t;
clock_gettime(CLOCK_REALTIME,&t);
double realtime = t.tv_sec + (double)t.tv_nsec*1e-9;
if (time == 0.0) {
  initial_pose = robot_state.O_T_EE_c;
} else {
  double diff = realtime - lasttime;
  printf("%f ",diff);
}
lasttime = realtime;
count++;
if(tosec == 0.0){
  tosecZero++;
} else if(tosec == 0.001){
  tosecP001++;
} else if(tosec == 0.002){
  tosecP002++;
} else if(tosec == 0.003){
  tosecP003++;
}

FILE *fp;
fp = fopen("tick.txt","w");
if(fp == NULL){
  return -1;
}
struct timespec t;
clock_gettime(CLOCK_REALTIME,&t);
double lasttim = t.tv_sec + t.tv_nsec * 1e-9;
std::array<double, 16> initial_pose;
double time = 0.0;
robot.control([&time, &initial_pose, &fp, &lasttim](const franka::RobotState& robot_state, franka::Duration period) -> franka::CartesianPose {
struct timespec t;
clock_gettime(CLOCK_REALTIME,&t);
double tim = t.tv_sec + t.tv_nsec * 1e-9;
time += period.toSec();
if (time == 0.0) {
  initial_pose = robot_state.O_T_EE_c;
}
double diff = tim - lasttim;
lasttim = tim;
fprintf(fp,"%f\n",diff);