Franka机械臂代码学习笔记0_setcartesianimpedance

深度解析Franka机器人的运动生成与控制——libfranka - 古月居

注意：Franka机器人的控制频率为1kHz，但是实际留给用户代码的执行时间只有300us，所以不能让代码冗长，也不要有太多额外的外部读写操作。

• franka::Robot类: 用于与机器人通信。
• franka::RobotState：保管机械臂状态的结构体，包含的信息有关节空间信息：如关节角度、角速度、关节力矩、估计出的外部力矩、碰撞/接触状态等。笛卡尔空间信息：末端参数、末端位置、速度、估计出的外部扭矩等。接口信号：最后一个控制信号和期望信号等。
• franka::Duration: 类，毫秒级时间。通过 franka::Duration 类对象的 toSec() 方法返回时间信息（double，通常是0.001）可以实现插值。
• franka::JointPositions:控制模式。
• franka::MotionFinished: 结束。
• 回调函数：是一个被作为参数传递的函数。因为机械臂的工作频率是1kHz，所以要在1ms内完成。

例子：

double time = 0.0;
auto control_callback = [&time](const franka::RobotState& robot_state,
                                franka::Duration time_step) -> franka::JointPositions {
  time += time_step.toSec();  // Update time at the beginning of the callback.
  franka::JointPositions output = getJointPositions(time);
  if (time >= max_time) {
    // Return MotionFinished at the end of the trajectory.
    return franka::MotionFinished(output);
  }
  return output;
}

• <cmath>：是C++语言中标准库头文件。
• <iostream>：输入输出流库。
• <franka/exception.h>：包含用于异常处理的类。
• std::cerr：标准出错输出对象，默认输出信息到屏幕上。与std::cout 不同的地方在于cerr会输出报错信息到屏幕上，方便调试。
• try: 是C++的一个关键字，用以异常处理。常常和catch、throw关键字连用。如果发生异常，使用throw来抛出异常。如果要处理可能引发的异常，需要紧接在try模块之后实现一个或多个catch模块。
• robot.SetJointImpedance: 指定内部控制器的关节刚度（阻尼自动从刚度中导出）。
• robot.SetCartesianImpedance: 指定内部控制器的笛卡尔刚度（阻尼自动从刚度中导出）。
• cin.ignore(): cin.sync()是清空缓存区，cin.ignore()也是删除缓冲区中数据的作用，但其对缓冲区中的删除数据控制的较精确。
• std::array: array数组容器，比内置数组有很多优势。
• e.what(): 捕获异常，然后程序结束。

关节位置运动生成示例：

#include <cmath>
#include <iostream>
#include <franka/exception.h>
#include <franka/robot.h>
 
int main(int argc, char** argv) {
    if (argc != 2) {
    std::cerr << "Usage: " << argv[0] << "IP-Address" << std::endl;
    return -1;
  }
  try
  {
      franka::Robot robot(argv[1]);
      robot.setCollisionBehavior(
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}},
      {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}},
      {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}});
      robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}});
      robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}});
      std::cout << "Please make sure to have the user stop button at hand!" << std::endl
              << "Press Enter to continue..." << std::endl;
      std::cin.ignore();   
      std::array<double, 7> initial_position;
      double time = 0.0;
      robot.control([&initial_position, &time](const franka::RobotState& robot_state,
                                             franka::Duration period) -> franka::JointPositions {
      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],
                                        initial_position[4], initial_position[5],
                                        initial_position[6] + delta_angle}};
      if (time >= 5) {
        std::cout << std::endl << "Finished motion, shutting down robot" << std::endl;
        return franka::MotionFinished(output);
      }
      return output;
    });
  }
  catch(const franka::Exception& e)
  {
      std::cerr << e.what() << std::endl;
      return -1;
  }
  return 0;
}