《Python机器人基础原理》第一百一十五篇：装配产线仿真——TCP标定存在微小误差，观测闭环系统末端实际跟踪偏差

 

声明：本篇全部为课程离线仿真代码，不能直接用于真实机器人控制器，规避工程误导风险。

 

1. 实际工业应用场景

 

周三下午，装配工位。小赵刚完成一台新到货的2R桌面装配机器人的TCP（Tool Center Point）标定——用四点法，示教器上显示标定残差0.8 mm，在手册允许范围内。接着他把一条圆弧插装路径灌进系统，PD参数沿用上篇验证过的Kp=300、Kd=50，加了重力前馈。仿真跑完，末端误差曲线让老郑皱了皱眉。

 

"标定残差不到1 mm，但末端跟踪误差峰值到了5.8 mm。"老郑指着屏幕。"你看，路径远端误差更大。问题出在哪？TCP标定误差是工具坐标系下的偏移——它随手臂姿态旋转。控制器用错误的TCP做逆运动学，算出来的关节角参考轨迹本身就是偏的。PD忠实地跟踪这个被污染的参考，末端自然到不了该到的地方。"

 

小赵回过神来："所以不是PD的问题，是标定误差通过IK'骗'了整个控制链？"

 

"对。而且这个偏移不是常数——工具系下的δ随q₁旋转，投影到世界坐标系后，在不同路径位置分量不同。这就是误差随位置变化的原因。"

 

构型约定：竖直平面内2R机械臂。连杆参数：L1 a₁=0.50 m / m₁=6.5 kg / I₁=0.45 kg·m² / com₁=0.25 m；L2 a₂=0.40 m / m₂=4.0 kg / I₂=0.20 kg·m² / com₂=0.20 m。TCP标定误差模型：真实工具尖端在工具坐标系（固连于连杆2末端法兰）下偏移δ_tool = [0.003, -0.002] m（合成幅值3.6 mm）。控制器标定得到δ_calib ≠ δ_tool，本篇简化为δ_calib = [0.003, -0.002]（即控制器"以为"的工具偏移），而真实工具尖端就在法兰中心（δ_true = 0）。因此控制器的FK和IK都基于错误的偏移，产生系统性偏差。末端目标路径：圆心(0.55, 0.25) m、半径0.12 m、角度-30°~+30°的圆弧，21个航点，总时长4.0 s。该路径r∈[0.634, 0.715] m，q₂∈[-107°, -75°]（elbow_down），远离奇异区（q₂=0°, r=0.90 m）。PD参数：Kp=[300, 200] N·m/rad，Kd=[50, 35] N·m·s/rad。重力前馈：τ_ff = G(q_des)。力矩限幅±120/±90 N·m。RK4步长0.001 s，仿真5.0 s（留1.0 s收敛余量）。技术栈：numpy + scipy（interpolate） + matplotlib。

⚠️ 声明：本篇全部代码为课程离线仿真，运行在普通PC上，用于TCP标定误差影响验证。代码中不包含任何真实机器人控制器实时运算或安全逻辑。不可直接用于产线设备。

 

2. 痛点分析

 

课程模块 本篇对应的工程痛点

第3章 连杆参数及运动学求解 → TCP标定 标定得到的工具偏移δ_calib与真实δ_tool不一致，导致IK和FK均基于错误模型

第3章 → 运动学映射 工具系偏移随关节角旋转，投影到世界系后产生位形相关的末端偏差

第5章 机器人关节驱动与控制 → PD跟踪 控制器基于被IK污染的关节参考轨迹运行，PD无法弥补上游模型错误

 

本篇代码解决 L2 离线仿真校核层。输入：2R连杆参数、理想笛卡尔路径航点、TCP标定误差向量、PD增益。处理：①控制器用错误TCP做IK，得到"被污染的"关节参考；②关节PD+重力前馈跟踪该参考；③正运动学用真实模型（无偏移）计算实际末端位置；④对比理想路径与实际末端。输出：4张TCP标定误差分析图表 + 误差报告。

 

不覆盖：在线TCP标定修正、视觉引导补偿、力觉柔顺装配。

 

3. 核心逻辑讲解

 

物理直觉

 

TCP标定误差的本质是"模型错位"。机器人控制器内部维护一个工具坐标系偏移δ_calib，它告诉控制器"末端执行器尖端相对于法兰中心在哪里"。如果标定不准，这个偏移就是错的。

 

