Franka Robot demo 关节速度生成器连续运动 结合错误恢复(generate_consecutive_motions.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/exception.h> #include <franka/robot.h> #include "examples_common.h" /** * @example generate_consecutive_motions.cpp * An example showing how to execute consecutive motions with error recovery. * * @warning Before executing this example, make sure there is enough space in front and to the side * of the robot. */ /** * @example generate_consecutive_motions.cpp * 一个演示如何执行连续运动并进行错误恢复的示例。 * * @warning 在执行此示例之前,请确保机器人前方和侧面有足够的空间。 */ int main(int argc, char** argv) { 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(// 重设碰撞行为 {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 9.0, 9.0, 8.0, 7.0, 6.0}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}, {{10.0, 10.0, 10.0, 12.5, 12.5, 12.5}}); for (size_t i = 0; i < 5; i++) { std::cout << "Executing motion." << std::endl; try { double time_max = 4.0; // 最大时间 double omega_max = 0.2; // 最大角速度 double time = 0.0; robot.control([=, &time](const franka::RobotState&, franka::Duration period) -> franka::JointVelocities {// 关节速度运动 time += period.toSec(); double cycle = std::floor(std::pow(-1.0, (time - std::fmod(time, time_max)) / time_max)); double omega = cycle * omega_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); franka::JointVelocities velocities = {{0.0, 0.0, omega, 0.0, 0.0, 0.0, 0.0}}; if (time >= 2 * time_max) { std::cout << std::endl << "Finished motion." << std::endl; return franka::MotionFinished(velocities); } return velocities; }); } catch (const franka::ControlException& e) { std::cout << e.what() << std::endl; std::cout << "Running error recovery..." << std::endl; robot.automaticErrorRecovery();// 自动恢复错误 } } } catch (const franka::Exception& e) { std::cout << e.what() << std::endl; return -1; } std::cout << "Finished." << std::endl; return 0; }
代码解释
- 初始化和连接:通过命令行参数连接到机器人,并设置默认行为。
- 运动到初始位置:将机器人移动到一个预定义的关节配置,以确保其处于合适的初始位置。
- 设置碰撞行为:在控制循环之前设置碰撞行为参数,以确保机器人在运动过程中能够正确处理碰撞。
- 执行连续运动并进行错误恢复:通过控制关节速度,机器人进行一系列连续运动。在每次运动过程中,捕获并处理控制异常,执行自动错误恢复。
- 异常处理:捕获并处理可能出现的异常,确保程序能够正确终止。
这个示例展示了如何生成和控制机器人的连续运动,同时确保运动的安全性和稳定性,并在发生错误时进行自动恢复。