From 107e5b87ee97c5906a0684446354a1a7291265c0 Mon Sep 17 00:00:00 2001 From: A-0-A-0-A <3131489786@qq,com> Date: Mon, 27 Apr 2026 23:20:12 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=A3=E7=A0=81=E4=B8=8A=E4=BC=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- tactile/code/tests/test_ik_solver.py | 200 +++++++++++++++++++++++++++ 1 file changed, 200 insertions(+) create mode 100644 tactile/code/tests/test_ik_solver.py diff --git a/tactile/code/tests/test_ik_solver.py b/tactile/code/tests/test_ik_solver.py new file mode 100644 index 00000000..5bf716cd --- /dev/null +++ b/tactile/code/tests/test_ik_solver.py @@ -0,0 +1,200 @@ +""" +逆运动学求解器测试 + +测试改进的IK算法的正确性和性能 +""" + +import unittest +import numpy as np +from math import pi +import sys +sys.path.insert(0, '..') + +from fingers_sim.ik_solver import ImprovedIKSolver, compute_fingertip_position + + +class TestImprovedIKSolver(unittest.TestCase): + """测试改进的IK求解器""" + + def setUp(self): + """测试前设置""" + self.solver = ImprovedIKSolver() + + # 测试用的连杆参数 + self.test_links = [ + 0.025, 0.007, 0.018, # 偏移量 + 0.005, 0.007, # L1 + 0.005, 0.007, # L2 + 0.002, 0.002 # L3 + ] + + # 单位旋转矩阵 + self.R = np.eye(3) + + def test_jacobian_computation(self): + """测试雅可比矩阵计算""" + q = np.array([pi/4, pi/4, pi/4]) + + Jp = self.solver.compute_jacobian(q, self.test_links, self.R) + + # 检查维度 + self.assertEqual(Jp.shape, (3, 3)) + + # 检查第一列（x方向）应该为0（手指在yz平面运动） + self.assertAlmostEqual(Jp[0, 0], 0.0, places=5) + self.assertAlmostEqual(Jp[0, 1], 0.0, places=5) + self.assertAlmostEqual(Jp[0, 2], 0.0, places=5) + + def test_distance_jacobian(self): + """测试距离雅可比计算""" + q = np.array([pi/4, pi/4, pi/4]) + p = compute_fingertip_position(q, self.test_links, self.R) + Jp = self.solver.compute_jacobian(q, self.test_links, self.R) + + Jd = self.solver.compute_distance_jacobian(p, Jp) + + # 检查维度 + self.assertEqual(Jd.shape, (1, 3)) + + # 检查是否非零 + self.assertTrue(np.any(Jd != 0)) + + def test_damping_computation(self): + """测试阻尼系数计算""" + # 正常情况 + damping_normal = self.solver.compute_damping(0.1) + self.assertEqual(damping_normal, self.solver.damping_base) + + # 奇异点附近 + damping_singular = self.solver.compute_damping(1e-5) + self.assertGreater(damping_singular, self.solver.damping_base) + self.assertLessEqual(damping_singular, self.solver.damping_max) + + def test_joint_limit_avoidance(self): + """测试关节限位避免""" + # 正常位置 + q_normal = np.array([pi/8, pi/8, pi/8]) + avoidance = self.solver.compute_joint_limit_avoidance(q_normal) + self.assertTrue(np.allclose(avoidance, 0)) + + # 接近下限 + q_near_min = np.array([0.01, pi/8, pi/8]) + avoidance = self.solver.compute_joint_limit_avoidance(q_near_min) + self.assertGreater(avoidance[0], 0) + + # 接近上限 + q_near_max = np.array([5*pi/16 - 0.01, pi/8, pi/8]) + avoidance = self.solver.compute_joint_limit_avoidance(q_near_max) + self.assertLess(avoidance[0], 0) + + def test_ik_solution_convergence(self): + """测试IK解的收敛性""" + # 初始关节角度 + q_current = np.array([pi/8, pi/8, pi/8]) + + # 计算当前位置 + p_current = compute_fingertip_position(q_current, self.test_links, self.R) + d_current = np.linalg.norm(p_current) + + # 目标距离（稍微小一点） + target_distance = d_current * 0.8 + + # 迭代求解 + for _ in range(50): + q_dot = self.solver.solve(target_distance, q_current, p_current, + self.test_links, self.R, Kp=50.0) + q_current = q_current + q_dot * 0.01 # 模拟时间步 + q_current = np.clip(q_current, 0, pi/2) # 限位 + p_current = compute_fingertip_position(q_current, self.test_links, self.R) + + # 检查是否收敛 + d_final = np.linalg.norm(p_current) + error = abs(d_final - target_distance) + self.assertLess(error, 0.01, f"IK did not converge: error = {error}") + + def test_null_space_projection(self): + """测试零空间投影""" + J = np.array([[1, 0, 0], [0, 1, 0]], dtype=float) + + N = self.solver.compute_null_space_projection(J) + + # 检查维度 + self.assertEqual(N.shape, (3, 3)) + + # 检查投影性质: J @ N 应该接近0 + JN = J @ N + self.assertTrue(np.allclose(JN, 0, atol=1e-10)) + + def test_speed_limit(self): + """测试速度限制""" + q = np.array([pi/4, pi/4, pi/4]) + p = compute_fingertip_position(q, self.test_links, self.R) + + # 设置一个很大的目标距离差，产生大速度 + target_distance = np.linalg.norm(p) + 1.0 + + q_dot = self.solver.solve(target_distance, q, p, self.test_links, self.R, Kp=1000.0) + + # 检查速度是否被限制 + speed = np.linalg.norm(q_dot) + self.assertLessEqual(speed, 5.0 + 1e-6) + + +class TestForwardKinematics(unittest.TestCase): + """测试正向运动学""" + + def test_fingertip_position(self): + """测试指尖位置计算""" + links = [0.0, 0.0, 0.0, 0.01, 0.01, 0.01, 0.01, 0.005, 0.005] + R = np.eye(3) + + # 零角度时 + q_zero = np.array([0, 0, 0]) + p_zero = compute_fingertip_position(q_zero, links, R) + + # 检查z坐标应该是正的（指尖在手掌上方） + self.assertGreater(p_zero[2], 0) + + # 弯曲时 + q_bent = np.array([pi/4, pi/4, pi/4]) + p_bent = compute_fingertip_position(q_bent, links, R) + + # 弯曲后y坐标应该变化 + self.assertNotEqual(p_zero[1], p_bent[1]) + + +class TestPerformance(unittest.TestCase): + """性能测试""" + + def test_ik_computation_speed(self): + """测试IK计算速度""" + import time + + solver = ImprovedIKSolver() + links = [0.025, 0.007, 0.018, 0.005, 0.007, 0.005, 0.007, 0.002, 0.002] + R = np.eye(3) + + q = np.array([pi/4, pi/4, pi/4]) + p = compute_fingertip_position(q, links, R) + target = np.linalg.norm(p) * 0.9 + + # 预热 + for _ in range(10): + solver.solve(target, q, p, links, R) + + # 计时 + start = time.time() + iterations = 1000 + for _ in range(iterations): + solver.solve(target, q, p, links, R) + elapsed = time.time() - start + + avg_time = elapsed / iterations * 1000 # ms + print(f"\nAverage IK solve time: {avg_time:.3f} ms") + + # 应该小于1ms + self.assertLess(avg_time, 1.0) + + +if __name__ == '__main__': + unittest.main()