关键机制：δ_tool定义在工具坐标系（固连于连杆2末端）中。当手臂运动时，工具系随q₁+q₂旋转。因此δ_calib在世界系中的投影 = R(q₁+q₂)·δ_calib。这意味着：同样的3.6 mm工具系偏移，在不同关节角下指向不同方向，投影到路径法向上的分量也不同——所以末端误差不是常数。

 

本篇如何让误差进入闭环：控制器用带偏移的IK求解关节角——它把"期望笛卡尔点p_des"当成"法兰中心应该到达的点p_des - R(q₁+q₂)·δ_calib"来做逆解。由于δ_calib是错的（本篇中它非零而真实δ=0），解出的关节角q_des本身就是偏的。PD忠实地跟踪这个有毛病的参考，真实末端就到不了p_des。

 

业务逻辑 → 代码映射

 

输入：理想笛卡尔航点 + δ_calib（标定误差）+ 连杆参数 + PD增益

  ↓

【步骤1：控制器视角IK（核心——误差从这里进入）】

  对每个路径点p_des(t)：

    控制器"以为"法兰中心应该到 p_target = p_des - R(q₁_des+q₂_des)·δ_calib

    但q_des未知，用迭代/近似：先按无偏移IK得初始q，再迭代修正

    → 得到被污染的关节参考 q_des(t)

  ↓

【步骤2：关节轨迹插值】

  对q_des(t)三次样条

  ↓

【步骤3：仿真循环】

  对每个时刻t：

    ① RK4积分得到实际关节角q(t)

    ② 控制器用q_des(t)做PD（标准关节空间PD）

    ③ 重力前馈G(q_des)

    ④ 正运动学用真实模型（无偏移）算实际末端：p_actual = FK_no_offset(q)

    ⑤ 记录误差 e = |p_des - p_actual|

  ↓

【步骤4：指标计算】

  ① 真实末端线误差

  ② 控制器视角误差（用带偏移FK算的）

  ↓

【步骤5：4张图表】

  ① 末端轨迹对比

  ② 真实末端线误差时序

  ③ 控制器视角误差 vs 真实误差

  ④ TCP偏移在世界系中的投影分量（解释误差变化原因）

 

关键公式文字翻译

 

工具系偏移的世界投影：δ_world(q) = R_z(q₁+q₂) · δ_tool。R_z是二维旋转矩阵[cos, -sin; sin, cos]。当手臂伸直（q₂≈0, q₁变化），δ_world的方向随q₁旋转——这就是误差随位置变化的根源。

 

被污染的IK：控制器求解的是 FK_with_offset(q) = FK_no_offset(q) + δ_world(q) = p_des。移项：FK_no_offset(q) = p_des - δ_world(q)。控制器用这个等式做逆解，但δ_world(q)依赖q本身，所以是一个隐式方程——本篇用单次近似（假设δ_world用上一步q估算）来模拟。

 

重力前馈用G(q_des)：延续113、114篇方案，在期望位形求值，开环补偿，不进入反馈回路。

 

4. OOP代码实现

 

技术栈

 

限定：numpy, scipy（interpolate仅用于轨迹插值）, matplotlib。不引入pybullet、roboticstoolbox等任何第三方重型库。

 

项目结构树

 

tcp_calibration_simulation/

├── run_tcp_sim.py # 主入口

├── requirements.txt

├── robot/

│ ├── __init__.py

│ ├── models.py # dataclass数据模型

│ ├── kinematics.py # 正/逆运动学（支持工具系偏移旋转）

│ ├── dynamics.py # 2R闭式动力学

│ ├── trajectory_cartesian.py # 笛卡尔路径 + 被污染的IK

│ ├── controller.py # PD+重力前馈（关节空间，参考已被IK污染）

│ ├── simulator.py # RK4仿真器

│ ├── metrics.py # 误差指标

│ └── visualize.py # 4张图表封装

├── tests/

│ ├── __init__.py

│ └── test_tcp_sim.py # 3类测试

├── results/ # 4张图表

└── output/ # 误差报告txt

 

核心源码

 

"robot/models.py"

 

"""

Data models for 2R TCP calibration error simulation.

"""

 

 

from dataclasses import dataclass, field

import numpy as np

 

 

@dataclass

