Franka Robot Demo 笛卡尔速度运动(generate_cartesian_velocity_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_cartesian_velocity_motion.cpp * An example showing how to generate a Cartesian velocity motion. * * @warning Before executing this example, make sure there is enough space in front of the robot. */ /** * @example generate_cartesian_velocity_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 the joint impedance. robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); // 设置关节阻抗 // Set the collision behavior. std::array<double, 7> lower_torque_thresholds_nominal{ {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.}}; std::array<double, 7> upper_torque_thresholds_nominal{ {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}}; std::array<double, 7> lower_torque_thresholds_acceleration{ {25.0, 25.0, 22.0, 20.0, 19.0, 17.0, 14.0}}; std::array<double, 7> upper_torque_thresholds_acceleration{ {35.0, 35.0, 32.0, 30.0, 29.0, 27.0, 24.0}}; std::array<double, 6> lower_force_thresholds_nominal{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}}; std::array<double, 6> upper_force_thresholds_nominal{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}}; std::array<double, 6> lower_force_thresholds_acceleration{{30.0, 30.0, 30.0, 25.0, 25.0, 25.0}}; std::array<double, 6> upper_force_thresholds_acceleration{{40.0, 40.0, 40.0, 35.0, 35.0, 35.0}}; robot.setCollisionBehavior( lower_torque_thresholds_acceleration, upper_torque_thresholds_acceleration,// 加速条件下关节低扭矩和高扭矩 lower_torque_thresholds_nominal, upper_torque_thresholds_nominal, // 正常工作条件下关节低扭矩和高扭矩 lower_force_thresholds_acceleration, upper_force_thresholds_acceleration, // 加速条件下末端执行器受力限制 lower_force_thresholds_nominal, upper_force_thresholds_nominal); // 正常工作条件下 末端执行器受力限制 double time_max = 4.0; // 最长时限 double v_max = 0.1; // 最大速度 double angle = M_PI / 4.0; // 角度 double time = 0.0; // 运行时间 robot.control([=, &time](const franka::RobotState&, franka::Duration period) -> franka::CartesianVelocities {// 迪卡尔速度生成 time += period.toSec(); // 时长累加 double cycle = std::floor(pow(-1.0, (time - std::fmod(time, time_max)) / time_max));//周期 double v = cycle * v_max / 2.0 * (1.0 - std::cos(2.0 * M_PI / time_max * time)); // 速度 double v_x = std::cos(angle) * v; // x轴速度分量 double v_z = -std::sin(angle) * v;// y轴速度分量 franka::CartesianVelocities output = {{v_x, 0.0, v_z, 0.0, 0.0, 0.0}}; if (time >= 2 * time_max) { std::cout << std::endl << "Finished motion, shutting down example" << std::endl; return franka::MotionFinished(output); } return output; // 返回规划好的速度 }); } catch (const franka::Exception& e) { std::cout << e.what() << std::endl; return -1; } return 0; }
-
初始化和连接:
- 检查命令行参数,确保提供了机器人的主机名。
- 通过
franka::Robot
类连接到机器人,并调用setDefaultBehavior
设置默认行为。
-
初始位置设置:
- 定义目标关节配置
q_goal
,包含七个关节角度。 - 创建
MotionGenerator
对象,用于生成移动到目标位置的关节运动。 - 提示用户按下回车键以继续,并将机器人移动到初始关节配置。
- 定义目标关节配置
-
设置关节阻抗和碰撞行为:
- 调用
setJointImpedance
设置每个关节的阻抗值。 - 调用
setCollisionBehavior
设置各种碰撞行为参数,包括加速和正常工作条件下的扭矩和力的阈值。
- 调用
-
生成笛卡尔速度运动:
- 定义运动参数,如最大时间
time_max
、最大速度v_max
和角度angle
。 - 使用
robot.control
方法定义一个控制循环,在该循环中生成笛卡尔速度。 - 在控制循环中,通过时间周期计算出当前速度
v
,并将其分解为 x 和 z 方向的速度分量v_x
和v_z
。 - 创建并返回
franka::CartesianVelocities
对象,包含当前的速度分量。 - 如果总时间超过两倍的最大时间,则结束运动并返回
MotionFinished
。
- 定义运动参数,如最大时间
-
异常处理:
- 捕获并处理
franka::Exception
,输出错误信息并返回-1。
- 捕获并处理
总结来说,该代码的主要步骤包括连接机器人、设置初始位置、配置运动参数、生成笛卡尔速度运动并处理异常。这些步骤确保了机器人按照预定义的轨迹安全地运动。