force-position-hybrid

name	force-position-hybrid
description	机械臂力位混合控制技能 - 任务空间选择矩阵、力/位置并行控制、装配应用、ROS2实现
argument-hint	力位混合 OR hybrid force position OR 任务空间 OR 力控制 OR 装配
user-invocable	true

机械臂力位混合控制技能

用于实现机械臂的混合力位控制，在任务空间同时进行力控制和位置控制，典型应用于装配、插销、磨削等场景

---

何时使用

当需要以下帮助时使用此技能：

• 同时控制某些方向的位置和其他方向的压力
• 精密装配（销钉插入、齿轮咬合）
• 曲面跟踪（法向力保持恒定）
• 力/位置切换控制
• 接触任务规划

---

快速参考

控制原理

力位混合控制核心:
在任务空间定义选择矩阵 S
- S[i]=1 → 该方向力控制
- S[i]=0 → 该方向位置控制

常用配置（擦拭任务）:
方向: X     Y     Z     Rx    Ry    Rz
      0     0     1     0     0     0     → Z向力控制，其余位置控制

典型应用

任务	位置控制方向	力控制方向
擦玻璃	X, Y, Rz	Z (法向力)
销钉插孔	X, Y	Z (插入力)
磨削	Rz	X, Y, Z (法向力)
开门	X, Y, Z, Rz	Ry (力矩)

---

核心算法

选择矩阵与雅可比

import numpy as np
from typing import Tuple, List


class TaskSpaceModel:
    """任务空间模型"""

    def __init__(self, n_joints: int):
        self.n = n_joints
        self.joint_limits = np.zeros((n_joints, 2))

        # 雅可比矩阵（需要实时更新）
        self.J = np.zeros((6, n_joints))
        self.J_dot = np.zeros((6, n_joints))  # 雅可比时间导数

    def compute_jacobian(self, q: np.ndarray, robot_model) -> np.ndarray:
        """计算雅可比矩阵"""
        # 末端执行器雅可比
        # J = [Jv; Jw] - 线速度和角速度雅可比
        return robot_model.get_jacobian(q)

    def joint_to_task_torque(
        self,
        tau_task: np.ndarray,
        J: np.ndarray
    ) -> np.ndarray:
        """
        将任务空间力矩转换为关节空间力矩

        Args:
            tau_task: 6D 任务空间力矩 (Fx, Fy, Fz, Mx, My, Mz)
            J: 6xn 雅可比矩阵

        Returns:
            tau_joints: n 关节力矩
        """
        # 逆雅可比转置（常用简化）
        return J.T @ tau_task

混合控制器

class HybridForcePositionController:
    """
    混合力位控制器

    基于任务空间选择矩阵，在不同方向上分别执行力控制和位置控制
    """

    def __init__(self, n_joints: int):
        self.n = n_joints

        # === 选择矩阵 ===
        # S[i] = 1 → i 方向力控制
        # S[i] = 0 → i 方向位置控制
        self.S = np.zeros(6)  # 默认全位置控制

        # === 位置控制增益 ===
        self.Kp_pos = np.diag([50.0] * 6)
        self.Kd_pos = np.diag([15.0] * 6)

        # === 力控制增益 ===
        self.Kp_force = np.diag([2.0] * 6)
        self.Ki_force = np.diag([0.5] * 6)
        self.Kd_force = np.diag([0.2] * 6)

        # 积分状态
        self.force_integral = np.zeros(6)

        # 期望值
        self.Xd = np.zeros(6)      # 期望位置
        self.Fd = np.zeros(6)     # 期望力

        # 任务空间惯性矩阵（用于动力学前馈）
        self.M_task = np.eye(6)

    def set_force_control_directions(self, directions: []):
        
        .S[:] = 
         d  directions:
            .S[d] = 
        
        .force_integral = np.zeros()

     ():
        
        .S[:] = 
         d  directions:
            .S[d] =   
        .force_integral = np.zeros()

     () -> np.ndarray:
        
        
        pos_error = Xd - X
        pos_error_dot = -X_dot  
        F_pos = .Kp_pos @ pos_error + .Kd_pos @ pos_error_dot

        
        force_error = Fd - F_ext
        .force_integral += force_error * dt

        
        max_integral = np.array([] * )
        .force_integral = np.clip(.force_integral, -max_integral, max_integral)

        F_force = .Kp_force @ force_error + \
                  .Ki_force @ .force_integral

        
        
        S = .S
        F_cmd = np.zeros()

         i  ():
             S[i] > :  
                F_cmd[i] = F_force[i]
            :           
                F_cmd[i] = F_pos[i]

        
        

         F_cmd

     () -> np.ndarray:
        
        
        tau = J.T @ F_cmd

        
        

         tau

