深度解析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;
}