跳转至

Algorithm Domain Services

controller.domain.services.algorithm.kinematic_domain_service

KinematicDomainService 

KinematicDomainService()

运动学领域服务。

提供机械臂正逆运动学解算功能，包括DH参数管理、坐标变换矩阵计算等。

属性：

名称 类型 描述
dh_param DHParam

机械臂 DH 参数对象。
kinematic_dh ndarray

运动学 DH 参数矩阵。
kinematic_utils KinematicUtils

运动学工具类实例。
gripper2base ndarray

当前末端到基座的变换矩阵。

初始化运动学服务。

源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
21
22
23
24
25
26
def __init__(self):
    """初始化运动学服务。"""
    self.dh_param = DHParam()
    self.kinematic_dh = self.dh_param.get_kinematic_dh()
    self.kinematic_utils = KinematicUtils()
    self.gripper2base = self.get_gripper2base(self.kinematic_dh[:, 3])

get_kinematic_dh 

get_kinematic_dh() -> np.ndarray

获取运动学 DH 参数矩阵。

返回：

类型 描述
ndarray

np.ndarray: DH 参数矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
28
29
30
31
32
33
34
def get_kinematic_dh(self) -> np.ndarray:
    """获取运动学 DH 参数矩阵。

    Returns:
        np.ndarray: DH 参数矩阵。
    """
    return self.kinematic_dh

get_gripper2base 

get_gripper2base(theta_list: list[float] | ndarray = None) -> tuple[np.ndarray, np.ndarray]

计算末端执行器相对于基座的位姿（四元数 + 位置）。

参数：

名称 类型 描述 默认
theta_list list[float] | ndarray

关节角度列表（弧度）。如果不传则使用当前内部状态。

 None

返回：

名称 类型 描述
tuple tuple[ndarray, ndarray]

(quat, pos) - quat (np.ndarray): 姿态四元数 [x, y, z, w]。 - pos (np.ndarray): 位置坐标 [x, y, z]。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
def get_gripper2base(self, theta_list: list[float] | np.ndarray = None) -> tuple[np.ndarray, np.ndarray]:
    """计算末端执行器相对于基座的位姿（四元数 + 位置）。

    Args:
        theta_list (list[float] | np.ndarray, optional): 关节角度列表（弧度）。如果不传则使用当前内部状态。

    Returns:
        tuple: (quat, pos)
            - quat (np.ndarray): 姿态四元数 [x, y, z, w]。
            - pos (np.ndarray): 位置坐标 [x, y, z]。
    """
    if theta_list is not None:
        self.kinematic_dh[:, 3] = theta_list
        rm_list = []
        for dh in self.kinematic_dh:
            rm_list.append(self.kinematic_utils.dh2rm(*dh))
        self.gripper2base = reduce(lambda x, y: x @ y, rm_list, np.eye(4))
    quat = R.from_matrix(self.gripper2base[:3, :3]).as_quat()
    return quat, self.gripper2base[:3, 3]

get_gripper2base_rm 

get_gripper2base_rm(theta_list: list[float]) -> np.ndarray

获取末端执行器相对于基座的变换矩阵。

参数：

名称 类型 描述 默认
theta_list list[float]