---

装配应用：销钉插入

class PegInHoleController:
    """
    销钉插入控制器

    阶段1: 接近（位置控制移动到孔上方）
    阶段2: 搜索（力控制法向，位置控制切向）
    阶段3: 插入（Z向位置控制 + 径向力适应）
    阶段4: 到位（位置锁定）
    """

    def __init__(self, n_joints: int):
        self.hybrid = HybridForcePositionController(n_joints)
        self.phase = "approach"
        self.insertion_depth = 0.0
        self.max_insertion = 0.05  # 5cm

        # 接近位置
        self.approach_pose = np.zeros(6)
        self.approach_pose[2] = 0.1  # 孔上方 10cm

        # 插入时的力阈值
        self.force_threshold = 5.0  # N

        # 成功标志
        self.insertion_complete = False

    def plan(self, hole_position: np.ndarray) -> dict:
        """
        规划插入任务

        Args:
            hole_position: 孔的位置 (x, y, z)

        Returns:
            控制参数
        """
        # 接近点在孔上方 5cm
        self.approach_pose[:3] = hole_position
        self.approach_pose[2] += 0.05

        return {
            "approach_pose": self.approach_pose,
            "insertion_force": ,  
            : [, ],  
        }

     () -> np.ndarray:
        
         .phase == :
             ._phase_approach(X, dt)
         .phase == :
             ._phase_search(X, F_ext, dt)
         .phase == :
             ._phase_insertion(X, F_ext, dt)
        :
             np.zeros()

     () -> np.ndarray:
        
        
        .hybrid.set_position_control_directions([, , , , , ])
         .hybrid.compute_task_wrench(
            X, np.zeros(), np.zeros(),
            .approach_pose, np.zeros(), dt
        )

     () -> np.ndarray:
        
        
        .hybrid.set_force_control_directions([, ])  
        .hybrid.set_position_control_directions([, , , ])

        
         (F_ext[]) > .force_threshold:
            .phase = 
            .insertion_depth = 

        
        target = .approach_pose.copy()
        target[] -=  * dt  

         .hybrid.compute_task_wrench(
            X, np.zeros(), F_ext,
            target, np.zeros(), dt
        )

     () -> np.ndarray:
        
        
        .hybrid.set_position_control_directions([, , , ])  

        
        Fd = np.array([, , , , , ])

        
        target = .approach_pose.copy()
        target[] -= .max_insertion

         .hybrid.compute_task_wrench(
            X, np.zeros(), F_ext,
            target, Fd, dt
        )

---

ROS2 实现

混合力位控制节点

#!/usr/bin/env python3
"""混合力位控制 ROS2 节点"""

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from geometry_msgs.msg import WrenchStamped
from std_msgs.msg import Float64MultiArray
import numpy as np


class HybridControlNode(Node):
    def __init__(self):
        super().__init__('hybrid_force_position_control')

        # 参数
        self.declare_parameter('n_joints', 6)
        self.declare_parameter('control_mode', 'hybrid')  # hybrid / position / force
        self.n = self.get_parameter('n_joints').value

        # 控制器
        self.hybrid = HybridForcePositionController(self.n)
        self.task_model = TaskSpaceModel(self.n)

        # 状态
        self.q = np.zeros(self.n)
        self.q_dot = np.zeros(self.n)
        self.end_effector_pose = np.zeros(6)
        self.F_ext = np.zeros()

        
        .target_pose = np.zeros()
        .target_force = np.zeros()

        
        .joint_state_sub = .create_subscription(
            JointState, , .joint_callback, )
        .ft_sub = .create_subscription(
            WrenchStamped, , .ft_callback, )
        .target_sub = .create_subscription(
            Float64MultiArray, , .target_callback, )

        
        .tau_pub = .create_publisher(
            Float64MultiArray, , )

        
        .timer = .create_timer(, .control_loop)  

        .get_logger().info()

     ():
         (msg.position) >= .n:
            .q = np.array(msg.position[:.n])
         (msg.velocity) >= .n:
            .q_dot = np.array(msg.velocity[:.n])

        
        .end_effector_pose = .fkine(.q)

     ():
        .F_ext[] = msg.wrench.force.x
        .F_ext[] = msg.wrench.force.y
        .F_ext[] = msg.wrench.force.z
        .F_ext[] = msg.wrench.torque.x
        .F_ext[] = msg.wrench.torque.y
        .F_ext[] = msg.wrench.torque.z

     ():
        
        data = np.array(msg.data)
         (data) >= :
            .target_pose = data[:]
            .target_force = data[:]

            
             i  ():
                 (.target_force[i]) > :
                    .hybrid.S[i] = 
                :
                    .hybrid.S[i] = 

     ():
        dt = 

        
        J = .task_model.compute_jacobian(.q, .robot_model)

        
        F_cmd = .hybrid.compute_task_wrench(
            .end_effector_pose, np.zeros(), .F_ext,
            .target_pose, .target_force, dt
        )

        
        tau_cmd = J.T @ F_cmd

        
        cmd = Float64MultiArray()
        cmd.data = tau_cmd.tolist()
        .tau_pub.publish(cmd)

     () -> np.ndarray:
        
         np.zeros()  