class LinkInertia:

    name: str

    mass: float # kg

    length: float # m

    inertia: float # kg·m^2

    com_offset: float # m

 

    def __post_init__(self):

        if self.mass <= 0:

            raise ValueError(f"{self.name}: mass must be positive")

        if self.length <= 0:

            raise ValueError(f"{self.name}: length must be positive")

        if self.inertia <= 0:

            raise ValueError(f"{self.name}: inertia must be positive")

        if not 0 < self.com_offset <= self.length:

            raise ValueError(f"{self.name}: com_offset must be in (0, length]")

 

 

@dataclass

class TCPOffset:

    """TCP calibration error expressed in tool frame (attached to link 2 flange)."""

    dx_tool: float # m, x in tool frame (along link 2 direction)

    dy_tool: float # m, y in tool frame (perpendicular to link 2)

 

    def as_array(self):

        return np.array([self.dx_tool, self.dy_tool], dtype=float)

 

    @property

    def magnitude(self):

        return np.linalg.norm(self.as_array())

 

 

@dataclass

class PDGains:

    Kp: np.ndarray

    Kd: np.ndarray

 

    def __post_init__(self):

        # Validate BEFORE reshape — ensures wrong shapes raise ValueError

        kp = np.asarray(self.Kp, dtype=float)

        kd = np.asarray(self.Kd, dtype=float)

        if kp.shape != (2,):

            raise ValueError(f"Kp must be length-2, got shape {kp.shape}")

        if kd.shape != (2,):

            raise ValueError(f"Kd must be length-2, got shape {kd.shape}")

        self.Kp = kp

        self.Kd = kd

 

 

@dataclass

class TorqueLimits:

    tau_max: np.ndarray

 

    def __post_init__(self):

        self.tau_max = np.asarray(self.tau_max, dtype=float).reshape(2,)

        if self.tau_max.shape != (2,) or np.any(self.tau_max <= 0):

            raise ValueError("tau_max must be length-2 positive array")

 

 

@dataclass

class SimConfig:

    links: list = field(default_factory=list)

    gravity: float = 9.81

    dt: float = 0.001

    t_sim: float = 5.0

 

    def __post_init__(self):

        if not self.links:

            self.links = [

                LinkInertia("L1", 6.5, 0.50, 0.45, 0.25),

                LinkInertia("L2", 4.0, 0.40, 0.20, 0.20),

            ]

 

"robot/kinematics.py"

 

"""

2R planar forward/inverse kinematics.

Tool-frame TCP offset rotates with link 2:

  p_world = FK_no_offset(q) + R_z(q1+q2) * delta_tool

This is the physically correct model — offset direction changes with arm posture.

"""

 

 

import numpy as np

import math

from robot.models import SimConfig

 

 

class Kinematics2R:

    def __init__(self, config: SimConfig):

        self.l1 = config.links[0].length

        self.l2 = config.links[1].length

 

    def forward_no_offset(self, q):

        """Forward kinematics of flange center (no TCP offset)."""

        q1, q2 = q[0], q[1]

        x = self.l1 * math.cos(q1) + self.l2 * math.cos(q1 + q2)

        y = self.l1 * math.sin(q1) + self.l2 * math.sin(q1 + q2)

        return np.array([x, y], dtype=float)

 

    def rotation_tool_to_world(self, q):

        """Rotation matrix from tool frame to world frame: R_z(q1+q2)."""

        theta = q[0] + q[1]

        c, s = math.cos(theta), math.sin(theta)

        return np.array([[c, -s], [s, c]], dtype=float)

 

    def forward_with_offset(self, q, tcp_offset):

        """

        Forward kinematics including tool-frame TCP offset.

        tcp_offset is expressed in tool frame — rotates with link 2.

        """

        p_flange = self.forward_no_offset(q)

        R = self.rotation_tool_to_world(q)

        delta_world = R @ tcp_offset.as_array()

        return p_flange + delta_world

 

    def inverse(self, x, y, elbow_down=True):

        """

        Standard cosine-law IK (no offset). Used for ideal path.

        """

        r2 = x**2 + y**2

        r = math.sqrt(r2)

        r_min = abs(self.l1 - self.l2)

        r_max = self.l1 + self.l2

        if r < r_min - 1e-6 or r > r_max + 1e-6:

            raise ValueError(f"Point ({x:.4f}, {y:.4f}) outside workspace "

                            f"[{r_min:.3f}, {r_max:.3f}] m")

 

        cos_q2 = (r2 - self.l1**2 - self.l2**2) / (2.0 * self.l1 * self.l2)

        cos_q2 = max(-1.0, min(1.0, cos_q2))

        q2 = math.acos(cos_q2)

        if elbow_down:

            q2 = -q2

 

        alpha = math.atan2(y, x)

        beta = math.atan2(self.l2 * math.sin(abs(q2)),

                          self.l1 + self.l2 * math.cos(abs(q2)))

        q1 = alpha - beta if elbow_down else alpha + beta

        return np.array([q1, q2], dtype=float)

 

    def inverse_with_offset_approx(self, p_des, tcp_offset, elbow_down=True,

                                    q_guess=None):

        """

        IK that accounts for tool-frame TCP offset.

        Solves: FK_no_offset(q) + R(q)·δ = p_des

        Uses single-step approximation: compute R from q_guess, subtract from p_des.

        This is how a real controller would do it — it uses its believed TCP.

        """

        if q_guess is None:

            q_guess = np.array([0.0, -math.pi/2])

 

        # Controller's belief: flange should reach p_des - R(q_guess)·δ

        R = self.rotation_tool_to_world(q_guess)

        delta_world = R @ tcp_offset.as_array()

        p_flange_target = p_des - delta_world

 

        # IK for flange target (standard, no offset)

        q = self.inverse(p_flange_target[0], p_flange_target[1],

                         elbow_down=elbow_down)

        return q

 

