Franka Robot demo 外部控制循环生成笛卡尔位姿运动(generate_cartesian_pose_motion_external_control_loop.cpp)
// Copyright (c) 2023 Franka Robotics GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE #include <cmath> #include <iostream> #include <franka/active_control.h> #include <franka/active_motion_generator.h> #include <franka/exception.h> #include <franka/robot.h> #include "examples_common.h" /** * @example generate_cartesian_pose_motion_external_control_loop.cpp * An example showing how to generate a Cartesian motion with an external control loop. * * @warning Before executing this example, make sure there is enough space in front of the robot. */ /** * @example generate_cartesian_pose_motion_external_control_loop.cpp * 一个演示如何通过外部控制循环生成笛卡尔位姿运动的示例。 * * @warning 在执行此示例之前,请确保机器人前方有足够的空间。 */ int main(int argc, char** argv) { // Check whether the required arguments were passed if (argc != 2) { std::cerr << "Usage: " << argv[0] << " <robot-hostname>" << std::endl; return -1; } try { 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, 16> initial_pose; // 初始化位姿 double time = 0.0; auto callback_control = [&time, &initial_pose](// 迪卡尔位姿 const franka::RobotState& robot_state, franka::Duration period) -> franka::CartesianPose { time += period.toSec(); if (time == 0.0) { initial_pose = robot_state.O_T_EE_c; } constexpr double kRadius = 0.3; // 运动半经 double angle = M_PI / 4 * (1 - std::cos(M_PI / 5.0 * time)); // 运动角度 随时间运动 周期运动 double delta_x = kRadius * std::sin(angle); // x轴运动距离 double delta_z = kRadius * (std::cos(angle) - 1); //y轴运动距离 std::array<double, 16> new_pose = initial_pose; // 初始化位姿 new_pose[12] += delta_x; // x轴位移分量 new_pose[14] += delta_z; // z轴位移分量 if (time >= 10.0) {// 超过10s则停止 std::cout << std::endl << "Finished motion, shutting down example" << std::endl; return franka::MotionFinished(new_pose); } return new_pose; }; bool motion_finished = false; auto active_control = robot.startCartesianPoseControl(//开启外部迪卡尔位姿控制循环 research_interface::robot::Move::ControllerMode::kJointImpedance); while (!motion_finished) { auto read_once_return = active_control->readOnce();// 读取机器人状态 auto robot_state = read_once_return.first; auto duration = read_once_return.second; auto cartesian_positions = callback_control(robot_state, duration); // 计算迪卡尔位姿 motion_finished = cartesian_positions.motion_finished; active_control->writeOnce(cartesian_positions); // 发送迪卡尔位姿指令 } } catch (const franka::Exception& e) { std::cout << e.what() << std::endl; return -1; } return 0; }
总结:
这段代码通过外部控制循环生成一个笛卡尔位姿运动。首先,连接到机器人并设置默认行为,然后将机器人移动到初始关节配置。接着,设置碰撞行为,初始化位姿和时间变量,定义控制回调函数。在控制循环中,读取机器人状态,计算新的笛卡尔位姿,并发送控制指令。最后,捕获并处理可能发生的异常。