跳转至

规划器 API

所有规划器共享接口 get_state(t) -> (pos, vel, acc)（世界系线量前馈）。 SE3ToppTrajectory 还提供 get_motion_state(t) -> (T_d, V_d, Vdot_d)，其中 twist 与其导数在期望 body frame 表达。get_motion_reference 将两类接口统一成 SE(3) Lie 控制器所需的 body 运动参考。

SE(3)-TOPP

两段 SE(3) 测地线 + 每段时间最优参数化的对接轨迹。

路点：T1（起始位姿）→ T2（预对接：终点沿接近轴后撤 standoff，姿态同 终点）→ T3（终点位姿）。接近段用宽松限速、对接段用严格限速（论文 Table 8：接近 0.10 m/s·0.20 rad/s，对接 0.02 m/s·0.05 rad/s）。

源代码位于： src/compliant_docking/planning/se3_topp.py 
 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
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
class SE3ToppTrajectory:
    """两段 SE(3) 测地线 + 每段时间最优参数化的对接轨迹。

    路点：T1（起始位姿）→ T2（预对接：终点沿接近轴后撤 standoff，姿态同
    终点）→ T3（终点位姿）。接近段用宽松限速、对接段用严格限速（论文
    Table 8：接近 0.10 m/s·0.20 rad/s，对接 0.02 m/s·0.05 rad/s）。
    """

    def __init__(self, start_pos: np.ndarray, start_ori: np.ndarray,
                 final_pos: np.ndarray, final_ori: np.ndarray, *,
                 standoff: float,
                 v_max_approach: float, a_max_approach: float,
                 omega_max_approach: float, alpha_max_approach: float,
                 v_max_docking: float, a_max_docking: float,
                 omega_max_docking: float, alpha_max_docking: float):
        assert start_pos.shape == (3,) and final_pos.shape == (3,)
        assert start_ori.shape == (3, 3) and final_ori.shape == (3, 3)
        assert standoff > 0, "Stand-off must be positive"

        stroke = final_pos - start_pos
        norm = float(np.linalg.norm(stroke))
        assert norm > 0, "start 与 final 位置不可重合"
        self.axis = -stroke / norm  # 接近轴（从终点指向起始）

        self._p0 = start_pos.copy()
        self._R0 = np.array(start_ori, dtype=float)
        self._pf = final_pos.copy()
        self._Rf = np.array(final_ori, dtype=float)

        # 路点：T2 = 终点后撤（姿态同终点），T3 = 终点
        p2 = self._pf + standoff * self.axis
        T1 = pin.SE3(self._R0, self._p0)
        T2 = pin.SE3(self._Rf, p2)
        T3 = pin.SE3(self._Rf, self._pf)

        segs = [
            self._make_segment(T1, T2,
                               np.array([v_max_approach] * 3 + [omega_max_approach] * 3),
                               np.array([a_max_approach] * 3 + [alpha_max_approach] * 3)),
            self._make_segment(T2, T3,
                               np.array([v_max_docking] * 3 + [omega_max_docking] * 3),
                               np.array([a_max_docking] * 3 + [alpha_max_docking] * 3)),
        ]
        self._segments = segs
        # 元组布局：(T_a, R_a, ξ, ṡ_max, s̈_max, T)——时长在索引 5
        self._t1 = segs[0][5]
        self._t2 = segs[1][5]

    @staticmethod
    def _make_segment(T_a: pin.SE3, T_b: pin.SE3,
                      V_max6: np.ndarray, A_max6: np.ndarray) -> tuple:
        """段数据：(T_a, R_a, ξ, ṡ_max, s̈_max, T)。Theorem 1 的线性映射。"""
        T_rel = T_a.inverse() * T_b
        xi = pin.log6(T_rel).vector  # 体坐标系常螺旋 [v; ω]
        denom = np.abs(xi)
        mask = denom > 1e-12
        assert np.any(mask), "段螺旋为零（重合路点）"
        s_dot_max = float(np.min(V_max6[mask] / denom[mask]))
        s_ddot_max = float(np.min(A_max6[mask] / denom[mask]))
        T = _topp_duration(s_dot_max, s_ddot_max)
        return (T_a, np.array(T_a.rotation), xi, s_dot_max, s_ddot_max, T)

    # ------------------------------------------------------------------
    @property
    def durations(self) -> tuple[float, float]:
        """(接近段, 对接段) 时长 [s]。"""
        return self._t1, self._t2

    @property
    def total_duration(self) -> float:
        return self._t1 + self._t2

    def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        """世界系 (pos, vel, acc) 线量前馈（t≥总时长停在终点）。"""
        t = float(t)
        if t < 0.0:
            return self._p0.copy(), np.zeros(3), np.zeros(3)
        if t < self._t1:
            return self._segment_state(0, t)
        return self._segment_state(1, t - self._t1)

    def get_pose(self, t: float) -> pin.SE3:
        """t 时刻的完整位姿 T_r(t)（供按步姿态参考与测试使用）。"""
        t = float(t)
        if t < 0.0:
            return pin.SE3(self._R0, self._p0)
        if t < self._t1:
            T_a, _, xi, _, _, _ = self._segments[0]
            s, _, _ = _topp_profile_exact(t, self._segments[0][3], self._segments[0][4], self._segments[0][5])
            return T_a * pin.exp6(pin.Motion(xi * s))
        T_a, _, xi, s_dot_max, s_ddot_max, T = self._segments[1]
        s, _, _ = _topp_profile_exact(t - self._t1, s_dot_max, s_ddot_max, T)
        return T_a * pin.exp6(pin.Motion(xi * s))

    def get_motion_state(self, t: float) -> tuple[pin.SE3, np.ndarray, np.ndarray]:
        """t 时刻的期望运动参考 (T_d, V_d, Vdot_d)——**body（体坐标）量**。

        每段为常螺旋测地线 Td(s) = Ta·Exp(ξ·s)，ξ 在段内为常量，严格有::

            [V_d] = Td⁻¹·Ṫd = [ξ]·ṡ   →   V_d = ξ·ṡ
            Vdot_d = d(V_d)/dt = ξ·s̈

        （body twist 的向量导数，正是 SE(3) Lie 阻抗控制器 Eq. 62 需要的
        V̇_d；不得从世界系 pos/vel/acc 反推。）t≥总时长停在终点，t<0 在起点。

        供 control/se3_impedance.py（Kim et al. 2025 Eq. 44-66 控制器）与
        planning/motion_reference.py 适配器使用。
        """
        t = float(t)
        if t < 0.0:
            return pin.SE3(self._R0, self._p0), np.zeros(6), np.zeros(6)
        if t >= self.total_duration:
            # 停驻在终点：速度/加速度参考为零（TOPP 剖面在 t=T 的
            # s̈=-a_max 是减速段末端的 bang-bang 残留，参考已静止）
            T_a, _, xi, _, _, _ = self._segments[1]
            return T_a * pin.exp6(pin.Motion(xi)), np.zeros(6), np.zeros(6)
        if t < self._t1:
            seg_idx, t_local = 0, t
        else:
            seg_idx, t_local = 1, t - self._t1
        T_a, _, xi, s_dot_max, s_ddot_max, T = self._segments[seg_idx]
        s, s_dot, s_ddot = _topp_profile_exact(t_local, s_dot_max, s_ddot_max, T)
        T_d = T_a * pin.exp6(pin.Motion(xi * s))
        return T_d, xi * s_dot, xi * s_ddot

    def _segment_state(self, seg_idx: int, t_local: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        T_a, R_a, xi, s_dot_max, s_ddot_max, T = self._segments[seg_idx]
        s, s_dot, s_ddot = _topp_profile_exact(t_local, s_dot_max, s_ddot_max, T)

        v_b = xi[:3] * s_dot
        w_b = xi[3:] * s_dot
        M_s = T_a * pin.exp6(pin.Motion(xi * s))
        R_s = np.array(M_s.rotation)
        # 体坐标系螺旋映射到世界系：ṗ = R·v_b；
        # p̈ = R·(ω_b×v_b) + R·ξ_v·s̈（ω_b×v_b = (ξ_w×ξ_v)·ṡ² 已含 ṡ²，
        # 不得再乘 ṡ²；有限差分审计见 tests/test_se3_topp.py）
        acc = R_s @ (np.cross(w_b, v_b) + xi[:3] * s_ddot)
        return np.array(M_s.translation), R_s @ v_b, acc

属性

durations property 

durations: tuple[float, float]

(接近段, 对接段) 时长 [s]。

total_duration property 

total_duration: float

方法：

__init__ 

__init__(
    start_pos: ndarray,
    start_ori: ndarray,
    final_pos: ndarray,
    final_ori: ndarray,
    *,
    standoff: float,
    v_max_approach: float,
    a_max_approach: float,
    omega_max_approach: float,
    alpha_max_approach: float,
    v_max_docking: float,
    a_max_docking: float,
    omega_max_docking: float,
    alpha_max_docking: float,
)
源代码位于： src/compliant_docking/planning/se3_topp.py 
 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
def __init__(self, start_pos: np.ndarray, start_ori: np.ndarray,
             final_pos: np.ndarray, final_ori: np.ndarray, *,
             standoff: float,
             v_max_approach: float, a_max_approach: float,
             omega_max_approach: float, alpha_max_approach: float,
             v_max_docking: float, a_max_docking: float,
             omega_max_docking: float, alpha_max_docking: float):
    assert start_pos.shape == (3,) and final_pos.shape == (3,)
    assert start_ori.shape == (3, 3) and final_ori.shape == (3, 3)
    assert standoff > 0, "Stand-off must be positive"

    stroke = final_pos - start_pos
    norm = float(np.linalg.norm(stroke))
    assert norm > 0, "start 与 final 位置不可重合"
    self.axis = -stroke / norm  # 接近轴（从终点指向起始）

    self._p0 = start_pos.copy()
    self._R0 = np.array(start_ori, dtype=float)
    self._pf = final_pos.copy()
    self._Rf = np.array(final_ori, dtype=float)

    # 路点：T2 = 终点后撤（姿态同终点），T3 = 终点
    p2 = self._pf + standoff * self.axis
    T1 = pin.SE3(self._R0, self._p0)
    T2 = pin.SE3(self._Rf, p2)
    T3 = pin.SE3(self._Rf, self._pf)

    segs = [
        self._make_segment(T1, T2,
                           np.array([v_max_approach] * 3 + [omega_max_approach] * 3),
                           np.array([a_max_approach] * 3 + [alpha_max_approach] * 3)),
        self._make_segment(T2, T3,
                           np.array([v_max_docking] * 3 + [omega_max_docking] * 3),
                           np.array([a_max_docking] * 3 + [alpha_max_docking] * 3)),
    ]
    self._segments = segs
    # 元组布局：(T_a, R_a, ξ, ṡ_max, s̈_max, T)——时长在索引 5
    self._t1 = segs[0][5]
    self._t2 = segs[1][5]

get_state 

get_state(
    t: float,
) -> tuple[ndarray, ndarray, ndarray]

世界系 (pos, vel, acc) 线量前馈（t≥总时长停在终点）。

源代码位于： src/compliant_docking/planning/se3_topp.py 
129
130
131
132
133
134
135
136
def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """世界系 (pos, vel, acc) 线量前馈（t≥总时长停在终点）。"""
    t = float(t)
    if t < 0.0:
        return self._p0.copy(), np.zeros(3), np.zeros(3)
    if t < self._t1:
        return self._segment_state(0, t)
    return self._segment_state(1, t - self._t1)

get_pose 

get_pose(t: float) -> SE3

t 时刻的完整位姿 T_r(t)（供按步姿态参考与测试使用）。

源代码位于： src/compliant_docking/planning/se3_topp.py 
138
139
140
141
142
143
144
145
146
147
148
149
def get_pose(self, t: float) -> pin.SE3:
    """t 时刻的完整位姿 T_r(t)（供按步姿态参考与测试使用）。"""
    t = float(t)
    if t < 0.0:
        return pin.SE3(self._R0, self._p0)
    if t < self._t1:
        T_a, _, xi, _, _, _ = self._segments[0]
        s, _, _ = _topp_profile_exact(t, self._segments[0][3], self._segments[0][4], self._segments[0][5])
        return T_a * pin.exp6(pin.Motion(xi * s))
    T_a, _, xi, s_dot_max, s_ddot_max, T = self._segments[1]
    s, _, _ = _topp_profile_exact(t - self._t1, s_dot_max, s_ddot_max, T)
    return T_a * pin.exp6(pin.Motion(xi * s))

get_motion_state 

get_motion_state(
    t: float,
) -> tuple[SE3, ndarray, ndarray]

t 时刻的期望运动参考 (T_d, V_d, Vdot_d)——body（体坐标）量。

每段为常螺旋测地线 Td(s) = Ta·Exp(ξ·s)，ξ 在段内为常量，严格有::

[V_d] = Td⁻¹·Ṫd = [ξ]·ṡ   →   V_d = ξ·ṡ
Vdot_d = d(V_d)/dt = ξ·s̈

（body twist 的向量导数，正是 SE(3) Lie 阻抗控制器 Eq. 62 需要的 V̇_d；不得从世界系 pos/vel/acc 反推。）t≥总时长停在终点，t<0 在起点。

供 control/se3_impedance.py（Kim et al. 2025 Eq. 44-66 控制器）与 planning/motion_reference.py 适配器使用。

源代码位于： src/compliant_docking/planning/se3_topp.py 
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
def get_motion_state(self, t: float) -> tuple[pin.SE3, np.ndarray, np.ndarray]:
    """t 时刻的期望运动参考 (T_d, V_d, Vdot_d)——**body（体坐标）量**。

    每段为常螺旋测地线 Td(s) = Ta·Exp(ξ·s)，ξ 在段内为常量，严格有::

        [V_d] = Td⁻¹·Ṫd = [ξ]·ṡ   →   V_d = ξ·ṡ
        Vdot_d = d(V_d)/dt = ξ·s̈

    （body twist 的向量导数，正是 SE(3) Lie 阻抗控制器 Eq. 62 需要的
    V̇_d；不得从世界系 pos/vel/acc 反推。）t≥总时长停在终点，t<0 在起点。

    供 control/se3_impedance.py（Kim et al. 2025 Eq. 44-66 控制器）与
    planning/motion_reference.py 适配器使用。
    """
    t = float(t)
    if t < 0.0:
        return pin.SE3(self._R0, self._p0), np.zeros(6), np.zeros(6)
    if t >= self.total_duration:
        # 停驻在终点：速度/加速度参考为零（TOPP 剖面在 t=T 的
        # s̈=-a_max 是减速段末端的 bang-bang 残留，参考已静止）
        T_a, _, xi, _, _, _ = self._segments[1]
        return T_a * pin.exp6(pin.Motion(xi)), np.zeros(6), np.zeros(6)
    if t < self._t1:
        seg_idx, t_local = 0, t
    else:
        seg_idx, t_local = 1, t - self._t1
    T_a, _, xi, s_dot_max, s_ddot_max, T = self._segments[seg_idx]
    s, s_dot, s_ddot = _topp_profile_exact(t_local, s_dot_max, s_ddot_max, T)
    T_d = T_a * pin.exp6(pin.Motion(xi * s))
    return T_d, xi * s_dot, xi * s_ddot

SE(3) 运动参考适配

get_motion_reference(planner, t, r_des) -> tuple[pin.SE3, np.ndarray, np.ndarray]

返回 (T_d, V_d, Vdot_d)。若规划器实现 get_motion_state 则直接透传；否则 调用 get_state，以固定 r_des 构造位姿并把世界系线速度/加速度旋转到 desired body frame，角速度与角加速度置零。

解耦五次 / 两段式对接 / 圆+8字

三轴解耦的五次多项式轨迹规划器（x/y/z 分别独立） Decoupled quintic polynomial trajectory planner for x, y, z axes

源代码位于： src/compliant_docking/planning/trajectory.py 
 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
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
class DecoupledQuinticTrajectory:
    """
    三轴解耦的五次多项式轨迹规划器（x/y/z 分别独立）
    Decoupled quintic polynomial trajectory planner for x, y, z axes
    """
    def __init__(self, start_pos: np.ndarray, target_pos: np.ndarray, duration: float):
        """
        Initialize the trajectory planner with decoupled planning for each axis

        Args:
            start_pos: 初始位置 (x0, y0, z0) / Initial position
            target_pos: 目标位置 (xf, yf, zf) / Target position
            duration: 轨迹持续时间（秒） / Trajectory duration (s)

        Note:
            三轴各自满足端点速度/加速度为零，生成 C2 连续的平滑轨迹 /
            Each axis satisfies zero vel/acc at endpoints (C2 continuity)
        """
        assert start_pos.shape == (3,), "Start position must be 3D vector"
        assert target_pos.shape == (3,), "Target position must be 3D vector"
        assert duration > 0, "Duration must be positive"

        self.p0 = start_pos
        self.pf = target_pos
        self.T = duration

        self.ax = self._solve_quintic_coefficients(start_pos[0], target_pos[0])
        self.ay = self._solve_quintic_coefficients(start_pos[1], target_pos[1])
        self.az = self._solve_quintic_coefficients(start_pos[2], target_pos[2])

        self.coefficients = np.vstack([self.ax, self.ay, self.az])

    def _solve_quintic_coefficients(self, p0: float, pf: float) -> np.ndarray:
        """
        单轴五次多项式系数求解 / Solve coefficients of 1D quintic polynomial:
        p(t) = a0 t^5 + a1 t^4 + a2 t^3 + a3 t^2 + a4 t + a5
        约束 / Constraints：p(0)=p0, p(T)=pf, p'(0)=p'(T)=0, p''(0)=p''(T)=0
        parameters / 参数:
            p0: 初始位置 / Initial position
            pf: 目标位置 / Target position
        """
        A = np.array([
            [0, 0, 0, 0, 0, 1],
            [self.T**5, self.T**4, self.T**3, self.T**2, self.T, 1],
            [0, 0, 0, 0, 1, 0],
            [5*self.T**4, 4*self.T**3, 3*self.T**2, 2*self.T, 1, 0],
            [0, 0, 0, 2, 0, 0],
            [20*self.T**3, 12*self.T**2, 6*self.T, 2, 0, 0]
        ])

        b = np.array([p0, pf, 0, 0, 0, 0])

        return np.linalg.solve(A, b)

    def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        """
        获取时刻 t 的位置/速度/加速度；三轴独立计算 /
        Get position, velocity and acceleration at time t; axes computed independently

        Args:
            t: 当前时间（秒） / Current time (s)

        Returns:
            (pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
        """
        t = np.clip(t, 0, self.T)

        pos = np.zeros(3)
        vel = np.zeros(3)
        acc = np.zeros(3)

        t_pos = np.array([t**5, t**4, t**3, t**2, t, 1])
        t_vel = np.array([5*t**4, 4*t**3, 3*t**2, 2*t, 1, 0])
        t_acc = np.array([20*t**3, 12*t**2, 6*t, 2, 0, 0])

        for i in range(3):
            pos[i] = self.coefficients[i] @ t_pos
            vel[i] = self.coefficients[i] @ t_vel
            acc[i] = self.coefficients[i] @ t_acc

        return pos, vel, acc

    def verify_boundary_conditions(self, tol: float = 1e-10) -> bool:
        """
        验证边界条件（起止位置、速度=0、加速度=0）是否满足 /
        Verify that endpoint position/velocity/acceleration constraints hold

        Args:
            tol: 浮点比较容差 / Tolerance for comparisons

        Returns:
            是否全部满足 / True if all constraints satisfied
        """
        pos_start, vel_start, acc_start = self.get_state(0)
        pos_end, vel_end, acc_end = self.get_state(self.T)

        conditions = [
            np.allclose(pos_start, self.p0, atol=tol),
            np.allclose(pos_end, self.pf, atol=tol),
            np.allclose(vel_start, np.zeros(3), atol=tol),
            np.allclose(vel_end, np.zeros(3), atol=tol),
            np.allclose(acc_start, np.zeros(3), atol=tol),
            np.allclose(acc_end, np.zeros(3), atol=tol)
        ]

        return all(conditions)

方法：

__init__ 

__init__(
    start_pos: ndarray,
    target_pos: ndarray,
    duration: float,
)

Initialize the trajectory planner with decoupled planning for each axis

参数：

名称 类型 描述 默认
start_pos ndarray

初始位置 (x0, y0, z0) / Initial position

 必需
target_pos ndarray

目标位置 (xf, yf, zf) / Target position

 必需
duration float

轨迹持续时间（秒） / Trajectory duration (s)

 必需
Note

三轴各自满足端点速度/加速度为零，生成 C2 连续的平滑轨迹 / Each axis satisfies zero vel/acc at endpoints (C2 continuity)

源代码位于： src/compliant_docking/planning/trajectory.py 
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
def __init__(self, start_pos: np.ndarray, target_pos: np.ndarray, duration: float):
    """
    Initialize the trajectory planner with decoupled planning for each axis

    Args:
        start_pos: 初始位置 (x0, y0, z0) / Initial position
        target_pos: 目标位置 (xf, yf, zf) / Target position
        duration: 轨迹持续时间（秒） / Trajectory duration (s)

    Note:
        三轴各自满足端点速度/加速度为零，生成 C2 连续的平滑轨迹 /
        Each axis satisfies zero vel/acc at endpoints (C2 continuity)
    """
    assert start_pos.shape == (3,), "Start position must be 3D vector"
    assert target_pos.shape == (3,), "Target position must be 3D vector"
    assert duration > 0, "Duration must be positive"

    self.p0 = start_pos
    self.pf = target_pos
    self.T = duration

    self.ax = self._solve_quintic_coefficients(start_pos[0], target_pos[0])
    self.ay = self._solve_quintic_coefficients(start_pos[1], target_pos[1])
    self.az = self._solve_quintic_coefficients(start_pos[2], target_pos[2])

    self.coefficients = np.vstack([self.ax, self.ay, self.az])

get_state 

get_state(
    t: float,
) -> tuple[ndarray, ndarray, ndarray]

获取时刻 t 的位置/速度/加速度；三轴独立计算 / Get position, velocity and acceleration at time t; axes computed independently

参数：

名称 类型 描述 默认
t float

当前时间（秒） / Current time (s)

 必需

返回：

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

(pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
源代码位于： src/compliant_docking/planning/trajectory.py 
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
def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    获取时刻 t 的位置/速度/加速度；三轴独立计算 /
    Get position, velocity and acceleration at time t; axes computed independently

    Args:
        t: 当前时间（秒） / Current time (s)

    Returns:
        (pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
    """
    t = np.clip(t, 0, self.T)

    pos = np.zeros(3)
    vel = np.zeros(3)
    acc = np.zeros(3)

    t_pos = np.array([t**5, t**4, t**3, t**2, t, 1])
    t_vel = np.array([5*t**4, 4*t**3, 3*t**2, 2*t, 1, 0])
    t_acc = np.array([20*t**3, 12*t**2, 6*t, 2, 0, 0])

    for i in range(3):
        pos[i] = self.coefficients[i] @ t_pos
        vel[i] = self.coefficients[i] @ t_vel
        acc[i] = self.coefficients[i] @ t_acc

    return pos, vel, acc

verify_boundary_conditions 

verify_boundary_conditions(
    tol: float = 1e-10,
) -> bool

验证边界条件（起止位置、速度=0、加速度=0）是否满足 / Verify that endpoint position/velocity/acceleration constraints hold

参数：

名称 类型 描述 默认
tol float

浮点比较容差 / Tolerance for comparisons

 1e-10

返回：

类型 描述
bool

是否全部满足 / True if all constraints satisfied
源代码位于： src/compliant_docking/planning/trajectory.py 
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
def verify_boundary_conditions(self, tol: float = 1e-10) -> bool:
    """
    验证边界条件（起止位置、速度=0、加速度=0）是否满足 /
    Verify that endpoint position/velocity/acceleration constraints hold

    Args:
        tol: 浮点比较容差 / Tolerance for comparisons

    Returns:
        是否全部满足 / True if all constraints satisfied
    """
    pos_start, vel_start, acc_start = self.get_state(0)
    pos_end, vel_end, acc_end = self.get_state(self.T)

    conditions = [
        np.allclose(pos_start, self.p0, atol=tol),
        np.allclose(pos_end, self.pf, atol=tol),
        np.allclose(vel_start, np.zeros(3), atol=tol),
        np.allclose(vel_end, np.zeros(3), atol=tol),
        np.allclose(acc_start, np.zeros(3), atol=tol),
        np.allclose(acc_end, np.zeros(3), atol=tol)
    ]

    return all(conditions)

两段式对接轨迹：接近段（approach，宽松限速）+ 对接段（docking，严格限速） Two-phase docking trajectory: approach phase (loose limits) + docking phase (strict limits)

任务结构取自 Ren & Shan 2026 (Acta Astronautica) 的两段式对接：末端先以宽松限速 从 start_pos 接近到预对接点（pre-dock，= final_pos 沿接近轴后撤 standoff），再以 严格限速（0.02 m/s 量级）完成最后一段进给。峰值接触力主要由接触前速度决定， 因此对接段的低限速是压低峰值力的关键。

与论文 SE(3)-TOPP 的关系与简化点 / Relation to the paper's SE(3)-TOPP and simplifications: - 论文对整条 SE(3) 路径做时间最优参数化（TOPP），全程不停顿、速度沿路径连续变化； - 本实现是其保守简化版：两段各自独立做 rest-to-rest 五次多项式，途经预对接点处 瞬时停顿（速度/加速度归零）后再进入对接段——时间上不是最优，更保守； - 每段时长由限速解析反推（quintic_rest_to_rest_duration）：三轴同步使用同一段 时长 T，并用该段位移的 3D 标量长度 L 保守估计合成速度/加速度峰值 （v_peak = 1.875·L/T、a_peak = 5.7735·L/T²）。对直线段而言三轴共享同一无量纲 时间形状，该估计恰为精确值。

位置/速度/加速度全程 C2 连续：接合点（预对接点）两侧均为 rest-to-rest，自然衔接。

源代码位于： src/compliant_docking/planning/trajectory.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
class TwoPhaseDockingTrajectory:
    """
    两段式对接轨迹：接近段（approach，宽松限速）+ 对接段（docking，严格限速）
    Two-phase docking trajectory: approach phase (loose limits) + docking phase (strict limits)

    任务结构取自 Ren & Shan 2026 (Acta Astronautica) 的两段式对接：末端先以宽松限速
    从 start_pos 接近到预对接点（pre-dock，= final_pos 沿接近轴后撤 standoff），再以
    严格限速（0.02 m/s 量级）完成最后一段进给。峰值接触力主要由接触前速度决定，
    因此对接段的低限速是压低峰值力的关键。

    与论文 SE(3)-TOPP 的关系与简化点 / Relation to the paper's SE(3)-TOPP and simplifications:
    - 论文对整条 SE(3) 路径做时间最优参数化（TOPP），全程不停顿、速度沿路径连续变化；
    - 本实现是其保守简化版：两段各自独立做 rest-to-rest 五次多项式，途经预对接点处
      瞬时停顿（速度/加速度归零）后再进入对接段——时间上不是最优，更保守；
    - 每段时长由限速解析反推（quintic_rest_to_rest_duration）：三轴同步使用同一段
      时长 T，并用该段位移的 3D 标量长度 L 保守估计合成速度/加速度峰值
      （v_peak = 1.875·L/T、a_peak = 5.7735·L/T²）。对直线段而言三轴共享同一无量纲
      时间形状，该估计恰为精确值。

    位置/速度/加速度全程 C2 连续：接合点（预对接点）两侧均为 rest-to-rest，自然衔接。
    """

    def __init__(self,
                 start_pos: np.ndarray,
                 final_pos: np.ndarray,
                 standoff: float,
                 v_max_approach: float,
                 a_max_approach: float,
                 v_max_docking: float,
                 a_max_docking: float):
        """
        Initialize the two-phase docking trajectory

        Args:
            start_pos: 初始位置 (3D) / Initial position
            final_pos: 最终对接目标位置 (3D) / Final docking target position
            standoff: 预对接点沿接近轴的后撤距离 [m] / stand-off retreat distance [m]
            v_max_approach: 接近段线速度上限 / approach phase limits
            a_max_approach: 接近段线加速度上限 / approach phase limits
            v_max_docking: 对接段线速度上限 / docking phase limits
            a_max_docking: 对接段线加速度上限 / docking phase limits
        """
        assert start_pos.shape == (3,), "Start position must be 3D vector"
        assert final_pos.shape == (3,), "Final position must be 3D vector"
        assert standoff > 0, "Stand-off must be positive"
        assert v_max_approach > 0 and a_max_approach > 0, "Approach limits must be positive"
        assert v_max_docking > 0 and a_max_docking > 0, "Docking limits must be positive"

        self.p0 = start_pos
        self.pf = final_pos
        self.standoff = standoff

        # 接近轴 = 推进方向（start→final）的反向，即从目标指向"上方"
        stroke = final_pos - start_pos
        self.axis = -stroke / np.linalg.norm(stroke)

        # 预对接点 = 最终目标沿接近轴后撤 standoff
        self._pre_dock = final_pos + standoff * self.axis

        # 两段时长分别由各自限速反推（L 用该段位移的 3D 范数）
        length_approach = float(np.linalg.norm(self._pre_dock - start_pos))
        length_docking = float(np.linalg.norm(final_pos - self._pre_dock))
        self._t1 = quintic_rest_to_rest_duration(length_approach, v_max_approach, a_max_approach)
        self._t2 = quintic_rest_to_rest_duration(length_docking, v_max_docking, a_max_docking)

        # 复用解耦五次规划器：两段各自 rest-to-rest，接合点自然 C2
        self._approach = DecoupledQuinticTrajectory(start_pos, self._pre_dock, self._t1)
        self._docking = DecoupledQuinticTrajectory(self._pre_dock, final_pos, self._t2)

    @property
    def durations(self) -> tuple[float, float]:
        """(接近段时长 t1, 对接段时长 t2)，单位秒 / (approach, docking) durations in s"""
        return (self._t1, self._t2)

    @property
    def pre_dock_pos(self) -> np.ndarray:
        """预对接点位置（副本）/ Pre-dock waypoint position (copy)"""
        return self._pre_dock.copy()

    @property
    def total_duration(self) -> float:
        """轨迹总时长 t1 + t2（秒）/ Total trajectory duration t1 + t2 (s)"""
        return self._t1 + self._t2

    def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        """
        获取时刻 t 的位置/速度/加速度 / Get position, velocity and acceleration at time t

        时间分段：
        - t < 0：按 t = 0 处理（停在起点）；
        - t ∈ [0, t1)：接近段（段内局部时间采样）；
        - t ∈ [t1, t1+t2)：对接段（段内局部时间采样）；
        - t ≥ t1+t2：停在 final_pos，速度/加速度为 0。

        Args:
            t: 当前时间（秒） / Current time (s)

        Returns:
            (pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
        """
        if t < 0.0:
            return self._approach.get_state(0.0)
        if t < self._t1:
            return self._approach.get_state(t)
        # t ≥ t1：第二段按局部时间采样；超过 t2 时内部 clip 停在 final（vel/acc = 0）
        return self._docking.get_state(t - self._t1)

属性

durations property 

durations: tuple[float, float]

(接近段时长 t1, 对接段时长 t2)，单位秒 / (approach, docking) durations in s

total_duration property 

total_duration: float

轨迹总时长 t1 + t2（秒）/ Total trajectory duration t1 + t2 (s)

pre_dock_pos property 

pre_dock_pos: ndarray

预对接点位置（副本）/ Pre-dock waypoint position (copy)

方法：

__init__ 

__init__(
    start_pos: ndarray,
    final_pos: ndarray,
    standoff: float,
    v_max_approach: float,
    a_max_approach: float,
    v_max_docking: float,
    a_max_docking: float,
)

Initialize the two-phase docking trajectory

参数：

名称 类型 描述 默认
start_pos ndarray

初始位置 (3D) / Initial position

 必需
final_pos ndarray

最终对接目标位置 (3D) / Final docking target position

 必需
standoff float

预对接点沿接近轴的后撤距离 [m] / stand-off retreat distance [m]

 必需
v_max_approach float

接近段线速度上限 / approach phase limits

 必需
a_max_approach float

接近段线加速度上限 / approach phase limits

 必需
v_max_docking float

对接段线速度上限 / docking phase limits

 必需
a_max_docking float

对接段线加速度上限 / docking phase limits

 必需
源代码位于： src/compliant_docking/planning/trajectory.py 
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
def __init__(self,
             start_pos: np.ndarray,
             final_pos: np.ndarray,
             standoff: float,
             v_max_approach: float,
             a_max_approach: float,
             v_max_docking: float,
             a_max_docking: float):
    """
    Initialize the two-phase docking trajectory

    Args:
        start_pos: 初始位置 (3D) / Initial position
        final_pos: 最终对接目标位置 (3D) / Final docking target position
        standoff: 预对接点沿接近轴的后撤距离 [m] / stand-off retreat distance [m]
        v_max_approach: 接近段线速度上限 / approach phase limits
        a_max_approach: 接近段线加速度上限 / approach phase limits
        v_max_docking: 对接段线速度上限 / docking phase limits
        a_max_docking: 对接段线加速度上限 / docking phase limits
    """
    assert start_pos.shape == (3,), "Start position must be 3D vector"
    assert final_pos.shape == (3,), "Final position must be 3D vector"
    assert standoff > 0, "Stand-off must be positive"
    assert v_max_approach > 0 and a_max_approach > 0, "Approach limits must be positive"
    assert v_max_docking > 0 and a_max_docking > 0, "Docking limits must be positive"

    self.p0 = start_pos
    self.pf = final_pos
    self.standoff = standoff

    # 接近轴 = 推进方向（start→final）的反向，即从目标指向"上方"
    stroke = final_pos - start_pos
    self.axis = -stroke / np.linalg.norm(stroke)

    # 预对接点 = 最终目标沿接近轴后撤 standoff
    self._pre_dock = final_pos + standoff * self.axis

    # 两段时长分别由各自限速反推（L 用该段位移的 3D 范数）
    length_approach = float(np.linalg.norm(self._pre_dock - start_pos))
    length_docking = float(np.linalg.norm(final_pos - self._pre_dock))
    self._t1 = quintic_rest_to_rest_duration(length_approach, v_max_approach, a_max_approach)
    self._t2 = quintic_rest_to_rest_duration(length_docking, v_max_docking, a_max_docking)

    # 复用解耦五次规划器：两段各自 rest-to-rest，接合点自然 C2
    self._approach = DecoupledQuinticTrajectory(start_pos, self._pre_dock, self._t1)
    self._docking = DecoupledQuinticTrajectory(self._pre_dock, final_pos, self._t2)

get_state 

get_state(
    t: float,
) -> tuple[ndarray, ndarray, ndarray]

获取时刻 t 的位置/速度/加速度 / Get position, velocity and acceleration at time t

时间分段： - t < 0：按 t = 0 处理（停在起点）； - t ∈ [0, t1)：接近段（段内局部时间采样）； - t ∈ [t1, t1+t2)：对接段（段内局部时间采样）； - t ≥ t1+t2：停在 final_pos，速度/加速度为 0。

参数：

名称 类型 描述 默认
t float

当前时间（秒） / Current time (s)

 必需

返回：

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

(pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
源代码位于： src/compliant_docking/planning/trajectory.py 
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    获取时刻 t 的位置/速度/加速度 / Get position, velocity and acceleration at time t

    时间分段：
    - t < 0：按 t = 0 处理（停在起点）；
    - t ∈ [0, t1)：接近段（段内局部时间采样）；
    - t ∈ [t1, t1+t2)：对接段（段内局部时间采样）；
    - t ≥ t1+t2：停在 final_pos，速度/加速度为 0。

    Args:
        t: 当前时间（秒） / Current time (s)

    Returns:
        (pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
    """
    if t < 0.0:
        return self._approach.get_state(0.0)
    if t < self._t1:
        return self._approach.get_state(t)
    # t ≥ t1：第二段按局部时间采样；超过 t2 时内部 clip 停在 final（vel/acc = 0）
    return self._docking.get_state(t - self._t1)

圆形 + 8 字形轨迹跟踪测试规划器：过渡 → 竖直圆 → 过渡 → 平面 8 字 → 保持 Circle + figure-8 trajectory planner for controller tracking tests (hold at the end)

在不考虑对接的条件下测试控制器的轨迹跟踪能力（轨迹形状参考同类任务空间 跟踪实验，速度/加速度改为对位置公式解析求导，无数值差分）。

时间结构（四段 + 保持）： - 段1 过渡 [0, T_tr)：start_pos → 圆起点 P1（rest-to-rest 五次时间缩放）； - 段2 圆周 [T_tr, T_tr+T_c)：竖直圆（y-z 平面），θ = 2π·f_c·(t-t0)； - 段3 过渡 [T_tr+T_c, 2·T_tr+T_c)：圆终点 P2 → 8 字中心 C（rest-to-rest）； - 段4 8字 [2·T_tr+T_c, total)：水平 8 字（x-y 平面），φ = 2π·f_8·(t-t0)； - t ≥ total：保持在 8 字结束点，速度/加速度为零。

几何 / Geometry: - 圆心 C = start_pos + [0, 0, circle_center_offset]（默认在起点正下方 0.06 m）； - 圆起点 P1 = C + [0, r, 0]；圆终点 P2 = 段2 结束时刻的圆周位置 （f_c·T_c 非整圈时 P2 ≠ P1，过渡段3 必须从 P2 出发）； - 8 字为 Lissajous 曲线 p = C + [ax·sinφ, ay·sin(2φ), 0]。

连续性说明 / Continuity note: 每一段均使用五次时间缩放。圆周和 8 字的几何相位也由该缩放推进，因此在每个 段边界位置、速度和加速度都连续（C2）；这使测试聚焦于轨迹跟踪，而不是人为的 速度阶跃。circle_frequency / figure8_frequency 表示该段总圈数除以该段 时长的平均频率，故总相位仍为 2π·frequency·duration。

源代码位于： src/compliant_docking/planning/trajectory.py 
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
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
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
class CircleFigure8Trajectory:
    """
    圆形 + 8 字形轨迹跟踪测试规划器：过渡 → 竖直圆 → 过渡 → 平面 8 字 → 保持
    Circle + figure-8 trajectory planner for controller tracking tests (hold at the end)

    在不考虑对接的条件下测试控制器的轨迹跟踪能力（轨迹形状参考同类任务空间
    跟踪实验，速度/加速度改为对位置公式解析求导，无数值差分）。

    时间结构（四段 + 保持）：
    - 段1 过渡 [0, T_tr)：start_pos → 圆起点 P1（rest-to-rest 五次时间缩放）；
    - 段2 圆周 [T_tr, T_tr+T_c)：竖直圆（y-z 平面），θ = 2π·f_c·(t-t0)；
    - 段3 过渡 [T_tr+T_c, 2·T_tr+T_c)：圆终点 P2 → 8 字中心 C（rest-to-rest）；
    - 段4 8字 [2·T_tr+T_c, total)：水平 8 字（x-y 平面），φ = 2π·f_8·(t-t0)；
    - t ≥ total：保持在 8 字结束点，速度/加速度为零。

    几何 / Geometry:
    - 圆心 C = start_pos + [0, 0, circle_center_offset]（默认在起点正下方 0.06 m）；
    - 圆起点 P1 = C + [0, r, 0]；圆终点 P2 = 段2 结束时刻的圆周位置
      （f_c·T_c 非整圈时 P2 ≠ P1，过渡段3 必须从 P2 出发）；
    - 8 字为 Lissajous 曲线 p = C + [ax·sinφ, ay·sin(2φ), 0]。

    连续性说明 / Continuity note:
    每一段均使用五次时间缩放。圆周和 8 字的几何相位也由该缩放推进，因此在每个
    段边界位置、速度和加速度都连续（C2）；这使测试聚焦于轨迹跟踪，而不是人为的
    速度阶跃。``circle_frequency`` / ``figure8_frequency`` 表示该段总圈数除以该段
    时长的平均频率，故总相位仍为 ``2π·frequency·duration``。
    """

    def __init__(self, start_pos: np.ndarray, *,
                 transition_duration: float = 1.5,
                 circle_duration: float = 5.0,
                 circle_radius: float = 0.10,
                 circle_frequency: float = 0.2,
                 circle_center_offset: float = -0.06,
                 figure8_duration: float = 5.0,
                 figure8_radius_x: float = 0.10,
                 figure8_radius_y: float = 0.07,
                 figure8_frequency: float = 0.2):
        """
        Initialize the circle + figure-8 tracking-test trajectory

        Args:
            start_pos: 轨迹出发点 (3D) / Start position (3D)
            transition_duration: 过渡段时长 T_tr [s] / Transition segment duration
            circle_duration: 圆周段时长 T_c [s] / Circle segment duration
            circle_radius: 圆周半径 r [m] / Circle radius
            circle_frequency: 圆周频率 f_c [Hz] / Circle frequency
            circle_center_offset: 圆心相对 start_pos 的 z 偏移 [m]（可正可负） /
                Circle center offset below/above the start position
            figure8_duration: 8 字段时长 T_8 [s] / Figure-8 segment duration
            figure8_radius_x: 8 字 x 半幅值 [m] / Figure-8 radius
            figure8_radius_y: 8 字 y 半幅值 [m] / Figure-8 radius
            figure8_frequency: 8 字频率 f_8 [Hz] / Figure-8 frequency
        """
        assert start_pos.shape == (3,), "Start position must be 3D vector"
        assert transition_duration > 0, "Transition duration must be positive"
        assert circle_duration > 0, "Circle duration must be positive"
        assert circle_radius > 0, "Circle radius must be positive"
        assert circle_frequency > 0, "Circle frequency must be positive"
        assert np.isfinite(circle_center_offset), "Circle center offset must be finite"
        assert figure8_duration > 0, "Figure-8 duration must be positive"
        assert figure8_radius_x > 0, "Figure-8 radius x must be positive"
        assert figure8_radius_y > 0, "Figure-8 radius y must be positive"
        assert figure8_frequency > 0, "Figure-8 frequency must be positive"

        # 存副本，防外部修改 / Store a copy so external mutation cannot affect us
        self.start_pos = np.array(start_pos, dtype=float)

        self._t_tr = float(transition_duration)
        self._t_c = float(circle_duration)
        self._r_c = float(circle_radius)
        self._f_c = float(circle_frequency)
        self._ax8 = float(figure8_radius_x)
        self._ay8 = float(figure8_radius_y)
        self._f_8 = float(figure8_frequency)

        # 关键几何点 / Key waypoints
        self._center = self.start_pos + np.array([0.0, 0.0, float(circle_center_offset)])
        self._p1 = self._center + np.array([0.0, self._r_c, 0.0])
        # 圆终点 = 段2 结束时刻的圆周位置（f_c·T_c 非整圈时不在 P1）
        theta_end = 2.0 * np.pi * self._f_c * self._t_c
        self._p2 = self._center + self._r_c * np.array(
            [0.0, np.cos(theta_end), np.sin(theta_end)])
        # 8 字结束点（t ≥ total 时保持于此）
        phi_end = 2.0 * np.pi * self._f_8 * float(figure8_duration)
        self._end_pos = self._center + np.array(
            [self._ax8 * np.sin(phi_end), self._ay8 * np.sin(2.0 * phi_end), 0.0])

        # 段边界时间 / Segment boundary times
        self._t_8 = float(figure8_duration)
        self._t1 = self._t_tr
        self._t2 = self._t_tr + self._t_c
        self._t3 = 2.0 * self._t_tr + self._t_c
        self._total = 2.0 * self._t_tr + self._t_c + self._t_8

    # ---- 结构属性（供指标分段统计） / Structural properties (for per-segment metrics) ----

    @property
    def durations(self) -> tuple[float, float, float]:
        """(过渡段 T_tr, 圆周段 T_c, 8 字段 T_8) 时长（秒）/ (transition, circle, figure-8) durations"""
        return (self._t_tr, self._t_c, self._t_8)

    @property
    def total_duration(self) -> float:
        """轨迹总时长 2·T_tr + T_c + T_8（秒）/ Total duration 2·T_tr + T_c + T_8 (s)"""
        return self._total

    @property
    def segments(self) -> tuple[tuple[str, float, float], ...]:
        """四段 (名称, t_start, t_end)：过渡1/圆周/过渡2/8字 /
        The four segments as (name, t_start, t_end): transition/circle/transition/figure-8"""
        return (
            ("过渡1", 0.0, self._t1),
            ("圆周", self._t1, self._t2),
            ("过渡2", self._t2, self._t3),
            ("8字", self._t3, self._total),
        )

    # ---- 采样 / Sampling ----

    @staticmethod
    def _quintic_transition(a: np.ndarray, b: np.ndarray,
                            u: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        """
        rest-to-rest 五次时间缩放的过渡段状态（u = (t-t0)/T 已归一化）/
        Quintic rest-to-rest transition state at normalized time u

        s(u) = 10u³ − 15u⁴ + 6u⁵，p = A + s(u)·(B−A)，
        v = s'(u)/T·(B−A)，a = s''(u)/T²·(B−A)；T 因子已折算进返回值的调用侧 /
        The 1/T and 1/T² factors are applied by the caller
        """
        s = 10.0 * u**3 - 15.0 * u**4 + 6.0 * u**5
        ds = 30.0 * u**2 - 60.0 * u**3 + 30.0 * u**4
        dds = 60.0 * u - 180.0 * u**2 + 120.0 * u**3
        delta = b - a
        return a + s * delta, ds * delta, dds * delta

    @staticmethod
    def _quintic_scale(u: float) -> tuple[float, float, float]:
        """五次时间缩放及其对归一化时间的前两阶导数。"""
        return (
            10.0 * u**3 - 15.0 * u**4 + 6.0 * u**5,
            30.0 * u**2 - 60.0 * u**3 + 30.0 * u**4,
            60.0 * u - 180.0 * u**2 + 120.0 * u**3,
        )

    def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
        """
        获取时刻 t 的位置/速度/加速度 / Get position, velocity and acceleration at time t

        时间分段：
        - t < 0：按 t = 0 处理（停在起点）；
        - t ∈ [0, T_tr)：过渡段1（start_pos → P1）；
        - t ∈ [T_tr, T_tr+T_c)：圆周段（竖直圆，y-z 平面）；
        - t ∈ [T_tr+T_c, 2·T_tr+T_c)：过渡段2（P2 → C）；
        - t ∈ [2·T_tr+T_c, total)：8 字段（水平，x-y 平面）；
        - t ≥ total：保持在 8 字结束点，速度/加速度为 0。

        Args:
            t: 当前时间（秒） / Current time (s)

        Returns:
            (pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
        """
        if t < 0.0:
            t = 0.0
        if t >= self._total:
            return self._end_pos.copy(), np.zeros(3), np.zeros(3)

        if t < self._t1:
            # 段1 过渡：start_pos → P1（1/T、1/T² 因子在此折算）
            pos, vel, acc = self._quintic_transition(self.start_pos, self._p1, t / self._t_tr)
            return pos, vel / self._t_tr, acc / self._t_tr**2
        if t < self._t2:
            # 段2 圆周：相位以五次时间缩放推进，边界速度/加速度均为零。
            u = (t - self._t1) / self._t_c
            s, ds, dds = self._quintic_scale(u)
            theta_total = 2.0 * np.pi * self._f_c * self._t_c
            theta = theta_total * s
            theta_dot = theta_total * ds / self._t_c
            theta_ddot = theta_total * dds / self._t_c**2
            pos = self._center + self._r_c * np.array([0.0, np.cos(theta), np.sin(theta)])
            vel = self._r_c * theta_dot * np.array([0.0, -np.sin(theta), np.cos(theta)])
            acc = self._r_c * (
                theta_ddot * np.array([0.0, -np.sin(theta), np.cos(theta)])
                + theta_dot**2 * np.array([0.0, -np.cos(theta), -np.sin(theta)]))
            return pos, vel, acc
        if t < self._t3:
            # 段3 过渡：P2 → C
            u = (t - self._t2) / self._t_tr
            pos, vel, acc = self._quintic_transition(self._p2, self._center, u)
            return pos, vel / self._t_tr, acc / self._t_tr**2

        # 段4 8 字：相位以五次时间缩放推进，保证与前段和保持段 C2 连续。
        u = (t - self._t3) / self._t_8
        s, ds, dds = self._quintic_scale(u)
        phi_total = 2.0 * np.pi * self._f_8 * self._t_8
        phi = phi_total * s
        phi_dot = phi_total * ds / self._t_8
        phi_ddot = phi_total * dds / self._t_8**2
        pos = self._center + np.array(
            [self._ax8 * np.sin(phi), self._ay8 * np.sin(2.0 * phi), 0.0])
        vel = phi_dot * np.array(
            [self._ax8 * np.cos(phi), 2.0 * self._ay8 * np.cos(2.0 * phi), 0.0])
        acc = phi_ddot * np.array(
            [self._ax8 * np.cos(phi), 2.0 * self._ay8 * np.cos(2.0 * phi), 0.0])
        acc += phi_dot**2 * np.array(
            [-self._ax8 * np.sin(phi), -4.0 * self._ay8 * np.sin(2.0 * phi), 0.0])
        return pos, vel, acc

属性

durations property 

durations: tuple[float, float, float]

(过渡段 T_tr, 圆周段 T_c, 8 字段 T_8) 时长（秒）/ (transition, circle, figure-8) durations

total_duration property 

total_duration: float

轨迹总时长 2·T_tr + T_c + T_8（秒）/ Total duration 2·T_tr + T_c + T_8 (s)

segments property 

segments: tuple[tuple[str, float, float], ...]

四段 (名称, t_start, t_end)：过渡1/圆周/过渡2/8字 / The four segments as (name, t_start, t_end): transition/circle/transition/figure-8

方法：

__init__ 

__init__(
    start_pos: ndarray,
    *,
    transition_duration: float = 1.5,
    circle_duration: float = 5.0,
    circle_radius: float = 0.1,
    circle_frequency: float = 0.2,
    circle_center_offset: float = -0.06,
    figure8_duration: float = 5.0,
    figure8_radius_x: float = 0.1,
    figure8_radius_y: float = 0.07,
    figure8_frequency: float = 0.2,
)

Initialize the circle + figure-8 tracking-test trajectory

参数：

名称 类型 描述 默认
start_pos ndarray

轨迹出发点 (3D) / Start position (3D)

 必需
transition_duration float

过渡段时长 T_tr [s] / Transition segment duration

 1.5
circle_duration float

圆周段时长 T_c [s] / Circle segment duration

 5.0
circle_radius float

圆周半径 r [m] / Circle radius

 0.1
circle_frequency float

圆周频率 f_c [Hz] / Circle frequency

 0.2
circle_center_offset float

圆心相对 start_pos 的 z 偏移 [m]（可正可负） / Circle center offset below/above the start position

 -0.06
figure8_duration float

8 字段时长 T_8 [s] / Figure-8 segment duration

 5.0
figure8_radius_x float

8 字 x 半幅值 [m] / Figure-8 radius

 0.1
figure8_radius_y float

8 字 y 半幅值 [m] / Figure-8 radius

 0.07
figure8_frequency float

8 字频率 f_8 [Hz] / Figure-8 frequency

 0.2
源代码位于： src/compliant_docking/planning/trajectory.py 
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
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
def __init__(self, start_pos: np.ndarray, *,
             transition_duration: float = 1.5,
             circle_duration: float = 5.0,
             circle_radius: float = 0.10,
             circle_frequency: float = 0.2,
             circle_center_offset: float = -0.06,
             figure8_duration: float = 5.0,
             figure8_radius_x: float = 0.10,
             figure8_radius_y: float = 0.07,
             figure8_frequency: float = 0.2):
    """
    Initialize the circle + figure-8 tracking-test trajectory

    Args:
        start_pos: 轨迹出发点 (3D) / Start position (3D)
        transition_duration: 过渡段时长 T_tr [s] / Transition segment duration
        circle_duration: 圆周段时长 T_c [s] / Circle segment duration
        circle_radius: 圆周半径 r [m] / Circle radius
        circle_frequency: 圆周频率 f_c [Hz] / Circle frequency
        circle_center_offset: 圆心相对 start_pos 的 z 偏移 [m]（可正可负） /
            Circle center offset below/above the start position
        figure8_duration: 8 字段时长 T_8 [s] / Figure-8 segment duration
        figure8_radius_x: 8 字 x 半幅值 [m] / Figure-8 radius
        figure8_radius_y: 8 字 y 半幅值 [m] / Figure-8 radius
        figure8_frequency: 8 字频率 f_8 [Hz] / Figure-8 frequency
    """
    assert start_pos.shape == (3,), "Start position must be 3D vector"
    assert transition_duration > 0, "Transition duration must be positive"
    assert circle_duration > 0, "Circle duration must be positive"
    assert circle_radius > 0, "Circle radius must be positive"
    assert circle_frequency > 0, "Circle frequency must be positive"
    assert np.isfinite(circle_center_offset), "Circle center offset must be finite"
    assert figure8_duration > 0, "Figure-8 duration must be positive"
    assert figure8_radius_x > 0, "Figure-8 radius x must be positive"
    assert figure8_radius_y > 0, "Figure-8 radius y must be positive"
    assert figure8_frequency > 0, "Figure-8 frequency must be positive"

    # 存副本，防外部修改 / Store a copy so external mutation cannot affect us
    self.start_pos = np.array(start_pos, dtype=float)

    self._t_tr = float(transition_duration)
    self._t_c = float(circle_duration)
    self._r_c = float(circle_radius)
    self._f_c = float(circle_frequency)
    self._ax8 = float(figure8_radius_x)
    self._ay8 = float(figure8_radius_y)
    self._f_8 = float(figure8_frequency)

    # 关键几何点 / Key waypoints
    self._center = self.start_pos + np.array([0.0, 0.0, float(circle_center_offset)])
    self._p1 = self._center + np.array([0.0, self._r_c, 0.0])
    # 圆终点 = 段2 结束时刻的圆周位置（f_c·T_c 非整圈时不在 P1）
    theta_end = 2.0 * np.pi * self._f_c * self._t_c
    self._p2 = self._center + self._r_c * np.array(
        [0.0, np.cos(theta_end), np.sin(theta_end)])
    # 8 字结束点（t ≥ total 时保持于此）
    phi_end = 2.0 * np.pi * self._f_8 * float(figure8_duration)
    self._end_pos = self._center + np.array(
        [self._ax8 * np.sin(phi_end), self._ay8 * np.sin(2.0 * phi_end), 0.0])

    # 段边界时间 / Segment boundary times
    self._t_8 = float(figure8_duration)
    self._t1 = self._t_tr
    self._t2 = self._t_tr + self._t_c
    self._t3 = 2.0 * self._t_tr + self._t_c
    self._total = 2.0 * self._t_tr + self._t_c + self._t_8

get_state 

get_state(
    t: float,
) -> tuple[ndarray, ndarray, ndarray]

获取时刻 t 的位置/速度/加速度 / Get position, velocity and acceleration at time t

时间分段： - t < 0：按 t = 0 处理（停在起点）； - t ∈ [0, T_tr)：过渡段1（start_pos → P1）； - t ∈ [T_tr, T_tr+T_c)：圆周段（竖直圆，y-z 平面）； - t ∈ [T_tr+T_c, 2·T_tr+T_c)：过渡段2（P2 → C）； - t ∈ [2·T_tr+T_c, total)：8 字段（水平，x-y 平面）； - t ≥ total：保持在 8 字结束点，速度/加速度为 0。

参数：

名称 类型 描述 默认
t float

当前时间（秒） / Current time (s)

 必需

返回：

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

(pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
源代码位于： src/compliant_docking/planning/trajectory.py 
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
def get_state(self, t: float) -> tuple[np.ndarray, np.ndarray, np.ndarray]:
    """
    获取时刻 t 的位置/速度/加速度 / Get position, velocity and acceleration at time t

    时间分段：
    - t < 0：按 t = 0 处理（停在起点）；
    - t ∈ [0, T_tr)：过渡段1（start_pos → P1）；
    - t ∈ [T_tr, T_tr+T_c)：圆周段（竖直圆，y-z 平面）；
    - t ∈ [T_tr+T_c, 2·T_tr+T_c)：过渡段2（P2 → C）；
    - t ∈ [2·T_tr+T_c, total)：8 字段（水平，x-y 平面）；
    - t ≥ total：保持在 8 字结束点，速度/加速度为 0。

    Args:
        t: 当前时间（秒） / Current time (s)

    Returns:
        (pos, vel, acc) 三个 3D 向量 / 3D numpy arrays
    """
    if t < 0.0:
        t = 0.0
    if t >= self._total:
        return self._end_pos.copy(), np.zeros(3), np.zeros(3)

    if t < self._t1:
        # 段1 过渡：start_pos → P1（1/T、1/T² 因子在此折算）
        pos, vel, acc = self._quintic_transition(self.start_pos, self._p1, t / self._t_tr)
        return pos, vel / self._t_tr, acc / self._t_tr**2
    if t < self._t2:
        # 段2 圆周：相位以五次时间缩放推进，边界速度/加速度均为零。
        u = (t - self._t1) / self._t_c
        s, ds, dds = self._quintic_scale(u)
        theta_total = 2.0 * np.pi * self._f_c * self._t_c
        theta = theta_total * s
        theta_dot = theta_total * ds / self._t_c
        theta_ddot = theta_total * dds / self._t_c**2
        pos = self._center + self._r_c * np.array([0.0, np.cos(theta), np.sin(theta)])
        vel = self._r_c * theta_dot * np.array([0.0, -np.sin(theta), np.cos(theta)])
        acc = self._r_c * (
            theta_ddot * np.array([0.0, -np.sin(theta), np.cos(theta)])
            + theta_dot**2 * np.array([0.0, -np.cos(theta), -np.sin(theta)]))
        return pos, vel, acc
    if t < self._t3:
        # 段3 过渡：P2 → C
        u = (t - self._t2) / self._t_tr
        pos, vel, acc = self._quintic_transition(self._p2, self._center, u)
        return pos, vel / self._t_tr, acc / self._t_tr**2

    # 段4 8 字：相位以五次时间缩放推进，保证与前段和保持段 C2 连续。
    u = (t - self._t3) / self._t_8
    s, ds, dds = self._quintic_scale(u)
    phi_total = 2.0 * np.pi * self._f_8 * self._t_8
    phi = phi_total * s
    phi_dot = phi_total * ds / self._t_8
    phi_ddot = phi_total * dds / self._t_8**2
    pos = self._center + np.array(
        [self._ax8 * np.sin(phi), self._ay8 * np.sin(2.0 * phi), 0.0])
    vel = phi_dot * np.array(
        [self._ax8 * np.cos(phi), 2.0 * self._ay8 * np.cos(2.0 * phi), 0.0])
    acc = phi_ddot * np.array(
        [self._ax8 * np.cos(phi), 2.0 * self._ay8 * np.cos(2.0 * phi), 0.0])
    acc += phi_dot**2 * np.array(
        [-self._ax8 * np.sin(phi), -4.0 * self._ay8 * np.sin(2.0 * phi), 0.0])
    return pos, vel, acc

逆运动学

使用 Pinocchio 进行逆运动学（阻尼最小二乘）：返回关节角与是否收敛 / Compute inverse kinematics (damped least squares) using Pinocchio

参数：

名称 类型 描述 默认
pin_model Model

Pinocchio 模型 / Pinocchio model

 必需
pin_data Data

Pinocchio 数据 / Pinocchio data

 必需
target_pose SE3

目标末端位姿 pin.SE3 / target end-effector pose

 必需
initial_q ndarray | None

初始关节角，None 则取 neutral / initial joint config

 _DEFAULT_INITIAL_Q
max_iters int

最大迭代步数 / maximum iterations

 3000
eps float

收敛阈值 / convergence threshold

 1e-07
ee_frame str

末端 frame 名（由模型/场景决定；默认值为组合 URDF 的 cylinder_link，即 iiwa14 + 公头圆柱场景的历史名称） / end-effector frame name (decided by model/scene; default is the historical name of the iiwa14 combined URDF)

 'cylinder_link'

返回：

名称 类型 描述
q ndarray

关节角解 / joint configuration
success bool

是否收敛 / convergence flag
源代码位于： src/compliant_docking/planning/kinematics.py 
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
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
def compute_ik(pin_model, pin_data, target_pose, initial_q=_DEFAULT_INITIAL_Q, max_iters=3000, eps=1e-7,
               *, ee_frame: str = "cylinder_link"):
    """
    使用 Pinocchio 进行逆运动学（阻尼最小二乘）：返回关节角与是否收敛 /
    Compute inverse kinematics (damped least squares) using Pinocchio

    Args:
        pin_model (pin.Model): Pinocchio 模型 / Pinocchio model
        pin_data (pin.Data): Pinocchio 数据 / Pinocchio data
        target_pose (pin.SE3): 目标末端位姿 pin.SE3 / target end-effector pose
        initial_q (np.ndarray | None): 初始关节角，None 则取 neutral / initial joint config
        max_iters (int): 最大迭代步数 / maximum iterations
        eps (float): 收敛阈值 / convergence threshold
        ee_frame: 末端 frame 名（由模型/场景决定；默认值为组合 URDF 的
            ``cylinder_link``，即 iiwa14 + 公头圆柱场景的历史名称） /
            end-effector frame name (decided by model/scene; default is the
            historical name of the iiwa14 combined URDF)

    Returns:
        q (np.ndarray): 关节角解 / joint configuration
        success (bool): 是否收敛 / convergence flag
    """
    # 若未提供初始值，则使用模型的中性位姿作为初值
    if initial_q is None:
        q = pin.neutral(pin_model)
    else:
        q = initial_q.copy()

    # Get end effector frame ID（frame 名由 ee_frame 参数决定，默认为历史名称）
    ee_frame_id = pin_model.getFrameId(ee_frame)

    # Damping factor for numerical stability
    damp = 1e-8  # 阻尼因子，提高最小二乘求解的数值稳定性

    for i in range(max_iters):
        # Update robot kinematics
        pin.forwardKinematics(pin_model, pin_data, q)
        pin.updateFramePlacements(pin_model, pin_data)

        # Get current end-effector pose
        current_pose = pin_data.oMf[ee_frame_id]

        # Compute position error
        error_pos = target_pose.translation - current_pose.translation

        # Compute orientation error using matrix logarithm
        # 姿态误差采用李代数对数映射：log(Rd Rc^T)
        error_rot = pin.log3(target_pose.rotation @ current_pose.rotation.T)

        # Combine errors
        error = np.concatenate([error_pos, error_rot])

        # Check convergence
        if np.linalg.norm(error) < eps:
            print(f"IK converged in {i+1} iterations")
            return q, True

        # Compute task Jacobian
        pin.computeJointJacobians(pin_model, pin_data, q)
        J = pin.getFrameJacobian(pin_model, pin_data, ee_frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)

        # Compute joint update using damped least squares
        Jt = J.T
        JJt = J @ Jt
        lambda_eye = damp * np.eye(6)  # 6 DOF task space

        # Solve using damped least squares
        # 阻尼最小二乘（等价于 J^T (J J^T + λI)^{-1} e）
        v = np.linalg.solve(JJt + lambda_eye, error)
        dq = Jt @ v

        # Update joint positions
        q = pin.integrate(pin_model, q, dq)

        # Joint limits handling (if needed)
        q = np.clip(q, pin_model.lowerPositionLimit, pin_model.upperPositionLimit)

    print("IK failed to converge")
    return q, False