"robot/dynamics.py"

 

"""

2R planar rigid-body dynamics (vertical plane).

Reused from art.113/114/115, unchanged.

"""

 

 

import numpy as np

import math

from robot.models import SimConfig

 

 

class Dynamics2R:

    def __init__(self, config: SimConfig):

        self.cfg = config

        self.links = config.links

        self.g = config.gravity

        self.m1 = self.links[0].mass

        self.m2 = self.links[1].mass

        self.l1 = self.links[0].length

        self.l2 = self.links[1].length

        self.r1 = self.links[0].com_offset

        self.r2 = self.links[1].com_offset

        self.I1 = self.links[0].inertia

        self.I2 = self.links[1].inertia

 

    def mass_matrix(self, q):

        q2 = q[1]

        c2 = math.cos(q2)

        M11 = (self.I1 + self.I2 + self.m1*self.r1**2

               + self.m2*(self.l1**2 + self.r2**2 + 2*self.l1*self.r2*c2))

        M12 = self.I2 + self.m2*self.r2*(self.r2 + self.l1*c2)

        M22 = self.I2 + self.m2*self.r2**2

        return np.array([[M11, M12], [M12, M22]], dtype=float)

 

    def coriolis_vector(self, q, qd):

        q2 = q[1]

        s2 = math.sin(q2)

        h = self.m2 * self.l1 * self.r2 * s2

        C1 = h * qd[1] * (2.0*qd[0] + qd[1])

        C2 = -h * qd[0]**2

        return np.array([[C1], [C2]], dtype=float)

 

    def gravity_vector(self, q):

        q1 = q[0]

        q2 = q[1]

        c1 = math.cos(q1)

        c12 = math.cos(q1 + q2)

        G1 = self.m1*self.g*self.r1*c1 + self.m2*self.g*(self.l1*c1 + self.r2*c12)

        G2 = self.m2*self.g*self.r2*c12

        return np.array([[G1], [G2]], dtype=float)

 

"robot/trajectory_cartesian.py"

 

"""

Cartesian path for assembly line simulation.

Path is in healthy workspace region (远离奇异区).

Includes contaminated IK: controller uses its believed TCP offset.

"""

 

 

import numpy as np

from scipy.interpolate import CubicSpline

from robot.kinematics import Kinematics2R

from robot.models import TCPOffset

 

 

