Franka 怎么控制其停止

了确保运动的停止条件，必须添加一些机制来决定控制过程何时结束。通常，有几个常见的方法来设置停止条件：

1. 基于位移或位置的停止条件：

   • 当机器人的末端执行器达到目标位置或超过某个位移阈值时，停止运动。
   • 可以通过比较当前的末端位置和目标位置来判断是否应该停止。

2. 基于力的停止条件：

   • 当外部施加的力达到或超过某个预设的力阈值时，停止运动。
   • 这在力控任务中尤其常见，确保机器人在接触物体时不会施加过大的力。

3. 基于时间的停止条件：

   • 设置一个最大运行时间，当达到该时间时，停止控制。
   • 可以确保控制过程不会无限期进行。

4. 基于外部信号的停止条件：

   • 接收到外部传感器或系统发送的停止信号时，停止控制。
   • 这可以与其他系统或操作流程集成。

下面，我们在之前的代码示例基础上，添加一个基于位移和力的停止条件：

#include <franka/exception.h>
#include <franka/robot.h>
#include <franka/model.h>
#include <franka/rate_limiting.h>
#include <iostream>
#include <array>
#include <cmath>

// 初始位置和允许的最大位移
std::array<double, 3> initial_position = {0.0, 0.0, 0.5};  // 例如，z方向上的初始位置是0.5米
double max_position_z = 0.6;  // 最大目标位置z，设定为0.6米
double min_position_z = 0.4;  // 最小目标位置z，设定为0.4米

int main() {
    try {
        franka::Robot robot("192.168.0.1");  // 替换为机器人的实际IP地址

        // 定义力控制和位置控制的目标
        double desired_force_z = -10.0;  // 期望在z轴方向上的力（N）
        double max_force_deviation = 5.0;  // 最大允许的力偏差（N）
        double max_displacement_z = 0.01;  // 最大允许的位移调整（m）每个控制周期

        // 设置初始位置和阻抗控制参数
        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}}
        );

        // 获取初始位置
        franka::RobotState initial_state = robot.readOnce();
        initial_position = initial_state.O_T_EE_c[14];

        // 控制回调函数
        auto control_callback = [&](const franka::RobotState& state, franka::Duration period) -> franka::CartesianVelocities {
            // 获取当前末端执行器的外力
            std::array<double, 6> external_forces = state.K_F_ext_hat_K;
            double current_force_z = external_forces[2];

            // 计算力偏差
            double force_error = desired_force_z - current_force_z;

            // 计算位移调整（简单的比例控制）
            double displacement_z = force_error * 0.001;  // 比例系数0.001

            // 限制位移调整量
            if (displacement_z > max_displacement_z) {
                displacement_z = max_displacement_z;
            } else if (displacement_z < -max_displacement_z) {
                displacement_z = -max_displacement_z;
            }

            // 获取当前末端执行器的z轴位置
            double current_position_z = state.O_T_EE[14];

            // 判断是否超过了允许的最大位移范围
            if (current_position_z + displacement_z > max_position_z || 
                current_position_z + displacement_z < min_position_z) {
                std::cout << "Reached position limit. Stopping control." << std::endl;
                return franka::MotionFinished(franka::CartesianVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
            }

            // 判断是否超过了力偏差的最大允许值
            if (std::abs(force_error) > max_force_deviation) {
                std::cout << "Force deviation too large. Stopping control." << std::endl;
                return franka::MotionFinished(franka::CartesianVelocities{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
            }

            // 设置末端执行器的速度，沿z轴调整
            franka::CartesianVelocities velocities = {{0.0, 0.0, displacement_z, 0.0, 0.0, 0.0}};
            return franka::rate::limit(franka::kMaxCartesianVelocity, velocities);
        };

        // 开始控制循环
        robot.control(control_callback);

    } catch (const franka::Exception& e) {
        std::cerr << e.what() << std::endl;
        return -1;
    }

    return 0;
}

代码解释

1. 初始位置和最大位移设置：

   • initial_position 用于存储机器人末端执行器的初始位置。
   • max_position_z 和 min_position_z 分别定义了z轴上的最大和最小允许位置。

2. 获取初始位置：

   • 使用 robot.readOnce() 获取机器人的初始状态，并从中提取z轴的初始位置。

3. 控制回调函数：

   • 计算当前外力与期望力的偏差，调整位移。
   • 限制位移调整量，确保不会发生过大移动。
   • 判断当前z轴位置是否超出允许范围，如果超出则停止控制。
   • 判断当前力偏差是否超出允许范围，如果超出则停止控制。

4. 停止控制条件：

   • 当z轴位置超出最大或最小允许位置时，停止控制。
   • 当力偏差超过最大允许值时，停止控制。
   • 通过 franka::MotionFinished 返回零速度，表示控制任务结束。

注意事项

• 安全性：在实际应用中，必须仔细设置力和位移的阈值，确保机器人不会发生过大移动或施加过大力。
• 调试和验证：在实际环境中测试和验证代码，确保控制逻辑能够有效地工作并满足应用需求。
• 代码优化：根据实际需求，可以进一步优化控制逻辑和停止条件，以提高控制的精确性和响应速度。

通过这种方式，您可以实现基于力和位置控制的复杂任务，并确保控制过程在满足特定条件时自动停止。