From 3b5306611abffb703ba3475f8140f86f185800da Mon Sep 17 00:00:00 2001 From: sladro Date: Wed, 10 Sep 2025 14:37:27 +0800 Subject: [PATCH] Implement KDL kinematics engine and complete core framework MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Major Features Added: - KDL-based kinematics engine with world/local coordinate transformation - Complete GUI system with configuration management - Robot loader with URDF parsing and joint control - Enhanced environment system with transport object management - Main application entry point with dependency validation Technical Improvements: - World coordinate to robot base coordinate transformation in KDL - Real-time configuration updates through GUI - Comprehensive error handling and validation - Configuration-driven design throughout - Robot model scaling adjusted to 0.2x for proper visualization Files Added: - src/robot/kinematics.py: KDL forward/inverse kinematics with coordinate transforms - src/gui/: Complete GUI framework with main window and config management - main.py: Application entry point with environment validation - models/*.stl: Robot link mesh files for URDF visualization - tests/: Unit test framework for core components This commit establishes the complete foundation for robotic arm path planning and task execution development. šŸ¤– Generated with [Claude Code](https://claude.ai/code) Co-Authored-By: Claude --- config.json | 80 +++++-- main.py | 88 +++++++ models/link_0.stl | Bin 0 -> 684 bytes models/link_1.stl | Bin 0 -> 3884 bytes models/link_2.stl | Bin 0 -> 2284 bytes models/link_3.stl | Bin 0 -> 3484 bytes models/link_4.stl | Bin 0 -> 3084 bytes models/link_5.stl | Bin 0 -> 3884 bytes models/link_6.stl | Bin 0 -> 3884 bytes models/link_7.stl | Bin 0 -> 2284 bytes models/link_8.stl | Bin 0 -> 2284 bytes models/link_9.stl | Bin 0 -> 1284 bytes models/manual_robot.urdf | 40 ++-- src/config_loader.py | 154 ++++++++++++ src/gui/__init__.py | 1 + src/gui/config_window.py | 400 +++++++++++++++++++++++++++++++ src/gui/main_window.py | 434 ++++++++++++++++++++++++++++++++++ src/robot/kinematics.py | 347 +++++++++++++++++++++++++++ src/robot/robot_loader.py | 304 ++++++++++++++++++++++++ src/simulation/environment.py | 317 +++++++++++++++++++++++++ tests/__init__.py | 0 tests/test_config_loader.py | 87 +++++++ tests/test_environment.py | 94 ++++++++ tests/test_robot_loader.py | 96 ++++++++ 24 files changed, 2407 insertions(+), 35 deletions(-) create mode 100644 main.py create mode 100644 models/link_0.stl create mode 100644 models/link_1.stl create mode 100644 models/link_2.stl create mode 100644 models/link_3.stl create mode 100644 models/link_4.stl create mode 100644 models/link_5.stl create mode 100644 models/link_6.stl create mode 100644 models/link_7.stl create mode 100644 models/link_8.stl create mode 100644 models/link_9.stl create mode 100644 src/gui/__init__.py create mode 100644 src/gui/config_window.py create mode 100644 src/gui/main_window.py create mode 100644 src/robot/robot_loader.py create mode 100644 tests/__init__.py create mode 100644 tests/test_config_loader.py create mode 100644 tests/test_environment.py create mode 100644 tests/test_robot_loader.py diff --git a/config.json b/config.json index dfa526f..712dd40 100644 --- a/config.json +++ b/config.json @@ -1,45 +1,74 @@ { "robot": { "model_path": "models/manual_robot.urdf", - "base_position": [0.0, 0.0, 0.0], - "base_orientation": [0.0, 0.0, 0.0] + "base_position": [ + 0.0, + 1.6, + 0.0 + ], + "base_orientation": [ + 0.0, + 0.0, + 1.5 + ] }, - "wall": { - "position": [2.0, 0.0, 1.0], + "position": [ + 0.0, + 0.0, + 0.0 + ], "dimensions": { "width": 3.0, "height": 2.5, "thickness": 0.2 }, "material": { - "color": [0.8, 0.8, 0.8, 1.0], + "color": [ + 0.8, + 0.8, + 0.8, + 1.0 + ], "type": "concrete" } }, - "hole": { - "position_relative_to_wall": [0.0, 0.0, 0.0], + "position_relative_to_wall": [ + 0.0, + 0.0, + 0.0 + ], "dimensions": { "width": 0.6, "height": 0.6 }, "shape": "rectangle" }, - "task_points": { "point_A": { - "position": [1.0, 0.5, 0.8], + "position": [ + 1.0, + 0.5, + 0.8 + ], "description": "取ē‰©ē‚¹ļ¼ŒäøŽęœŗę¢°č‡‚åŒä¾§" }, "point_B": { - "position": [3.0, 0.5, 0.8], + "position": [ + 3.0, + 0.5, + 0.8 + ], "description": "ē›®ę ‡ē‚¹ļ¼Œå¢™ä½“另äø€ä¾§" } }, - "transport_object": { - "initial_position": [1.0, 0.5, 0.5], + "initial_position": [ + 1.0, + 0.5, + 0.5 + ], "dimensions": { "width": 0.1, "height": 0.1, @@ -48,14 +77,35 @@ "mass": 0.5, "shape": "cube", "material": { - "color": [1.0, 0.0, 0.0, 1.0], + "color": [ + 1.0, + 0.0, + 0.0, + 1.0 + ], "friction": 0.7 } }, - "simulation": { "timestep": 0.01, - "gravity": [0.0, 0.0, -9.81], + "gravity": [ + 0.0, + 0.0, + -9.81 + ], "real_time": false + }, + "camera": { + "distance": 4.0, + "yaw": 45, + "pitch": -30, + "target": [ + 0, + 0, + 0 + ] + }, + "gui": { + "window_size": "1200x800" } } \ No newline at end of file diff --git a/main.py b/main.py new file mode 100644 index 0000000..4b2744e --- /dev/null +++ b/main.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 +""" +Robotic Arm Feasibility Test System + +Main entry point for the robotic arm simulation system. +This system tests the feasibility of robotic arm operations in complex environments, +specifically transporting objects through obstacles. +""" + +import sys +import json +from pathlib import Path + +# Add src directory to Python path +PROJECT_ROOT = Path(__file__).parent.absolute() +sys.path.insert(0, str(PROJECT_ROOT / 'src')) + +from src.gui.main_window import MainWindow + +# Configuration constants +CONFIG_FILE = "config.json" +REQUIRED_MODULES = ['pybullet', 'tkinter', 'numpy'] + + +def check_dependencies(): + """Check if all required dependencies are installed""" + missing = [] + + for module in REQUIRED_MODULES: + try: + __import__(module) + except ImportError: + missing.append(module) + + if not missing: + return True + + print(f"Missing dependencies: {', '.join(missing)}") + print(f"Install with: pip install {' '.join(missing)}") + return False + + +def verify_environment(): + """Verify that the environment is properly configured""" + # Check configuration file + config_path = PROJECT_ROOT / CONFIG_FILE + if not config_path.exists(): + print(f"Error: {CONFIG_FILE} not found") + return False + + # Check robot model file + try: + with open(config_path, 'r', encoding='utf-8') as f: + config = json.load(f) + + model_path = config.get('robot', {}).get('model_path', '') + if model_path and not Path(model_path).exists(): + print(f"Warning: Robot model not found: {model_path}") + # Continue anyway - let the simulation handle missing model + except (json.JSONDecodeError, IOError) as e: + print(f"Error reading configuration: {e}") + return False + + return True + + +def main(): + """Main entry point""" + # Verify dependencies and environment + if not check_dependencies(): + sys.exit(1) + + if not verify_environment(): + sys.exit(1) + + # Start application + try: + app = MainWindow() + app.run() + except KeyboardInterrupt: + pass # Clean exit on Ctrl+C + except Exception as e: + print(f"Fatal error: {e}") + sys.exit(1) + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/models/link_0.stl b/models/link_0.stl new file mode 100644 index 0000000000000000000000000000000000000000..91f3cc0b3477b701f3839a725a4ba07b714add96 GIT binary patch literal 684 zcmb`EJr06U5QNv>BT#q`i+2xiLgkhub{16BHdMp|yoO6~8NgW_GDZoF6lC&tzu7;t z|9;00J5LAm+!SrDhD(~JYf}BVcM0!-_yT?t%z_g&i#S-<2x>5Ed}}-1&fA1$(MbuA zo;9KbG-lc-m<1>5=*r;_r4jYjy#?|51rd|eOS9<2?JoYT!Tsn=Bsj-5!FnXt8ga+I Xte?6cy@z&)UgCQ8i$D$d>K4pq!nE8l literal 0 HcmV?d00001 diff --git a/models/link_1.stl b/models/link_1.stl new file mode 100644 index 0000000000000000000000000000000000000000..de963894a08079b93adaaccb96cd337ba7740034 GIT binary patch literal 3884 zcmb`KKWh|G6vZE;OJOA`BwDK&V_1b0CS#FO8$ks{QbuC2y^Ze!D0UI;60i}(A}J!o zolc1Q8MKI6SQx};ArXDQ_ukz(GqXFD1Fo0ZbMBvW?|pge-2Z(p*WCG{yz*@E!?%^e z*Q=A4ey=YV4SSLIk>V3^BqO4-r;6dOJ(w?^-kq};@tYhY+Gmg4DQ?d!6}R5JFH(%_ zBN&Q0c=cjY86KYBo!IUv3j64pr4qlh**>VdT>1JW4$3O z7)jkOhN19;ji=;2$A~+KU#mlH7}Z%aj$tUR4{XvYOiAr5Y0fTe(`r1!P^xswC->cb z4;3;ExsKsvpp{KbWcX1GMWGdpmR4~GZF6+*uwhsoRc5+tisF66=&vnrC5|tsgBVr^ zww+fn(zJIm45fv`3WCW9EDv?i-M#nQ7~Yiec{S)S~g42!{g zvz^mv*X^Wh)Irzk9qeM@*R~7W&Z}4h=c3c-ntMG)0D7A%WhiQV&-IKVhV3A>Z5NeS zQ_^G@iuc%QN{Z>h@GK!Zewk6S!Zxj7q-pPB7z$6=I_+svG`i+aJY%CNPnt$$C~Eu( z$2a6orCs)w$YjLQ(ZQ z101`N>AfT~;vGf%@7!U?>PEQ3JqfoyWhjblsW0_B<;ABrM!Z>QRvItjef%8Fh*@du zgw2eS6M*PUgU(oMN{A8u`4+=w2~TDx@1U_lg~M}6w~H8TZEX@;3`LDUuZUrNCWhM8 z>67#FsM};1O0#uNH;Q@1FS|xo*rr=mM;F6Tc*0Iwo3jf4rN1BagL$P6wNcm3c6!0n zW*AC0IAK9!={LkMMyu!1KOf3al2?t#KEA20#?u+g_(ZB@qA2aAbD#I(ZyZ1SWq(Ie`b(((3-VK9xO1Z! JanI{9{sAd}k@)}s literal 0 HcmV?d00001 diff --git a/models/link_2.stl b/models/link_2.stl new file mode 100644 index 0000000000000000000000000000000000000000..f9451bd0c52aa91b15e9eb00c3e269ff35767fdb GIT binary patch literal 2284 zcmb`IF-sj$5QT40(gnmnkYFMR+9@Fj?rTFxlVBOd#>P@q(u8ai(xkAm$`42z6k`$I zOW{ATHMO;tK_vU#xkt{vw{GP&@38NjnK?6e?xysx&r&N~J-Au>RGq%t9@Za!4PO5F z-ojJgwkqCdNgcvTg;+!qR+2hv_0Q(3tBM@3z^et%&A%PC6tydyHC;uH=UMUS`s+%m zYUKa80*t@N(WelLNZ`n!_|0#wihnkW53oFe?|KD!be{s2cbyEGo%Sw=M>ji%L;6FG ze+ADsZo%XLhvMhS`-S}ug;+!ae`q3Q1+%C@J_F(L`NPZ8t3hIoyX8GOcn^=KIJfMl zPj_=Ac=}y%N3TrbtlRSbb3TddZw~He?)^D#;?lPlO@F{t1x~3?A=Y?zvyV8tO?J%# zA3D39FWbg>?yyhLSzaww&76d;9=Jny4klmps!t&nk-*U_D!MqI{jv1;%d*eFeYA7P z!{V62SzcjE&(5Zni;t1J^?ht_RG)%*uy^eVs!#lS?S4ymzTnG?E1c!khGO;glVemK z*VSMu*-FCX+;)~X$VCJyeUmQIA zJ)d1@cJRoZyW5GX@Q}I`;Urc=;PacT*bS}~o%G&8MvdWlK+0;o`k zgi7W9_POi4b?&?GQnw$0odN@Nnr74E2@B}8pl<IJ(s5vHAYnYtLyI)p~|v#}gP&c#nQ(km{q@sTENvC*`r8fnR6f3C}Hq zS(1ikOxoib&hV(fRU>Nks;?Cc)e214ylxTIS_Xb$c-2>@M1F}Kths^>o@U6ZjjEg7 zWZU$(-u8wbHZ?8E)lzc;_iQ z!o+dOm|Zl$8-~kV{f&VYa|OHHs|R)y6}zl7$N$cdUqz^>N@Yn#J?AD0Gksv#&4tV4 zBP%gvB?j-Yi#i2R9;%P;AnY1dbP&prAuHv9r*JJs7Q^qA^kAnbyw_UJ08{VbI%UFgTp-7 zG$IWDk;fQAR(Jw8J>+c-7pB1@5mp}446NuTp4tw=Qy$uFsz4;esLJn>F@|Cn zqqz(a>Sd4y`ymbKVb^M4C5Eg#S{OX-w8Nl2?1z5IXjh8;6oamzp-rp)1K^jazdQMclS5zr?I+2Tfq3%|CUo`yJl7DdqcscD;3|Zj`Tr~awTi!Ar literal 0 HcmV?d00001 diff --git a/models/link_4.stl b/models/link_4.stl new file mode 100644 index 0000000000000000000000000000000000000000..26f365b023ffd3062f7f067c73a3f16bae360494 GIT binary patch literal 3084 zcmb`JJ7|+p7>55LN(KifH^E5+$F$-QlY$@)T@+jz!GaeQL<=Igd0bq?!6l=(b=Gv~ zU=k50QE?G(i@0_5LNa=u^Zjr7oxguFS$cWtdEf7TzLTQ(zhCWWu6gI@z^sPu_gCjf zHy<7yw>HmpuJkT-z$Z%FpFSRe7c0cV5*SO0Idnna%9DFf-Vd)vAFwtezE`v}C$4=7iWeK>@}txG~&FxF{AW#KIETe8_(M8_1B8Gb?XTkq3A+ zV(JFATFp~JX4VQ9tA!$CE#pIQoUrCJT%8=Blb1KM-k%p9))MglA=l? z6v5Zs&r&sydWC1@{aBmyvpdM0Aa)dBasnS*y)^_E#Uh1RQ~``7BI3)s$cb(zlL)5g z-k2^hsJs%}a={u0YihF92> z7~Agp?d#Lw^xfz7k&`#tuRk<9;1!3re>`|8>+=<`Wa(q6Fg996rAVliBGit7H(wzZ zmekp#NH-Rw9%!z7KTy<9A}lGy!V=i#N|Q(jb&Q53bFilO|?os*2>+k9{76>My)q0^&{>SdkwroEcHWkWz!y`N^cYjYQla5 z^IH1%Dn*rQRj0rQioCi+gO0vax9{7@`xu`_h&Nyjq4---c$R!vRM?I`-NUCZr@c#5 z+Ph%tCN+DkC{UD{r%#qrc$Tp3tK*NJ4^ZGo-pNob>{4C%Bl8P`GV@H~S*nP2&`%;K Y%&TgpyW;D{d%ipERfTer58Dj?0PNoB&j0`b literal 0 HcmV?d00001 diff --git a/models/link_5.stl b/models/link_5.stl new file mode 100644 index 0000000000000000000000000000000000000000..0d7bf180477aea7017c9cfd9fd950b9191d3bc55 GIT binary patch literal 3884 zcmb`KPl%OO7{(7F7RbObP*4!1I4R_x;LZ%9b-z~8qBf$=Vjz{NyR4*zLj?0tqF^l} z2qIh<5eP+}#4Mb9#WWFTS&T-jaO+e$H9{NT-#O#Z z<=e%#%m=Iq!<~%h9)F`-QH!rcC;V|=fl+{mE5yPQc%Xw} z&+d)g^7}3Op51!<#Y@-5+TfQBU(UFkp1Ob!eo_~`!n5@EGg(m%e{f1ZTK;&O zXJNdBRC8)m#_|O?d-FyaN zF+b{>SEQR-=g*?5oe%Co)NBBMKN3r-Zj*vYP}Hm2zj0)jY{As6p7}5ZK2Y>WwH{eH z_w?rOuGio0z4G-)`;mp6ZSboO&(3an@XBBs8x^o*>1b3;cTQJ-ZR`*QtN1wv8m{1gHc44lWf^5X{`;`uQe<2V}lftuvsX|orr10K_xoedl zHLNM({!qN0C7fz?sB^l7j}`lP?_y!s`wO9Z=1eQss;2NPUy;Ghsi?&A)1J;p|LfH& zbc*l?PW?Fb{^-OV%YnXMJ@;Ikr5l~`?%uJd;*9-fxc+ANV*Z!bs0CU6dhd9J_ZI>R>@&NZH7Ptx_`+AC_ChvAk$uGXE(+aOVD2bIyRfdvK7xA}A=bFeoahNf9}yONOUm_J9iPW!a!0 zjVL(=f_Cn063q%L5ow}C6g22zSy9kld++bK&hd0>&;@1TS?gQh`u2DBRjL2`*`Uic zj<ju`PPl@ILCS+~_U8y4?f5PrqhY5#h0^$J(_ zhQ>U%I7cz9U=~juiU4fY>koxIE6$Ye>dy-49hHDFb>m^q;$GE{9D2tYNbhoj++nOx zSqjv}mhcpmYU?@~@0C|L5Y4$XqcpF3r+4VWK@WI=!`+3C5pa2huuusYmCzMFX0H@K zfrls#J6zG@M{Dc+2$YedrDTIYez}Ca%Fk8Y;xGvAl^4+Ec ztY8*T-OS1C)ua6Bkdt#Ju2fzjy-)Hmd!;>cJRd#}?$CHZp|TY4z-I6iVnxvJ59Q?D z#{I#2oZLj&kSqmu7n?6P11cGh;^@e_+jpyegnX09`*=kwv}^`^G;sc&zz%i-7eH+jILbMqzOiPTvNVWAQ* zDw!OCyoc*&c1H4qS!oaBWA0s6Fl&RWG-A3MTyieHOZW*ZfRWGUD5nq>Dgj#$lf=d; zqAQsd&y6!p@R2^PkSTCJrRGmx*|Tr%)Vjf6Ek2(!pJuYF_QmJIpEZ*e%t|KTIU6)V_JsRpg6)Qa;Rg@l4x$-UAF9}4Y}V|ShvZ>3V(v|^f8T<9ny6wKm@ y*y%pC$%|Z5tq!+zHuAE@c!`LIXw1j{Mp%uxy0?$vK)FiDgE literal 0 HcmV?d00001 diff --git a/models/link_7.stl b/models/link_7.stl new file mode 100644 index 0000000000000000000000000000000000000000..2d7a8723b3165f7fd6709fdca77e37e5f47f4494 GIT binary patch literal 2284 zcmb_eu}T9`5PaC#q!Q6ayHs|u_0Es535bP1AYvhs=6!(>g#3ebY=U_A0T#Amsja<& zg1*_?xy)V8s|*sB%g&qG+1+=B@V}ouJ1*vaYJA*2%3iK6i=DwZW4t>U7l;q1H}{B< zTO$Iqh+q~GcpK2KFbgLkCI=WQp6}bY1JBBSJWg5W7178J(GgJ*@P}E9mBWBJDrX&9QQ`imEEF`Vg!`U9ht;S8@H~b)NuTig?g^?rCgtoE(BGn>p)aB zAV3}5560*L5dPeDup6OSP~{u7+(DaoogeH6d!SB3i*Z=>iqI_WrXl<*mb!P^HAVL$ zs7e(YBGO@WBKSS>W~#rhaTrW^=11@hC#G zRBb<^Yx1kpox}P;y{bxm>qq$P+D*;4-lzz_IIpdTXm)*U12aSmk08Vj1yHakGpV*y;>LAroR`0t1g@8Cf_#({C#CK1SaCrO7hh--a4uv<@>Ebrcw5 zp@vCN#p*cXZbUa_XQtKS%D?>3Wm>$Cg6CYiCl>(qG8+~MyGRm(2K44yOB`x}E%$fWhs8U|kR z#!UrH*gkf>I)hR1JUqwG9Gj>vawpuO!=ocNb7nA#>uFd1e6S9l2m6-Ra3_};jA9p3 zhC3s?KYS)wQ^YXLh*0E++ufX3V-h}hVCarTY;1c~hR&Hg^w~p=LZpz+3cH>%47Mc8 z&}nAp;ireYgnQ?k2=UU@%9-A9enWm@C<>Vn>&@VA(fY-S9CyVNMEo~~qL4}JsdB$p zBMt368>4$Y3#&Exxez~2*OaBb@f7c$5aTiM2>~-0)fpIzHWP6A#CHk%mQ{`#{$6CY zjLIq(d9Cd#yx0%>&T6A>W#GE0>%pQc83v$XQxH_t&UI2xX&;c#odb7{0Y3;UcNkx^3uM>%9YXAUFWu4yNXcR dEeS)nn?mOf%JsLMU2tH`+&Aq5jB4060V-f%W literal 0 HcmV?d00001 diff --git a/models/link_9.stl b/models/link_9.stl new file mode 100644 index 0000000000000000000000000000000000000000..b86f550cc4d0844486774a96874185cab10f50cb GIT binary patch literal 1284 zcmb`Fy=p>15QVq1&<9ARUG`>JUe1!;t5*x8uu@Eh6LRt&oAWev+ zduDeo=c3*!7r_HF-<iPU{{Tx%Uan zLM3QaO4XecRUCnb5tFR9H3=m`nBc5g_ShNGNt^_ZFaeF33Xi*lWuX#u;SnvgKj8@1 z<-HYiV4@PiS-f=+Hvf9Tlk-gMT}@c;S01>9#9y2D!}s70)kCj$i+~4S25-T+ZtYOK ztH0-F=$#vWxoP>)%cF;0Wj$QJtF9(23zeWz30>h!?}{VvFk+nb*Qtdo=%<7*!CAlA z<6DV1M>|u2BTPVJrsPpYSQaWlCl9M1|Aix5m-m+PV=^Wx5uC+a2a(nE$6EwE@Je_K&RqaQ9_iBn literal 0 HcmV?d00001 diff --git a/models/manual_robot.urdf b/models/manual_robot.urdf index 5e7126a..02657c1 100644 --- a/models/manual_robot.urdf +++ b/models/manual_robot.urdf @@ -55,7 +55,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -64,7 +64,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -118,7 +118,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -126,7 +126,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -179,7 +179,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -187,7 +187,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -235,7 +235,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -243,7 +243,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -287,7 +287,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -295,7 +295,7 @@ Link masses decrease gradually from base to end effector, following robot arm de - + @@ -327,14 +327,14 @@ Link masses decrease gradually from base to end effector, following robot arm de - + - + @@ -358,14 +358,14 @@ Link masses decrease gradually from base to end effector, following robot arm de - + - + @@ -389,14 +389,14 @@ Link masses decrease gradually from base to end effector, following robot arm de - + - + @@ -420,14 +420,14 @@ Link masses decrease gradually from base to end effector, following robot arm de - + - + @@ -457,14 +457,14 @@ Link masses decrease gradually from base to end effector, following robot arm de - + - + diff --git a/src/config_loader.py b/src/config_loader.py index e69de29..d8a59ab 100644 --- a/src/config_loader.py +++ b/src/config_loader.py @@ -0,0 +1,154 @@ +import json +import os +from typing import Dict, Any, Optional + + +class ConfigLoader: + _instance: Optional['ConfigLoader'] = None + _config: Optional[Dict[str, Any]] = None + + def __new__(cls, config_path: str = "config.json"): + if cls._instance is None: + cls._instance = super().__new__(cls) + return cls._instance + + def __init__(self, config_path: str = "config.json"): + if self._config is None: + self.config_path = config_path + self.load_config() + self.validate_config() + + def load_config(self) -> None: + if not os.path.exists(self.config_path): + raise FileNotFoundError(f"Configuration file not found: {self.config_path}") + + try: + with open(self.config_path, 'r', encoding='utf-8') as f: + self._config = json.load(f) + except json.JSONDecodeError as e: + raise ValueError(f"Invalid JSON format in config file: {e}") + except Exception as e: + raise RuntimeError(f"Failed to load config file: {e}") + + def validate_config(self) -> None: + required_sections = ['robot', 'wall', 'hole', 'task_points', 'transport_object', 'simulation'] + + for section in required_sections: + if section not in self._config: + raise ValueError(f"Missing required config section: {section}") + + self._validate_robot_config() + self._validate_wall_config() + self._validate_hole_config() + self._validate_task_points() + self._validate_transport_object_config() + self._validate_simulation_config() + + def _validate_robot_config(self) -> None: + robot_config = self._config['robot'] + + if 'model_path' not in robot_config: + raise ValueError("Robot config missing model_path") + + model_path = robot_config['model_path'] + if not os.path.exists(model_path): + raise FileNotFoundError(f"Robot model file not found: {model_path}") + + if 'base_position' not in robot_config or len(robot_config['base_position']) != 3: + raise ValueError("Robot base_position must contain 3 coordinate values") + + if 'base_orientation' not in robot_config or len(robot_config['base_orientation']) != 3: + raise ValueError("Robot base_orientation must contain 3 angle values") + + def _validate_wall_config(self) -> None: + wall_config = self._config['wall'] + + if 'position' not in wall_config or len(wall_config['position']) != 3: + raise ValueError("Wall position must contain 3 coordinate values") + + if 'dimensions' not in wall_config: + raise ValueError("Wall config missing dimensions") + + dimensions = wall_config['dimensions'] + required_dims = ['width', 'height', 'thickness'] + for dim in required_dims: + if dim not in dimensions: + raise ValueError(f"Wall dimensions missing {dim}") + if dimensions[dim] <= 0: + raise ValueError(f"Wall {dim} must be greater than 0") + + def _validate_hole_config(self) -> None: + hole_config = self._config['hole'] + + if 'position_relative_to_wall' not in hole_config or len(hole_config['position_relative_to_wall']) != 3: + raise ValueError("Hole position_relative_to_wall must contain 3 coordinate values") + + if 'dimensions' not in hole_config: + raise ValueError("Hole config missing dimensions") + + dimensions = hole_config['dimensions'] + for dim in ['width', 'height']: + if dim not in dimensions: + raise ValueError(f"Hole dimensions missing {dim}") + if dimensions[dim] <= 0: + raise ValueError(f"Hole {dim} must be greater than 0") + + def _validate_task_points(self) -> None: + task_points = self._config['task_points'] + + for point_name in ['point_A', 'point_B']: + if point_name not in task_points: + raise ValueError(f"Task points missing {point_name}") + + point = task_points[point_name] + if 'position' not in point or len(point['position']) != 3: + raise ValueError(f"{point_name} position must contain 3 coordinate values") + + def _validate_transport_object_config(self) -> None: + obj_config = self._config['transport_object'] + + if 'initial_position' not in obj_config or len(obj_config['initial_position']) != 3: + raise ValueError("Transport object initial_position must contain 3 coordinate values") + + if 'dimensions' not in obj_config: + raise ValueError("Transport object config missing dimensions") + + dimensions = obj_config['dimensions'] + for dim in ['width', 'height', 'depth']: + if dim not in dimensions: + raise ValueError(f"Transport object dimensions missing {dim}") + if dimensions[dim] <= 0: + raise ValueError(f"Transport object {dim} must be greater than 0") + + if 'mass' not in obj_config or obj_config['mass'] <= 0: + raise ValueError("Transport object mass must be greater than 0") + + def _validate_simulation_config(self) -> None: + sim_config = self._config['simulation'] + + if 'timestep' not in sim_config or sim_config['timestep'] <= 0: + raise ValueError("Simulation timestep must be greater than 0") + + if 'gravity' not in sim_config or len(sim_config['gravity']) != 3: + raise ValueError("Simulation gravity must contain 3 components") + + def get_robot_config(self) -> Dict[str, Any]: + return self._config['robot'].copy() + + def get_wall_config(self) -> Dict[str, Any]: + return self._config['wall'].copy() + + def get_hole_config(self) -> Dict[str, Any]: + return self._config['hole'].copy() + + def get_task_points(self) -> Dict[str, Any]: + return self._config['task_points'].copy() + + def get_transport_object_config(self) -> Dict[str, Any]: + return self._config['transport_object'].copy() + + def get_simulation_config(self) -> Dict[str, Any]: + return self._config['simulation'].copy() + + def get_full_config(self) -> Dict[str, Any]: + return self._config.copy() \ No newline at end of file diff --git a/src/gui/__init__.py b/src/gui/__init__.py new file mode 100644 index 0000000..37b7fd9 --- /dev/null +++ b/src/gui/__init__.py @@ -0,0 +1 @@ +# GUI module initialization \ No newline at end of file diff --git a/src/gui/config_window.py b/src/gui/config_window.py new file mode 100644 index 0000000..2340f41 --- /dev/null +++ b/src/gui/config_window.py @@ -0,0 +1,400 @@ +import tkinter as tk +from tkinter import ttk, messagebox +import json +from typing import Dict, Any, Callable, Tuple, List + + +class ConfigWindow: + # UI Constants extracted from hardcoded values + WINDOW_GEOMETRY = "800x600" + BUTTON_COLORS = { + "apply": "#4CAF50", + "save": "#2196F3", + "reset": "#f44336" + } + FONT_TITLE = ("Arial", 12, "bold") + FONT_SECTION = ("Arial", 10, "bold") + PADDING_STANDARD = 5 + PADDING_LARGE = 10 + ENTRY_WIDTH_STANDARD = 10 + ENTRY_WIDTH_LONG = 40 + BUTTON_PADDING = 20 + + def __init__(self, parent, config_path: str = "config.json", on_apply_callback: Callable = None): + self.parent = parent + self.config_path = config_path + self.on_apply_callback = on_apply_callback + self.window = None + self.config_data = {} + self.entry_widgets = {} + + # Load current configuration + self.load_config() + + def load_config(self): + """Load configuration from file""" + try: + with open(self.config_path, 'r', encoding='utf-8') as f: + self.config_data = json.load(f) + except Exception as e: + # Follow fail-fast principle - raise exception instead of hiding it + raise RuntimeError(f"Failed to load configuration from {self.config_path}: {e}") + + def save_config(self): + """Save configuration to file""" + try: + with open(self.config_path, 'w', encoding='utf-8') as f: + json.dump(self.config_data, f, indent=2, ensure_ascii=False) + return True + except Exception as e: + # Log error but allow UI to handle it + raise RuntimeError(f"Failed to save configuration to {self.config_path}: {e}") + + def show(self): + """Show configuration window""" + if self.window and self.window.winfo_exists(): + self.window.lift() + return + + self.window = tk.Toplevel(self.parent) + self.window.title("Configuration Management") + self.window.geometry(self.WINDOW_GEOMETRY) + + # Create notebook for tabs + notebook = ttk.Notebook(self.window) + notebook.pack(fill=tk.BOTH, expand=True, padx=self.PADDING_LARGE, pady=self.PADDING_LARGE) + + # Create tabs + self.create_robot_tab(notebook) + self.create_wall_tab(notebook) + self.create_hole_tab(notebook) + self.create_task_points_tab(notebook) + self.create_transport_object_tab(notebook) + self.create_simulation_tab(notebook) + + # Create buttons frame + button_frame = tk.Frame(self.window) + button_frame.pack(fill=tk.X, padx=self.PADDING_LARGE, pady=self.PADDING_STANDARD) + + # Apply button + apply_btn = tk.Button(button_frame, text="Apply", command=self.apply_changes, + bg=self.BUTTON_COLORS["apply"], fg="white", padx=self.BUTTON_PADDING) + apply_btn.pack(side=tk.LEFT, padx=self.PADDING_STANDARD) + + # Save button + save_btn = tk.Button(button_frame, text="Save to File", command=self.save_changes, + bg=self.BUTTON_COLORS["save"], fg="white", padx=self.BUTTON_PADDING) + save_btn.pack(side=tk.LEFT, padx=self.PADDING_STANDARD) + + # Reset button + reset_btn = tk.Button(button_frame, text="Reset", command=self.reset_values, + bg=self.BUTTON_COLORS["reset"], fg="white", padx=self.BUTTON_PADDING) + reset_btn.pack(side=tk.LEFT, padx=self.PADDING_STANDARD) + + # Close button + close_btn = tk.Button(button_frame, text="Close", command=self.window.destroy, + padx=self.BUTTON_PADDING) + close_btn.pack(side=tk.RIGHT, padx=self.PADDING_STANDARD) + + def _create_vector_input(self, parent: tk.Frame, row: int, label: str, key_prefix: str, + values: List[float], labels: List[str] = None) -> None: + """Create vector input fields (x, y, z or custom labels)""" + if labels is None: + labels = ["X:", "Y:", "Z:"] + + tk.Label(parent, text=label).grid(row=row, column=0, sticky="e", + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + + for i, (lbl, val) in enumerate(zip(labels, values)): + tk.Label(parent, text=lbl).grid(row=row + i, column=1, sticky="e", + padx=self.PADDING_STANDARD) + entry_key = f"{key_prefix}.{lbl.lower().rstrip(':')}" + self.entry_widgets[entry_key] = tk.Entry(parent, width=self.ENTRY_WIDTH_STANDARD) + self.entry_widgets[entry_key].grid(row=row + i, column=2, + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets[entry_key].insert(0, str(val)) + + def _create_title_label(self, parent: tk.Frame, text: str, row: int = 0, + font_type: str = "title") -> None: + """Create a title label""" + font = self.FONT_TITLE if font_type == "title" else self.FONT_SECTION + tk.Label(parent, text=text, font=font).grid( + row=row, column=0, columnspan=6, pady=self.PADDING_LARGE) + + def _get_config_value(self, *keys, default=None): + """Safely get nested configuration value""" + value = self.config_data + for key in keys: + if isinstance(value, dict): + value = value.get(key, {}) + else: + return default + return value if value != {} else default + + def create_robot_tab(self, notebook): + """Create robot configuration tab""" + frame = ttk.Frame(notebook) + notebook.add(frame, text="Robot") + + # Title + self._create_title_label(frame, "Robot Configuration") + + # Model path + tk.Label(frame, text="Model Path:").grid(row=1, column=0, sticky="e", + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets['robot.model_path'] = tk.Entry(frame, width=self.ENTRY_WIDTH_LONG) + self.entry_widgets['robot.model_path'].grid(row=1, column=1, columnspan=3, + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets['robot.model_path'].insert(0, self._get_config_value('robot', 'model_path', default='')) + + # Base position + base_pos = self._get_config_value('robot', 'base_position', default=[0, 0, 0]) + self._create_vector_input(frame, 2, "Base Position:", "robot.base_position", base_pos) + + # Base orientation + base_orient = self._get_config_value('robot', 'base_orientation', default=[0, 0, 0]) + self._create_vector_input(frame, 5, "Base Orientation:", "robot.base_orientation", + base_orient, labels=["Roll:", "Pitch:", "Yaw:"]) + + def _create_dimension_inputs(self, parent: tk.Frame, row: int, key_prefix: str, + dimensions: Dict[str, float], dim_names: List[str]) -> None: + """Create dimension input fields""" + tk.Label(parent, text="Dimensions:").grid(row=row, column=0, sticky="e", + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + + for i, dim_name in enumerate(dim_names): + tk.Label(parent, text=f"{dim_name.capitalize()}:").grid(row=row + i, column=1, sticky="e", + padx=self.PADDING_STANDARD) + entry_key = f"{key_prefix}.{dim_name}" + self.entry_widgets[entry_key] = tk.Entry(parent, width=self.ENTRY_WIDTH_STANDARD) + self.entry_widgets[entry_key].grid(row=row + i, column=2, + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + # Get default value from config or use 0 + default_val = dimensions.get(dim_name, 0.0) + self.entry_widgets[entry_key].insert(0, str(default_val)) + + def create_wall_tab(self, notebook): + """Create wall configuration tab""" + frame = ttk.Frame(notebook) + notebook.add(frame, text="Wall") + + # Title + self._create_title_label(frame, "Wall Configuration") + + # Wall position + wall_pos = self._get_config_value('wall', 'position', default=[2.0, 0.0, 1.0]) + self._create_vector_input(frame, 1, "Position:", "wall.position", wall_pos) + + # Wall dimensions + wall_dims = self._get_config_value('wall', 'dimensions', default={}) + # Set defaults if not in config + wall_dims.setdefault('width', 3.0) + wall_dims.setdefault('height', 2.5) + wall_dims.setdefault('thickness', 0.2) + self._create_dimension_inputs(frame, 4, "wall.dimensions", wall_dims, + ['width', 'height', 'thickness']) + + def create_hole_tab(self, notebook): + """Create hole configuration tab""" + frame = ttk.Frame(notebook) + notebook.add(frame, text="Hole") + + # Title + self._create_title_label(frame, "Hole Configuration") + + # Hole position relative to wall + hole_pos = self._get_config_value('hole', 'position_relative_to_wall', default=[0, 0, 0]) + self._create_vector_input(frame, 1, "Position (relative):", "hole.position", hole_pos) + + # Hole dimensions + hole_dims = self._get_config_value('hole', 'dimensions', default={}) + hole_dims.setdefault('width', 0.6) + hole_dims.setdefault('height', 0.6) + self._create_dimension_inputs(frame, 4, "hole.dimensions", hole_dims, ['width', 'height']) + + def _create_horizontal_vector_input(self, parent: tk.Frame, row: int, label: str, + key_prefix: str, values: List[float]) -> None: + """Create horizontal vector input fields (for compact layout)""" + labels = ["X:", "Y:", "Z:"] + + for i, (lbl, val) in enumerate(zip(labels, values)): + tk.Label(parent, text=lbl).grid(row=row, column=i*2, sticky="e", + padx=self.PADDING_STANDARD) + entry_key = f"{key_prefix}.{lbl.lower().rstrip(':')}" + self.entry_widgets[entry_key] = tk.Entry(parent, width=self.ENTRY_WIDTH_STANDARD) + self.entry_widgets[entry_key].grid(row=row, column=i*2+1, + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets[entry_key].insert(0, str(val)) + + def create_task_points_tab(self, notebook): + """Create task points configuration tab""" + frame = ttk.Frame(notebook) + notebook.add(frame, text="Task Points") + + # Title + self._create_title_label(frame, "Task Points Configuration") + + # Point A + self._create_title_label(frame, "Point A (Pick-up):", row=1, font_type="section") + + point_a = self._get_config_value('task_points', 'point_A', 'position', + default=[1.0, 0.5, 0.8]) + self._create_horizontal_vector_input(frame, 2, "Point A", "point_a", point_a) + + # Point B + self._create_title_label(frame, "Point B (Drop-off):", row=3, font_type="section") + + point_b = self._get_config_value('task_points', 'point_B', 'position', + default=[3.0, 0.5, 0.8]) + self._create_horizontal_vector_input(frame, 4, "Point B", "point_b", point_b) + + def create_transport_object_tab(self, notebook): + """Create transport object configuration tab""" + frame = ttk.Frame(notebook) + notebook.add(frame, text="Transport Object") + + # Title + self._create_title_label(frame, "Transport Object Configuration") + + # Initial position + obj_pos = self._get_config_value('transport_object', 'initial_position', + default=[1.0, 0.5, 0.5]) + self._create_vector_input(frame, 1, "Initial Position:", "object.position", obj_pos) + + # Dimensions + obj_dims = self._get_config_value('transport_object', 'dimensions', default={}) + obj_dims.setdefault('width', 0.1) + obj_dims.setdefault('height', 0.1) + obj_dims.setdefault('depth', 0.1) + self._create_dimension_inputs(frame, 4, "object.dimensions", obj_dims, + ['width', 'height', 'depth']) + + # Mass + mass = self._get_config_value('transport_object', 'mass', default=0.5) + tk.Label(frame, text="Mass (kg):").grid(row=7, column=0, sticky="e", + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets['object.mass'] = tk.Entry(frame, width=self.ENTRY_WIDTH_STANDARD) + self.entry_widgets['object.mass'].grid(row=7, column=2, + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets['object.mass'].insert(0, str(mass)) + + def create_simulation_tab(self, notebook): + """Create simulation configuration tab""" + frame = ttk.Frame(notebook) + notebook.add(frame, text="Simulation") + + # Title + self._create_title_label(frame, "Simulation Configuration") + + # Timestep + timestep = self._get_config_value('simulation', 'timestep', default=0.01) + tk.Label(frame, text="Timestep:").grid(row=1, column=0, sticky="e", + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets['simulation.timestep'] = tk.Entry(frame, width=self.ENTRY_WIDTH_STANDARD) + self.entry_widgets['simulation.timestep'].grid(row=1, column=1, + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.entry_widgets['simulation.timestep'].insert(0, str(timestep)) + + # Gravity + gravity = self._get_config_value('simulation', 'gravity', default=[0, 0, -9.81]) + self._create_vector_input(frame, 2, "Gravity:", "simulation.gravity", gravity) + + # Real-time mode + real_time = self._get_config_value('simulation', 'real_time', default=False) + tk.Label(frame, text="Real-time Mode:").grid(row=5, column=0, sticky="e", + padx=self.PADDING_STANDARD, pady=self.PADDING_STANDARD) + self.real_time_var = tk.BooleanVar() + self.real_time_var.set(real_time) + tk.Checkbutton(frame, variable=self.real_time_var).grid(row=5, column=1, + padx=self.PADDING_STANDARD, + pady=self.PADDING_STANDARD, sticky="w") + + def _get_vector_from_widgets(self, key_prefix: str) -> List[float]: + """Get vector values from widgets""" + return [ + float(self.entry_widgets[f'{key_prefix}.x'].get()), + float(self.entry_widgets[f'{key_prefix}.y'].get()), + float(self.entry_widgets[f'{key_prefix}.z'].get()) + ] + + def _get_vector_custom_from_widgets(self, key_prefix: str, labels: List[str]) -> List[float]: + """Get vector values from widgets with custom labels""" + return [float(self.entry_widgets[f'{key_prefix}.{label}'].get()) for label in labels] + + def _ensure_nested_dict(self, *keys) -> None: + """Ensure nested dictionary structure exists""" + current = self.config_data + for key in keys: + if key not in current: + current[key] = {} + current = current[key] + + def get_values_from_widgets(self) -> Dict[str, Any]: + """Get values from all entry widgets and update config_data""" + try: + # Ensure all necessary nested structures exist + self._ensure_nested_dict('robot') + self._ensure_nested_dict('wall', 'dimensions') + self._ensure_nested_dict('hole', 'dimensions') + self._ensure_nested_dict('task_points', 'point_A') + self._ensure_nested_dict('task_points', 'point_B') + self._ensure_nested_dict('transport_object', 'dimensions') + self._ensure_nested_dict('simulation') + + # Robot configuration + self.config_data['robot']['model_path'] = self.entry_widgets['robot.model_path'].get() + self.config_data['robot']['base_position'] = self._get_vector_from_widgets('robot.base_position') + self.config_data['robot']['base_orientation'] = self._get_vector_custom_from_widgets( + 'robot.base_orientation', ['roll', 'pitch', 'yaw']) + + # Wall configuration + self.config_data['wall']['position'] = self._get_vector_from_widgets('wall.position') + for dim in ['width', 'height', 'thickness']: + self.config_data['wall']['dimensions'][dim] = float( + self.entry_widgets[f'wall.dimensions.{dim}'].get()) + + # Hole configuration + self.config_data['hole']['position_relative_to_wall'] = self._get_vector_from_widgets('hole.position') + for dim in ['width', 'height']: + self.config_data['hole']['dimensions'][dim] = float( + self.entry_widgets[f'hole.dimensions.{dim}'].get()) + + # Task points + self.config_data['task_points']['point_A']['position'] = self._get_vector_from_widgets('point_a') + self.config_data['task_points']['point_B']['position'] = self._get_vector_from_widgets('point_b') + + # Transport object + self.config_data['transport_object']['initial_position'] = self._get_vector_from_widgets('object.position') + for dim in ['width', 'height', 'depth']: + self.config_data['transport_object']['dimensions'][dim] = float( + self.entry_widgets[f'object.dimensions.{dim}'].get()) + self.config_data['transport_object']['mass'] = float(self.entry_widgets['object.mass'].get()) + + # Simulation + self.config_data['simulation']['timestep'] = float(self.entry_widgets['simulation.timestep'].get()) + self.config_data['simulation']['gravity'] = self._get_vector_from_widgets('simulation.gravity') + self.config_data['simulation']['real_time'] = self.real_time_var.get() + + return self.config_data + except (ValueError, KeyError) as e: + # Allow UI to handle the error display + raise ValueError(f"Invalid input or configuration structure: {e}") + + def apply_changes(self): + """Apply configuration changes""" + config = self.get_values_from_widgets() + if self.on_apply_callback: + self.on_apply_callback(config) + + def save_changes(self): + """Save configuration changes to file""" + self.config_data = self.get_values_from_widgets() + self.save_config() + + def reset_values(self): + """Reset values to original configuration""" + self.load_config() + # Refresh window + if self.window: + self.window.destroy() + self.show() \ No newline at end of file diff --git a/src/gui/main_window.py b/src/gui/main_window.py new file mode 100644 index 0000000..affbb96 --- /dev/null +++ b/src/gui/main_window.py @@ -0,0 +1,434 @@ +import tkinter as tk +from tkinter import ttk, messagebox, scrolledtext +import pybullet as p +import pybullet_data +import threading +import time +from typing import Dict, Any +import sys +import os + +# Add parent directory to path for imports +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))) + +from src.config_loader import ConfigLoader +from src.robot.robot_loader import RobotLoader +from src.simulation.environment import Environment +from src.gui.config_window import ConfigWindow + + +class MainWindow: + def __init__(self): + self.root = tk.Tk() + self.root.title("Robotic Arm Simulation - Feasibility Test") + + # Get window dimensions from config or use defaults + self._setup_window_geometry() + + # Simulation components + self.physics_client = None + self.config_loader = None + self.robot_loader = None + self.environment = None + self.config_window = None + + # Simulation state + self.simulation_running = False + self.simulation_thread = None + + # Create GUI + self._create_control_panel() + self._create_log_panel() + self._create_bottom_panel() + + # Initialize simulation + self.initialize_simulation() + + def _setup_window_geometry(self): + """Setup window geometry from config or defaults""" + try: + # Try to load from config if exists + config = ConfigLoader().get_full_config() + window_size = config.get('gui', {}).get('window_size', "1200x800") + self.root.geometry(window_size) + except Exception as e: + # Use config default if loading fails + default_config = {"gui": {"window_size": "1200x800"}} + self.root.geometry(default_config['gui']['window_size']) + + def _create_control_panel(self): + """Create control panel on the left side""" + # Create main frame + self.main_frame = tk.Frame(self.root) + self.main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10) + + # Left panel - Control panel + left_panel = tk.Frame(self.main_frame, width=300) + left_panel.pack(side=tk.LEFT, fill=tk.Y, padx=(0, 10)) + + # Title + title_label = tk.Label(left_panel, text="Control Panel", + font=("Arial", 14, "bold")) + title_label.pack(pady=(0, 10)) + + # Configuration section + config_frame = tk.LabelFrame(left_panel, text="Configuration", padx=10, pady=10) + config_frame.pack(fill=tk.X, pady=(0, 10)) + + self.config_btn = tk.Button(config_frame, text="Open Config Manager", + command=self.open_config_window, + bg="#2196F3", fg="white", padx=10, pady=5) + self.config_btn.pack(fill=tk.X) + + self.reload_btn = tk.Button(config_frame, text="Reload Config", + command=self.reload_configuration, + padx=10, pady=5) + self.reload_btn.pack(fill=tk.X, pady=(5, 0)) + + # Simulation control section + control_frame = tk.LabelFrame(left_panel, text="Simulation Control", padx=10, pady=10) + control_frame.pack(fill=tk.X, pady=(0, 10)) + + self.start_btn = tk.Button(control_frame, text="Start Simulation", + command=self.start_simulation, + bg="#4CAF50", fg="white", padx=10, pady=5) + self.start_btn.pack(fill=tk.X) + + self.pause_btn = tk.Button(control_frame, text="Pause Simulation", + command=self.pause_simulation, + state=tk.DISABLED, padx=10, pady=5) + self.pause_btn.pack(fill=tk.X, pady=(5, 0)) + + self.reset_btn = tk.Button(control_frame, text="Reset Environment", + command=self.reset_environment, + bg="#FF9800", fg="white", padx=10, pady=5) + self.reset_btn.pack(fill=tk.X, pady=(5, 0)) + + # Reachability test section + test_frame = tk.LabelFrame(left_panel, text="Reachability Test", padx=10, pady=10) + test_frame.pack(fill=tk.X, pady=(0, 10)) + + self.test_btn = tk.Button(test_frame, text="Test Reachability", + command=self.test_reachability, + bg="#9C27B0", fg="white", padx=10, pady=5) + self.test_btn.pack(fill=tk.X) + + tk.Label(test_frame, text="Check if A and B points are reachable", + font=("Arial", 9)).pack(pady=(5, 0)) + + # Status section + status_frame = tk.LabelFrame(left_panel, text="Status", padx=10, pady=10) + status_frame.pack(fill=tk.X, pady=(0, 10)) + + # Robot status + tk.Label(status_frame, text="Robot:", font=("Arial", 9, "bold")).grid(row=0, column=0, sticky="w") + self.robot_status = tk.Label(status_frame, text="Not Loaded", fg="red") + self.robot_status.grid(row=0, column=1, sticky="w", padx=(5, 0)) + + # Environment status + tk.Label(status_frame, text="Environment:", font=("Arial", 9, "bold")).grid(row=1, column=0, sticky="w") + self.env_status = tk.Label(status_frame, text="Not Loaded", fg="red") + self.env_status.grid(row=1, column=1, sticky="w", padx=(5, 0)) + + # Simulation status + tk.Label(status_frame, text="Simulation:", font=("Arial", 9, "bold")).grid(row=2, column=0, sticky="w") + self.sim_status = tk.Label(status_frame, text="Stopped", fg="orange") + self.sim_status.grid(row=2, column=1, sticky="w", padx=(5, 0)) + + + def _create_log_panel(self): + """Create log panel with copyable text""" + from tkinter import scrolledtext + + # Right panel - Log display + right_panel = tk.Frame(self.main_frame) + right_panel.pack(side=tk.LEFT, fill=tk.BOTH, expand=True) + + # Log display frame + log_label = tk.Label(right_panel, text="System Log", font=("Arial", 12, "bold")) + log_label.pack(pady=(0, 5)) + + # Scrollable text widget for logs (can select and copy) + self.log_text = scrolledtext.ScrolledText(right_panel, height=25, wrap=tk.WORD) + self.log_text.pack(fill=tk.BOTH, expand=True, padx=5, pady=5) + + # Make it read-only initially but still selectable + self.log_text.config(state=tk.DISABLED) + + def _create_bottom_panel(self): + """Create bottom panel with viewer info and exit button""" + bottom_frame = tk.Frame(self.root) + bottom_frame.pack(fill=tk.X, padx=10, pady=(0, 10)) + + tk.Label(bottom_frame, text="PyBullet Viewer:", font=("Arial", 9, "bold")).pack(side=tk.LEFT, padx=(0, 5)) + tk.Label(bottom_frame, text="Use external PyBullet GUI window for 3D visualization", + fg="gray").pack(side=tk.LEFT) + + # Exit button + exit_btn = tk.Button(bottom_frame, text="Exit", command=self.on_closing, + bg="#f44336", fg="white", padx=20) + exit_btn.pack(side=tk.RIGHT) + + def initialize_simulation(self): + """Initialize PyBullet simulation""" + try: + # Connect to PyBullet in GUI mode + self.physics_client = p.connect(p.GUI) + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + + # Configure visualization + p.configureDebugVisualizer(p.COV_ENABLE_GUI, 1) + p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS, 1) + p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1) + + # Set camera from config or defaults + self._reset_camera_view() + + # Load configuration + self.config_loader = ConfigLoader() + + # Setup environment + self.environment = Environment(self.config_loader, self.physics_client) + self.environment.setup_environment() + self.env_status.config(text="Loaded", fg="green") + + # Load robot + self.robot_loader = RobotLoader(self.config_loader, self.physics_client) + self.robot_loader.load_robot() + self.robot_status.config(text="Loaded", fg="green") + + self.update_status("System ready") + + except Exception as e: + self.update_status(f"Initialization failed: {e}") + messagebox.showerror("Initialization Error", f"Failed to initialize simulation:\n{e}") + + def open_config_window(self): + """Open configuration management window""" + if not self.config_window: + self.config_window = ConfigWindow( + self.root, + on_apply_callback=self.on_config_applied + ) + self.config_window.show() + + def on_config_applied(self, new_config: Dict[str, Any]): + """Handle configuration changes from config window""" + self.update_status("Applying configuration...") + + try: + # Save current robot state if exists + robot_state = None + if self.robot_loader and self.robot_loader.robot_id is not None: + robot_state = self.robot_loader.get_joint_states() + + # Clear current environment + if self.environment: + self.environment.cleanup() + if self.robot_loader and self.robot_loader.robot_id is not None: + p.removeBody(self.robot_loader.robot_id, physicsClientId=self.physics_client) + + # Create new ConfigLoader with updated configuration + self.config_loader = ConfigLoader() + # Update the config loader's internal config directly + self.config_loader._config = new_config + + # Recreate environment + self.environment = Environment(self.config_loader, self.physics_client) + self.environment.setup_environment() + + # Reload robot + self.robot_loader = RobotLoader(self.config_loader, self.physics_client) + self.robot_loader.load_robot() + + # Restore robot state if available + if robot_state: + try: + positions = [state['position'] for state in robot_state.values()] + self.robot_loader.set_joint_positions(positions) + except Exception: + # State restoration failed, continue with defaults + pass + + self.update_status("Configuration applied") + + except Exception as e: + self.update_status(f"Configuration error: {e}") + messagebox.showerror("Configuration Error", f"Failed to apply configuration:\n{e}") + + def reload_configuration(self): + """Reload configuration from file""" + try: + self.config_loader = ConfigLoader() + self.on_config_applied(self.config_loader.get_full_config()) + self.update_status("Configuration reloaded") + except Exception as e: + self.update_status(f"Reload failed: {e}") + + def start_simulation(self): + """Start simulation loop""" + if not self.simulation_running: + self.simulation_running = True + self.sim_status.config(text="Running", fg="green") + self.start_btn.config(state=tk.DISABLED) + self.pause_btn.config(state=tk.NORMAL) + + # Start simulation thread + self.simulation_thread = threading.Thread(target=self.simulation_loop, daemon=True) + self.simulation_thread.start() + + self.update_status("Simulation running") + + def pause_simulation(self): + """Pause simulation loop""" + if self.simulation_running: + self.simulation_running = False + self.sim_status.config(text="Paused", fg="orange") + self.start_btn.config(state=tk.NORMAL) + self.pause_btn.config(state=tk.DISABLED) + self.update_status("Simulation paused") + + def simulation_loop(self): + """Main simulation loop (runs in separate thread)""" + while self.simulation_running: + try: + p.stepSimulation(physicsClientId=self.physics_client) + # Get timestep from config + timestep = self.config_loader.get_full_config().get('simulation', {}).get('timestep', 0.01) + time.sleep(timestep) + except Exception as e: + self.update_status(f"Simulation error: {e}") + self.simulation_running = False + break + + def reset_environment(self): + """Reset environment to initial state""" + try: + # Reset robot + if self.robot_loader: + self.robot_loader.reset_robot() + + # Reset environment objects + if self.environment: + self.environment.reset_environment() + + # Reset camera + self._reset_camera_view() + + self.update_status("Environment reset") + + except Exception as e: + self.update_status(f"Reset failed: {e}") + + def test_reachability(self): + """Test reachability of task points""" + self.update_status("Testing reachability...") + + try: + # Get task points + task_points = self.config_loader.get_task_points() + point_a = task_points['point_A']['position'] + point_b = task_points['point_B']['position'] + + # Perform reachability check + self._check_reachability(point_a, point_b) + + except Exception as e: + self.update_status(f"Reachability test failed: {e}") + raise + + def _check_reachability(self, point_a, point_b): + """Check if points are reachable by the robot""" + # Calculate distances + distance_a = self._calculate_distance(point_a) + distance_b = self._calculate_distance(point_b) + + # Get robot reach + ee_pos, _ = self.robot_loader.get_end_effector_pose() + current_reach = self._calculate_distance(ee_pos) + + # Check reachability + a_reachable = distance_a <= current_reach + b_reachable = distance_b <= current_reach + + if a_reachable and b_reachable: + self.update_status("Both points are reachable") + elif not a_reachable: + self.update_status(f"Point A out of reach ({distance_a:.2f}m > {current_reach:.2f}m)") + elif not b_reachable: + self.update_status(f"Point B out of reach ({distance_b:.2f}m > {current_reach:.2f}m)") + + def _calculate_distance(self, point): + """Calculate Euclidean distance from origin""" + return (point[0]**2 + point[1]**2 + point[2]**2) ** 0.5 + + def _reset_camera_view(self): + """Reset camera to default view using config values""" + try: + # Get camera settings from config + config = self.config_loader.get_full_config() if self.config_loader else {} + camera_config = config.get('camera', {}) + + # Use config values with explicit defaults from config structure + default_camera = {'distance': 4.0, 'yaw': 45, 'pitch': -30, 'target': [0, 0, 0]} + distance = camera_config.get('distance', default_camera['distance']) + yaw = camera_config.get('yaw', default_camera['yaw']) + pitch = camera_config.get('pitch', default_camera['pitch']) + target = camera_config.get('target', default_camera['target']) + + p.resetDebugVisualizerCamera( + cameraDistance=distance, + cameraYaw=yaw, + cameraPitch=pitch, + cameraTargetPosition=target, + physicsClientId=self.physics_client + ) + except Exception as e: + # Re-raise exception for proper error handling + raise Exception(f"Failed to reset camera view: {e}") + + def update_status(self, message: str): + """Update status display with copyable text""" + import time + if hasattr(self, 'log_text'): + # Enable text widget to add content + self.log_text.config(state=tk.NORMAL) + # Add timestamp and message + timestamp = time.strftime("%H:%M:%S") + self.log_text.insert(tk.END, f"[{timestamp}] {message}\n") + # Auto-scroll to bottom + self.log_text.see(tk.END) + # Make read-only again but still selectable + self.log_text.config(state=tk.DISABLED) + + def on_closing(self): + """Handle window closing""" + if messagebox.askokcancel("Quit", "Do you want to quit the simulation?"): + try: + # Stop simulation + self.simulation_running = False + + # Cleanup + if self.environment: + self.environment.cleanup() + + # Disconnect PyBullet + if self.physics_client is not None: + p.disconnect(self.physics_client) + + except Exception as e: + # Log cleanup failure but still close + print(f"Cleanup error: {e}") + + self.root.destroy() + + def run(self): + """Start the main window""" + self.root.protocol("WM_DELETE_WINDOW", self.on_closing) + self.root.mainloop() + + +if __name__ == "__main__": + # Test the main window directly + app = MainWindow() + app.run() \ No newline at end of file diff --git a/src/robot/kinematics.py b/src/robot/kinematics.py index e69de29..4160323 100644 --- a/src/robot/kinematics.py +++ b/src/robot/kinematics.py @@ -0,0 +1,347 @@ +#!/usr/bin/env python3 +""" +KDL-based Kinematics Engine + +Provides forward and inverse kinematics calculations using KDL library. +Supports URDF-based robot configurations for precise kinematic computations. +""" + +import PyKDL as kdl +import numpy as np +from typing import List, Tuple, Optional, Dict +from urdf_parser_py.urdf import URDF +import sys +import os + + +class KinematicsEngine: + """KDL-based kinematics engine for robot arm calculations""" + + def __init__(self, urdf_path: str, base_link: str, end_link: str, + base_position: List[float] = None, base_orientation: List[float] = None): + """Initialize KDL kinematics engine from URDF file""" + self.urdf_path = urdf_path + self.base_link = base_link + self.end_link = end_link + + # Base coordinate transformation (base pose in world frame) + self.base_position = base_position if base_position else [0.0, 0.0, 0.0] + self.base_orientation = base_orientation if base_orientation else [0.0, 0.0, 0.0] # RPY + self._base_to_world_frame = self._create_transform_frame(self.base_position, self.base_orientation) + self._world_to_base_frame = self._base_to_world_frame.Inverse() + + # KDL components + self.chain: Optional[kdl.Chain] = None + self.fk_solver: Optional[kdl.ChainFkSolverPos_recursive] = None + self.ik_solver: Optional[kdl.ChainIkSolverPos_NR_JL] = None + self.jac_solver: Optional[kdl.ChainJntToJacSolver] = None + self.joint_limits: List[Tuple[float, float]] = [] + + # Initialize KDL chain and solvers + self._build_kdl_chain() + self._create_solvers() + + def _build_kdl_chain(self) -> None: + """Build KDL chain from URDF file""" + try: + # Parse URDF + robot = URDF.from_xml_file(self.urdf_path) + + # Build chain from base to end effector + self.chain = self._urdf_to_kdl_chain(robot, self.base_link, self.end_link) + + if self.chain is None: + raise RuntimeError(f"Failed to build KDL chain from {self.base_link} to {self.end_link}") + + except Exception as e: + raise RuntimeError(f"Failed to build KDL chain: {e}") + + def _urdf_to_kdl_chain(self, robot: URDF, base_link: str, end_link: str) -> kdl.Chain: + """Convert URDF robot to KDL chain""" + chain = kdl.Chain() + + # Find path from base to end link + path = self._find_link_path(robot, base_link, end_link) + if not path: + raise RuntimeError(f"No path found from {base_link} to {end_link}") + + # Build chain along the path + for i in range(len(path) - 1): + current_link = path[i] + next_link = path[i + 1] + + # Find joint connecting these links + joint = self._find_joint_between_links(robot, current_link, next_link) + if joint is None: + continue + + # Convert joint to KDL segment + segment = self._joint_to_kdl_segment(joint, robot) + chain.addSegment(segment) + + # Store joint limits for controllable joints + if joint.type in ['revolute', 'prismatic']: + if joint.limit: + self.joint_limits.append((joint.limit.lower, joint.limit.upper)) + else: + # Default large limits if not specified + self.joint_limits.append((-3.14159, 3.14159)) + + return chain + + def _find_link_path(self, robot: URDF, start_link: str, end_link: str) -> List[str]: + """Find path between two links in URDF tree""" + if start_link == end_link: + return [start_link] + + # Build parent-child relationships + parent_map = {} + for joint in robot.joints: + parent_map[joint.child] = joint.parent + + # Find path from end to start + path = [end_link] + current = end_link + + while current != start_link and current in parent_map: + current = parent_map[current] + path.append(current) + + if current != start_link: + return [] # No path found + + # Reverse to get start->end path + return list(reversed(path)) + + def _find_joint_between_links(self, robot: URDF, parent_link: str, child_link: str): + """Find joint connecting two links""" + for joint in robot.joints: + if joint.parent == parent_link and joint.child == child_link: + return joint + return None + + def _joint_to_kdl_segment(self, joint, robot: URDF) -> kdl.Segment: + """Convert URDF joint to KDL segment""" + # Get joint origin + origin = joint.origin if joint.origin else URDF.Origin() + xyz = origin.xyz if origin.xyz else [0, 0, 0] + rpy = origin.rpy if origin.rpy else [0, 0, 0] + + # Create KDL frame + rotation = kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2]) + translation = kdl.Vector(xyz[0], xyz[1], xyz[2]) + frame = kdl.Frame(rotation, translation) + + # Create KDL joint + if joint.type == 'revolute': + axis = joint.axis if joint.axis else [0, 0, 1] + kdl_joint = kdl.Joint(joint.name, kdl.Joint.RotZ if axis == [0, 0, 1] + else kdl.Joint.RotY if axis == [0, 1, 0] + else kdl.Joint.RotX) + elif joint.type == 'prismatic': + axis = joint.axis if joint.axis else [0, 0, 1] + kdl_joint = kdl.Joint(joint.name, kdl.Joint.TransZ if axis == [0, 0, 1] + else kdl.Joint.TransY if axis == [0, 1, 0] + else kdl.Joint.TransX) + else: + # Fixed joint + kdl_joint = kdl.Joint(joint.name, kdl.Joint.None) + + return kdl.Segment(joint.child, kdl_joint, frame) + + def _create_solvers(self) -> None: + """Create KDL solvers for forward kinematics, inverse kinematics and Jacobian""" + if self.chain is None: + raise RuntimeError("KDL chain not initialized") + + try: + # Forward kinematics solver + self.fk_solver = kdl.ChainFkSolverPos_recursive(self.chain) + + # Jacobian solver + self.jac_solver = kdl.ChainJntToJacSolver(self.chain) + + # Inverse kinematics solver with joint limits + if self.joint_limits: + q_min = kdl.JntArray(len(self.joint_limits)) + q_max = kdl.JntArray(len(self.joint_limits)) + + for i, (lower, upper) in enumerate(self.joint_limits): + q_min[i] = lower + q_max[i] = upper + + # Create velocity IK solver first + ik_vel_solver = kdl.ChainIkSolverVel_pinv(self.chain) + + # Create position IK solver with joint limits + self.ik_solver = kdl.ChainIkSolverPos_NR_JL( + self.chain, q_min, q_max, self.fk_solver, ik_vel_solver + ) + else: + # Fallback without joint limits + ik_vel_solver = kdl.ChainIkSolverVel_pinv(self.chain) + self.ik_solver = kdl.ChainIkSolverPos_NR( + self.chain, self.fk_solver, ik_vel_solver + ) + + except Exception as e: + raise RuntimeError(f"Failed to create KDL solvers: {e}") + + def forward_kinematics(self, joint_positions: List[float]) -> Tuple[List[float], List[float]]: + """Calculate forward kinematics from joint positions to world coordinate end effector pose""" + if self.fk_solver is None: + raise RuntimeError("Forward kinematics solver not initialized") + + if len(joint_positions) != self.chain.getNrOfJoints(): + raise ValueError(f"Expected {self.chain.getNrOfJoints()} joint positions, got {len(joint_positions)}") + + # Create joint array + q = kdl.JntArray(len(joint_positions)) + for i, pos in enumerate(joint_positions): + q[i] = pos + + # Solve forward kinematics in base frame + base_end_frame = kdl.Frame() + result = self.fk_solver.JntToCart(q, base_end_frame) + + if result != 0: + raise RuntimeError("Forward kinematics calculation failed") + + # Transform from base frame to world frame + world_end_frame = self._base_to_world_frame * base_end_frame + + # Extract world position + position = [ + world_end_frame.p.x(), + world_end_frame.p.y(), + world_end_frame.p.z() + ] + + # Extract world orientation as quaternion + rotation = world_end_frame.M + orientation = self._rotation_to_quaternion(rotation) + + return position, orientation + + def inverse_kinematics(self, target_position: List[float], + target_orientation: List[float] = None, + seed_angles: List[float] = None) -> Optional[List[float]]: + """Calculate inverse kinematics from world coordinate target pose to joint positions""" + if self.ik_solver is None: + raise RuntimeError("Inverse kinematics solver not initialized") + + # Create world target frame + world_target_frame = kdl.Frame() + world_target_frame.p = kdl.Vector(target_position[0], target_position[1], target_position[2]) + + if target_orientation is not None: + world_target_frame.M = self._quaternion_to_rotation(target_orientation) + + # Transform from world frame to base frame + base_target_frame = self._world_to_base_frame * world_target_frame + + # Set seed angles + q_init = kdl.JntArray(self.chain.getNrOfJoints()) + if seed_angles: + if len(seed_angles) != self.chain.getNrOfJoints(): + raise ValueError(f"Expected {self.chain.getNrOfJoints()} seed angles, got {len(seed_angles)}") + for i, angle in enumerate(seed_angles): + q_init[i] = angle + else: + # Use zero as default seed + for i in range(self.chain.getNrOfJoints()): + q_init[i] = 0.0 + + # Solve inverse kinematics in base frame + q_out = kdl.JntArray(self.chain.getNrOfJoints()) + result = self.ik_solver.CartToJnt(q_init, base_target_frame, q_out) + + if result < 0: + return None # No solution found + + # Return joint angles + joint_angles = [] + for i in range(q_out.rows()): + joint_angles.append(q_out[i]) + + return joint_angles + + def compute_jacobian(self, joint_positions: List[float]) -> np.ndarray: + """Compute Jacobian matrix for given joint configuration""" + if self.jac_solver is None: + raise RuntimeError("Jacobian solver not initialized") + + if len(joint_positions) != self.chain.getNrOfJoints(): + raise ValueError(f"Expected {self.chain.getNrOfJoints()} joint positions, got {len(joint_positions)}") + + # Create joint array + q = kdl.JntArray(len(joint_positions)) + for i, pos in enumerate(joint_positions): + q[i] = pos + + # Compute Jacobian + jacobian = kdl.Jacobian(self.chain.getNrOfJoints()) + result = self.jac_solver.JntToJac(q, jacobian) + + if result != 0: + raise RuntimeError("Jacobian calculation failed") + + # Convert to numpy array + jac_array = np.zeros((6, self.chain.getNrOfJoints())) + for i in range(6): + for j in range(self.chain.getNrOfJoints()): + jac_array[i, j] = jacobian[i, j] + + return jac_array + + def get_joint_limits(self) -> List[Tuple[float, float]]: + """Get joint limits for all joints in the chain""" + return self.joint_limits.copy() + + def validate_joint_configuration(self, joint_positions: List[float]) -> bool: + """Validate if joint configuration is within limits""" + if len(joint_positions) != len(self.joint_limits): + return False + + for i, (pos, (lower, upper)) in enumerate(zip(joint_positions, self.joint_limits)): + if pos < lower or pos > upper: + return False + + return True + + def get_num_joints(self) -> int: + """Get number of joints in the kinematic chain""" + return self.chain.getNrOfJoints() if self.chain else 0 + + def _rotation_to_quaternion(self, rotation: kdl.Rotation) -> List[float]: + """Convert KDL rotation to quaternion [x, y, z, w]""" + # Get quaternion from KDL rotation + quat = rotation.GetQuaternion() + return [quat[0], quat[1], quat[2], quat[3]] # [x, y, z, w] + + def _quaternion_to_rotation(self, quaternion: List[float]) -> kdl.Rotation: + """Convert quaternion [x, y, z, w] to KDL rotation""" + return kdl.Rotation.Quaternion(quaternion[0], quaternion[1], quaternion[2], quaternion[3]) + + def _create_transform_frame(self, position: List[float], orientation_rpy: List[float]) -> kdl.Frame: + """Create KDL frame from position and RPY orientation""" + rotation = kdl.Rotation.RPY(orientation_rpy[0], orientation_rpy[1], orientation_rpy[2]) + translation = kdl.Vector(position[0], position[1], position[2]) + return kdl.Frame(rotation, translation) + + +def create_kinematics_engine(config_loader) -> KinematicsEngine: + """Create KinematicsEngine from configuration loader""" + robot_config = config_loader.get_robot_config() + urdf_path = robot_config['model_path'] + + # Get robot base pose from config + base_position = robot_config.get('base_position', [0.0, 0.0, 0.0]) + base_orientation = robot_config.get('base_orientation', [0.0, 0.0, 0.0]) + + # Get link names from config or use defaults + kinematics_config = robot_config.get('kinematics', {}) + base_link = kinematics_config.get('base_link', 'base_link') + end_link = kinematics_config.get('end_link', 'link_9') + + return KinematicsEngine(urdf_path, base_link, end_link, base_position, base_orientation) \ No newline at end of file diff --git a/src/robot/robot_loader.py b/src/robot/robot_loader.py new file mode 100644 index 0000000..5a40bd6 --- /dev/null +++ b/src/robot/robot_loader.py @@ -0,0 +1,304 @@ +import pybullet as p +import os +from typing import Dict, List, Tuple, Any, Optional +from ..config_loader import ConfigLoader + + +class RobotLoader: + def __init__(self, config_loader: ConfigLoader, physics_client: int): + self.config_loader = config_loader + self.physics_client = physics_client + self.robot_id: Optional[int] = None + self.num_joints: int = 0 + self.joint_indices: List[int] = [] + self.joint_info: Dict[int, Dict[str, Any]] = {} + self.link_info: Dict[int, Dict[str, Any]] = {} + self.joint_name_to_index: Dict[str, int] = {} + self.end_effector_index: Optional[int] = None + + # Get robot configuration + self.robot_config = config_loader.get_robot_config() + self.base_position = self.robot_config['base_position'] + self.base_orientation = self.robot_config['base_orientation'] + + # Get expected DOF from config if available + self.expected_dof = self.robot_config.get('dof', None) + + def load_robot(self) -> int: + """Load robot URDF model into pybullet simulation""" + model_path = self.robot_config['model_path'] + + if not os.path.exists(model_path): + raise FileNotFoundError(f"Robot URDF file not found: {model_path}") + + # Convert RPY to quaternion for pybullet + base_orientation_quat = p.getQuaternionFromEuler(self.base_orientation) + + # Load URDF model + self.robot_id = p.loadURDF( + model_path, + basePosition=self.base_position, + baseOrientation=base_orientation_quat, + physicsClientId=self.physics_client + ) + + if self.robot_id is None: + raise RuntimeError(f"Failed to load robot URDF: {model_path}") + + # Parse robot structure + self._parse_robot_structure() + + # Validate robot DOF if specified in config + if self.expected_dof is not None: + if len(self.joint_indices) != self.expected_dof: + raise ValueError(f"Expected {self.expected_dof} DOF robot, found {len(self.joint_indices)} controllable joints") + + # Set initial joint positions if available in config + self.reset_robot() + + return self.robot_id + + def _parse_robot_structure(self) -> None: + """Parse robot structure and extract joint/link information""" + self.num_joints = p.getNumJoints(self.robot_id, physicsClientId=self.physics_client) + + self.joint_indices = [] + self.joint_info = {} + self.link_info = {} + self.joint_name_to_index = {} + + for joint_index in range(self.num_joints): + joint_info = p.getJointInfo(self.robot_id, joint_index, physicsClientId=self.physics_client) + + joint_name = joint_info[1].decode('utf-8') + joint_type = joint_info[2] + + # Store joint information + self.joint_info[joint_index] = { + 'name': joint_name, + 'type': joint_type, + 'lower_limit': joint_info[8], + 'upper_limit': joint_info[9], + 'max_force': joint_info[10], + 'max_velocity': joint_info[11], + 'axis': joint_info[13] + } + + self.joint_name_to_index[joint_name] = joint_index + + # Only include revolute and prismatic joints as controllable + if joint_type in [p.JOINT_REVOLUTE, p.JOINT_PRISMATIC]: + self.joint_indices.append(joint_index) + + # Find end effector (last link) + if self.num_joints > 0: + self.end_effector_index = self.num_joints - 1 + + def get_joint_states(self) -> Dict[str, Dict[str, float]]: + """Get current states of all controllable joints""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + joint_states = {} + + for joint_index in self.joint_indices: + state = p.getJointState(self.robot_id, joint_index, physicsClientId=self.physics_client) + joint_name = self.joint_info[joint_index]['name'] + + joint_states[joint_name] = { + 'position': state[0], + 'velocity': state[1], + 'reaction_forces': state[2], + 'applied_torque': state[3] + } + + return joint_states + + def get_joint_limits(self) -> Dict[str, Tuple[float, float]]: + """Get joint limits for all controllable joints""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + joint_limits = {} + + for joint_index in self.joint_indices: + joint_name = self.joint_info[joint_index]['name'] + lower_limit = self.joint_info[joint_index]['lower_limit'] + upper_limit = self.joint_info[joint_index]['upper_limit'] + + joint_limits[joint_name] = (lower_limit, upper_limit) + + return joint_limits + + def get_end_effector_pose(self) -> Tuple[List[float], List[float]]: + """Get end effector pose (position and orientation)""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + if self.end_effector_index is None: + raise RuntimeError("End effector not identified") + + link_state = p.getLinkState( + self.robot_id, + self.end_effector_index, + physicsClientId=self.physics_client + ) + + position = list(link_state[0]) + orientation = list(link_state[1]) + + return position, orientation + + def get_link_state(self, link_index: int) -> Dict[str, Any]: + """Get state of a specific link""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + link_state = p.getLinkState( + self.robot_id, + link_index, + physicsClientId=self.physics_client + ) + + return { + 'position': list(link_state[0]), + 'orientation': list(link_state[1]), + 'local_inertial_position': list(link_state[2]), + 'local_inertial_orientation': list(link_state[3]), + 'world_position': list(link_state[4]), + 'world_orientation': list(link_state[5]) + } + + def set_joint_positions(self, joint_positions: List[float]) -> None: + """Set target positions for all controllable joints""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + if len(joint_positions) != len(self.joint_indices): + raise ValueError(f"Expected {len(self.joint_indices)} joint positions, got {len(joint_positions)}") + + # Validate joint positions + if not self.validate_joint_positions(joint_positions): + raise ValueError("Joint positions exceed limits") + + # Set joint positions + for i, joint_index in enumerate(self.joint_indices): + p.setJointMotorControl2( + self.robot_id, + joint_index, + p.POSITION_CONTROL, + targetPosition=joint_positions[i], + physicsClientId=self.physics_client + ) + + def set_joint_velocities(self, joint_velocities: List[float]) -> None: + """Set target velocities for all controllable joints""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + if len(joint_velocities) != len(self.joint_indices): + raise ValueError(f"Expected {len(self.joint_indices)} joint velocities, got {len(joint_velocities)}") + + # Set joint velocities + for i, joint_index in enumerate(self.joint_indices): + p.setJointMotorControl2( + self.robot_id, + joint_index, + p.VELOCITY_CONTROL, + targetVelocity=joint_velocities[i], + physicsClientId=self.physics_client + ) + + def reset_robot(self) -> None: + """Reset robot to initial configuration""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + # Get initial positions from config or use zeros + initial_positions = self.robot_config.get('initial_joint_positions', [0.0] * len(self.joint_indices)) + + # Ensure initial positions list matches joint count + if len(initial_positions) != len(self.joint_indices): + initial_positions = [0.0] * len(self.joint_indices) + + for i, joint_index in enumerate(self.joint_indices): + p.resetJointState( + self.robot_id, + joint_index, + initial_positions[i], + physicsClientId=self.physics_client + ) + + def validate_joint_positions(self, positions: List[float]) -> bool: + """Validate if joint positions are within limits""" + if len(positions) != len(self.joint_indices): + return False + + for i, joint_index in enumerate(self.joint_indices): + lower_limit = self.joint_info[joint_index]['lower_limit'] + upper_limit = self.joint_info[joint_index]['upper_limit'] + + # Skip validation for unlimited joints + if lower_limit == 0.0 and upper_limit == -1.0: + continue + + if positions[i] < lower_limit or positions[i] > upper_limit: + return False + + return True + + def check_self_collision(self) -> bool: + """Check if robot has self-collision""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + # Get contact points for self-collision detection + contact_points = p.getContactPoints( + bodyA=self.robot_id, + bodyB=self.robot_id, + physicsClientId=self.physics_client + ) + + return len(contact_points) > 0 + + def get_joint_names(self) -> List[str]: + """Get names of all controllable joints""" + return [self.joint_info[idx]['name'] for idx in self.joint_indices] + + def get_joint_info(self) -> Dict[int, Dict[str, Any]]: + """Get complete joint information""" + return self.joint_info.copy() + + def get_robot_id(self) -> Optional[int]: + """Get pybullet robot ID""" + return self.robot_id + + def get_dof(self) -> int: + """Get number of degrees of freedom""" + return len(self.joint_indices) + + def enable_torque_control(self) -> None: + """Switch to torque control mode""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + for joint_index in self.joint_indices: + p.setJointMotorControl2( + self.robot_id, + joint_index, + p.TORQUE_CONTROL, + force=0, + physicsClientId=self.physics_client + ) + + def enable_position_control(self) -> None: + """Switch to position control mode""" + if self.robot_id is None: + raise RuntimeError("Robot not loaded. Call load_robot() first.") + + # Get current positions + current_states = self.get_joint_states() + current_positions = [state['position'] for state in current_states.values()] + + # Set position control with current positions as targets + self.set_joint_positions(current_positions) \ No newline at end of file diff --git a/src/simulation/environment.py b/src/simulation/environment.py index e69de29..fa853af 100644 --- a/src/simulation/environment.py +++ b/src/simulation/environment.py @@ -0,0 +1,317 @@ +import pybullet as p +import pybullet_data +from typing import Dict, List, Any, Optional, Tuple +from ..config_loader import ConfigLoader + + +class Environment: + def __init__(self, config_loader: ConfigLoader, physics_client: int): + self.config_loader = config_loader + self.physics_client = physics_client + + # Get configurations + self.wall_config = config_loader.get_wall_config() + self.hole_config = config_loader.get_hole_config() + self.transport_object_config = config_loader.get_transport_object_config() + self.simulation_config = config_loader.get_simulation_config() + + # Environment object IDs + self.wall_id: Optional[int] = None + self.transport_object_id: Optional[int] = None + self.ground_plane_id: Optional[int] = None + + def setup_simulation(self) -> None: + """Setup basic simulation environment""" + # Set additional search path for pybullet data + p.setAdditionalSearchPath(pybullet_data.getDataPath()) + + # Set gravity + gravity = self.simulation_config['gravity'] + p.setGravity(gravity[0], gravity[1], gravity[2], physicsClientId=self.physics_client) + + # Set time step + timestep = self.simulation_config['timestep'] + p.setTimeStep(timestep, physicsClientId=self.physics_client) + + # Set real time simulation + real_time = self.simulation_config.get('real_time', False) + if real_time: + p.setRealTimeSimulation(1, physicsClientId=self.physics_client) + else: + p.setRealTimeSimulation(0, physicsClientId=self.physics_client) + + def create_ground_plane(self) -> int: + """Create ground plane""" + self.ground_plane_id = p.loadURDF( + "plane.urdf", + physicsClientId=self.physics_client + ) + return self.ground_plane_id + + def create_wall_with_hole(self) -> int: + """Create wall with hole obstacle""" + wall_pos = self.wall_config['position'] + wall_dims = self.wall_config['dimensions'] + hole_dims = self.hole_config['dimensions'] + hole_pos_rel = self.hole_config['position_relative_to_wall'] + + # Calculate wall dimensions + wall_width = wall_dims['width'] + wall_height = wall_dims['height'] + wall_thickness = wall_dims['thickness'] + + # Calculate hole dimensions + hole_width = hole_dims['width'] + hole_height = hole_dims['height'] + + # Calculate absolute hole position + hole_pos = [ + wall_pos[0] + hole_pos_rel[0], + wall_pos[1] + hole_pos_rel[1], + wall_pos[2] + hole_pos_rel[2] + ] + + # Create wall with hole using multiple boxes + wall_parts = [] + + # Wall parameters + wall_color = self.wall_config.get('material', {}).get('color', [0.8, 0.8, 0.8, 1.0]) + + # Bottom part (below hole) + bottom_height = hole_pos[2] - wall_pos[2] - hole_height / 2 + if bottom_height > 0: + bottom_pos = [wall_pos[0], wall_pos[1], wall_pos[2] + bottom_height / 2] + bottom_id = self._create_box( + [wall_width / 2, wall_thickness / 2, bottom_height / 2], + bottom_pos, + wall_color + ) + wall_parts.append(bottom_id) + + # Top part (above hole) + top_height = wall_pos[2] + wall_height - (hole_pos[2] + hole_height / 2) + if top_height > 0: + top_pos = [wall_pos[0], wall_pos[1], hole_pos[2] + hole_height / 2 + top_height / 2] + top_id = self._create_box( + [wall_width / 2, wall_thickness / 2, top_height / 2], + top_pos, + wall_color + ) + wall_parts.append(top_id) + + # Left part (left of hole) + left_width = hole_pos[0] - wall_pos[0] - hole_width / 2 + wall_width / 2 + if left_width > 0: + left_pos = [wall_pos[0] - wall_width / 2 + left_width / 2, wall_pos[1], hole_pos[2]] + left_id = self._create_box( + [left_width / 2, wall_thickness / 2, hole_height / 2], + left_pos, + wall_color + ) + wall_parts.append(left_id) + + # Right part (right of hole) + right_width = wall_pos[0] + wall_width / 2 - (hole_pos[0] + hole_width / 2) + if right_width > 0: + right_pos = [hole_pos[0] + hole_width / 2 + right_width / 2, wall_pos[1], hole_pos[2]] + right_id = self._create_box( + [right_width / 2, wall_thickness / 2, hole_height / 2], + right_pos, + wall_color + ) + wall_parts.append(right_id) + + # Store the first wall part as main wall ID + if wall_parts: + self.wall_id = wall_parts[0] + + return wall_parts + + def _create_box(self, half_extents: List[float], position: List[float], color: List[float]) -> int: + """Create a box collision shape and body""" + # Create collision shape + collision_shape = p.createCollisionShape( + p.GEOM_BOX, + halfExtents=half_extents, + physicsClientId=self.physics_client + ) + + # Create visual shape + visual_shape = p.createVisualShape( + p.GEOM_BOX, + halfExtents=half_extents, + rgbaColor=color, + physicsClientId=self.physics_client + ) + + # Create multi-body + body_id = p.createMultiBody( + baseMass=0, # Static object + baseCollisionShapeIndex=collision_shape, + baseVisualShapeIndex=visual_shape, + basePosition=position, + physicsClientId=self.physics_client + ) + + return body_id + + def create_transport_object(self) -> int: + """Create the object to be transported""" + obj_config = self.transport_object_config + + # Get object parameters + initial_pos = obj_config['initial_position'] + dimensions = obj_config['dimensions'] + mass = obj_config['mass'] + shape = obj_config.get('shape', 'cube') + color = obj_config.get('material', {}).get('color', [1.0, 0.0, 0.0, 1.0]) + + if shape == 'cube': + # Create cube + half_extents = [ + dimensions['width'] / 2, + dimensions['height'] / 2, + dimensions['depth'] / 2 + ] + + collision_shape = p.createCollisionShape( + p.GEOM_BOX, + halfExtents=half_extents, + physicsClientId=self.physics_client + ) + + visual_shape = p.createVisualShape( + p.GEOM_BOX, + halfExtents=half_extents, + rgbaColor=color, + physicsClientId=self.physics_client + ) + else: + raise ValueError(f"Unsupported transport object shape: {shape}") + + # Create multi-body + self.transport_object_id = p.createMultiBody( + baseMass=mass, + baseCollisionShapeIndex=collision_shape, + baseVisualShapeIndex=visual_shape, + basePosition=initial_pos, + physicsClientId=self.physics_client + ) + + return self.transport_object_id + + def setup_environment(self) -> Dict[str, Any]: + """Setup complete environment and return object IDs""" + # Setup basic simulation parameters + self.setup_simulation() + + # Create ground plane + ground_id = self.create_ground_plane() + + # Create wall with hole + wall_parts = self.create_wall_with_hole() + + # Create transport object + transport_obj_id = self.create_transport_object() + + return { + 'ground_plane_id': ground_id, + 'wall_parts': wall_parts, + 'transport_object_id': transport_obj_id + } + + def get_transport_object_pose(self) -> Tuple[List[float], List[float]]: + """Get current pose of transport object""" + if self.transport_object_id is None: + raise RuntimeError("Transport object not created") + + pos, orient = p.getBasePositionAndOrientation( + self.transport_object_id, + physicsClientId=self.physics_client + ) + + return list(pos), list(orient) + + def set_transport_object_pose(self, position: List[float], orientation: List[float] = None) -> None: + """Set pose of transport object""" + if self.transport_object_id is None: + raise RuntimeError("Transport object not created") + + if orientation is None: + orientation = [0, 0, 0, 1] # Default quaternion + + p.resetBasePositionAndOrientation( + self.transport_object_id, + position, + orientation, + physicsClientId=self.physics_client + ) + + def check_collision(self, body_a: int, body_b: int = None) -> bool: + """Check collision between bodies""" + if body_b is None: + # Check collision with any other body + contact_points = p.getContactPoints( + bodyA=body_a, + physicsClientId=self.physics_client + ) + else: + # Check collision between specific bodies + contact_points = p.getContactPoints( + bodyA=body_a, + bodyB=body_b, + physicsClientId=self.physics_client + ) + + return len(contact_points) > 0 + + def get_hole_info(self) -> Dict[str, Any]: + """Get hole information for path planning""" + wall_pos = self.wall_config['position'] + hole_pos_rel = self.hole_config['position_relative_to_wall'] + hole_dims = self.hole_config['dimensions'] + + # Calculate absolute hole position + hole_center = [ + wall_pos[0] + hole_pos_rel[0], + wall_pos[1] + hole_pos_rel[1], + wall_pos[2] + hole_pos_rel[2] + ] + + return { + 'center': hole_center, + 'dimensions': hole_dims, + 'shape': self.hole_config.get('shape', 'rectangle') + } + + def get_wall_info(self) -> Dict[str, Any]: + """Get wall information for path planning""" + return { + 'position': self.wall_config['position'], + 'dimensions': self.wall_config['dimensions'] + } + + def reset_environment(self) -> None: + """Reset environment to initial state""" + if self.transport_object_id is not None: + initial_pos = self.transport_object_config['initial_position'] + self.set_transport_object_pose(initial_pos) + + def cleanup(self) -> None: + """Clean up environment objects""" + objects_to_remove = [] + + if self.wall_id is not None: + objects_to_remove.append(self.wall_id) + + if self.transport_object_id is not None: + objects_to_remove.append(self.transport_object_id) + + if self.ground_plane_id is not None: + objects_to_remove.append(self.ground_plane_id) + + for obj_id in objects_to_remove: + try: + p.removeBody(obj_id, physicsClientId=self.physics_client) + except: + pass # Object might already be removed \ No newline at end of file diff --git a/tests/__init__.py b/tests/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/tests/test_config_loader.py b/tests/test_config_loader.py new file mode 100644 index 0000000..6108408 --- /dev/null +++ b/tests/test_config_loader.py @@ -0,0 +1,87 @@ +#!/usr/bin/env python3 + +import sys +import os +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +from src.config_loader import ConfigLoader + +def test_config_loader(): + print("Testing ConfigLoader...") + + # Test singleton pattern + loader1 = ConfigLoader() + loader2 = ConfigLoader() + assert loader1 is loader2, "ConfigLoader should be singleton" + print("āœ“ Singleton pattern works") + + # Test config loading + config = loader1.get_full_config() + assert 'robot' in config, "Config should contain robot section" + assert 'wall' in config, "Config should contain wall section" + assert 'hole' in config, "Config should contain hole section" + assert 'task_points' in config, "Config should contain task_points section" + assert 'transport_object' in config, "Config should contain transport_object section" + assert 'simulation' in config, "Config should contain simulation section" + print("āœ“ All required config sections present") + + # Test robot config + robot_config = loader1.get_robot_config() + assert robot_config['model_path'] == "models/manual_robot.urdf", "Robot model path should be correct" + assert len(robot_config['base_position']) == 3, "Robot base position should have 3 coordinates" + assert len(robot_config['base_orientation']) == 3, "Robot base orientation should have 3 angles" + print("āœ“ Robot config validation passed") + + # Test wall config + wall_config = loader1.get_wall_config() + assert len(wall_config['position']) == 3, "Wall position should have 3 coordinates" + assert 'dimensions' in wall_config, "Wall should have dimensions" + assert wall_config['dimensions']['width'] > 0, "Wall width should be positive" + assert wall_config['dimensions']['height'] > 0, "Wall height should be positive" + assert wall_config['dimensions']['thickness'] > 0, "Wall thickness should be positive" + print("āœ“ Wall config validation passed") + + # Test hole config + hole_config = loader1.get_hole_config() + assert len(hole_config['position_relative_to_wall']) == 3, "Hole position should have 3 coordinates" + assert 'dimensions' in hole_config, "Hole should have dimensions" + assert hole_config['dimensions']['width'] > 0, "Hole width should be positive" + assert hole_config['dimensions']['height'] > 0, "Hole height should be positive" + print("āœ“ Hole config validation passed") + + # Test task points + task_points = loader1.get_task_points() + assert 'point_A' in task_points, "Should have point_A" + assert 'point_B' in task_points, "Should have point_B" + assert len(task_points['point_A']['position']) == 3, "Point A should have 3 coordinates" + assert len(task_points['point_B']['position']) == 3, "Point B should have 3 coordinates" + print("āœ“ Task points validation passed") + + # Test transport object config + obj_config = loader1.get_transport_object_config() + assert len(obj_config['initial_position']) == 3, "Object initial position should have 3 coordinates" + assert 'dimensions' in obj_config, "Object should have dimensions" + assert obj_config['mass'] > 0, "Object mass should be positive" + print("āœ“ Transport object config validation passed") + + # Test simulation config + sim_config = loader1.get_simulation_config() + assert sim_config['timestep'] > 0, "Timestep should be positive" + assert len(sim_config['gravity']) == 3, "Gravity should have 3 components" + print("āœ“ Simulation config validation passed") + + print("\nāœ… All tests passed! ConfigLoader is working correctly.") + + # Print sample data + print("\nSample configuration data:") + print(f"Robot model: {robot_config['model_path']}") + print(f"Wall position: {wall_config['position']}") + print(f"Wall dimensions: {wall_config['dimensions']}") + print(f"Hole dimensions: {hole_config['dimensions']}") + print(f"Point A: {task_points['point_A']['position']}") + print(f"Point B: {task_points['point_B']['position']}") + print(f"Object mass: {obj_config['mass']} kg") + print(f"Simulation timestep: {sim_config['timestep']} s") + +if __name__ == "__main__": + test_config_loader() \ No newline at end of file diff --git a/tests/test_environment.py b/tests/test_environment.py new file mode 100644 index 0000000..15dba0a --- /dev/null +++ b/tests/test_environment.py @@ -0,0 +1,94 @@ +#!/usr/bin/env python3 + +import sys +import os +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +import pybullet as p +from src.config_loader import ConfigLoader +from src.simulation.environment import Environment + +def test_environment(): + print("Testing Environment...") + + # Initialize pybullet in DIRECT mode for testing + physics_client = p.connect(p.DIRECT) + + try: + # Test environment initialization + config_loader = ConfigLoader() + environment = Environment(config_loader, physics_client) + print("āœ“ Environment initialization successful") + + # Test environment setup + env_objects = environment.setup_environment() + print("āœ“ Environment setup successful") + + # Verify all objects were created + assert 'ground_plane_id' in env_objects, "Ground plane should be created" + assert 'wall_parts' in env_objects, "Wall parts should be created" + assert 'transport_object_id' in env_objects, "Transport object should be created" + print(f"āœ“ Created objects: ground={env_objects['ground_plane_id']}, wall_parts={len(env_objects['wall_parts'])}, object={env_objects['transport_object_id']}") + + # Test transport object pose + obj_pos, obj_orient = environment.get_transport_object_pose() + assert len(obj_pos) == 3, "Object position should have 3 coordinates" + assert len(obj_orient) == 4, "Object orientation should have 4 quaternion values" + print(f"āœ“ Transport object pose: pos={obj_pos}, orient={obj_orient}") + + # Test setting object pose + new_pos = [1.5, 0.0, 0.5] + environment.set_transport_object_pose(new_pos) + updated_pos, _ = environment.get_transport_object_pose() + assert abs(updated_pos[0] - new_pos[0]) < 0.01, "Object position should be updated" + print("āœ“ Transport object pose setting works") + + # Test hole info + hole_info = environment.get_hole_info() + assert 'center' in hole_info, "Hole info should include center" + assert 'dimensions' in hole_info, "Hole info should include dimensions" + assert 'shape' in hole_info, "Hole info should include shape" + print(f"āœ“ Hole info: center={hole_info['center']}, dims={hole_info['dimensions']}") + + # Test wall info + wall_info = environment.get_wall_info() + assert 'position' in wall_info, "Wall info should include position" + assert 'dimensions' in wall_info, "Wall info should include dimensions" + print(f"āœ“ Wall info: pos={wall_info['position']}, dims={wall_info['dimensions']}") + + # Test environment reset + environment.reset_environment() + reset_pos, _ = environment.get_transport_object_pose() + expected_pos = config_loader.get_transport_object_config()['initial_position'] + assert abs(reset_pos[0] - expected_pos[0]) < 0.01, "Object should be reset to initial position" + print("āœ“ Environment reset successful") + + # Test collision detection + ground_id = env_objects['ground_plane_id'] + transport_id = env_objects['transport_object_id'] + has_collision = environment.check_collision(transport_id, ground_id) + print(f"āœ“ Collision detection: {has_collision}") + + # Test cleanup + environment.cleanup() + print("āœ“ Environment cleanup successful") + + print("\nāœ… All Environment tests passed!") + + # Print environment information + print(f"\nEnvironment Information:") + print(f"- Wall parts created: {len(env_objects['wall_parts'])}") + print(f"- Transport object mass: {config_loader.get_transport_object_config()['mass']} kg") + print(f"- Hole dimensions: {hole_info['dimensions']}") + print(f"- Wall dimensions: {wall_info['dimensions']}") + + except Exception as e: + print(f"āŒ Test failed: {e}") + raise + finally: + # Cleanup + p.disconnect(physics_client) + print("āœ“ PyBullet disconnected") + +if __name__ == "__main__": + test_environment() \ No newline at end of file diff --git a/tests/test_robot_loader.py b/tests/test_robot_loader.py new file mode 100644 index 0000000..89619ce --- /dev/null +++ b/tests/test_robot_loader.py @@ -0,0 +1,96 @@ +#!/usr/bin/env python3 + +import sys +import os +sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) + +import pybullet as p +from src.config_loader import ConfigLoader +from src.robot.robot_loader import RobotLoader + +def test_robot_loader(): + print("Testing RobotLoader...") + + # Initialize pybullet in DIRECT mode for testing + physics_client = p.connect(p.DIRECT) + + try: + # Test config loading + config_loader = ConfigLoader() + robot_loader = RobotLoader(config_loader, physics_client) + print("āœ“ RobotLoader initialization successful") + + # Test robot loading + robot_id = robot_loader.load_robot() + assert robot_id is not None, "Robot ID should not be None" + print(f"āœ“ Robot loaded successfully with ID: {robot_id}") + + # Test DOF + dof = robot_loader.get_dof() + print(f"āœ“ Robot has {dof} degrees of freedom") + + # Test joint names + joint_names = robot_loader.get_joint_names() + assert len(joint_names) == dof, "Joint names count should match DOF" + print(f"āœ“ Joint names: {joint_names[:3]}..." if len(joint_names) > 3 else f"āœ“ Joint names: {joint_names}") + + # Test joint limits + joint_limits = robot_loader.get_joint_limits() + assert len(joint_limits) == dof, "Joint limits count should match DOF" + print("āœ“ Joint limits retrieved successfully") + + # Test joint states + joint_states = robot_loader.get_joint_states() + assert len(joint_states) == dof, "Joint states count should match DOF" + print("āœ“ Joint states retrieved successfully") + + # Test end effector pose + ee_pos, ee_orient = robot_loader.get_end_effector_pose() + assert len(ee_pos) == 3, "End effector position should have 3 coordinates" + assert len(ee_orient) == 4, "End effector orientation should have 4 quaternion values" + print(f"āœ“ End effector pose: pos={ee_pos}, orient={ee_orient}") + + # Test joint position validation + zero_positions = [0.0] * dof + is_valid = robot_loader.validate_joint_positions(zero_positions) + print(f"āœ“ Zero positions validation: {is_valid}") + + # Test setting joint positions + robot_loader.set_joint_positions(zero_positions) + print("āœ“ Joint positions set successfully") + + # Test robot reset + robot_loader.reset_robot() + print("āœ“ Robot reset successful") + + # Test self-collision check + has_collision = robot_loader.check_self_collision() + print(f"āœ“ Self-collision check: {has_collision}") + + # Test control mode switching + robot_loader.enable_position_control() + print("āœ“ Position control mode enabled") + + robot_loader.enable_torque_control() + print("āœ“ Torque control mode enabled") + + print("\nāœ… All RobotLoader tests passed!") + + # Print robot information + print(f"\nRobot Information:") + print(f"- DOF: {dof}") + print(f"- Robot ID: {robot_id}") + print(f"- Joint count: {robot_loader.num_joints}") + print(f"- Controllable joints: {len(robot_loader.joint_indices)}") + print(f"- End effector index: {robot_loader.end_effector_index}") + + except Exception as e: + print(f"āŒ Test failed: {e}") + raise + finally: + # Cleanup + p.disconnect(physics_client) + print("āœ“ PyBullet disconnected") + +if __name__ == "__main__": + test_robot_loader() \ No newline at end of file