class CartesianTrajectory:

    def __init__(self, waypoint_times, waypoints_xy, kinematics: Kinematics2R):

        self.t_wp = np.asarray(waypoint_times, dtype=float)

        self.p_wp = np.asarray(waypoints_xy, dtype=float)

 

        if self.t_wp.ndim != 1:

            raise ValueError("waypoint_times must be 1-D")

        if self.p_wp.ndim != 2 or self.p_wp.shape[1] != 2:

            raise ValueError(f"waypoints_xy must be (N, 2), got {self.p_wp.shape}")

        if len(self.t_wp) != self.p_wp.shape[0]:

            raise ValueError("time and position arrays must have same length")

        if not np.all(np.diff(self.t_wp) > 0):

            raise ValueError("waypoint_times must be strictly increasing")

 

        self.kin = kinematics

        self._spline_x = CubicSpline(self.t_wp, self.p_wp[:, 0], bc_type='natural')

        self._spline_y = CubicSpline(self.t_wp, self.p_wp[:, 1], bc_type='natural')

        self.t_start = self.t_wp[0]

        self.t_end = self.t_wp[-1]

 

    def evaluate_cartesian(self, t):

        t = np.clip(t, self.t_start, self.t_end)

        x = self._spline_x(t)

        y = self._spline_y(t)

        xd = self._spline_x(t, 1)

        yd = self._spline_y(t, 1)

        xdd = self._spline_x(t, 2)

        ydd = self._spline_y(t, 2)

        return np.array([x, y]), np.array([xd, yd]), np.array([xdd, ydd])

 

    def generate_contaminated_joint_trajectory(self, num_samples=200,

                                               elbow_down=True,

                                               tcp_offset=None,

                                               continuity_tol=0.5):

        """

        Controller's IK: uses believed TCP offset.

        This produces a CONTAMINATED joint trajectory — the core mechanism.

        """

        t_samples = np.linspace(self.t_start, self.t_end, num_samples)

        q_samples = np.zeros((num_samples, 2))

        q_prev = np.array([0.0, -math.pi/2])

 

        for i, t in enumerate(t_samples):

            p_des, _, _ = self.evaluate_cartesian(t)

 

            if tcp_offset is not None:

                # Controller "believes" TCP offset — solves contaminated IK

                q = self.kin.inverse_with_offset_approx(

                    p_des, tcp_offset, elbow_down=elbow_down, q_guess=q_prev)

            else:

                # No offset — ideal IK

                q = self.kin.inverse(p_des[0], p_des[1], elbow_down=elbow_down)

 

            q_samples[i, :] = q

            q_prev = q

 

        # Continuity check

        diff = np.abs(np.diff(q_samples, axis=0))

        max_jump = np.max(diff)

        if max_jump > continuity_tol:

            print(f" ⚠ Joint trajectory discontinuity: max jump = {max_jump:.4f} rad")

 

        return t_samples, q_samples

 

    @classmethod

    def builtin_assembly_arc(cls, kinematics: Kinematics2R):

        """

        Assembly arc in healthy workspace region.

        Center (0.55, 0.25) m, radius 0.12 m, angle -30° to +30°.

        21 waypoints, 4.0s total.

        Verified: r range [0.634, 0.715] m, q2 range [-107°, -75°].

        Far from singularity (r=0.90m, q2=0°).

        """

        center_x, center_y = 0.55, 0.25

        radius = 0.12

        angles = np.linspace(-30 * np.pi / 180, 30 * np.pi / 180, 21)

        t_wp = np.linspace(0.0, 4.0, 21)

 

        waypoints_xy = np.zeros((21, 2))

        for i, theta in enumerate(angles):

            waypoints_xy[i, 0] = center_x + radius * np.cos(theta)

            waypoints_xy[i, 1] = center_y + radius * np.sin(theta)

 

        return cls(t_wp, waypoints_xy, kinematics)

 

"robot/controller.py"

 

"""

PD + gravity feedforward (joint space).

The contamination enters via the REFERENCE trajectory (q_des is already contaminated

from IK-with-offset in trajectory_cartesian.py).

Controller itself just does standard joint-space PD — no dead calculations.

"""

 

 

import numpy as np

from robot.models import PDGains, TorqueLimits

from robot.dynamics import Dynamics2R

 

 

class PDGravityController:

    def __init__(self, gains: PDGains, torque_limits: TorqueLimits,

                 dynamics: Dynamics2R):

        self.Kp = gains.Kp

        self.Kd = gains.Kd

        self.tau_max = torque_limits.tau_max.reshape(2, 1)

        self.dyn = dynamics

 

    def compute_torque(self, q, q_dot, q_des, qd_des):

        q = np.asarray(q).reshape(2, 1)

        q_dot = np.asarray(q_dot).reshape(2, 1)

        q_des = np.asarray(q_des).reshape(2, 1)

        qd_des = np.asarray(qd_des).reshape(2, 1)

 

        # Standard joint-space PD

        e_q = q_des - q

        e_qd = qd_des - q_dot

 

        # Gravity feedforward at DESIRED posture (open-loop, no feedback coupling)

        G_ff = self.dyn.gravity_vector(q_des)

 

        tau_raw = (self.Kp.reshape(2, 1) * e_q

                   + self.Kd.reshape(2, 1) * e_qd

                   + G_ff)

        tau_clipped = np.clip(tau_raw, -self.tau_max, self.tau_max)

        return tau_clipped

 