Launch 配置

# launch/hybrid_control.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node


def generate_launch_description():
    return LaunchDescription([
        # 控制器管理器
        Node(
            package='controller_manager',
            executable='spawner',
            arguments=['joint_effort_controller'],
        ),

        # 力位混合控制器
        Node(
            package='manipulator_controllers',
            executable='hybrid_control_node',
            name='hybrid_control',
            parameters=[{
                'n_joints': 6,
                'control_mode': 'hybrid',
            }],
            remappings=[
                ('/joint_states', '/manipulator_controller/joint_states'),
                ('/ft_sensor/wrench', '/ft300/wrench'),
            ],
        ),

        # 位置目标发布（可选：任务规划器）
        Node(
            package='manipulator_planners',
            executable='insertion_planner',
            name='insertion_planner',
        ),
    ])

---

轨迹生成

class HybridTrajectoryGenerator:
    """混合力位轨迹生成器"""

    def __init__(self):
        self.time = 0.0
        self.dt = 0.001

    def generate_approach_trajectory(
        self,
        start: np.ndarray,
        hole_pos: np.ndarray,
        approach_height: float = 0.05,
        approach_time: float = 5.0
    ) -> np.ndarray:
        """生成接近轨迹（位置控制段）"""
        num_steps = int(approach_time / self.dt)
        trajectory = np.zeros((num_steps, 6))

        # 接近点
        target = start.copy()
        target[:3] = hole_pos
        target[2] = hole_pos[2] + approach_height

        for i in range(num_steps):
            t = i / num_steps
            # 5次多项式插值
            s = 10 * t**5 - 15 * t**4 + 6 * t**3
            trajectory[i] = start + s * (target - start)

        self.time += approach_time
        return trajectory

    def generate_insertion_trajectory(
        self,
        hole_pos: np.ndarray,
        insertion_depth: float = 0.05,
        insertion_time: float = 10.0
    ) -> np.ndarray:
        
        num_steps = (insertion_time / .dt)
        trajectory = np.zeros((num_steps, ))

        start_z = hole_pos[] +   
        end_z = hole_pos[] - insertion_depth

         i  (num_steps):
            t = i / num_steps
            
            z = start_z + (end_z - start_z) * t
            trajectory[i, :] = hole_pos
            trajectory[i, ] = z

        .time += insertion_time
         trajectory

---

故障排查

问题	原因	解决方案
力控制振荡	增益过高或传感器噪声	减小 Kp_force，添加低通滤波
插入卡住	切向刚度过高	减小 X/Y 方向刚度
位置跟踪误差大	位置增益不足	增加 Kp_pos
接触后力跳变	接触检测阈值太高	降低力阈值，提前切换
关节力矩饱和	轨迹规划不合理	减小速度/加速度，规划平滑轨迹

调试命令

# 监听末端力
ros2 topic echo /ft_sensor/wrench --field wrench.force

# 监听末端位姿
ros2 topic echo /end_effector_pose

# 手动发送目标
ros2 topic pub /hybrid_target std_msgs/Float64MultiArray \
  'data: [0.4, 0.0, 0.1, 0, 0, 0, 0, 0, 5, 0, 0, 0]' --once

# 录制数据
ros2 bag record /joint_states /ft_sensor/wrench /hybrid_target -o hybrid_data

---

相关技能

• manipulator/impedance-control — 阻抗控制
• manipulator/force-control — 力控制基础
• manipulator/grasp-planning — 抓取规划
• manipulator/motion-control/trajectory — 轨迹规划
• perception/kalman-filtering — 传感器滤波