关节角度列表（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
def get_gripper2base_rm(self, theta_list: list[float]) -> np.ndarray:
    """获取末端执行器相对于基座的变换矩阵。

    Args:
        theta_list (list[float]): 关节角度列表（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    self.kinematic_dh[:, 3] = theta_list
    rm_list = []
    for dh in self.kinematic_dh:
        rm_list.append(self.kinematic_utils.dh2rm(*dh))
    self.gripper2base = reduce(lambda x, y: x @ y, rm_list, np.eye(4))
    return self.gripper2base

inverse_kinematic 

inverse_kinematic(rm: ndarray, pos: ndarray, initial_theta: list[float] | None = None) -> list[float]

机械臂逆运动学求解。

参数：

名称 类型 描述 默认
rm ndarray

目标姿态旋转矩阵 (3x3)。

 必需
pos ndarray

目标位置坐标 [x, y, z]。

 必需
initial_theta list[float] | None

初始关节角度猜测值，用于选择最优解。

 None

返回：

类型 描述
list[float]

list[float]: 最优关节角度解（弧度）。

引发：

类型 描述
ValueError

如果未找到有效的逆运动学解。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
def inverse_kinematic(self, rm: np.ndarray, pos: np.ndarray, initial_theta: list[float] | None = None) -> list[float]:
    """机械臂逆运动学求解。

    Args:
        rm (np.ndarray): 目标姿态旋转矩阵 (3x3)。
        pos (np.ndarray): 目标位置坐标 [x, y, z]。
        initial_theta (list[float] | None, optional): 初始关节角度猜测值，用于选择最优解。

    Returns:
        list[float]: 最优关节角度解（弧度）。

    Raises:
        ValueError: 如果未找到有效的逆运动学解。
    """
    if not initial_theta:
        initial_theta = self.kinematic_dh[:, 3]
    valid_solutions = []
    # rotation matrix
    nx, ny, nz = rm[:, 0]
    ox, oy, oz = rm[:, 1]
    ax, ay, az = rm[:, 2]
    px, py, pz = pos

    # DH参数
    a3 = 0.425 
    a4 = 0.401 
    d4 = 0.0856 
    d5 = 0.086 
    d6 = 0.231

    # 添加奇异性判断
    if abs(ax**2 + ay**2 - d4**2) < 1e-6:
        return valid_solutions

    # theta1计算
    m = ay * d6 - py
    n = ax * d6 - px
    try:
        delta = m**2 + n**2 - d4**2

        if delta < 0:
            return valid_solutions

        theta1_0 = atan2(m, n) - atan2(-d4, sqrt(m**2 + n**2 - d4**2))
        theta1_1 = atan2(m, n) - atan2(-d4, -sqrt(m**2 + n**2 - d4**2))
        theta1_candidate = [theta1_0, theta1_1]

    except Exception as e:
        return valid_solutions

    # theta5
    for i, theta1 in enumerate(theta1_candidate):
        s1 = sin(theta1)
        c1 = cos(theta1)
        # 修改theta5的计算方式
        cos_theta5 = -s1 * ax + c1 * ay
        if abs(cos_theta5) > 1:
            cos_theta5 = 1 if cos_theta5 > 0 else -1

        theta5_val = acos(cos_theta5)
        theta5_candidate = [theta5_val, -theta5_val]

        for j, theta5 in enumerate(theta5_candidate):
            s5 = sin(theta5)
            c5 = cos(theta5)

            # theta6
            if abs(s5) < 1e-6:
                # 腕部奇异性
                theta6 = initial_theta[5]
            else:
                m1 = -s1 * nx + c1 * ny
                n1 = -s1 * ox + c1 * oy
                theta6 = atan2(-n1 / s5, m1 / s5)

            # theta3
            s6 = sin(theta6)
            c6 = cos(theta6)
            m2 = d5 * (s6 * (nx * c1 + ny * s1) + c6 * (ox * c1 + oy * s1)) - d6 * (ax * c1 + ay * s1) + px * c1 + py * s1
            n2 = d5 * (oz * c6 + nz * s6) + pz - az * d6

            # 计算到关节3的距离
            r = sqrt(m2**2 + n2**2)

            if r > (a3 + a4) * 1.2 or r < abs(a3 - a4) * 0.8:  # 进一步放宽工作空间限制
                continue
            temp = (m2**2 + n2**2 - a3**2 - a4**2) / (2 * a3 * a4)
            if abs(temp) > 1:
                temp = 1 if temp > 0 else -1
            theta3_candidate = [acos(temp), -acos(temp)]

            for k, theta3 in enumerate(theta3_candidate):
                # theta2
                s3 = sin(theta3)
                c3 = cos(theta3)
                # 修改theta2的计算方式
                k1 = a3 + a4 * c3
                k2 = a4 * s3
                s2 = (k1 * n2 - k2 * m2) / (k1**2 + k2**2)
                c2 = (k1 * m2 + k2 * n2) / (k1**2 + k2**2)
                theta2 = -atan2(s2, c2)

                # theta4
                if abs(s5) < 1e-6:
                    # 在腕部奇异性时，使用更稳定的计算方式
                    if initial_theta is not None:
                        theta4_candidate = [initial_theta[3]]
                    else:
                        theta4_candidate = [initial_theta[3]]
                else:
                    T01 = self.kinematic_utils.dh2rm(self.kinematic_dh[0, 0], self.kinematic_dh[0, 1], self.kinematic_dh[0, 2], theta1)
                    T12 = self.kinematic_utils.dh2rm(self.kinematic_dh[1, 0], self.kinematic_dh[1, 1], self.kinematic_dh[1, 2], theta2)
                    T23 = self.kinematic_utils.dh2rm(self.kinematic_dh[2, 0], self.kinematic_dh[2, 1], self.kinematic_dh[2, 2], theta3)
                    R03 = (T01 @ T12 @ T23)[:3, :3]
                    R36 = R03.T @ rm
                    theta4 = atan2(R36[1, 2], R36[0, 2])
                    theta4_candidate = [-theta4, pi - theta4]
                for theta4 in theta4_candidate:
                    candidate = [theta1, theta2, theta3, theta4, theta5, theta6]
                    if self.verify_solution(candidate, (px, py, pz)):
                        valid_solutions.append(candidate)

    if not valid_solutions:
        raise ValueError("No valid solutions found")
    valid_solutions = [
        [self.kinematic_utils.normalize_angle(a) for a in sol] for sol in valid_solutions
    ]

    norm_initial_theta = [self.kinematic_utils.normalize_angle(a) for a in initial_theta]
    final_solution = min(valid_solutions, key=lambda sol: np.linalg.norm(np.array(sol) - np.array(norm_initial_theta)))

    gap = [n - i for n, i in zip(norm_initial_theta, initial_theta)]
    final_solution = [f - g for f, g in zip(final_solution, gap)]

    return final_solution

verify_solution 

verify_solution(theta_list: list[float], target_pos: ndarray) -> bool

验证逆运动学解的准确性。

参数：

名称 类型 描述 默认
theta_list list[float]

待验证的关节角度解。

 必需
target_pos ndarray

目标位置坐标。

 必需

返回：

名称 类型 描述
bool bool

如果正解位置与目标位置误差小于 1e-5 则返回 True。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
def verify_solution(self, theta_list: list[float], target_pos: np.ndarray) -> bool:
    """验证逆运动学解的准确性。

    Args:
        theta_list (list[float]): 待验证的关节角度解。
        target_pos (np.ndarray): 目标位置坐标。

    Returns:
        bool: 如果正解位置与目标位置误差小于 1e-5 则返回 True。
    """
    self.get_gripper2base(theta_list)
    current_pos = self.gripper2base[:3, 3]
    error = np.linalg.norm(current_pos - target_pos)

    return error < 1e-5

controller.domain.services.algorithm.trajectory_domain_service

SCurve 

SCurve(v_max=[pi / 4] * 6, acc_max=[pi / 8] * 6, t_j=0.5)

S曲线速度规划算法。

实现基于加加速度(Jerk)限制的S型速度曲线规划。

属性：

名称 类型 描述
v_max ndarray

最大速度限制。
acc_max ndarray

最大加速度限制。
t_j float

加加减速段时间。

初始化 S 曲线规划器。

参数：

名称 类型 描述 默认
v_max list[float]

最大速度. Defaults to [pi/4]*6.

 [pi / 4] * 6
acc_max list[float]

最大加速度. Defaults to [pi/8]*6.

 [pi / 8] * 6
t_j float

Jerk 时间参数. Defaults to 0.5.

 0.5
源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
16
17
18
19
20
21
22
23
24
25
26
def __init__(self, v_max=[pi/4] * 6, acc_max=[pi/8] * 6, t_j=0.5):
    """初始化 S 曲线规划器。

    Args:
        v_max (list[float], optional): 最大速度. Defaults to [pi/4]*6.
        acc_max (list[float], optional): 最大加速度. Defaults to [pi/8]*6.
        t_j (float, optional): Jerk 时间参数. Defaults to 0.5.
    """
    self.v_max = np.array(v_max)
    self.acc_max = np.array(acc_max)
    self.t_j = t_j

solve_cubic_numeric staticmethod 

solve_cubic_numeric(a, b, c, d, tol=1e-08)

求解三次方程实根。

源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
28
29
30
31
32
33
34
35
@staticmethod
def solve_cubic_numeric(a, b, c, d, tol = 1e-8):
    """求解三次方程实根。"""
    roots = np.roots([a, b, c, d])
    real_mask = np.isclose(roots.imag, 0, atol=tol)
    real_roots = roots[real_mask].real

    return real_roots

solve_quadratic staticmethod 

solve_quadratic(a, b, c, tol=1e-08)

求解二次方程实根。

源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
37
38
39
40
41
42
43
44
45
@staticmethod
def solve_quadratic(a, b, c, tol=1e-8):
    """求解二次方程实根。"""
    coeffs = [a, b, c]
    roots = np.roots(coeffs)
    real_mask = np.isclose(roots.imag, 0, atol=tol)
    real_roots = roots[real_mask].real

    return real_roots

base_method staticmethod 

base_method(t: float, jerk: float, a_start: float, v_start: float, s_start: float) -> tuple[float, float, float]

基础运动学方程计算。

返回：

名称 类型 描述
tuple tuple[float, float, float]

(a, v, s) 当前时刻的加速度、速度、位移。
源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
47
48
49
50
51
52
53
54
55
56
57
@staticmethod
def base_method(t: float, jerk: float, a_start: float, v_start: float, s_start: float) -> tuple[float, float, float]:
    """基础运动学方程计算。

    Returns:
        tuple: (a, v, s) 当前时刻的加速度、速度、位移。
    """
    a = jerk * t + a_start
    v = jerk * t**2 / 2 + a_start * t + v_start
    s = jerk * t**3 / 6 + a_start * t**2 / 2 + v_start * t + s_start
    return a, v, s

planning 

planning(start_angles: list[float], target_angles: list[float], v_start: list[float] = [0] * 6, dt: float = 0.01) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]

规划从起点到终点的S曲线轨迹。

参数：

名称 类型 描述 默认
start_angles list[float]

起点角度。

 必需
target_angles list[float]

终点角度。

 必需
v_start list[float]

起始速度. Defaults to [0]*6.

 [0] * 6
dt float

时间步长. Defaults to 0.01.

 0.01

返回：

名称 类型 描述
tuple tuple[ndarray, ndarray, ndarray, ndarray]

(times, accelerations, velocities, positions) 规划结果。
源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
def planning(self, start_angles: list[float], target_angles: list[float], v_start: list[float] = [0] * 6, dt: float = 0.01) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """规划从起点到终点的S曲线轨迹。

    Args:
        start_angles (list[float]): 起点角度。
        target_angles (list[float]): 终点角度。
        v_start (list[float], optional): 起始速度. Defaults to [0]*6.
        dt (float, optional): 时间步长. Defaults to 0.01.

    Returns:
        tuple: (times, accelerations, velocities, positions) 规划结果。
    """
    start_angles = np.array(start_angles)
    target_angles = np.array(target_angles)
    displacements = target_angles - start_angles
    abs_displacements = np.abs(displacements)
    if np.all(abs_displacements < 1e-6):
        num_samples = 1
        times = np.array([0.0])
        velocities = np.zeros((num_samples, 6))
        accelerations = np.zeros((num_samples, 6))
        positions = np.copy(start_angles).reshape((num_samples, 6))
        # 保持与其他返回路径一致的顺序：times, accelerations, velocities, positions
        return times, accelerations, velocities, positions
    max_displacement_idx = np.argmax(abs_displacements)
    max_displacement = abs_displacements[max_displacement_idx]
    a_max = self.acc_max[max_displacement_idx]
    v_max = self.v_max[max_displacement_idx]
    max_v_start = v_start[max_displacement_idx]
    jerk = a_max / self.t_j
    s_1, list_t_1, list_a_1, list_v_1, list_s_1 = self.get_boundary_1(jerk, 0, max_v_start, 0, v_max, a_max)
    s_2, t_2 = self.get_boundary_2(jerk, 0, max_v_start, 0)
    if s_1 < max_displacement:
        t_acc_1, t_acc_2, t_acc_3, t_dec_1, t_dec_2, t_dec_3 = list_t_1
        a_acc_1, a_acc_2, a_acc_3, a_dec_1, a_dec_2, a_dec_3 = list_a_1
        v_acc_1, v_acc_2, v_acc_3, v_dec_1, v_dec_2, v_dec_3 = list_v_1
        s_acc_1, s_acc_2, s_acc_3, s_dec_1, s_dec_2, s_dec_3 = list_s_1
        s_const = max_displacement - s_1
        t_const = s_const / v_max
        target_time = sum(list_t_1) + t_const
        param_arr = np.array([
            [0, 0, 0, 0, 0],
            [t_acc_1, jerk, 0, max_v_start, 0],
            [t_acc_1 + t_acc_2, 0, a_acc_1, v_acc_1, s_acc_1],
            [t_acc_1 + t_acc_2 + t_acc_3, -jerk, a_acc_2, v_acc_2, s_acc_2],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const, 0, a_acc_3, v_acc_3, s_acc_3],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const + t_dec_1, -jerk, a_acc_3, v_acc_3, s_acc_3 + s_const],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const + t_dec_1 + t_dec_2, 0, a_dec_1, v_dec_1, s_dec_1 + s_const],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const + t_dec_1 + t_dec_2 + t_dec_3, jerk, a_dec_2, v_dec_2, s_dec_2 + s_const],
        ])
        times, accelerations, velocities, positions = self.get_result(param_arr, target_time, dt, max_displacement_idx)
        accelerations, velocities, positions = self.scale_result(accelerations, 
                                                                 velocities,
                                                                 positions,
                                                                 max_displacement_idx, 
                                                                 displacements, 
                                                                 abs_displacements,
                                                                 start_angles, 
                                                                 target_angles)
        return times, accelerations, velocities, positions

    elif s_2 > max_displacement:
        t0 = max_v_start / jerk
        a = 2 * jerk
        b = jerk * t0 / 2
        c = 4 * max_v_start - jerk * t0**2 / 2
        d = -max_displacement
        t_j = self.solve_cubic_numeric(a, b, c, d)
        t_j = t_j[(t_j > 0) & (t_j < self.t_j)][0]
        s_acc_5, list_t_3, list_a_3, list_v_3, list_s_3 = self.get_stage_3(jerk, 0, max_v_start, 0, t_j, t0)
        t_acc_1, t_acc_2, t_dec_1, t_dec_2, t_dec_3 = list_t_3
        a_acc_1, a_acc_2, a_dec_1, a_dec_2, a_dec_3 = list_a_3
        v_acc_1, v_acc_2, v_dec_1, v_dec_2, v_dec_3 = list_v_3
        s_acc_1, s_acc_2, s_dec_1, s_dec_2, s_dec_3 = list_s_3
        target_time = sum(list_t_3)
        param_arr = np.array([
            [0, 0, 0, 0, 0],
            [t_acc_1, jerk, 0, max_v_start, 0],
            [t_acc_1 + t_acc_2, -jerk, a_acc_1, v_acc_1, s_acc_1],
            [t_acc_1 + t_acc_2 + t_dec_1, -jerk, a_acc_2, v_acc_2, s_acc_2],
            [t_acc_1 + t_acc_2 + t_dec_1 + t_dec_2, 0, a_dec_1, v_dec_1, s_dec_1],
            [t_acc_1 + t_acc_2 + t_dec_1 + t_dec_2 + t_dec_3, jerk, a_dec_2, v_dec_2, s_dec_2],
        ])
        times, accelerations, velocities, positions = self.get_result(param_arr, target_time, dt, max_displacement_idx)
        accelerations, velocities, positions = self.scale_result(accelerations, 
                                                                 velocities,
                                                                 positions,
                                                                 max_displacement_idx, 
                                                                 displacements, 
                                                                 abs_displacements,
                                                                 start_angles, 
                                                                 target_angles)
        return times, accelerations, velocities, positions
    else:
        a_acc_1, v_acc_1, s_acc_1 = self.base_method(self.t_j, jerk, 0, max_v_start, 0)
        t0 = max_v_start / a_acc_1
        s_t0 = a_max * t0**2 / 2 + jerk * self.t_j ** 2 * t0 / 2
        s_dec_3 = jerk * self.t_j ** 3 / 6
        delta_s = max_displacement - s_t0 - s_dec_3 - s_acc_1
        a = a_max / 2
        b = 3 / 2 * a_max * self.t_j + max_v_start
        c = 5 / 6 * a_max * self.t_j ** 2 + max_v_start * self.t_j - delta_s / 2 
        t_k = self.solve_quadratic(a, b, c)
        t_k = t_k[(4 * self.t_j + 2 * t_k + t0 > t_2) & (4 * self.t_j + 2 * t_k + t0 < sum(list_t_1))][0]
        s_dec_4, list_t_2, list_a_2, list_v_2, list_s_2 = self.get_stage_2(jerk, 0, max_v_start, 0, t_k, t0)
        t_acc_1, t_acc_2, t_acc_3, t_dec_1, t_dec_2, t_dec_3, t_dec_4 = list_t_2
        a_acc_1, a_acc_2, a_acc_3, a_dec_1, a_dec_2, a_dec_3, a_dec_4 = list_a_2
        v_acc_1, v_acc_2, v_acc_3, v_dec_1, v_dec_2, v_dec_3, v_dec_4 = list_v_2
        s_acc_1, s_acc_2, s_acc_3, s_dec_1, s_dec_2, s_dec_3, s_dec_4 = list_s_2

        target_time = sum(list_t_2)
        param_arr = np.array([
            [0, 0, 0, 0, 0],
            [t_acc_1, jerk, 0, max_v_start, 0],
            [t_acc_1 + t_acc_2, 0, a_acc_1, v_acc_1, s_acc_1],
            [t_acc_1 + t_acc_2 + t_acc_3, -jerk, a_acc_2, v_acc_2, s_acc_2],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1, -jerk, a_acc_3, v_acc_3, s_acc_3],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1 + t_dec_2, 0, a_dec_1, v_dec_1, s_dec_1],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1 + t_dec_2 + t_dec_3, 0, a_dec_2, v_dec_2, s_dec_2],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1 + t_dec_2 + t_dec_3 + t_dec_4, jerk, a_dec_3, v_dec_3, s_dec_3],
        ])
        times, accelerations, velocities, positions = self.get_result(param_arr, target_time, dt, max_displacement_idx)
        accelerations, velocities, positions = self.scale_result(accelerations, 
                                                        velocities,
                                                        positions,
                                                        max_displacement_idx, 
                                                        displacements, 
                                                        abs_displacements,
                                                        start_angles, 
                                                        target_angles)
        return times, accelerations, velocities, positions

controller.domain.services.algorithm.curve_motion

CurveMotionDomainService 

CurveMotionDomainService(kinematic_solver: KinematicDomainService, v_max=[pi / 4] * 6, a_max=[pi / 8] * 6, j_max=[pi / 16] * 6, dt=0.01)

曲线运动规划服务。

提供基于弧长参数化的空间曲线规划、姿态平滑插值以及基于 TOPPRA 的时间参数化功能。

属性：

名称 类型 描述
v_max ndarray

各关节最大速度 (rad/s)。
a_max ndarray

各关节最大加速度 (rad/s²)。
j_max ndarray

各关节最大加加速度 (rad/s³)。
dt float

时间步长 (s)。
kinematic_solver KinematicDomainService

运动学求解器实例。

初始化曲线运动服务。

参数：

名称 类型 描述 默认
kinematic_solver KinematicDomainService

运动学求解器服务实例。

 必需
v_max list[float]

最大速度列表. Defaults to [pi/4]*6.

 [pi / 4] * 6
a_max list[float]

最大加速度列表. Defaults to [pi/8]*6.

 [pi / 8] * 6
j_max list[float]

最大加加速度列表. Defaults to [pi/16]*6.

 [pi / 16] * 6
dt float

采样时间步长. Defaults to 0.01.

 0.01
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
def __init__(
    self, 
    kinematic_solver: KinematicDomainService,
    v_max=[pi / 4] * 6, 
    a_max=[pi / 8] * 6, 
    j_max=[pi / 16] * 6, 
    dt=0.01,
):
    """初始化曲线运动服务。

    Args:
        kinematic_solver: 运动学求解器服务实例。
        v_max (list[float], optional): 最大速度列表. Defaults to [pi/4]*6.
        a_max (list[float], optional): 最大加速度列表. Defaults to [pi/8]*6.
        j_max (list[float], optional): 最大加加速度列表. Defaults to [pi/16]*6.
        dt (float, optional): 采样时间步长. Defaults to 0.01.
    """
    self.v_max = np.asarray(v_max, dtype=float)
    self.a_max = np.asarray(a_max, dtype=float)
    self.j_max = np.asarray(j_max, dtype=float)
    self.dt = float(dt)
    self.kinematic_solver = kinematic_solver
    self.nearest_position = None

clamp 

clamp(x, low, high)

将值 x 限制在 [low, high] 区间内。

源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
50
51
52
def clamp(self, x, low, high):
    """将值 x 限制在 [low, high] 区间内。"""
    return max(low, min(x, high))

curve_motion 

curve_motion(pos_fun, u0: float, u1: float, start_position: list[float], end_position: list[float] | None, ds: float = 0.002, include_end: bool = True, orientation_mode: str = 'slerp', tool_axis: str = 'z', up_hint: ndarray = np.array([0, 0, 1.0])) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

执行曲线运动规划。

参数：

名称 类型 描述 默认
pos_fun callable

位置函数，接受参数 u 返回 (3,) 坐标。

 必需
u0 float

参数起始值。

 必需
u1 float

参数终止值。

 必需
start_position list[float]

起点关节角度。

 必需
end_position list[float] | None

终点关节角度。

 必需
ds float

采样弧长步长. Defaults to 0.002.

 0.002
include_end bool

是否包含终点. Defaults to True.

 True
orientation_mode str

姿态插值模式 ('fixed'|'slerp'|'tangent'). Defaults to "slerp".

 'slerp'
tool_axis str

工具轴方向 ('x'|'y'|'z'). Defaults to "z".

 'z'
up_hint ndarray

向上向量提示，用于切向跟随模式. Defaults to [0, 0, 1.0].

 array([0, 0, 1.0])

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t_list, positions, qd, qdd) - t_list: 时间戳列表 - positions: 关节角度轨迹列表 - qd: 关节速度轨迹列表 - qdd: 关节加速度轨迹列表

引发：

类型 描述
ValueError

如果 orientation_mode 无效或 slerp 模式下未提供 end_position。
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
def curve_motion(
        self,
        pos_fun,  # callable u -> (3,)
        u0: float,
        u1: float,
        start_position: list[float],
        end_position: list[float] | None,
        ds: float = 0.002,
        include_end: bool = True,
        orientation_mode: str = "slerp",
        tool_axis: str = "z",
        up_hint: np.ndarray = np.array([0, 0, 1.0])
) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """执行曲线运动规划。

    Args:
        pos_fun (callable): 位置函数，接受参数 u 返回 (3,) 坐标。
        u0 (float): 参数起始值。
        u1 (float): 参数终止值。
        start_position (list[float]): 起点关节角度。
        end_position (list[float] | None): 终点关节角度。
        ds (float, optional): 采样弧长步长. Defaults to 0.002.
        include_end (bool, optional): 是否包含终点. Defaults to True.
        orientation_mode (str, optional): 姿态插值模式 ('fixed'|'slerp'|'tangent'). Defaults to "slerp".
        tool_axis (str, optional): 工具轴方向 ('x'|'y'|'z'). Defaults to "z".
        up_hint (np.ndarray, optional): 向上向量提示，用于切向跟随模式. Defaults to [0, 0, 1.0].

    Returns:
        tuple: (t_list, positions, qd, qdd)
            - t_list: 时间戳列表
            - positions: 关节角度轨迹列表
            - qd: 关节速度轨迹列表
            - qdd: 关节加速度轨迹列表

    Raises:
        ValueError: 如果 orientation_mode 无效或 slerp 模式下未提供 end_position。
    """
    # 起点姿态来自起点关节角的正解
    start_quat, start_pos_fk = self.kinematic_solver.get_gripper2base(start_position)
    # 1) 等弧长采样
    pos_list, u_eq, n_seg = self.sample_parametric_equal_arclen(pos_fun, u0, u1, ds, include_end)

    # 2) 姿态生成
    if orientation_mode == "fixed":
        quat_list = np.repeat(self._q_normalize(start_quat)[None, :], len(pos_list), axis=0)

    elif orientation_mode == "slerp":
        if end_position is None:
            raise ValueError("slerp 模式需要提供 end_position 以确定终止姿态。")
        end_quat, _ = self.kinematic_solver.get_gripper2base(end_position)
        q0 = self._q_normalize(start_quat)
        q1 = self._q_normalize(end_quat)
        tau = np.linspace(0, 1, len(pos_list))
        quat_list = np.vstack([self._q_slerp(q0, q1, ti) for ti in tau])

    elif orientation_mode == "tangent":
        quat_list = self._quat_follow_tangent(pos_list, tool_axis=tool_axis, up_hint=up_hint)
    else:
        raise ValueError("orientation_mode 必须是 {'fixed','slerp','tangent'}")

    # 3) 原有平滑/IK/TOPPRA 管线复用
    t_list, positions, qd, qdd = self.smooth(quat_list, pos_list, n_seg)
    return t_list, positions, qd, qdd

make_pos_fun_spline 

make_pos_fun_spline(start_position: list[float], end_position: list[float], mid_points: list, bc_type: str = 'natural') -> tuple[callable, np.ndarray]

构造基于三次样条的位置函数。

参数：

名称 类型 描述 默认
start_position list[float]

起点关节角度。

 必需
end_position list[float]

终点关节角度。

 必需
mid_points list

中间点坐标列表 (N, 3)。

 必需
bc_type str

边界条件类型. Defaults to "natural".

 'natural'

返回：

名称 类型 描述
tuple tuple[callable, ndarray]

(pos_fun, s) - pos_fun: 位置函数 callable - s: 累积弦长数组

引发：

类型 描述
ValueError

如果点集形状不正确。
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
def make_pos_fun_spline(
    self, 
    start_position: list[float],
    end_position: list[float],
    mid_points: list, 
    bc_type: str = "natural") -> tuple[callable, np.ndarray]:
    """构造基于三次样条的位置函数。

    Args:
        start_position (list[float]): 起点关节角度。
        end_position (list[float]): 终点关节角度。
        mid_points (list): 中间点坐标列表 (N, 3)。
        bc_type (str, optional): 边界条件类型. Defaults to "natural".

    Returns:
        tuple: (pos_fun, s)
            - pos_fun: 位置函数 callable
            - s: 累积弦长数组

    Raises:
        ValueError: 如果点集形状不正确。
    """
    _, start_pos = self.kinematic_solver.get_gripper2base(start_position)
    _, end_pos = self.kinematic_solver.get_gripper2base(end_position)
    points = np.vstack([start_pos, mid_points, end_pos])
    P = np.asarray(points, dtype=float)
    if P.ndim != 2 or P.shape[0] < 2 or P.shape[1] not in (2, 3):
        raise ValueError("points 形状必须是 (N,3) 或 (N,2)，且 N>=2")
    if P.shape[1] == 2:
        P = np.hstack([P, np.zeros((P.shape[0], 1))])  # 2D 自动补 z=0

    # --- 弦长参数化，避免参数拥挤 ---
    d = np.linalg.norm(np.diff(P, axis=0), axis=1)
    s = np.concatenate([[0.0], np.cumsum(d)])
    if s[-1] == 0:
        raise ValueError("所有点重合，无法构造曲线")
    t = s / s[-1]  # 归一化到 [0,1]

    # 分别构造 x,y,z 的样条
    cs_x = CubicSpline(t, P[:, 0], bc_type=bc_type)
    cs_y = CubicSpline(t, P[:, 1], bc_type=bc_type)
    cs_z = CubicSpline(t, P[:, 2], bc_type=bc_type)

    def _eval(u):
        u = np.asarray(u, dtype=float)
        # 可选：夹紧到 [0,1]，避免数值越界
        u = np.clip(u, 0.0, 1.0)
        X = np.stack([cs_x(u), cs_y(u), cs_z(u)], axis=-1)  # (..., 3)
        if X.ndim == 1:  # 标量 u -> (3,)
            return X
        return X  # 数组 u -> (M,3)

    return _eval, s

sample_parametric_equal_arclen 

sample_parametric_equal_arclen(pos_fun, u0: float, u1: float, ds: float = 0.002, include_end: bool = True, dense: int = 4000) -> tuple[np.ndarray, np.ndarray, int]

对参数曲线 r(u) 做等弧长采样。

参数：

名称 类型 描述 默认
pos_fun callable

参数曲线函数 r(u)。

 必需
u0 float

参数起始值。

 必需
u1 float

参数终止值。

 必需
ds float

目标弧长采样间隔. Defaults to 0.002.

 0.002
include_end bool

结果是否必须包含终点. Defaults to True.

 True
dense int

初始密采样的点数. Defaults to 4000.

 4000

返回：

名称 类型 描述
tuple tuple[ndarray, ndarray, int]

(pos_list, u_eq, n_seg) - pos_list: 等弧长采样后的坐标列表 (M, 3) - u_eq: 对应的参数值列表 (M,) - n_seg: 分段数
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
def sample_parametric_equal_arclen(
        self,
        pos_fun,  # callable: u -> (3,) numpy array
        u0: float,
        u1: float,
        ds: float = 0.002,
        include_end: bool = True,
        dense: int = 4000
) -> tuple[np.ndarray, np.ndarray, int]:
    """对参数曲线 r(u) 做等弧长采样。

    Args:
        pos_fun (callable): 参数曲线函数 r(u)。
        u0 (float): 参数起始值。
        u1 (float): 参数终止值。
        ds (float, optional): 目标弧长采样间隔. Defaults to 0.002.
        include_end (bool, optional): 结果是否必须包含终点. Defaults to True.
        dense (int, optional): 初始密采样的点数. Defaults to 4000.

    Returns:
        tuple: (pos_list, u_eq, n_seg)
            - pos_list: 等弧长采样后的坐标列表 (M, 3)
            - u_eq: 对应的参数值列表 (M,)
            - n_seg: 分段数
    """
    # 1) 先在参数上做均匀密采样，估算弧长
    u_dense = np.linspace(u0, u1, max(1000, int(abs(u1 - u0) * dense)))
    XYZ = np.vstack([np.asarray(pos_fun(ui), dtype=float).reshape(3) for ui in u_dense])
    seg = np.linalg.norm(np.diff(XYZ, axis=0), axis=1)
    s_cum = np.concatenate([[0.0], np.cumsum(seg)])
    L = float(s_cum[-1])
    if L < 1e-12:
        # 退化：几乎零长
        pos_list = XYZ[[0, -1], :] if include_end else XYZ[[0], :]
        u_eq = np.array([u_dense[0], u_dense[-1]]) if include_end else np.array([u_dense[0]])
        return pos_list, u_eq, 0.0

    # 2) 弧长 -> u 的反查，用插值近似
    s2u = interp1d(s_cum, u_dense, kind="linear", bounds_error=False, fill_value=(u_dense[0], u_dense[-1]))

    # 3) 目标等弧长序列
    n_seg = max(1, int(np.ceil(L / ds)))
    s_target = np.linspace(0.0, L, n_seg + 1 if include_end else n_seg)
    u_eq = s2u(s_target)

    # 4) 得到等弧长位置
    pos_list = np.vstack([np.asarray(pos_fun(ui), dtype=float).reshape(3) for ui in u_eq])
    return pos_list, u_eq, n_seg

wrap_to_pi 

wrap_to_pi(q: ndarray) -> np.ndarray

将角度包裹到 (-pi, pi]，避免 2π 跳变影响计算。

源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
303
304
305
def wrap_to_pi(self, q: np.ndarray) -> np.ndarray:
    """将角度包裹到 (-pi, pi]，避免 2π 跳变影响计算。"""
    return (q + np.pi) % (2 * np.pi) - np.pi

ensure_2d_array 

ensure_2d_array(arr: ndarray) -> np.ndarray

将输入转换为二维数组 (N, dof) 并做基本校验。

Raises

ValueError 当路标数少于 2（缺少起点或终点）时抛出。

源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
def ensure_2d_array(self, arr: np.ndarray) -> np.ndarray:
    """
    将输入转换为二维数组 (N, dof) 并做基本校验。

    Raises
    ------
    ValueError
        当路标数少于 2（缺少起点或终点）时抛出。
    """
    q = np.asarray(arr, dtype=float)
    if q.ndim == 1:
        q = q[None, :]
    if not (q.ndim == 2 and q.shape[0] >= 2):
        raise ValueError("Q_waypoints 至少需要两个点（起点与终点）且维度为 (N, dof)。")
    return q

toppra_time_parameterize 

toppra_time_parameterize(waypoints: ndarray, v_max: ndarray | float, a_max: ndarray | float, dt: float = 0.01, grid_n: int = 400) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

使用 TOPPRA 对路径进行时间参数化。

参数：

名称 类型 描述 默认
waypoints ndarray

路径点数组 (N, dof)。

 必需
v_max ndarray | float

最大速度约束。

 必需
a_max ndarray | float

最大加速度约束。

 必需
dt float

输出轨迹的时间步长. Defaults to 0.01.

 0.01
grid_n int

TOPPRA 离散化网格点数. Defaults to 400.

 400

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t, q, qd, qdd) - t: 时间戳列表 - q: 关节位置列表 - qd: 关节速度列表 - qdd: 关节加速度列表

引发：

类型 描述
ValueError

如果 waypoints 维度不正确或约束格式错误。
RuntimeError

如果 TOPPRA 求解失败。
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
def toppra_time_parameterize(self,
                             waypoints: np.ndarray,
                             v_max: np.ndarray | float,
                             a_max: np.ndarray | float,
                             dt: float = 0.01,
                             grid_n: int = 400,
                             ) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """使用 TOPPRA 对路径进行时间参数化。

    Args:
        waypoints (np.ndarray): 路径点数组 (N, dof)。
        v_max (np.ndarray | float): 最大速度约束。
        a_max (np.ndarray | float): 最大加速度约束。
        dt (float, optional): 输出轨迹的时间步长. Defaults to 0.01.
        grid_n (int, optional): TOPPRA 离散化网格点数. Defaults to 400.

    Returns:
        tuple: (t, q, qd, qdd)
            - t: 时间戳列表
            - q: 关节位置列表
            - qd: 关节速度列表
            - qdd: 关节加速度列表

    Raises:
        ValueError: 如果 waypoints 维度不正确或约束格式错误。
        RuntimeError: 如果 TOPPRA 求解失败。
    """
    waypoints = np.asarray(waypoints, dtype=float)
    if waypoints.ndim != 2 or waypoints.shape[1] != 6 or waypoints.shape[0] < 2:
        raise ValueError("waypoints 必须是 (N,6) 且 N>=2")

    dof = waypoints.shape[1]

    # 1) 几何路径：用样条在路径参数 s∈[0,1] 上插值
    breaks = np.linspace(0.0, 1.0, waypoints.shape[0])
    path = ta.SplineInterpolator(breaks, waypoints)  # 官方示例同款 API

    # 2) 速度/加速度约束（上下界格式）
    v_max = np.full(dof, float(v_max)) if np.isscalar(v_max) else np.asarray(v_max, dtype=float)
    a_max = np.full(dof, float(a_max)) if np.isscalar(a_max) else np.asarray(a_max, dtype=float)
    if v_max.shape != (dof,) or a_max.shape != (dof,):
        raise ValueError("v_max/a_max 需为标量或 shape=(6,)")

    v_bounds = np.column_stack((-np.abs(v_max), np.abs(v_max)))  # (2,6) -> [v_min; v_max]
    a_bounds = np.column_stack((-np.abs(a_max), np.abs(a_max)))  # (2,6) -> [a_min; a_max]

    pc_vel = constraint.JointVelocityConstraint(v_bounds)
    pc_acc = constraint.JointAccelerationConstraint(a_bounds)

    # 3) TOPPRA 主算法 + 常用参数化器（常用且满足边界/约束）
    gridpoints = np.linspace(0, path.duration, int(grid_n))
    instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints, parametrizer="ParametrizeConstAccel")

    # 4) 求解时间参数化，得到可按时间采样的 jnt_traj
    jnt_traj = instance.compute_trajectory(sd_start=0.0, sd_end=0.0)
    if jnt_traj is None:
        raise RuntimeError("TOPPRA 求解失败：给定约束下不可行，或路径异常。")

    # 5) 按 dt 采样
    T = float(jnt_traj.duration)
    M = max(2, int(np.ceil(T / dt)) + 1)
    t = np.linspace(0.0, T, M)

    q = jnt_traj.eval(t)
    qd = jnt_traj.evald(t)
    qdd = jnt_traj.evaldd(t)
    return t.tolist(), q.tolist(), qd.tolist(), qdd.tolist()

controller.domain.services.algorithm.hand_eye_transform_domain_service

手眼标定变换服务 - Domain层

HandEyeTransformDomainService 

HandEyeTransformDomainService(config: HandEyeCalibrationConfig, kinematic_service: KinematicDomainService)

手眼标定变换服务。

职责： 1. 像素坐标 → 相机坐标系 2. 相机坐标系 → 基坐标系 3. 计算目标位姿（包含偏移和姿态调整） 4. 提供逆运动学求解

属性：

名称 类型 描述
config HandEyeCalibrationConfig

手眼标定配置。
kinematic_service KinematicDomainService

运动学服务实例。

初始化手眼标定变换服务。

参数：

名称 类型 描述 默认
config HandEyeCalibrationConfig

手眼标定配置（通过DI注入）。

 必需
kinematic_service KinematicDomainService

运动学服务。

 必需
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
25
26
27
28
29
30
31
32
33
34
35
36
37
def __init__(
    self,
    config: HandEyeCalibrationConfig,
    kinematic_service: KinematicDomainService
):
    """初始化手眼标定变换服务。

    Args:
        config: 手眼标定配置（通过DI注入）。
        kinematic_service: 运动学服务。
    """
    self.config = config
    self.kinematic_service = kinematic_service

calculate_target_joint_angles 

calculate_target_joint_angles(central_center: List[float], depth: float, real_center: List[float], real_depth: float, angle: float, current_joint_angles: List[float]) -> Optional[List[float]]

计算运动到零件位置所需的目标关节角度。

完整流程： 1. 像素坐标 → 相机坐标系 2. 正运动学：当前关节角度 → 末端位姿 3. 相机坐标系 → 基坐标系 4. 计算零件方向 + 偏移量 5. 逆运动学：目标位姿 → 目标关节角度

参数：

名称 类型 描述 默认
central_center List[float]

中心点像素坐标 [u, v]。

 必需
depth float

中心点深度 (mm)。

 必需
real_center List[float]

实际中心点像素坐标 [u, v]。

 必需
real_depth float

实际中心点深度 (mm)。

 必需
angle float

零件角度（弧度）。

 必需
current_joint_angles List[float]

当前关节角度列表（弧度）。

 必需

返回：

类型 描述
Optional[List[float]]

Optional[List[float]]: 目标关节角度列表（弧度），如果无解则返回 None。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
def calculate_target_joint_angles(
    self,
    central_center: List[float],      # [u, v] 像素坐标
    depth: float,                     # 深度 (mm)
    real_center: List[float],         # 实际中心点 [u, v]
    real_depth: float,                # 实际深度 (mm)
    angle: float,                     # 零件角度（弧度）
    current_joint_angles: List[float] # 当前关节角度（弧度）
) -> Optional[List[float]]:
    """计算运动到零件位置所需的目标关节角度。

    完整流程：
    1. 像素坐标 → 相机坐标系
    2. 正运动学：当前关节角度 → 末端位姿
    3. 相机坐标系 → 基坐标系
    4. 计算零件方向 + 偏移量
    5. 逆运动学：目标位姿 → 目标关节角度

    Args:
        central_center (List[float]): 中心点像素坐标 [u, v]。
        depth (float): 中心点深度 (mm)。
        real_center (List[float]): 实际中心点像素坐标 [u, v]。
        real_depth (float): 实际中心点深度 (mm)。
        angle (float): 零件角度（弧度）。
        current_joint_angles (List[float]): 当前关节角度列表（弧度）。

    Returns:
        Optional[List[float]]: 目标关节角度列表（弧度），如果无解则返回 None。
    """
    # 获取配置参数
    intrinsics = self.config.camera_intrinsics

    # 1. 像素坐标 → 相机坐标系（齐次坐标）
    px1, py1, pz1 = central_center[0], central_center[1], depth
    px2, py2, pz2 = real_center[0], real_center[1], real_depth

    # 深度单位转换：mm → m
    pz1 *= 0.001
    pz2 *= 0.001

    # 使用针孔相机模型
    px1 = (px1 - intrinsics.cx) * pz1 / intrinsics.fx
    py1 = (py1 - intrinsics.cy) * pz1 / intrinsics.fy
    px2 = (px2 - intrinsics.cx) * pz2 / intrinsics.fx
    py2 = (py2 - intrinsics.cy) * pz2 / intrinsics.fy

    p1_cam = np.array([px1, py1, pz1, 1])
    p2_cam = np.array([px2, py2, pz2, 1])

    # 2. 正运动学：获取当前末端位姿（4x4变换矩阵）
    forward_matrix = self.kinematic_service.get_gripper2base_rm(current_joint_angles)

    # 3. 相机坐标系 → 基坐标系
    T_cam2base = forward_matrix @ self.config.hand_eye_matrix

    p1_base = T_cam2base @ p1_cam
    p2_base = T_cam2base @ p2_cam
    v_base = p2_base - p1_base

    # 4. 计算目标位姿
    # 4.1 计算零件方向向量
    theta = np.arctan2(v_base[1], v_base[0])

    # 4.2 构建绕 Z 轴旋转的变换矩阵
    T_target2base = self.get_z_rotation_matrix(theta)
    T_target2base[:, 3] = p2_base

    # 4.3 添加偏移量
    T_offset2target = np.eye(4)
    T_offset2target[:3, 3] = self.config.target_offset.to_array()
    T_target2base = T_target2base @ T_offset2target

    # 4.4 调整末端姿态
    adjustment = self.config.end_effector_adjustment
    T_target2base = (
        T_target2base 
        @ self.get_z_rotation_matrix(adjustment.z_rotation) 
        @ self.get_y_rotation_matrix(adjustment.y_rotation)
    )

    # 5. 逆运动学求解
    theta_list = self.kinematic_service.inverse_kinematic(
        T_target2base[:3, :3], 
        T_target2base[:3, 3],
        initial_theta=current_joint_angles
    )

    return theta_list

get_z_rotation_matrix 

get_z_rotation_matrix(angle: float) -> np.ndarray

获取绕 Z 轴旋转的齐次变换矩阵。

参数：

名称 类型 描述 默认
angle float

旋转角度（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
def get_z_rotation_matrix(self, angle: float) -> np.ndarray:
    """获取绕 Z 轴旋转的齐次变换矩阵。

    Args:
        angle (float): 旋转角度（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    c = cos(angle)
    s = sin(angle)
    return np.array([
        [c, -s, 0, 0],
        [s,  c, 0, 0],
        [0,  0, 1, 0],
        [0,  0, 0, 1]
    ])

get_y_rotation_matrix 

get_y_rotation_matrix(angle: float) -> np.ndarray

获取绕 Y 轴旋转的齐次变换矩阵。

参数：

名称 类型 描述 默认
angle float

旋转角度（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
def get_y_rotation_matrix(self, angle: float) -> np.ndarray:
    """获取绕 Y 轴旋转的齐次变换矩阵。

    Args:
        angle (float): 旋转角度（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    c = cos(angle)
    s = sin(angle)
    return np.array([
        [ c, 0, s, 0],
        [ 0, 1, 0, 0],
        [-s, 0, c, 0],
        [ 0, 0, 0, 1]
    ])

get_x_rotation_matrix 

get_x_rotation_matrix(angle: float) -> np.ndarray

获取绕 X 轴旋转的齐次变换矩阵。

参数：

名称 类型 描述 默认
angle float

旋转角度（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
def get_x_rotation_matrix(self, angle: float) -> np.ndarray:
    """获取绕 X 轴旋转的齐次变换矩阵。

    Args:
        angle (float): 旋转角度（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    c = cos(angle)
    s = sin(angle)
    return np.array([
        [1,  0,  0, 0],
        [0,  c, -s, 0],
        [0,  s,  c, 0],
        [0,  0,  0, 1]
    ])