"robot/simulator.py"

 

"""

RK4 simulator.

Records both desired, actual (no offset), and believed (with offset) end positions.

"""

 

 

import numpy as np

from robot.dynamics import Dynamics2R

from robot.controller import PDGravityController

from robot.models import SimConfig

from robot.kinematics import Kinematics2R

from scipy.interpolate import CubicSpline

 

 

class EndTrackingSimulator:

    def __init__(self, dynamics: Dynamics2R,

                 controller: PDGravityController,

                 kinematics: Kinematics2R,

                 joint_traj_times, joint_traj_q,

                 config: SimConfig,

                 tcp_offset=None):

        self.dyn = dynamics

        self.ctrl = controller

        self.kin = kinematics

        self.tcp_offset = tcp_offset

        self.cfg = config

        self.dt = config.dt

        self.n_steps = int(config.t_sim / config.dt)

 

        self._spl_q1 = CubicSpline(joint_traj_times, joint_traj_q[:, 0],

                                   bc_type='natural')

        self._spl_q2 = CubicSpline(joint_traj_times, joint_traj_q[:, 1],

                                   bc_type='natural')

        self._t_min = joint_traj_times[0]

        self._t_max = joint_traj_times[-1]

 

    def _get_ref(self, t):

        t_clip = max(self._t_min, min(t, self._t_max))

        q_des = np.array([self._spl_q1(t_clip), self._spl_q2(t_clip)])

        qd_des = np.array([self._spl_q1(t_clip, 1), self._spl_q2(t_clip, 1)])

        return q_des, qd_des

 

    def _derivatives(self, x, t):

        q = x[0:2]

        qd = x[2:4]

        q_des, qd_des = self._get_ref(t)

        tau = self.ctrl.compute_torque(q, qd, q_des, qd_des)

        M = self.dyn.mass_matrix(q)

        C = self.dyn.coriolis_vector(q, qd)

        G = self.dyn.gravity_vector(q)

        qdd = np.linalg.solve(M, tau - C - G)

        return np.concatenate([qd, qdd.flatten()])

 

    def run(self, q_start=None, q_dot_start=None):

        if q_start is None:

            q_start = np.zeros(2)

        if q_dot_start is None:

            q_dot_start = np.zeros(2)

 

        x = np.concatenate([q_start, q_dot_start])

        x_hist = np.zeros((self.n_steps + 1, 4))

        x_hist[0] = x

 

        p_true_hist = np.zeros((self.n_steps + 1, 2))

        p_believed_hist = np.zeros((self.n_steps + 1, 2))

 

        # Initial positions

        p_true_hist[0] = self.kin.forward_no_offset(q_start)

        if self.tcp_offset is not None:

            p_believed_hist[0] = self.kin.forward_with_offset(

                q_start, self.tcp_offset)

        else:

            p_believed_hist[0] = p_true_hist[0]

 

        t_arr = np.arange(self.n_steps + 1) * self.dt

        dt = self.dt

 

        for i in range(self.n_steps):

            t = t_arr[i]

            xi = x_hist[i]

            k1 = self._derivatives(xi, t)

            k2 = self._derivatives(xi + 0.5*dt*k1, t + 0.5*dt)

            k3 = self._derivatives(xi + 0.5*dt*k2, t + 0.5*dt)

            k4 = self._derivatives(xi + dt*k3, t + dt)

            x_next = xi + (dt/6.0)*(k1 + 2*k2 + 2*k3 + k4)

            x_hist[i+1] = x_next

 

            q_next = x_next[0:2]

            p_true_hist[i+1] = self.kin.forward_no_offset(q_next)

            if self.tcp_offset is not None:

                p_believed_hist[i+1] = self.kin.forward_with_offset(

                    q_next, self.tcp_offset)

            else:

                p_believed_hist[i+1] = p_true_hist[i+1]

 

        return t_arr, x_hist, p_true_hist, p_believed_hist

 

"robot/metrics.py"

 

"""

Metrics for TCP calibration error analysis.

"""

 

 

import numpy as np

from robot.kinematics import Kinematics2R

 

 

def compute_errors(t, p_true_hist, p_believed_hist, cartesian_traj):

    """

    Compute true error and controller-believed error.

    """

    n = len(t)

    p_des_hist = np.zeros((n, 2))

    e_true_norm = np.z

利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！