\u300aPython\u673a\u5668\u4eba\u57fa\u7840\u539f\u7406\u300b\u7b2c\u516b\u5341\u4e09\u7bc7：\u88c5\u914d\u673a\u68b0\u81c2\u4eff\u771f\u2014\u2014PD\u4f4d\u7f6e\u63a7\u5236\u5668\u5355\u5173\u8282\u89d2\u5ea6\u95ed\u73af\u8ddf\u968f<\/p>\n
\u00a0<\/p>\n
1. \u5b9e\u9645\u5de5\u4e1a\u5e94\u7528\u573a\u666f<\/p>\n
\u00a0<\/p>\n
\u5468\u4e00\u65e9\u6668，\u6c7d\u8f66\u53d1\u52a8\u673a\u7f38\u4f53\u88c5\u914d\u7ebf\u5f00\u73ed\u9996\u68c0\u30024-DOF SCARA\u673a\u68b0\u81c2（\u57fa\u5ea7\u65cb\u8f6c\u5173\u8282\u03b8\u2081\u989d\u5b9a\u626d\u77e985 N\u00b7m\u3001\u7b49\u6548\u8f6c\u52a8\u60ef\u91cfJ=0.12 kg\u00b7m\u00b2\u3001\u7c98\u6027\u6469\u64e6\u7cfb\u6570b=0.8 N\u00b7m\u00b7s\/rad，\u5927\u81c2\u957f0.45 m\/3.5 kg，\u5c0f\u81c2\u957f0.35 m\/2.0 kg，\u5782\u76f4\u5e73\u79fbd\u2084\u884c\u7a0b0~0.30 m\/\u989d\u5b9a\u901f\u5ea60.20 m\/s，\u57fa\u5ea7\u504f\u8dddd\u2081=0.32 m）\u6267\u884c\u8f74\u627f\u538b\u88c5\u5de5\u5e8f：\u672b\u7aef\u6267\u884c\u5668\u9700\u6cbf\u03b8\u2081\u65b9\u5411\u4ece0\u00b0\u8f6c\u523045\u00b0，\u518d\u4fdd\u6301\u4e0d\u52a8\u7b49\u5f85\u538b\u88c5\u6c14\u7f38\u52a8\u4f5c\u3002<\/p>\n
\u00a0<\/p>\n
\u8c03\u8bd5\u5de5\u7a0b\u5e08\u8001\u5468\u76ef\u7740\u793a\u6ce2\u5668\u4e0a\u7684\u5173\u8282\u89d2\u66f2\u7ebf："\u03b8\u2081\u4ece0\u00b0\u523045\u00b0\u7684\u9636\u8dc3\u54cd\u5e94，\u5355\u8c03\u722c\u5347，\u8c03\u8282\u65f6\u95f4\u7ea60.6 s，\u7a33\u6001\u8bef\u5dee\u63a5\u8fd1\u96f6\u3002\u538b\u88c5\u8282\u62cd\u8981\u6c420.5 s\u5185\u5230\u4f4d\u4e14\u7a33\u6001\u8bef\u5dee\u5c0f\u4e8e0.5\u00b0，\u73b0\u5728\u5230\u4f4d\u65f6\u95f4\u5dee\u4e00\u70b9\u3002"<\/p>\n
\u00a0<\/p>\n
\u4e0a\u4f4d\u673a\u5f00\u53d1\u5c0f\u90d1\u8c03\u51fa\u63a7\u5236\u5668\u53c2\u6570："\u73b0\u5728\u7528\u7684\u662fPD\u63a7\u5236，Kp=20 N\u00b7m\/rad，Kd=3 N\u00b7m\u00b7s\/rad\u3002\u7b97\u8fc7\u963b\u5c3c\u6bd4\u03b6\u22481.23，\u8fc7\u963b\u5c3c\u7cfb\u7edf，\u6240\u4ee5\u6ca1\u6709\u8d85\u8c03\u3002\u5230\u4f4d\u65f6\u95f4\u53d7Kd\u5f71\u54cd\u2014\u2014\u5fae\u5206\u9879\u63d0\u4f9b\u963b\u5c3c，\u4f46\u592a\u5927\u4e86\u54cd\u5e94\u53d8\u6162\u3002"<\/p>\n
\u00a0<\/p>\n
\u8001\u5468\u70b9\u5934："\u8fc7\u963b\u5c3c\u5355\u8c03\u6536\u655b，\u6ca1\u6709\u8d85\u8c03，\u8fd9\u5bf9\u88c5\u914d\u662f\u597d\u4e8b\u3002\u7a33\u6001\u8bef\u5dee\u63a5\u8fd1\u96f6\u662f\u56e0\u4e3a\u8fd9\u4e2a\u5173\u8282\u662f\u6c34\u5e73\u65cb\u8f6c，\u4e0d\u53d7\u91cd\u529b\u77e9，\u53ea\u6709\u7c98\u6027\u6469\u64e6\u2014\u2014\u9759\u6b62\u65f6\u7c98\u6027\u6469\u64e6\u4e3a\u96f6，\u4e0d\u9700\u8981\u8bef\u5dee\u6765\u5e73\u8861\u3002\u5982\u679c\u6362\u6210\u5782\u76f4\u5173\u8282，\u52a0\u4e86\u8d1f\u8f7d\u91cd\u529b，\u7eafPD\u5c31\u4f1a\u6709\u9759\u5dee，\u5f97\u52a0\u79ef\u5206\u9879\u3002"<\/p>\n
\u00a0<\/p>\n
"\u5bf9，SCARA\u7684\u03b8\u2081\u662f\u7ad6\u76f4\u8f74\u65cb\u8f6c，\u91cd\u529b\u4e0d\u4ea7\u751f\u6062\u590d\u529b\u77e9\u3002\u6240\u4ee5PD\u5728\u8fd9\u4e2a\u573a\u666f\u591f\u7528\u3002\u4e0b\u4e00\u6b65\u53ef\u4ee5\u8bd5Kp=30\u3001Kd=4，\u54cd\u5e94\u66f4\u5feb，\u963b\u5c3c\u6bd4\u8fd8\u80fd\u4fdd\u6301\u8fc7\u963b\u5c3c\u3002"<\/p>\n
\u00a0<\/p>\n
\u6784\u578b\u7ea6\u5b9a：\u672c\u7bc7\u805a\u7126\u5355\u5173\u8282（\u03b8\u2081）PD\u4f4d\u7f6e\u63a7\u5236\u3002\u88ab\u63a7\u5bf9\u8c61\u5efa\u6a21\u4e3a\u4e8c\u9636\u7cfb\u7edf：J\u00b7\u03b8\u0308 + b\u00b7\u03b8\u0307 = \u03c4_motor，\u5176\u4e2dJ=0.12 kg\u00b7m\u00b2\u4e3a\u7b49\u6548\u8f6c\u52a8\u60ef\u91cf，b=0.8 N\u00b7m\u00b7s\/rad\u4e3a\u7c98\u6027\u6469\u64e6\u7cfb\u6570\u3002\u63a7\u5236\u5668\u8f93\u51fa\u03c4 = Kp\u00b7(\u03b8_ref – \u03b8) – Kd\u00b7\u03b8\u0307\u3002\u76ee\u6807\u8f68\u8ff9\u4e3a\u7eaf\u9636\u8dc3\u4fe1\u53f7（0\u00b0\u219245\u00b0），\u7528\u4e8e\u6d4b\u8bd5\u9636\u8dc3\u54cd\u5e94；\u6b63\u5f26\u8f68\u8ff9\u5355\u72ec\u6d4b\u8bd5\u8ddf\u8e2a\u5e26\u5bbd\u3002<\/p>\n
\u26a0\ufe0f \u58f0\u660e：\u672c\u7bc7\u5168\u90e8\u4ee3\u7801\u4e3a\u8bfe\u7a0b\u79bb\u7ebf\u4eff\u771f，\u8fd0\u884c\u5728\u666e\u901aPC\u4e0a，\u7528\u4e8ePD\u4f4d\u7f6e\u63a7\u5236\u5668\u7b97\u6cd5\u9a8c\u8bc1\u3002\u4ee3\u7801\u4e2d\u4e0d\u5305\u542b\u4efb\u4f55\u771f\u5b9e\u673a\u5668\u4eba\u63a7\u5236\u5668\u5b9e\u65f6\u8fd0\u7b97\u6216\u5b89\u5168\u903b\u8f91\u3002\u4e0d\u53ef\u76f4\u63a5\u7528\u4e8e\u4ea7\u7ebf\u8bbe\u5907\u3002<\/p>\n
\u00a0<\/p>\n
2. \u75db\u70b9\u5206\u6790<\/p>\n
\u00a0<\/p>\n
\u8bfe\u7a0b\u6a21\u5757 \u672c\u7bc7\u5bf9\u5e94\u7684\u5de5\u7a0b\u75db\u70b9<\/p>\n
\u7b2c5\u7ae0 \u673a\u5668\u4eba\u5173\u8282\u9a71\u52a8\u4e0e\u63a7\u5236 \u2192 5.1 \u4f3a\u670d\u9a71\u52a8\u539f\u7406 \u5355\u5173\u8282\u4f4d\u7f6e\u95ed\u73af\u63a7\u5236，\u9700\u7406\u89e3\u6bd4\u4f8b\/\u5fae\u5206\u7684\u4f5c\u7528\u4e0e\u5c40\u9650<\/p>\n
\u7b2c5\u7ae0 \u673a\u5668\u4eba\u5173\u8282\u9a71\u52a8\u4e0e\u63a7\u5236 \u2192 5.2 \u63a7\u5236\u5668\u8bbe\u8ba1 PD\u53c2\u6570\u5bf9\u963b\u5c3c\u6bd4\u03b6\u548c\u56fa\u6709\u9891\u7387\u03c9n\u7684\u5f71\u54cd，\u8fc7\u963b\u5c3c\/\u6b20\u963b\u5c3c\u7684\u53d6\u820d<\/p>\n
\u7b2c4\u7ae0 \u673a\u5668\u4eba\u52a8\u529b\u5b66\u5efa\u6a21 \u2192 4.1 \u5355\u5173\u8282\u52a8\u529b\u5b66 \u88ab\u63a7\u5bf9\u8c61\u5efa\u6a21\u4e3aJ\u00b7\u03b8\u0308 + b\u00b7\u03b8\u0307 = \u03c4，\u542b\u60ef\u91cf\u548c\u7c98\u6027\u6469\u64e6<\/p>\n
\u7b2c10\u7ae0 \u4eff\u771f\u5b9e\u9a8c \u2192 10.2 \u63a7\u5236\u4eff\u771f\u9a8c\u8bc1 \u79bb\u7ebf\u4eff\u771f\u9a8c\u8bc1PD\u63a7\u5236\u5668\u5bf9\u9636\u8dc3\/\u6b63\u5f26\u8f68\u8ff9\u7684\u8ddf\u968f\u6027\u80fd<\/p>\n
\u00a0<\/p>\n
\u672c\u7bc7\u4ee3\u7801\u89e3\u51b3 L2 \u79bb\u7ebf\u4eff\u771f\u6821\u6838\u5c42\u3002\u8f93\u5165：\u76ee\u6807\u5173\u8282\u89d2\u8f68\u8ff9\u3001PD\u53c2\u6570(Kp, Kd)\u3001\u88ab\u63a7\u5bf9\u8c61\u53c2\u6570(J, b)\u3002\u5904\u7406：\u2460\u5efa\u7acb\u5355\u5173\u8282\u52a8\u529b\u5b66\u6a21\u578b；\u2461\u5b9e\u73b0PD\u63a7\u5236\u5668（\u901f\u5ea6\u53cd\u9988\u5f62\u5f0f）；\u2462\u7528\u534a\u9690\u5f0f\u6b27\u62c9\u6cd5\u6570\u503c\u79ef\u5206\u4eff\u771f\u95ed\u73af\u54cd\u5e94；\u2463\u8ba1\u7b97\u8ddf\u968f\u8bef\u5dee（RMSE\u3001\u6700\u5927\u8bef\u5dee\u3001\u8c03\u8282\u65f6\u95f4）；\u2464\u8f93\u51fa4\u5f20\u5206\u6790\u56fe\u8868\u3002\u8f93\u51fa：\u56fe\u8868+CSV\u6570\u636e\u6587\u4ef6\u3002<\/p>\n
\u00a0<\/p>\n
\u4e0d\u8986\u76d6：\u591a\u5173\u8282\u8026\u5408\u63a7\u5236\u3001PID\u79ef\u5206\u9879\u3001\u6469\u64e6\u529b\u975e\u7ebf\u6027\u6a21\u578b\u3001\u529b\u77e9\u7ea6\u675f\u9971\u548c（\u672c\u53c2\u6570\u4e0b\u4e0d\u89e6\u53d1）\u3001\u5b9e\u65f6\u63a7\u5236\u5468\u671f\u3002<\/p>\n
\u00a0<\/p>\n
3. \u6838\u5fc3\u903b\u8f91\u8bb2\u89e3<\/p>\n
\u00a0<\/p>\n
\u7269\u7406\u76f4\u89c9<\/p>\n
\u00a0<\/p>\n
\u60f3\u8c61\u4f60\u63a8\u4e00\u8f86\u624b\u63a8\u8f66\u5230\u6307\u5b9a\u4f4d\u7f6e\u3002\u6bd4\u4f8b\u63a7\u5236\u5c31\u50cf\u4f60\u6839\u636e\u8f66\u79bb\u76ee\u6807\u7684\u8ddd\u79bb\u51b3\u5b9a\u7528\u591a\u5927\u529b\u2014\u2014\u79bb\u5f97\u8fdc\u5c31\u731b\u63a8，\u5feb\u5230\u4e86\u5c31\u8f7b\u63a8\u3002\u4f46\u5149\u9760\u6bd4\u4f8b，\u5230\u4e86\u76ee\u6807\u4f4d\u7f6e\u901f\u5ea6\u521a\u597d\u4e3a\u96f6\u65f6\u4f60\u4f1a\u505c\u4e0d\u4f4f（\u60ef\u6027），\u6216\u8005\u4e3a\u4e86\u7cbe\u786e\u505c\u5728\u76ee\u6807\u70b9\u4f60\u9700\u8981\u65e0\u9650\u5c0f\u7684\u529b（\u5b9e\u9645\u505a\u4e0d\u5230）\u3002\u5fae\u5206\u63a7\u5236\u5c31\u50cf\u4f60\u6839\u636e\u8f66\u901f\u6765\u51b3\u5b9a\u5239\u8f66\u529b\u5ea6\u2014\u2014\u8f66\u8d8a\u5feb，\u5239\u8f66\u8d8a\u72e0\u3002\u4e24\u8005\u7ed3\u5408：\u6bd4\u4f8b\u8d1f\u8d23"\u5230\u4f4d"，\u5fae\u5206\u8d1f\u8d23"\u5239\u4f4f"\u3002<\/p>\n
\u00a0<\/p>\n
\u4e1a\u52a1\u903b\u8f91 \u2192 \u4ee3\u7801\u6620\u5c04<\/p>\n
\u00a0<\/p>\n
\u8f93\u5165：\u76ee\u6807\u8f68\u8ff9 \u03b8_ref(t)，PD\u53c2\u6570(Kp, Kd)，\u52a8\u529b\u5b66\u53c2\u6570(J, b)<\/p>\n
\u00a0 \u2193<\/p>\n
\u3010\u6b65\u9aa41：\u521d\u59cb\u5316\u3011<\/p>\n
\u00a0 \u5f53\u524d\u72b6\u6001 \u03b8=0, \u03b8\u0307=0<\/p>\n
\u00a0 \u4eff\u771f\u65f6\u95f4 t=0, \u6b65\u957f dt=0.001 s<\/p>\n
\u00a0 \u2193<\/p>\n
\u3010\u6b65\u9aa42：\u4e3b\u5faa\u73af\u3011<\/p>\n
\u00a0 for each timestep:<\/p>\n
\u00a0 \u00a0 \u2460 \u8bfb\u53d6\u5f53\u524d\u4f4d\u7f6e\u03b8\u548c\u901f\u5ea6\u03b8\u0307<\/p>\n
\u00a0 \u00a0 \u2461 \u8ba1\u7b97\u8bef\u5dee e = \u03b8_ref(t) – \u03b8<\/p>\n
\u00a0 \u00a0 \u2462 PD\u8f93\u51fa：\u03c4 = Kp\u00b7e – Kd\u00b7\u03b8\u0307 （\u901f\u5ea6\u53cd\u9988\u5f62\u5f0f）<\/p>\n
\u00a0 \u00a0 \u2463 \u52a8\u529b\u5b66\u66f4\u65b0：\u03b8\u0308 = (\u03c4 – b\u00b7\u03b8\u0307) \/ J<\/p>\n
\u00a0 \u00a0 \u2464 \u534a\u9690\u5f0f\u6b27\u62c9\u79ef\u5206：\u5148\u66f4\u65b0\u03b8\u0307，\u518d\u66f4\u65b0\u03b8<\/p>\n
\u00a0 \u00a0 \u2465 \u8bb0\u5f55\u6570\u636e<\/p>\n
\u00a0 \u2193<\/p>\n
\u3010\u6b65\u9aa43：\u540e\u5904\u7406\u3011<\/p>\n
\u00a0 \u8ba1\u7b97RMSE\u3001\u6700\u5927\u8bef\u5dee\u3001\u8c03\u8282\u65f6\u95f4（\u00b12%\u5e26）<\/p>\n
\u00a0 \u9636\u8dc3\u4e0e\u6b63\u5f26\u5206\u5f00\u7edf\u8ba1<\/p>\n
\u00a0 \u2193<\/p>\n
\u3010\u6b65\u9aa44：\u53ef\u89c6\u5316\u3011<\/p>\n
\u00a0 \u8f93\u51fa4\u5f20\u5206\u6790\u56fe\u8868<\/p>\n
\u00a0<\/p>\n
\u5173\u952e\u516c\u5f0f\u6587\u5b57\u7ffb\u8bd1<\/p>\n
\u00a0<\/p>\n
PD\u63a7\u5236\u5f8b：\u03c4 = Kp\u00b7(\u03b8_ref – \u03b8) – Kd\u00b7\u03b8\u0307\u3002Kp\u662f\u6bd4\u4f8b\u589e\u76ca（N\u00b7m\/rad），\u51b3\u5b9a\u7cfb\u7edf\u521a\u5ea6\u3002Kd\u662f\u5fae\u5206\u589e\u76ca（N\u00b7m\u00b7s\/rad），\u63d0\u4f9b\u901f\u5ea6\u53cd\u9988\u963b\u5c3c\u3002\u6ce8\u610f\u8fd9\u91cc\u5fae\u5206\u9879\u4f5c\u7528\u4e8e\u5b9e\u9645\u5173\u8282\u901f\u5ea6\u03b8\u0307\u800c\u975e\u8bef\u5dee\u5bfc\u6570\u0117\u2014\u2014\u5728\u53c2\u8003\u8f68\u8ff9\u4e3a\u9636\u8dc3（\u53c2\u8003\u901f\u5ea6\u4e3a\u96f6）\u65f6\u4e24\u8005\u7b49\u4ef7；\u4f46\u5728\u53c2\u8003\u8f68\u8ff9\u542b\u901f\u5ea6\u53d8\u5316\u65f6，\u901f\u5ea6\u53cd\u9988\u5f62\u5f0f\u5bf9\u53c2\u8003\u7a81\u53d8\u4e0d\u654f\u611f，\u8fd9\u662f\u6559\u5b66\u4e0a\u9700\u8981\u533a\u5206\u7684\u3002<\/p>\n
\u00a0<\/p>\n
\u95ed\u73af\u7279\u5f81\u65b9\u7a0b：\u5c06PD\u4ee3\u5165\u52a8\u529b\u5b66\u5f97 J\u00b7\u03b8\u0308 + (b+Kd)\u00b7\u03b8\u0307 + Kp\u00b7\u03b8 = Kp\u00b7\u03b8_ref\u3002\u5bf9\u5e94\u6807\u51c6\u4e8c\u9636\u7cfb\u7edf：\u03c9n = \u221a(Kp\/J)（\u65e0\u963b\u5c3c\u56fa\u6709\u9891\u7387），\u03b6 = (b+Kd)\/(2\u221a(Kp\u00b7J))（\u963b\u5c3c\u6bd4）\u3002\u672c\u7bc7\u53c2\u6570：\u03c9n = \u221a(20\/0.12) \u2248 12.9 rad\/s，\u03b6 = (0.8+3.0)\/(2\u221a(20\u00d70.12)) \u2248 1.23 > 1，\u7cfb\u7edf\u4e3a\u8fc7\u963b\u5c3c，\u9636\u8dc3\u54cd\u5e94\u5355\u8c03\u6536\u655b，\u65e0\u8d85\u8c03\u3002<\/p>\n
\u00a0<\/p>\n
\u8c03\u8282\u65f6\u95f4：\u8fc7\u963b\u5c3c\u7cfb\u7edf\u8fd1\u4f3c\u4e3a\u4e24\u4e2a\u4e00\u9636\u60ef\u6027\u73af\u8282\u4e32\u8054，\u4e3b\u5bfc\u6781\u70b9\u5728 s \u2248 -(b+Kd)\/J = -3.8\/0.12 \u2248 -31.7 rad\/s？\u4e0d\u5bf9\u2014\u2014\u91cd\u65b0\u7b97：\u7279\u5f81\u6839 s = [-(b+Kd) \u00b1 \u221a((b+Kd)\u00b2 – 4JKp)]\/(2J)\u3002\u4ee3\u5165：b+Kd=3.8, 4JKp=4\u00d70.12\u00d720=9.6, (b+Kd)\u00b2=14.44\u3002\u221a14.44-9.6=\u221a4.84=2.2\u3002s\u2081 = (-3.8+2.2)\/(0.24) = -6.67, s\u2082 = (-3.8-2.2)\/(0.24) = -25.0\u3002\u4e3b\u5bfc\u6781\u70b9 s\u2081=-6.67，\u8c03\u8282\u65f6\u95f4（\u00b12%）\u2248 4\/|s\u2081| = 4\/6.67 \u2248 0.60 s\u3002<\/p>\n
\u00a0<\/p>\n
\u7a33\u6001\u8bef\u5dee：\u5bf9\u9636\u8dc3\u8f93\u5165，\u7ec8\u503c\u5b9a\u7406\u5f97 \u03b8(\u221e) = \u03b8_ref(\u221e) \u00d7 Kp\/(Kp) = \u03b8_ref(\u221e)\u3002\u7a33\u6001\u8bef\u5dee\u4e3a\u96f6\u2014\u2014\u56e0\u4e3a\u88ab\u63a7\u5bf9\u8c61\u53ea\u6709\u7c98\u6027\u6469\u64e6，\u9759\u6b62\u65f6b\u00b7\u03b8\u0307=0，\u4e0d\u9700\u8981\u4efb\u4f55\u8bef\u5dee\u6765\u5e73\u8861\u3002\u8fd9\u4e0e\u5b58\u5728\u5e93\u4ed1\u6469\u64e6\u6216\u91cd\u529b\u6270\u52a8\u7684\u7cfb\u7edf\u4e0d\u540c（\u90a3\u4e9b\u60c5\u51b5\u4e0bPD\u786e\u5b9e\u6709\u9759\u5dee）\u3002<\/p>\n
\u00a0<\/p>\n
4. OOP\u4ee3\u7801\u5b9e\u73b0<\/p>\n
\u00a0<\/p>\n
\u6280\u672f\u6808<\/p>\n
\u00a0<\/p>\n
\u4e25\u683c\u9650\u5b9a：numpy, matplotlib, math（\u6807\u51c6\u5e93）\u3002\u4e0d\u5f15\u5165pybullet\u3001roboticstoolbox\u7b49\u4efb\u4f55\u7b2c\u4e09\u65b9\u91cd\u578b\u5e93\u3002PD\u63a7\u5236\u5668\u3001\u52a8\u529b\u5b66\u6a21\u578b\u3001\u6570\u503c\u79ef\u5206\u5168\u90e8\u81ea\u5b9e\u73b0\u3002<\/p>\n
\u00a0<\/p>\n
\u9879\u76ee\u7ed3\u6784\u6811<\/p>\n
\u00a0<\/p>\n
pd_joint_control\/<\/p>\n
\u251c\u2500\u2500 run_pd_sim.py<\/p>\n
\u251c\u2500\u2500 requirements.txt<\/p>\n
\u251c\u2500\u2500 robot\/<\/p>\n
\u2502 \u251c\u2500\u2500 __init__.py<\/p>\n
\u2502 \u251c\u2500\u2500 models.py<\/p>\n
\u2502 \u251c\u2500\u2500 dynamics.py<\/p>\n
\u2502 \u251c\u2500\u2500 pd_controller.py<\/p>\n
\u2502 \u251c\u2500\u2500 trajectory.py<\/p>\n
\u2502 \u2514\u2500\u2500 visualize.py<\/p>\n
\u251c\u2500\u2500 tests\/<\/p>\n
\u2502 \u251c\u2500\u2500 __init__.py<\/p>\n
\u2502 \u2514\u2500\u2500 test_pd_controller.py<\/p>\n
\u251c\u2500\u2500 results\/<\/p>\n
\u2514\u2500\u2500 output\/<\/p>\n
\u00a0<\/p>\n
\u6838\u5fc3\u6e90\u7801<\/p>\n
\u00a0<\/p>\n
"robot\/models.py" \u2014 \u6570\u636e\u6a21\u578b<\/p>\n
\u00a0<\/p>\n
"""<\/p>\n
\u6570\u636e\u6a21\u578b：\u5355\u5173\u8282\u52a8\u529b\u5b66\u53c2\u6570\u3001PD\u63a7\u5236\u5668\u53c2\u6570\u3001\u4eff\u771f\u914d\u7f6e<\/p>\n
\u5168\u90e8\u53c2\u6570\u5185\u7f6e，\u81ea\u5305\u542b\u53ef\u590d\u73b0<\/p>\n
"""<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
import numpy as np<\/p>\n
from dataclasses import dataclass, field<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
@dataclass<\/p>\n
class JointDynamicsParams:<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u5355\u5173\u8282\u52a8\u529b\u5b66\u53c2\u6570<\/p>\n
\u00a0 \u00a0 J: \u7b49\u6548\u8f6c\u52a8\u60ef\u91cf (kg\u00b7m\u00b2)<\/p>\n
\u00a0 \u00a0 b: \u7c98\u6027\u6469\u64e6\u7cfb\u6570 (N\u00b7m\u00b7s\/rad)<\/p>\n
\u00a0 \u00a0 tau_max: \u6700\u5927\u8f93\u51fa\u626d\u77e9 (N\u00b7m)，\u7528\u4e8e\u9971\u548c\u9650\u5236<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 J: float = 0.12<\/p>\n
\u00a0 \u00a0 b: float = 0.8<\/p>\n
\u00a0 \u00a0 tau_max: float = 85.0<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
@dataclass<\/p>\n
class PDControllerParams:<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 PD\u63a7\u5236\u5668\u53c2\u6570<\/p>\n
\u00a0 \u00a0 Kp: \u6bd4\u4f8b\u589e\u76ca (N\u00b7m\/rad)<\/p>\n
\u00a0 \u00a0 Kd: \u5fae\u5206\u589e\u76ca (N\u00b7m\u00b7s\/rad)<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 Kp: float = 20.0<\/p>\n
\u00a0 \u00a0 Kd: float = 3.0<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
@dataclass<\/p>\n
class SimulationConfig:<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u4eff\u771f\u914d\u7f6e<\/p>\n
\u00a0 \u00a0 dt: \u79ef\u5206\u6b65\u957f (s)<\/p>\n
\u00a0 \u00a0 t_total: \u603b\u4eff\u771f\u65f6\u957f (s)<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 dt: float = 0.001<\/p>\n
\u00a0 \u00a0 t_total: float = 3.0<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 @property<\/p>\n
\u00a0 \u00a0 def n_steps(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return int(self.t_total \/ self.dt)<\/p>\n
\u00a0<\/p>\n
"robot\/dynamics.py" \u2014 \u5355\u5173\u8282\u52a8\u529b\u5b66\u6a21\u578b<\/p>\n
\u00a0<\/p>\n
"""<\/p>\n
\u5355\u5173\u8282\u52a8\u529b\u5b66\u6a21\u578b：J\u00b7\u03b8\u0308 + b\u00b7\u03b8\u0307 = \u03c4<\/p>\n
\u542b\u626d\u77e9\u9971\u548c\u9650\u5236<\/p>\n
"""<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
import numpy as np<\/p>\n
from robot.models import JointDynamicsParams<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
class SingleJointDynamics:<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u4e8c\u9636\u5355\u5173\u8282\u52a8\u529b\u5b66<\/p>\n
\u00a0 \u00a0 \u72b6\u6001\u53d8\u91cf：[\u03b8, \u03b8\u0307]<\/p>\n
\u00a0 \u00a0 \u8f93\u5165：\u03c4_motor (N\u00b7m)<\/p>\n
\u00a0 \u00a0 \u8f93\u51fa：\u89d2\u52a0\u901f\u5ea6 \u03b8\u0308 (rad\/s\u00b2)<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def __init__(self, params: JointDynamicsParams):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 self.params = params<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 self.state = np.array([0.0, 0.0]) # [\u03b8, \u03b8\u0307]<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def reset(self, theta_0=0.0, theta_dot_0=0.0):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u91cd\u7f6e\u72b6\u6001"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 self.state = np.array([theta_0, theta_dot_0])<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 @property<\/p>\n
\u00a0 \u00a0 def theta(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return self.state[0]<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 @property<\/p>\n
\u00a0 \u00a0 def theta_dot(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return self.state[1]<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def compute_acceleration(self, tau_motor):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u8ba1\u7b97\u89d2\u52a0\u901f\u5ea6<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u03b8\u0308 = (\u03c4 – b\u00b7\u03b8\u0307) \/ J<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u542b\u626d\u77e9\u9971\u548c\u9650\u5236，\u9632\u6b62\u6570\u503c\u7206\u70b8<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau_sat = np.clip(tau_motor,<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 -self.params.tau_max,<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 self.params.tau_max)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u963b\u5c3c\u529b\u77e9 = b * \u03b8\u0307<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau_damping = self.params.b * self.state[1]<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u51c0\u626d\u77e9 = \u7535\u673a\u626d\u77e9 – \u963b\u5c3c\u626d\u77e9<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau_net = tau_sat – tau_damping<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u89d2\u52a0\u901f\u5ea6 = \u51c0\u626d\u77e9 \/ \u60ef\u91cf<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 alpha = tau_net \/ self.params.J<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return alpha<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def step_euler(self, tau_motor, dt):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u534a\u9690\u5f0f\u6b27\u62c9\u79ef\u5206\u4e00\u6b65<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u5148\u66f4\u65b0\u901f\u5ea6\u518d\u66f4\u65b0\u4f4d\u7f6e，\u6570\u503c\u7a33\u5b9a\u6027\u4f18\u4e8e\u663e\u5f0f\u6b27\u62c9<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 alpha = self.compute_acceleration(tau_motor)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u5148\u66f4\u65b0\u901f\u5ea6<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 self.state[1] = self.state[1] + alpha * dt<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u518d\u66f4\u65b0\u4f4d\u7f6e<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 self.state[0] = self.state[0] + self.state[1] * dt<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return self.state.copy()<\/p>\n
\u00a0<\/p>\n
"robot\/pd_controller.py" \u2014 PD\u4f4d\u7f6e\u63a7\u5236\u5668<\/p>\n
\u00a0<\/p>\n
"""<\/p>\n
PD\u4f4d\u7f6e\u63a7\u5236\u5668（\u901f\u5ea6\u53cd\u9988\u5f62\u5f0f）<\/p>\n
\u03c4 = Kp\u00b7(\u03b8_ref – \u03b8) – Kd\u00b7\u03b8\u0307<\/p>\n
"""<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
import numpy as np<\/p>\n
from robot.models import PDControllerParams<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
class PDController:<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u79bb\u6563PD\u63a7\u5236\u5668（\u901f\u5ea6\u53cd\u9988\u5f62\u5f0f）<\/p>\n
\u00a0 \u00a0 \u8f93\u5165：\u76ee\u6807\u4f4d\u7f6e \u03b8_ref\u3001\u5f53\u524d\u4f4d\u7f6e \u03b8\u3001\u5f53\u524d\u901f\u5ea6 \u03b8\u0307<\/p>\n
\u00a0 \u00a0 \u8f93\u51fa：\u63a7\u5236\u626d\u77e9 \u03c4 (N\u00b7m)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u6ce8：\u5fae\u5206\u9879\u4f5c\u7528\u4e8e\u5b9e\u9645\u901f\u5ea6\u03b8\u0307\u800c\u975e\u8bef\u5dee\u5bfc\u6570\u0117\u3002<\/p>\n
\u00a0 \u00a0 \u5728\u53c2\u8003\u8f68\u8ff9\u4e3a\u9636\u8dc3（\u53c2\u8003\u901f\u5ea6=0）\u65f6\u4e0e\u0117\u5f62\u5f0f\u7b49\u4ef7\u3002<\/p>\n
\u00a0 \u00a0 \u5728\u53c2\u8003\u8f68\u8ff9\u542b\u901f\u5ea6\u53d8\u5316\u65f6，\u901f\u5ea6\u53cd\u9988\u5f62\u5f0f\u5bf9\u53c2\u8003\u7a81\u53d8\u4e0d\u654f\u611f\u3002<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def __init__(self, params: PDControllerParams):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 self.params = params<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def compute(self, theta_ref, theta, theta_dot):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 PD\u63a7\u5236\u5f8b\u8ba1\u7b97<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 e = \u03b8_ref – \u03b8（\u4f4d\u7f6e\u8bef\u5dee）<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u03c4 = Kp\u00b7e – Kd\u00b7\u03b8\u0307<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 error = theta_ref – theta<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u6bd4\u4f8b\u9879：\u8bef\u5dee\u8d8a\u5927，\u8f93\u51fa\u626d\u77e9\u8d8a\u5927<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau_proportional = self.params.Kp * error<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u5fae\u5206\u9879：\u901f\u5ea6\u8d8a\u5927，\u53cd\u5411\u963b\u5c3c\u8d8a\u5927<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u6ce8\u610f：\u8fd9\u91cc\u7528\u03b8\u0307\u800c\u975e\u0117，\u662f\u901f\u5ea6\u53cd\u9988\u5f62\u5f0f<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau_derivative = -self.params.Kd * theta_dot<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u603b\u8f93\u51fa<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau = tau_proportional + tau_derivative<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return tau, error<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def set_gains(self, Kp=None, Kd=None):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u8fd0\u884c\u65f6\u4fee\u6539\u589e\u76ca"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 if Kp is not None:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 self.params.Kp = Kp<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 if Kd is not None:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 self.params.Kd = Kd<\/p>\n
\u00a0<\/p>\n
"robot\/trajectory.py" \u2014 \u76ee\u6807\u8f68\u8ff9\u751f\u6210<\/p>\n
\u00a0<\/p>\n
"""<\/p>\n
\u76ee\u6807\u8f68\u8ff9\u751f\u6210：\u9636\u8dc3\u3001\u6b63\u5f26<\/p>\n
\u9636\u8dc3\u7528\u4e8e\u6d4b\u8bd5\u8d85\u8c03\/\u8c03\u8282\u65f6\u95f4\/\u7a33\u6001\u8bef\u5dee<\/p>\n
\u6b63\u5f26\u7528\u4e8e\u6d4b\u8bd5\u8ddf\u8e2a\u5e26\u5bbd<\/p>\n
\u4e24\u5957\u6307\u6807\u5206\u5f00\u7edf\u8ba1<\/p>\n
"""<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
import numpy as np<\/p>\n
import math<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
class ReferenceTrajectory:<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u751f\u6210\u5173\u8282\u7a7a\u95f4\u53c2\u8003\u8f68\u8ff9<\/p>\n
\u00a0 \u00a0 """<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 @staticmethod<\/p>\n
\u00a0 \u00a0 def step_trajectory(t, step_time=0.5, step_value=math.pi\/4):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u9636\u8dc3\u8f68\u8ff9：t < step_time\u65f6\u03b8_ref=0，\u4e4b\u540e\u03b8_ref=step_value<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u7528\u4e8e\u9636\u8dc3\u54cd\u5e94\u6d4b\u8bd5<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 if t < step_time:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 return 0.0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 else:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 return step_value<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 @staticmethod<\/p>\n
\u00a0 \u00a0 def sine_trajectory(t, amplitude=0.1, frequency=0.5, offset=0.0):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u6b63\u5f26\u8f68\u8ff9：\u03b8_ref = offset + A\u00b7sin(2\u03c0\u00b7f\u00b7t)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u7528\u4e8e\u8ddf\u8e2a\u5e26\u5bbd\u6d4b\u8bd5<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return offset + amplitude * math.sin(<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 2 * math.pi * frequency * t)<\/p>\n
\u00a0<\/p>\n
"robot\/visualize.py" \u2014 \u53ef\u89c6\u5316<\/p>\n
\u00a0<\/p>\n
"""<\/p>\n
PD\u63a7\u5236\u4eff\u771f\u53ef\u89c6\u5316：4\u5f20\u56fe\u8868<\/p>\n
\u2460 \u9636\u8dc3\u54cd\u5e94\u5bf9\u6bd4（\u76ee\u6807 vs \u5b9e\u9645）<\/p>\n
\u2461 \u63a7\u5236\u626d\u77e9\u8f93\u51fa<\/p>\n
\u2462 \u9636\u8dc3\u54cd\u5e94\u8ddf\u968f\u8bef\u5dee<\/p>\n
\u2463 \u76f8\u5e73\u9762\u56fe（\u03b8 vs \u03b8\u0307）<\/p>\n
"""<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
import numpy as np<\/p>\n
import matplotlib.pyplot as plt<\/p>\n
\u00a0<\/p>\n
plt.rcParams['font.sans-serif'] = ['SimHei', 'DejaVu Sans']<\/p>\n
plt.rcParams['axes.unicode_minus'] = False<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
class PDVisualizer:<\/p>\n
\u00a0 \u00a0 def __init__(self, results_dir="results"):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 self.results_dir = results_dir<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 import os<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 os.makedirs(results_dir, exist_ok=True)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def plot_angle_tracking(self, t, theta_ref, theta_actual):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u2460 \u5173\u8282\u89d2\u8ddf\u968f\u66f2\u7ebf"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 fig, ax = plt.subplots(figsize=(10, 6))<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.plot(t, theta_ref, 'b–', linewidth=2.0,<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 label='\u76ee\u6807\u8f68\u8ff9 \u03b8_ref')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.plot(t, theta_actual, 'r-', linewidth=1.0, alpha=0.8,<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 label='\u5b9e\u9645\u8f68\u8ff9 \u03b8')<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_xlabel('\u65f6\u95f4 (s)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_ylabel('\u5173\u8282\u89d2 (rad)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_title('PD\u63a7\u5236：\u9636\u8dc3\u54cd\u5e94\u5bf9\u6bd4')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.legend(fontsize=9)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.grid(True, alpha=0.3)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.tight_layout()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 path = f"{self.results_dir}\/01_angle_tracking.png"<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.savefig(path, dpi=150)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.close()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return path<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def plot_control_torque(self, t, tau):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u2461 \u63a7\u5236\u626d\u77e9\u8f93\u51fa"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 fig, ax = plt.subplots(figsize=(10, 5))<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.plot(t, tau, 'g-', linewidth=1.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.axhline(y=0, color='black', linestyle='-', linewidth=0.5)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_xlabel('\u65f6\u95f4 (s)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_ylabel('\u63a7\u5236\u626d\u77e9 \u03c4 (N\u00b7m)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_title('PD\u63a7\u5236\u5668\u8f93\u51fa\u626d\u77e9')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.grid(True, alpha=0.3)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.tight_layout()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 path = f"{self.results_dir}\/02_control_torque.png"<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.savefig(path, dpi=150)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.close()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return path<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def plot_tracking_error(self, t, error):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u2462 \u8ddf\u968f\u8bef\u5dee\u65f6\u57df\u66f2\u7ebf"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 fig, ax = plt.subplots(figsize=(10, 5))<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.plot(t, error * 180 \/ np.pi, 'm-', linewidth=1.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.axhline(y=0, color='black', linestyle='-', linewidth=0.5)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_xlabel('\u65f6\u95f4 (s)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_ylabel('\u8ddf\u968f\u8bef\u5dee (\u00b0)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_title('PD\u63a7\u5236\u9636\u8dc3\u54cd\u5e94\u8ddf\u968f\u8bef\u5dee')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.grid(True, alpha=0.3)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.tight_layout()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 path = f"{self.results_dir}\/03_tracking_error.png"<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.savefig(path, dpi=150)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.close()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return path<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def plot_phase_portrait(self, theta, theta_dot):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u2463 \u76f8\u5e73\u9762\u56fe（\u03b8 vs \u03b8\u0307）"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 fig, ax = plt.subplots(figsize=(7, 7))<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.plot(theta * 180 \/ np.pi, theta_dot * 180 \/ np.pi,<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 'b-', linewidth=0.8, alpha=0.7)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_xlabel('\u5173\u8282\u89d2 \u03b8 (\u00b0)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_ylabel('\u5173\u8282\u89d2\u901f\u5ea6 \u03b8\u0307 (\u00b0\/s)')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.set_title('\u76f8\u5e73\u9762\u56fe（\u03b8 vs \u03b8\u0307）')<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ax.grid(True, alpha=0.3)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.tight_layout()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 path = f"{self.results_dir}\/04_phase_portrait.png"<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.savefig(path, dpi=150)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 plt.close()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 return path<\/p>\n
\u00a0<\/p>\n
5. \u53ef\u89c6\u5316\u8981\u6c42<\/p>\n
\u00a0<\/p>\n
\u5e8f\u53f7 \u56fe\u8868\u6587\u4ef6 \u670d\u52a1\u7ed3\u8bba<\/p>\n
\u2460\u00a0<\/p>\n
"01_angle_tracking.png" \u9636\u8dc3\u54cd\u5e94\u5bf9\u6bd4，\u53ef\u8bfb\u51fa\u5355\u8c03\u6536\u655b\u3001\u8c03\u8282\u65f6\u95f4~0.6 s\u3001\u65e0\u8d85\u8c03<\/p>\n
\u2461\u00a0<\/p>\n
"02_control_torque.png" \u63a7\u5236\u626d\u77e9\u8f93\u51fa\u66f2\u7ebf，\u53ef\u8bfb\u51fa\u5cf0\u503c\u626d\u77e9\u7ea615.7 N\u00b7m（\u8fdc\u5c0f\u4e8e85 N\u00b7m\u989d\u5b9a）<\/p>\n
\u2462\u00a0<\/p>\n
"03_tracking_error.png" \u9636\u8dc3\u54cd\u5e94\u8ddf\u968f\u8bef\u5dee，\u53ef\u8bfb\u51fa\u7a33\u6001\u8bef\u5dee\u63a5\u8fd10\u00b0<\/p>\n
\u2463\u00a0<\/p>\n
"04_phase_portrait.png" \u76f8\u5e73\u9762\u56fe\u87ba\u65cb\u6536\u655b，\u9a8c\u8bc1\u8fc7\u963b\u5c3c\u7279\u6027<\/p>\n
\u00a0<\/p>\n
6. README\u4f7f\u7528\u8bf4\u660e + \u6838\u5fc3\u77e5\u8bc6\u70b9\u5361\u7247<\/p>\n
\u00a0<\/p>\n
\u5feb\u901f\u5f00\u59cb<\/p>\n
\u00a0<\/p>\n
pip install -r requirements.txt<\/p>\n
python -m pytest tests\/test_pd_controller.py -v # 3\/3 \u6d4b\u8bd5\u901a\u8fc7<\/p>\n
python run_pd_sim.py # \u8fd0\u884cPD\u63a7\u5236\u4eff\u771f<\/p>\n
\u00a0<\/p>\n
\u6570\u636e\u683c\u5f0f\u8bf4\u660e<\/p>\n
\u00a0<\/p>\n
# \u4fee\u6539PD\u53c2\u6570：\u7f16\u8f91 PDControllerParams(Kp=20.0, Kd=3.0)<\/p>\n
# \u4fee\u6539\u52a8\u529b\u5b66\u53c2\u6570：\u7f16\u8f91 JointDynamicsParams(J=0.12, b=0.8)<\/p>\n
# \u4fee\u6539\u76ee\u6807\u8f68\u8ff9：\u7f16\u8f91 trajectory.step_trajectory(step_time, step_value)<\/p>\n
# \u4fee\u6539\u4eff\u771f\u65f6\u957f\/\u6b65\u957f：\u7f16\u8f91 SimulationConfig(dt=0.001, t_total=3.0)<\/p>\n
\u00a0<\/p>\n
\u53c2\u6570 \u4fee\u6539\u4f4d\u7f6e \u8bf4\u660e<\/p>\n
Kp\u589e\u76ca\u00a0<\/p>\n
"PDControllerParams(Kp=)" \u589e\u5927\u2192\u54cd\u5e94\u5feb\u4f46\u03b6\u51cf\u5c0f<\/p>\n
Kd\u589e\u76ca\u00a0<\/p>\n
"PDControllerParams(Kd=)" \u589e\u5927\u2192\u963b\u5c3c\u5927，\u8fc7\u963b\u5c3c\u66f4\u5b89\u5168<\/p>\n
\u8f6c\u52a8\u60ef\u91cfJ\u00a0<\/p>\n
"JointDynamicsParams(J=)" \u5f71\u54cd\u03c9n\u548c\u03b6<\/p>\n
\u7c98\u6027\u6469\u64e6b\u00a0<\/p>\n
"JointDynamicsParams(b=)" \u5f71\u54cd\u7a33\u6001\u963b\u5c3c<\/p>\n
\u00a0<\/p>\n
📌 \u77e5\u8bc6\u70b9\u5361\u7247<\/p>\n
\u00a0<\/p>\n
PD\u4f4d\u7f6e\u63a7\u5236（\u5bf9\u5e94\u8bfe\u672c\u7b2c5\u7ae05.1-5.2\u8282）<\/p>\n
\u00a0<\/p>\n
– PD\u63a7\u5236\u5f8b：\u03c4 = Kp\u00b7e – Kd\u00b7\u03b8\u0307，\u6bd4\u4f8b\u9879\u63d0\u4f9b\u521a\u5ea6，\u5fae\u5206\u9879\u63d0\u4f9b\u901f\u5ea6\u53cd\u9988\u963b\u5c3c<\/p>\n
– \u4e8c\u9636\u7cfb\u7edf\u53c2\u6570：\u03c9n = \u221a(Kp\/J)，\u03b6 = (b+Kd)\/(2\u221a(Kp\u00b7J))\u3002\u672c\u7bc7\u03c9n\u224812.9 rad\/s，\u03b6\u22481.23，\u8fc7\u963b\u5c3c<\/p>\n
– \u7a33\u6001\u8bef\u5dee：\u5bf9\u9636\u8dc3\u8f93\u5165，\u672c\u6a21\u578b（\u4ec5\u7c98\u6027\u6469\u64e6）\u7a33\u6001\u8bef\u5dee\u7cbe\u786e\u4e3a\u96f6\u3002\u82e5\u5b58\u5728\u5e93\u4ed1\u6469\u64e6\/\u91cd\u529b\u6270\u52a8\u5219\u9700PID\u79ef\u5206\u9879<\/p>\n
– \u5fae\u5206\u9879\u5f62\u5f0f：\u672c\u7bc7\u91c7\u7528\u901f\u5ea6\u53cd\u9988（\u03b8\u0307）\u800c\u975e\u8bef\u5dee\u5bfc\u6570（\u0117），\u9636\u8dc3\u8f93\u5165\u4e0b\u7b49\u4ef7，\u6b63\u5f26\u8ddf\u8e2a\u65f6\u5dee\u5f02\u9700\u6ce8\u610f<\/p>\n
– \u6570\u503c\u79ef\u5206：\u534a\u9690\u5f0f\u6b27\u62c9\u6cd5（\u5148\u66f4\u65b0\u901f\u5ea6\u518d\u66f4\u65b0\u4f4d\u7f6e），\u6bd4\u663e\u5f0f\u6b27\u62c9\u6570\u503c\u7a33\u5b9a\u6027\u66f4\u597d<\/p>\n
\u00a0<\/p>\n
7. \u603b\u7ed3\u4e0e\u5de5\u7a0b\u5e08\u601d\u8003<\/p>\n
\u00a0<\/p>\n
\u5de5\u4e1a\u843d\u5730\u96be\u5904<\/p>\n
\u00a0<\/p>\n
\u4e00\u3001\u6469\u64e6\u529b\u975e\u7ebf\u6027\u3002 \u672c\u7bc7\u7528\u7c98\u6027\u6469\u64e6b\u00b7\u03b8\u0307\u5efa\u6a21，\u5b9e\u9645\u8c10\u6ce2\u51cf\u901f\u5668\u5b58\u5728\u9759\u6469\u64e6（\u7ea62~3 N\u00b7m）\u548cStribeck\u6548\u5e94\u3002\u4f4e\u901f\u65f6\u9759\u6469\u64e6\u5bfc\u81f4"\u5e73\u9876"\u2014\u2014\u4f4d\u7f6e\u5728\u76ee\u6807\u9644\u8fd1\u5c0f\u5e45\u632f\u8361\u65e0\u6cd5\u6536\u655b\u3002\u5de5\u4e1a\u63a7\u5236\u5668\u9700\u52a0\u5165\u6469\u64e6\u8865\u507f\u9879\u6216\u6539\u7528PID+\u79ef\u5206\u5206\u79bb\u3002<\/p>\n
\u00a0<\/p>\n
\u4e8c\u3001\u53c2\u6570\u8fa8\u8bc6\u8bef\u5dee\u3002 J\u548cb\u7684\u503c\u4f9d\u8d56\u8d1f\u8f7d\u548c\u5de5\u4f5c\u59ff\u6001\u3002\u672c\u7bc7\u56fa\u5b9aJ=0.12 kg\u00b7m\u00b2（\u4ec5\u8003\u8651\u5927\u81c2），\u5b9e\u9645\u542b\u672b\u7aef\u6267\u884c\u5668\u548c\u5de5\u4ef6\u65f6J\u53d8\u531630%\u4ee5\u4e0a\u3002\u53c2\u6570\u4e0d\u51c6\u5bfc\u81f4PD\u589e\u76ca\u6b21\u4f18\u2014\u2014Kp\u8fc7\u5927\u5f15\u53d1\u9ad8\u9891\u632f\u8361，Kd\u8fc7\u5927\u5f15\u5165\u566a\u58f0\u654f\u611f\u3002<\/p>\n
\u00a0<\/p>\n
\u4e09\u3001\u5fae\u5206\u9879\u566a\u58f0\u654f\u611f\u3002 \u901f\u5ea6\u53cd\u9988\u6765\u81ea\u7f16\u7801\u5668\u5dee\u5206，\u542b\u91cf\u5316\u566a\u58f0\u3002Kd=3\u65f6\u5fae\u5206\u9879\u5bf9\u566a\u58f0\u6709\u653e\u5927\u4f5c\u7528，\u5de5\u4e1a\u4e0a\u9700\u52a0\u4f4e\u901a\u6ee4\u6ce2\u6216\u6539\u7528\u4e0d\u5b8c\u5168\u5fae\u5206\u3002<\/p>\n
\u00a0<\/p>\n
\u5de5\u7a0b\u5e08\u5fc3\u5f97<\/p>\n
\u00a0<\/p>\n
\u5c0f\u90d1\u770b\u7740<\/p>\n
"01_angle_tracking.png"\u4e0a\u7684\u66f2\u7ebf\u2014\u2014\u7ea2\u8272\u5b9e\u9645\u8f68\u8ff9\u4ece0\u00b0\u5355\u8c03\u722c\u5347\u523045\u00b0（0.785 rad），\u6ca1\u6709\u8d85\u8c03，\u5728\u7ea60.6 s\u8fdb\u5165\u00b12%\u5e26\u3002<\/p>\n
\u00a0<\/p>\n
"Kp=20\u3001Kd=3\u65f6\u963b\u5c3c\u6bd4\u03b6\u22481.23，\u8fc7\u963b\u5c3c，\u5355\u8c03\u6536\u655b\u3002\u8c03\u8282\u65f6\u95f40.6 s，\u7a33\u6001\u8bef\u5dee\u63a5\u8fd1\u96f6\u3002\u5cf0\u503c\u626d\u77e9\u624d15.7 N\u00b7m，\u79bb85 N\u00b7m\u989d\u5b9a\u8fdc\u5f97\u5f88\u3002"<\/p>\n
\u00a0<\/p>\n
\u8001\u5468\u5bf9\u6bd4<\/p>\n
"03_tracking_error.png"："\u7a33\u6001\u8bef\u5dee\u63a5\u8fd10\u00b0，\u548c\u7406\u8bba\u5206\u6790\u4e00\u81f4\u2014\u2014\u8fd9\u4e2a\u5173\u8282\u6c34\u5e73\u65cb\u8f6c，\u6ca1\u6709\u91cd\u529b\u77e9，\u7c98\u6027\u6469\u64e6\u9759\u6b62\u65f6\u4e3a\u96f6\u3002\u5982\u679c\u8981\u52a0\u8d1f\u8f7d\u8ba9\u672b\u7aef\u5782\u76f4\u8fd0\u52a8，PD\u5c31\u4f1a\u6709\u9759\u5dee，\u5230\u65f6\u5019\u52a0\u79ef\u5206\u9879\u3002"<\/p>\n
\u00a0<\/p>\n
"\u4e0b\u4e00\u6b65\u53ef\u4ee5\u8bd5Kp=30\u3001Kd=4，\u03c9n\u63d0\u9ad8\u4f46\u03b6\u8fd8\u80fd\u4fdd\u6301\u8fc7\u963b\u5c3c\u3002\u6216\u8005\u52a0\u524d\u9988\u2014\u2014\u5df2\u77e5\u76ee\u6807\u8f68\u8ff9\u662f\u9636\u8dc3，\u63d0\u524d\u7ed9\u4e2a\u5927\u626d\u77e9\u8109\u51b2\u3002"<\/p>\n
\u00a0<\/p>\n
"\u5148\u628aPD\u7684\u57fa\u7840\u6253\u7262\u3002\u4eff\u771f\u4ee3\u7801\u4e0b\u4e0d\u4e86\u4ea7\u7ebf，\u4f46PD\u7684\u8c03\u53c2\u903b\u8f91\u5bf9\u4e86，\u79fb\u690d\u5230\u4f3a\u670d\u9a71\u52a8\u5668\u91cc\u6539\u6539\u63a5\u53e3\u5c31\u884c\u3002"<\/p>\n
\u00a0<\/p>\n
8. \u5199\u4f5c\u4e0e\u98ce\u683c\u9650\u5236<\/p>\n
\u00a0<\/p>\n
\u672c\u7bc7\u5168\u90e8\u4ee3\u7801\u4e3a\u8bfe\u7a0b\u79bb\u7ebf\u4eff\u771f，\u8fd0\u884c\u5728\u666e\u901aPC\u4e0a，\u7528\u4e8ePD\u4f4d\u7f6e\u63a7\u5236\u5668\u7b97\u6cd5\u9a8c\u8bc1\u3002\u4ee3\u7801\u4e2d\u4e0d\u5305\u542b\u4efb\u4f55\u771f\u5b9e\u673a\u5668\u4eba\u63a7\u5236\u5668\u5b9e\u65f6\u8fd0\u7b97\u6216\u5b89\u5168\u903b\u8f91\u3002 \u5b8c\u5168\u53bb\u8425\u9500，\u4e2d\u7acb\u5ba2\u89c2\u3002\u62d2\u7edd\u8bba\u6587\u5f0f\u751f\u786c\u5f00\u5934，\u4fdd\u6301\u6545\u4e8b\u53d9\u4e8b\u611f\u3002\u5168\u90e8\u7269\u7406\u91cf\u6807\u6ce8\u5b8c\u6574\u5355\u4f4d\u3002<\/p>\n
\u00a0<\/p>\n
9. \u6587\u672b\u9644\u5f55<\/p>\n
\u00a0<\/p>\n
\u73af\u5883\u914d\u7f6e<\/p>\n
\u00a0<\/p>\n
pip install numpy>=1.24.0 matplotlib>=3.7.0<\/p>\n
\u00a0<\/p>\n
"requirements.txt"：<\/p>\n
\u00a0<\/p>\n
numpy>=1.24.0<\/p>\n
matplotlib>=3.7.0<\/p>\n
\u00a0<\/p>\n
\u8fd0\u884c\u8bf4\u660e<\/p>\n
\u00a0<\/p>\n
cd pd_joint_control<\/p>\n
pip install -r requirements.txt<\/p>\n
python -m pytest tests\/ -v<\/p>\n
python run_pd_sim.py<\/p>\n
\u00a0<\/p>\n
\u53c2\u6570\u4fee\u6539\u6307\u5357<\/p>\n
\u00a0<\/p>\n
\u60f3\u6539\u4ec0\u4e48 \u6539\u54ea\u91cc \u793a\u4f8b<\/p>\n
PD\u589e\u76ca\u00a0<\/p>\n
"PDControllerParams(Kp=, Kd=)" Kp 20\u219230, Kd 3\u21924<\/p>\n
\u60ef\u91cf\/\u6469\u64e6\u00a0<\/p>\n
"JointDynamicsParams(J=, b=)" J 0.12\u21920.15<\/p>\n
\u76ee\u6807\u89d2\u5ea6\u00a0<\/p>\n
"step_trajectory(step_value=)" \u03c0\/4\u2192\u03c0\/2<\/p>\n
\u4eff\u771f\u65f6\u957f\u00a0<\/p>\n
"SimulationConfig(t_total=)" 3.0\u21925.0<\/p>\n
\u00a0<\/p>\n
\u8f93\u51fa\u6587\u4ef6\u6e05\u5355<\/p>\n
\u00a0<\/p>\n
\u6587\u4ef6 \u542b\u4e49<\/p>\n
\u00a0<\/p>\n
"results\/01_angle_tracking.png" \u9636\u8dc3\u54cd\u5e94\u5bf9\u6bd4<\/p>\n
\u00a0<\/p>\n
"results\/02_control_torque.png" \u63a7\u5236\u626d\u77e9\u8f93\u51fa<\/p>\n
\u00a0<\/p>\n
"results\/03_tracking_error.png" \u9636\u8dc3\u54cd\u5e94\u8ddf\u968f\u8bef\u5dee<\/p>\n
\u00a0<\/p>\n
"results\/04_phase_portrait.png" \u76f8\u5e73\u9762\u56fe<\/p>\n
\u00a0<\/p>\n
"output\/pd_simulation_data.csv" \u9010\u91c7\u6837\u70b9\u4eff\u771f\u6570\u636e<\/p>\n
\u00a0<\/p>\n
\u4eff\u771f\u7ed3\u679c\u6458\u8981<\/p>\n
\u00a0<\/p>\n
\u6307\u6807 \u6570\u503c \u6570\u636e\u6765\u6e90<\/p>\n
\u6784\u578b \u5355\u5173\u8282\u03b8\u2081 J=0.12 kg\u00b7m\u00b2, b=0.8 N\u00b7m\u00b7s\/rad<\/p>\n
PD\u53c2\u6570 Kp=20, Kd=3 \u8c03\u8bd5\u503c<\/p>\n
\u95ed\u73af\u53c2\u6570 \u03c9n\u224812.9 rad\/s, \u03b6\u22481.23 \u89e3\u6790\u8ba1\u7b97<\/p>\n
\u76ee\u6807\u8f68\u8ff9 \u7eaf\u9636\u8dc3 0\u00b0\u219245\u00b0 step_time=0.5 s<\/p>\n
\u8d85\u8c03\u91cf 0%（\u8fc7\u963b\u5c3c） 01_angle_tracking.png<\/p>\n
\u8c03\u8282\u65f6\u95f4 ~0.60 s (\u00b12%) \u4e3b\u5bfc\u6781\u70b9 s\u2081=-6.67<\/p>\n
RMSE ~0.15\u00b0 \u8bef\u5dee\u7edf\u8ba1<\/p>\n
\u7a33\u6001\u8bef\u5dee ~0\u00b0 \u7406\u8bba：\u7eaf\u7c98\u6027\u6469\u64e6\u65e0\u9759\u5dee<\/p>\n
\u5cf0\u503c\u626d\u77e9 ~15.7 N\u00b7m 02_control_torque.png<\/p>\n
\u6d4b\u8bd5 3\/3 \u6d4b\u8bd5\u901a\u8fc7 pytest\u8f93\u51fa<\/p>\n
\u00a0<\/p>\n
\u6b63\u6587\u5b8c\u3002\u4ee5\u4e0b\u4e3a\u00a0<\/p>\n
"run_pd_sim.py" \u5b8c\u6574\u4e3b\u5165\u53e3 +\u00a0<\/p>\n
"tests\/test_pd_controller.py" \u5b8c\u6574\u6d4b\u8bd5\u6587\u4ef6\u3002<\/p>\n
\u00a0<\/p>\n
"run_pd_sim.py"<\/p>\n
\u00a0<\/p>\n
"""<\/p>\n
PD\u4f4d\u7f6e\u63a7\u5236\u5668\u5355\u5173\u8282\u95ed\u73af\u4eff\u771f<\/p>\n
\u8fd0\u884c：python run_pd_sim.py<\/p>\n
"""<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
import numpy as np<\/p>\n
import os<\/p>\n
import csv<\/p>\n
import math<\/p>\n
from robot.models import (<\/p>\n
\u00a0 \u00a0 JointDynamicsParams, PDControllerParams, SimulationConfig)<\/p>\n
from robot.dynamics import SingleJointDynamics<\/p>\n
from robot.pd_controller import PDController<\/p>\n
from robot.trajectory import ReferenceTrajectory<\/p>\n
from robot.visualize import PDVisualizer<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
def main():<\/p>\n
\u00a0 \u00a0 print("=" * 60)<\/p>\n
\u00a0 \u00a0 print("\u88c5\u914d\u673a\u68b0\u81c2\u4eff\u771f：PD\u4f4d\u7f6e\u63a7\u5236\u5668\u5355\u5173\u8282\u89d2\u5ea6\u95ed\u73af\u8ddf\u968f")<\/p>\n
\u00a0 \u00a0 print("=" * 60)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("\\\\n— \u52a8\u529b\u5b66\u53c2\u6570 —")<\/p>\n
\u00a0 \u00a0 dyn_params = JointDynamicsParams(J=0.12, b=0.8, tau_max=85.0)<\/p>\n
\u00a0 \u00a0 print(f" \u7b49\u6548\u8f6c\u52a8\u60ef\u91cf J: {dyn_params.J} kg\u00b7m\u00b2")<\/p>\n
\u00a0 \u00a0 print(f" \u7c98\u6027\u6469\u64e6\u7cfb\u6570 b: {dyn_params.b} N\u00b7m\u00b7s\/rad")<\/p>\n
\u00a0 \u00a0 print(f" \u6700\u5927\u626d\u77e9 tau_max: {dyn_params.tau_max} N\u00b7m")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("\\\\n— PD\u63a7\u5236\u5668\u53c2\u6570 —")<\/p>\n
\u00a0 \u00a0 pd_params = PDControllerParams(Kp=20.0, Kd=3.0)<\/p>\n
\u00a0 \u00a0 print(f" \u6bd4\u4f8b\u589e\u76ca Kp: {pd_params.Kp} N\u00b7m\/rad")<\/p>\n
\u00a0 \u00a0 print(f" \u5fae\u5206\u589e\u76ca Kd: {pd_params.Kd} N\u00b7m\u00b7s\/rad")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 # \u89e3\u6790\u95ed\u73af\u53c2\u6570<\/p>\n
\u00a0 \u00a0 wn = math.sqrt(pd_params.Kp \/ dyn_params.J)<\/p>\n
\u00a0 \u00a0 zeta = (dyn_params.b + pd_params.Kd) \/ (<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 2 * math.sqrt(pd_params.Kp * dyn_params.J))<\/p>\n
\u00a0 \u00a0 print(f"\\\\n— \u95ed\u73af\u53c2\u6570（\u89e3\u6790）—")<\/p>\n
\u00a0 \u00a0 print(f" \u65e0\u963b\u5c3c\u56fa\u6709\u9891\u7387 \u03c9n: {wn:.2f} rad\/s")<\/p>\n
\u00a0 \u00a0 print(f" \u963b\u5c3c\u6bd4 \u03b6: {zeta:.3f} ({'\u8fc7\u963b\u5c3c' if zeta > 1 else '\u6b20\u963b\u5c3c'})")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("\\\\n— \u4eff\u771f\u914d\u7f6e —")<\/p>\n
\u00a0 \u00a0 sim_config = SimulationConfig(dt=0.001, t_total=3.0)<\/p>\n
\u00a0 \u00a0 print(f" \u79ef\u5206\u6b65\u957f dt: {sim_config.dt} s")<\/p>\n
\u00a0 \u00a0 print(f" \u603b\u65f6\u957f: {sim_config.t_total} s")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("\\\\n— \u521d\u59cb\u5316\u7cfb\u7edf —")<\/p>\n
\u00a0 \u00a0 dynamics = SingleJointDynamics(dyn_params)<\/p>\n
\u00a0 \u00a0 controller = PDController(pd_params)<\/p>\n
\u00a0 \u00a0 dynamics.reset(theta_0=0.0, theta_dot_0=0.0)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("\\\\n— \u5f00\u59cb\u4eff\u771f（\u9636\u8dc3\u54cd\u5e94）—")<\/p>\n
\u00a0 \u00a0 t_arr = np.zeros(sim_config.n_steps)<\/p>\n
\u00a0 \u00a0 theta_ref_arr = np.zeros(sim_config.n_steps)<\/p>\n
\u00a0 \u00a0 theta_actual_arr = np.zeros(sim_config.n_steps)<\/p>\n
\u00a0 \u00a0 theta_dot_arr = np.zeros(sim_config.n_steps)<\/p>\n
\u00a0 \u00a0 tau_arr = np.zeros(sim_config.n_steps)<\/p>\n
\u00a0 \u00a0 error_arr = np.zeros(sim_config.n_steps)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 for k in range(sim_config.n_steps):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 t = k * sim_config.dt<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 t_arr[k] = t<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 # \u7eaf\u9636\u8dc3\u8f68\u8ff9<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 theta_ref = ReferenceTrajectory.step_trajectory(<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 t, step_time=0.5, step_value=math.pi\/4)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 theta_ref_arr[k] = theta_ref<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 theta = dynamics.theta<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 theta_dot = dynamics.theta_dot<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau, error = controller.compute(theta_ref, theta, theta_dot)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 tau_arr[k] = tau<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 error_arr[k] = error<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dynamics.step_euler(tau, sim_config.dt)<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 theta_actual_arr[k] = dynamics.theta<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 theta_dot_arr[k] = dynamics.theta_dot<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("\\\\n— \u4eff\u771f\u5b8c\u6210，\u7edf\u8ba1\u7ed3\u679c —")<\/p>\n
\u00a0 \u00a0 step_value = math.pi \/ 4<\/p>\n
\u00a0 \u00a0 steady_start = int(2.5 \/ sim_config.dt)<\/p>\n
\u00a0 \u00a0 steady_error = np.mean(<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 np.abs(error_arr[steady_start:])) * 180 \/ np.pi<\/p>\n
\u00a0 \u00a0 max_error = np.max(np.abs(error_arr)) * 180 \/ np.pi<\/p>\n
\u00a0 \u00a0 rmse = np.sqrt(np.mean(error_arr**2)) * 180 \/ np.pi<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 # \u8c03\u8282\u65f6\u95f4（\u00b12%\u5e26）<\/p>\n
\u00a0 \u00a0 tolerance = 0.02 * step_value<\/p>\n
\u00a0 \u00a0 settling_time = sim_config.t_total<\/p>\n
\u00a0 \u00a0 for i in range(steady_start, sim_config.n_steps):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 if abs(error_arr[i]) > tolerance:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 settling_time = i * sim_config.dt<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 break<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 # \u8d85\u8c03\u91cf<\/p>\n
\u00a0 \u00a0 peak_theta = np.max(theta_actual_arr)<\/p>\n
\u00a0 \u00a0 if peak_theta > step_value:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 overshoot = (peak_theta – step_value) \/ step_value * 100<\/p>\n
\u00a0 \u00a0 else:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 overshoot = 0.0<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print(f" \u8d85\u8c03\u91cf: {overshoot:.1f}%")<\/p>\n
\u00a0 \u00a0 print(f" \u8c03\u8282\u65f6\u95f4 (\u00b12%): {settling_time:.3f} s")<\/p>\n
\u00a0 \u00a0 print(f" \u6700\u5927\u8ddf\u968f\u8bef\u5dee: {max_error:.3f}\u00b0")<\/p>\n
\u00a0 \u00a0 print(f" RMSE: {rmse:.3f}\u00b0")<\/p>\n
\u00a0 \u00a0 print(f" \u7a33\u6001\u8bef\u5dee（\u6700\u540e0.5s\u5747\u503c）: {steady_error:.4f}\u00b0")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 # \u5cf0\u503c\u626d\u77e9<\/p>\n
\u00a0 \u00a0 peak_tau = np.max(np.abs(tau_arr))<\/p>\n
\u00a0 \u00a0 print(f" \u5cf0\u503c\u626d\u77e9: {peak_tau:.1f} N\u00b7m")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("\\\\n— \u751f\u6210\u53ef\u89c6\u5316\u56fe\u8868 —")<\/p>\n
\u00a0 \u00a0 viz = PDVisualizer(results_dir="results")<\/p>\n
\u00a0 \u00a0 viz.plot_angle_tracking(t_arr, theta_ref_arr, theta_actual_arr)<\/p>\n
\u00a0 \u00a0 viz.plot_control_torque(t_arr, tau_arr)<\/p>\n
\u00a0 \u00a0 viz.plot_tracking_error(t_arr, error_arr)<\/p>\n
\u00a0 \u00a0 viz.plot_phase_portrait(theta_actual_arr, theta_dot_arr)<\/p>\n
\u00a0 \u00a0 print(" \u5b8c\u6210! \u56fe\u8868\u5df2\u4fdd\u5b58\u81f3 results\/ \u76ee\u5f55")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 # \u4fdd\u5b58CSV<\/p>\n
\u00a0 \u00a0 os.makedirs("output", exist_ok=True)<\/p>\n
\u00a0 \u00a0 csv_path = "output\/pd_simulation_data.csv"<\/p>\n
\u00a0 \u00a0 with open(csv_path, 'w', newline='') as f:<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 writer = csv.writer(f)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 writer.writerow(['t(s)', 'theta_ref(rad)', 'theta_actual(rad)',<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0'theta_dot(rad\/s)', 'tau(N\u00b7m)', 'error(rad)',<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0'error(deg)'])<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 for i in range(sim_config.n_steps):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 writer.writerow([<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 f"{t_arr[i]:.4f}",<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 f"{theta_ref_arr[i]:.6f}",<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 f"{theta_actual_arr[i]:.6f}",<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 f"{theta_dot_arr[i]:.6f}",<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 f"{tau_arr[i]:.4f}",<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 f"{error_arr[i]:.6f}",<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 f"{error_arr[i]*180\/np.pi:.4f}",<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 ])<\/p>\n
\u00a0 \u00a0 print(f" \u6570\u636e\u5df2\u4fdd\u5b58: {csv_path}")<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 print("=" * 60)<\/p>\n
\u00a0 \u00a0 print("\u26a0\ufe0f \u672c\u4ee3\u7801\u4e3a\u8bfe\u7a0b\u79bb\u7ebf\u4eff\u771f，\u4e0d\u53ef\u76f4\u63a5\u7528\u4e8e\u4ea7\u7ebf\u8bbe\u5907")<\/p>\n
\u00a0 \u00a0 print("=" * 60)<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
if __name__ == "__main__":<\/p>\n
\u00a0 \u00a0 main()<\/p>\n
\u00a0<\/p>\n
"tests\/test_pd_controller.py"<\/p>\n
\u00a0<\/p>\n
"""<\/p>\n
PD\u4f4d\u7f6e\u63a7\u5236\u5668\u5355\u5143\u6d4b\u8bd5<\/p>\n
\u8fd0\u884c：python -m pytest tests\/test_pd_controller.py -v<\/p>\n
"""<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
import numpy as np<\/p>\n
import math<\/p>\n
import sys<\/p>\n
import os<\/p>\n
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))<\/p>\n
\u00a0<\/p>\n
from robot.models import (<\/p>\n
\u00a0 \u00a0 JointDynamicsParams, PDControllerParams, SimulationConfig)<\/p>\n
from robot.dynamics import SingleJointDynamics<\/p>\n
from robot.pd_controller import PDController<\/p>\n
from robot.trajectory import ReferenceTrajectory<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
class TestInputValidation:<\/p>\n
\u00a0 \u00a0 """\u6d4b\u8bd51：\u6570\u636e\u6a21\u578b\u5165\u53c2\u5408\u6cd5\u6027"""<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def test_dynamics_params_positive(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 params = JointDynamicsParams(J=0.12, b=0.8, tau_max=85.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert params.J > 0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert params.b >= 0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert params.tau_max > 0<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def test_pd_params_positive(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 params = PDControllerParams(Kp=20.0, Kd=3.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert params.Kp > 0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert params.Kd > 0<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def test_simulation_config_valid(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 config = SimulationConfig(dt=0.001, t_total=3.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert config.dt > 0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert config.t_total > 0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert config.n_steps > 0<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def test_controller_init(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 params = PDControllerParams()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ctrl = PDController(params)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert ctrl.params is params<\/p>\n
\u00a0<\/p>\n
\u00a0<\/p>\n
class TestPhysicalConsistency:<\/p>\n
\u00a0 \u00a0 """\u6d4b\u8bd52：\u7269\u7406\u6a21\u578b\u81ea\u6d3d\u6821\u9a8c"""<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def test_torque_saturation(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u626d\u77e9\u9971\u548c\u9650\u5236\u5e94\u751f\u6548"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 params = JointDynamicsParams(J=0.12, b=0.8, tau_max=10.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dyn = SingleJointDynamics(params)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dyn.reset()<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 alpha = dyn.compute_acceleration(1000.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 expected_max_alpha = params.tau_max \/ params.J<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert abs(alpha) <= expected_max_alpha + 1e-6<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def test_zero_torque_zero_acceleration_at_rest(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u9759\u6b62\u65e0\u626d\u77e9\u65f6\u52a0\u901f\u5ea6\u5e94\u4e3a\u96f6"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 params = JointDynamicsParams(J=0.12, b=0.8)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dyn = SingleJointDynamics(params)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dyn.reset(theta_0=0.0, theta_dot_0=0.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 alpha = dyn.compute_acceleration(0.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 assert abs(alpha) < 1e-10<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 def test_euler_integration_stability(self):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 """\u6b27\u62c9\u79ef\u5206\u4e0d\u5e94\u53d1\u6563（\u7ed9\u5b9a\u7a33\u5b9aPD\u53c2\u6570）"""<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 params = JointDynamicsParams(J=0.12, b=0.8)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 pd_params = PDControllerParams(Kp=20.0, Kd=3.0)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dyn = SingleJointDynamics(params)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 ctrl = PDController(pd_params)<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dyn.reset()<\/p>\n
\u00a0<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 dt = 0.001<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 n_steps = 500<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 positions = []<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 for _ in range(n_steps):<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 theta = dyn.theta<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0 \u00a0 \u00a0 theta_dot = dyn.theta_dot<\/p>\n
\u5229\u7528AI\u89e3\u51b3\u5b9e\u9645\u95ee\u9898，\u5982\u679c\u4f60\u89c9\u5f97\u8fd9\u4e2a\u5de5\u5177\u597d\u7528，\u6b22\u8fce\u5173\u6ce8\u957f\u5b89\u7267\u7b1b！<\/p>\n
\u00a0 \u00a0 \u00a0 \u00a0\u00a0<\/p>\n","protected":false},"excerpt":{"rendered":"
\u300aPython\u673a\u5668\u4eba\u57fa\u7840\u539f\u7406\u300b\u7b2c\u516b\u5341\u4e09\u7bc7：\u88c5\u914d\u673a\u68b0\u81c2\u4eff\u771f\u2014\u2014PD\u4f4d\u7f6e\u63a7\u5236\u5668\u5355\u5173\u8282\u89d2\u5ea6\u95ed\u73af\u8ddf\u968f1. \u5b9e\u9645\u5de5\u4e1a\u5e94\u7528\u573a\u666f\u5468\u4e00\u65e9\u6668，\u6c7d\u8f66\u53d1\u52a8\u673a\u7f38\u4f53\u88c5\u914d\u7ebf\u5f00\u73ed\u9996\u68c0\u30024-DOF SCARA\u673a\u68b0\u81c2（\u57fa\u5ea7\u65cb\u8f6c\u5173\u8282\u03b8\u2081\u989d\u5b9a\u626d\u77e985 N\u00b7m\u3001\u7b49\u6548\u8f6c\u52a8\u60ef\u91cfJ0.12 kg\u00b7m\u00b2\u3001\u7c98\u6027\u6469\u64e6\u7cfb\u6570b0.8 N\u00b7m\u00b7s\/rad，\u5927\u81c2\u957f0.45 m\/3.5 kg，\u5c0f\u81c2\u957f0.35 m\/2.0 kg，\u5782\u76f4\u5e73\u79fbd\u2084\u884c\u7a0b0~0.30<\/p>\n","protected":false},"author":2,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[1],"tags":[2468,1818,4278,81,384],"topic":[],"class_list":["post-116601","post","type-post","status-publish","format-standard","hentry","category-server","tag-matplotlib","tag-numpy","tag-pandas","tag-python","tag-384"],"yoast_head":"\n