Franka Robot demo 笛卡尔位姿生成器-肘部运动(generate_elbow_motion.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_elbow_motion.cpp * An example showing how to move the robot's elbow. * * @warning Before executing this example, make sure that the elbow has enough space to move. */ /** * @example generate_elbow_motion.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(// 重设碰撞行为 {{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; // 初始化位姿 std::array<double, 2> initial_elbow; // 初始化肘部 double time = 0.0; robot.control( [&time, &initial_pose, &initial_elbow](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; // 初始化迪卡尔位姿 initial_elbow = robot_state.elbow_c; // 初始化肘部运动 } double angle = M_PI / 10.0 * (1.0 - std::cos(M_PI / 5.0 * time)); auto elbow = initial_elbow; elbow[0] += angle; // 肘部角累加 if (time >= 10.0) {// 超过10s 运动停止 std::cout << std::endl << "Finished motion, shutting down example" << std::endl; return franka::MotionFinished({initial_pose, elbow}); } return {initial_pose, elbow};// 返回迪卡尔位姿 }); } catch (const franka::Exception& e) { std::cout << e.what() << std::endl; return -1; } return 0; }
- 设置碰撞行为:在控制循环之前设置碰撞行为参数,以确保机器人在运动过程中能够正确处理碰撞。
- 肘部运动控制循环:通过调整后的余弦函数生成肘部的角度,并控制肘部运动。在控制循环中,肘部会按照一个正弦波的模式进行运动,直到运动完成。
- 异常处理:捕获并处理可能出现的异常,确保程序能够正确终止。
这个示例展示了如何生成和控制机器人肘部的运动,同时确保运动的安全性和稳定性。