力雅可比矩阵和速度雅可比矩阵

力雅可比矩阵（Jacobian matrix of forces）描述了一个系统中每个力对于系统中每个自由度的偏导数。它可以用来计算系统中每个自由度上的力。

速度雅可比矩阵（Jacobian matrix of velocities）描述了一个系统中每个自由度对于系统中每个速度的偏导数。它可以用来计算系统中每个速度上的力。

在机器人学中，这两个雅可比矩阵经常用来控制机器人的运动。力雅可比矩阵用于控制机器人的力，而速度雅可比矩阵用于控制机器人的速度。

相关问题

平面2R力雅克比矩阵

### 平面2R力雅可比矩阵的应用及计算

在机器人学中，平面2R机械臂的力雅可比矩阵用于描述外部作用于末端执行器上的力如何映射到关节空间中的广义力。对于一个具有两个旋转关节（Revolute Joints）的平面机械臂而言，其运动学特性可以通过雅可比矩阵来表示。

#### 力雅可比矩阵定义

给定一个平面2R机械臂，在笛卡尔坐标系下，假设末端执行器受到外力 \( F \)，则该力可以被投影至关节空间形成相应的扭矩向量 \( τ \)[^1]：

\[
τ = J_f(q)^T F
\]

其中 \( q \) 表示当前配置下的关节角度矢量；\( J_f(q) \) 是指与位置速度相对应的位置雅可比矩阵转置后的版本，即所谓的“力雅可比”。

#### 计算过程

为了构建完整的6×2形式的雅可比矩阵，需要考虑线性和角动量部分的影响。具体来说，如果只关注平面上的操作，则只需处理前三个分量即可——即x轴方向、y轴方向以及绕z轴转动的角度变化率。因此，针对特定构型下的2R机械手，可以根据几何关系推导出如下表达式[^1]:

设第一个连杆长度为 \( l_1 \)，第二个连杆长度为 \( l_2 \)，关节变量分别为 \( θ_1,θ_2 \):

- 对于第i个关节(i=1,2),对应的偏移位姿变换矩阵 T_i 可由DH参数表征；
- 随后利用链式法则求得末端相对于基座的速度 V_e 关于各关节角度的一阶微分为:

\[
J_v=\begin{bmatrix}
-\sin(θ_1)(l_1+l_2\cos(θ_2)) & -l_2\sin(θ_1+θ_2)\newline
\cos(θ_1)(l_1+l_2\cos(θ_2))& l_2\cos(θ_1+θ_2)\newline
0& 1
\end{bmatrix}
\]

此矩阵代表了速度雅可比的一部分，而完整的力雅可比还需包括上述提到的姿态信息转换项。最终形成的综合雅可比矩阵能够有效地将操作空间内的物理交互反馈回控制系统内部，从而实现更精准的任务规划和障碍规避等功能.

import numpy as np

def compute_jacobian(theta1, theta2, l1, l2):
    """
    Computes the Jacobian matrix for a planar 2R robot.
    
    Parameters:
        theta1 (float): Angle of first joint in radians
        theta2 (float): Angle of second joint in radians
        l1     (float): Length of link one
        l2     (float): Length of link two
        
    Returns:
        ndarray: The computed Jacobian matrix with shape (3, 2)
    """
    jacobian = np.array([
        [-np.sin(theta1)*(l1 + l2*np.cos(theta2)), -l2 * np.sin(theta1 + theta2)],
        [np.cos(theta1)*(l1 + l2*np.cos(theta2)),  l2 * np.cos(theta1 + theta2)],
        [0                                           ,                           1]
    ])
    return jacobian.T  # Return transpose to get force Jacobian