From 722de584d2ccb6c8c4e01de95458e73c760e656d Mon Sep 17 00:00:00 2001 From: tangger Date: Sun, 13 Apr 2025 21:43:26 +0800 Subject: [PATCH] =?UTF-8?q?=E5=BD=93=E5=89=8D=E4=BB=A3=E7=A0=81=E6=9C=89?= =?UTF-8?q?=E9=97=AE=E9=A2=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lerobot_aloha/README.MD | 17 +++- .../__pycache__/agilex_robot.cpython-310.pyc | Bin 8573 -> 8516 bytes .../robot_components.cpython-310.pyc | Bin 13714 -> 13478 bytes lerobot_aloha/common/agilex_robot.py | 93 +++++++++--------- lerobot_aloha/common/robot_components.py | 32 +++--- .../__pycache__/control_utils.cpython-310.pyc | Bin 7078 -> 7078 bytes lerobot_aloha/common/utils/control_utils.py | 4 +- lerobot_aloha/configs/agilex.yaml | 84 ++++++++-------- lerobot_aloha/replay_data.py | 90 ++++++++++++----- lerobot_aloha/test.py | 21 +++- 10 files changed, 209 insertions(+), 132 deletions(-) diff --git a/lerobot_aloha/README.MD b/lerobot_aloha/README.MD index 87ace42..331ad20 100644 --- a/lerobot_aloha/README.MD +++ b/lerobot_aloha/README.MD @@ -1,5 +1,18 @@ export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libtiff.so.5 -python collect_data.py --robot.type=aloha --control.type=record --control.fps=30 --control.single_task="Grasp a lego block and put it in the bin." --control.repo_id=tangger/test --control.num_episodes=1 --control.root=./data +# fd token +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 -python lerobot/scripts/train.py --dataset.repo_id=maic/move_tube_on_scale --policy.type=act --output_dir=outputs/train/act_move_tube_on_scale --job_name=act_move_tube_on_scale --policy.device=cuda --wandb.enable=true --dataset.root=/home/ubuntu/LYT/aloha_lerobot/data1 \ No newline at end of file +# act +python lerobot/lerobot/scripts/train.py --policy.type=act --policy.device=cuda --wandb.enable=true --dataset.root=/home/ubuntu/LYT/lerobot_aloha/datasets/move_a_reagent_bottle_on_the_scale_with_head_camera/ --dataset.repo_id=maic/move_tube_on_scale_head --job_name=act_with_head --output_dir=outputs/train/act_move_bottle_on_scale_with_head + +python lerobot/lerobot/scripts/visualize_dataset_html.py --root /home/ubuntu/LYT/lerobot_aloha/datasets/move_a_reagent_bottle_on_a_scale_without_head_camera --repo-id xxx + +# pi0 ft +python lerobot/lerobot/scripts/train.py \ +--policy.path=lerobot/pi0 \ +--wandb.enable=true \ +--dataset.root=/home/ubuntu/LYT/lerobot_aloha/datasets/move_a_reagent_bottle_on_a_scale_without_head_camera \ +--dataset.repo_id=maic/move_a_reagent_bottle_on_a_scale_without_head_camera \ +--job_name=pi0_without_head \ +--output_dir=outputs/train/move_a_reagent_bottle_on_a_scale_without_head_camera diff --git a/lerobot_aloha/common/__pycache__/agilex_robot.cpython-310.pyc b/lerobot_aloha/common/__pycache__/agilex_robot.cpython-310.pyc index 5f8b55487c9a218caae468881ff7477941c9a25d..95e6da7856a50efbbcd42abf9b7c7b305f52c469 100644 GIT binary patch delta 2513 zcmaJ?du$YC5Z~F??QQSfyX#$BdZkop)d(sCjn$$Eia>a&=n1sf+iSV&W7}`HjpCkH z#Yj+z=$A-PiFZO`G#bzn6JmVC82{2l;{!ea@qsb?X-s@Z>&)H(MXkHp-}lYTH#5JR zo!R^I#`kXYn|{A8;PqDjA4BiI>VF*qB+|1AAL|4DF+8pnCBtX<9}@T%Vrys*6lF&! z$gJSVWv>z72gKx(Y1@OkhCN_t!!nyji;rX@m=p^BLZBS12o(Uq4mmRVD;Q`&DO^+? zb?ymAaioMWEU+UW81hzO;z>8pTZl?otOz#=JLdO!v+naUp4Qo^3;3M+Cl|_8a#Nk6 zM0va&#Rxqs73pU{IqW7|v0W@`MDhSIFH+Uc|CqB>YnF1PB7pFJ=d7PyYZ9Y3mZOe} z9wosDS478SM3RD|^#~K+qEW_x1zeRng*9_1tecktTINug_nfuDC9$hUqEvGt5=E~a zE5seI(TKNZXec$|8r|r)9Mlv5ogenLd%dnLrSL4IsSBGViQSUQUoR&Z#SPo$w@`LWPic!2*Fvf&Xv7M_A9`1j#1bum_nb@9f?tuuQg z*FwW;lIa`8ZsH#{wlv&~+p3fw9b&iepBl63B+AxNzCHRFX#B_M26&10#+HRwV&AGb z(nP|M*nq8+0}d)3Fcf4bvtd^wyBd}FGTYTCuEv8Jkvgvo?&l(k zp?yWaEtTbpPz>Nnl}c6&Izf!+b$rhVhq0$8c4K*h?1Z5^;*`KnV2lR+T#zPyJl=G7 zh^kzSI1zO8opXE^9mDJs!|ytNf{y;AP;9i7Ld0n-&>bzr>brQMX-GtiZpiyZj1oYy zfTa%3^fV=)w4MsLfnXED?x;Cn<=9ll&YNSGPvu6;Y~H?icVw>%hT$Pf$=WC;v}?kHPH`y9a`xGq`6f|(z)m)6%SYAe64tu>yOGievrvOC7EBU_PR8-J~RRd3p7*+$=}HDq0mz^G}` fLF8kZ?7Y@PT8)_&GY3qYdCA=!q*16Vu5cK6KeMsP~i zH9{1^#$$XzWNI{lgqWC3fcU}-CcdGG@uB#jPikVK55^b_;{Q(Hs8r`7zUtzrHD8`eBtHLvKIHESQo?fl|;8)g|&Z!-Wv$`h#d(8_V@3Tt8Qmv`n{<;VSiaJJ6e};!7lcI&G-c~wAitx2MgC_ zOJk9gWVu&M;|;W_)4;bOpT?rAn+1&2Yecv5VRRTU@NmLuFwzfFR&_I8N?1B;!LQCp zYQ^8!DL2tJCo>J>5scGlu{EUcMS6L@)gt}q|I%lgR!kko)beTxQ}4voUW`0HcN0iP zz3;)AO|Jr8C&O$}=W_vry1?;3q6C&s8Io*EVlS5S(QNs?N-zs107I)YX(@a#Bfx7m~MwvRLX{ z3MXd@zE716K3x2dX53z2cGwFoUmfg446zQ_LUCM;AMJnKwfpb0(`p0LGM7X+)Vj14 zC9DL>Yn|fy@uX>oRgzKfXBswZuoSYxb!_Ux66*WKP^epxjij(b1A!#QyDV*3#`>ki zmC*NaM4S#E0ZVuyTc9QeBPN^>Uq>e4w0Jh!r&0AVMLD{4@y%G}ppoa3S+iIfT{4*; zspN|0k(<%E+Whcvg_|>%qpQHTo&+?y<&SEbk zo1exkaF;DO%XS>g9p*P!(2^%5egm`M6*rq@5l-D-4L9dkjUb<9g>h3OEL4YT)QaNl z!gDwx_jBy9BC{lykR-})vZzCH6G`y=R$EM0sl{q3#qdify*X+vB?~fIyi7eMBS8x; z4RDuZGl|wD)V9$^Jclsk;rT4fWO+Gr)Snrv7#U_}jC9f9&y&kef)@xD5^O;j@;ec^ zY&p-fnWOsMfL`)Q5o{$lpX25ny9|Gn)Po59DZW{fY7$>bnmq^&)hw3t4gXl44`*_f ziBU7p`7W})M6j3OWr87sR|uXU*hjFRU;v?^{IG%b0M#Sf-UhQ{O8+*l#)WW_tRenQN zf}qOHC#LAK7v&B$sL*8*rm+%KM7r(q?iVpGCnMbX1aY9@=9Smc6zYk zy_OeWlA^Oc4VrkSy$9UlKzqD%$^<)Qw%P5LMt8@cgmOm#->10C`po<7AHoI`Gyd#Q z{A6D(ID@y|d6tuUUYNMw;D{5V+?k{@zwyE5$QmcfcTWb^G}FrOUFDN#Yv!IO`W{s? z8My$#4upnQu9y{Wh~GMwK|}O(#bB3M(Y4g&Y_J&ZT3&MY{O&e1ykipwM~cRwJU1Hd z+3}n<$xvq6^!{LTYpKpdfsw3XX71cjLq11rUQI*#Ft3y|MfTc|2y~}#620Ave9aD^ zT;pPQcQ@e*lkR*k_OH}%nG<6p`G(4hIkVZVuTwyofUYurLM%xSbmVj^ zu}7+T1DyU#^~d?sWTW>C-z$!!*Tous!(^GVQ81PuaO#1E_$D1sG<04uc;#TllVG1w=-c;R^^!1;@LA?smQFF0}X7b8nfO zylNoC4>ETI6BY#>FB0_wHYUUbqlt;eFGfG`;YJ~TG{(fl$OjXm&;OKdh%s@=etquq zJm)#*KIebVoji0^^BaD@Zyx<^Pyd;{eZwF0Fk8etcUYUa@3CTS6tO)Sbyz9pEWVJQ z%BX?WAJL!72GfjV|JycYgS4(GHSxX z7a+Pp+^DS))zvpPJ%z`o2^8wX7~eoKZ4PlQt@4DYoc$#H-n^MjSz`s&_6{kzTv{1A z#AgTF!sWl&j>nlYmD`HxoMw(tm5RA4d=sU}VRq?f|7&cOoYIpsvYI|@*xsy4>s0b8 zUyr=ynAfG{3%P{zK@BlIf7{;G<#{&J&yED+Tamj8K0oh--34?5JwP=O0@eWi1ly_U zmhDO#MviZRGYJd`>S*ISGLd5j2;O-v7I1{BBc4!IppvgEoeb*ERr`?r0^lIf;ts%0 z5p3s>l7Dp521QdSytoxPvm=v<>hebIhWiPiL+lAP)ytP&V2p)uL+h zRxuG;%65zG!5Z;R=qoqM*>&QxNL}fp+Kr5*#cy@BRT#}>$t@xnZf5Jn>TnMm5!vuB zRb_X1ru~taTlV-M3NlTci}bs7#B$R=L>SvAeym^YR^iBqis)kjxlwM~D3x=*Mm!#k zMa$_(k(!c9l_K3N3ejxSTzE5bWcX!RGAA$YMW>gb9Z8a`1^+Nm1P&8sq&g-S$i;Vy zK4h)pSi>kgUb@%dWprl#YFt-uyNk4ytJm<;82Acs3V2m)kF98t?U+3eG?85CaU{s) z8M62h@p0@HJ0l8B&Fq-;92HlZ)~Zrd?3qN(f6EGDDQoKxtPwy1q||o8$^#sz0NlU{ z;0SOOnq#}fzUDabWH2NqoA1&#-}f|@&bI7j!CA#ft?~VGeS@MpzB9N71@;1vVL8+= zNT!F;qe4cK4xfB|x%VC7QhX81qJsD~{y95{Tc9+(B**&RK!Hi1K(Jj3&!@P6>CP*r zrSfE=YVKm;ZzA(yttFQ>#-*N;i1ZR399i3oQaIDLPvIgNj};|q+msq({$ zr5SpMX(^VPyzYFcN6_@ly(JZQ35~CRhupoiKcMCL8kV)B}!|ZD@ z)j4q-`)Rv&Xrv6MZj9P)%Sf5(knMV&TFIY7v`RBm=19@X7^Aw4uf(Xr^|C@_aV`BXJf*Qa1=Zn$Ex2fmcuI;kK#3uc{bI&>V zckjFBcka8R&K!4ZZg*`B{675o&-DF!Zm*q4ti}EV>0onqqoo5Prah^oESUqXf#MG5MWI`A$~i4Jk<|)%*h-i1D1d3Bnx3DKoN7)rVmU3TgaulU z(SCMsSp##q?(G@C;Z6VuZHI?cJ6wqC397~w8n&0Rp9H_@$m;2sF_BkHM?%hI;&S3R ztxT4f)qTGUhh-Y)P$8aC^)XBdx$b<*Y)t zVO|)6RyCW?ge~(NY<9`e$VTwPIy>NTFihwm=59crn?}&?M(9E4MQ|bb5H=w^0bp9x zoMBqynwFuv(YXiVX$B|S+y^GUiS7Y#)L4l}@VNqZU!BKE`-0ZJL!OwZ!PNrW>9FR=YBoh$iqRQdrH;yWG3 z1irmACpXx~Ef2^P%ZFOYN$x4IcSBngt|>AYZuoCmp;$`VN}zQVK}O(Gi=w3<97C`n zhzO?;41@w|j_hMnYa8G>uaC{N{s?8Bvo{x~SB?-b9#K_m^jW^Y5$0-p#=9Rw2M|z( zrBKrdn05j~35+N-AAfyb`!05?Z3T%T1N*k^a}vi}pg6laLw3A`1Q!v;03@qSvoZQI znqpSg4TVO-b=8Z7Uc$_UT8pk}ZsU65jr2H#=UdivAQkqsR4Y>!^#{mh_TCzeOf!G` zU!(YTlhy&6ZKcO?lA|PrSCGFV1*+i?6-e_4c(x#q6KGR_y8@j?yB=Yo4Ar$qf;>`H z;8o1w3cSL8TKizW-lteG+!I1wB=9K9KP;FX{&;KYU$LfjeN$q8&FPwBHMdC8ew)xI z%68IvTR3A$3Wjjn0x=7Y&yy5b)v-#FwwspIBXa9B3OtkbT3lD?N!d`f+-S*)P81(+1Mtv1& zFC*|Cy@J-F&csdA8HSyCfu%Y_fY6!68=aq!rW+W08=!ofEI~)fi~A{_U;l+b2HD<7 zigdG&B3GwSxRUjtnpI3oP8*j*LyPH3Lb4u!TF|{1tyA@wK2|W2+IUXF&*8XCbEQ_` z%qwt>!w78%qX_K?cxRe6J)kg3=|K$JCt!=pp3DC_E zoZpSmgV2k>Kb8B?x`5yVFm0)fCL1)$MthqA^g6hXVm%#T;McKH2)OBW_DxSS+0K6L c*#OH0d)Egy<9trXugEIrbvRZ(o9ONO7ZF_w00000 diff --git a/lerobot_aloha/common/agilex_robot.py b/lerobot_aloha/common/agilex_robot.py index e7d11c5..be115da 100644 --- a/lerobot_aloha/common/agilex_robot.py +++ b/lerobot_aloha/common/agilex_robot.py @@ -42,7 +42,9 @@ class AgilexRobot(Robot): if arm_name not in self.sync_arm_queues or len(self.sync_arm_queues[arm_name]) == 0: print(f"can not get data from {arm_name} topic") return None - + + # 时间戳误差 + tolerance = 0.1 # 允许 100ms 的时间戳偏差 # 计算最小时间戳 timestamps = [ q[-1].header.stamp.to_sec() @@ -58,18 +60,16 @@ class AgilexRobot(Robot): min_time = min(timestamps) - # 检查数据同步性 + # 检查数据同步性（允许 100ms 偏差） for queue in list(self.sync_img_queues.values()) + list(self.sync_arm_queues.values()): - if queue[-1].header.stamp.to_sec() < min_time: + if queue[-1].header.stamp.to_sec() < min_time - tolerance: return None - if self.use_depth_image: for queue in self.sync_depth_queues.values(): - if queue[-1].header.stamp.to_sec() < min_time: + if queue[-1].header.stamp.to_sec() < min_time - tolerance: return None - if self.use_robot_base and len(self.sync_base_queue) > 0: - if self.sync_base_queue[-1].header.stamp.to_sec() < min_time: + if self.sync_base_queue[-1].header.stamp.to_sec() < min_time - tolerance: return None # 提取同步数据 @@ -81,33 +81,35 @@ class AgilexRobot(Robot): # 图像数据 for cam_name, queue in self.sync_img_queues.items(): - while queue[0].header.stamp.to_sec() < min_time: + while queue and queue[0].header.stamp.to_sec() < min_time - tolerance: queue.popleft() - frame_data['images'][cam_name] = self.bridge.imgmsg_to_cv2(queue.popleft(), 'passthrough') - + if queue: + frame_data['images'][cam_name] = self.bridge.imgmsg_to_cv2(queue.popleft(), 'passthrough') + # 深度数据 if self.use_depth_image: - frame_data['depths'] = {} for cam_name, queue in self.sync_depth_queues.items(): - while queue[0].header.stamp.to_sec() < min_time: + while queue and queue[0].header.stamp.to_sec() < min_time - tolerance: queue.popleft() - depth_img = self.bridge.imgmsg_to_cv2(queue.popleft(), 'passthrough') - # 保持原有的边界填充 - frame_data['depths'][cam_name] = cv2.copyMakeBorder( - depth_img, 40, 40, 0, 0, cv2.BORDER_CONSTANT, value=0 - ) - + if queue: + depth_img = self.bridge.imgmsg_to_cv2(queue.popleft(), 'passthrough') + frame_data['depths'][cam_name] = cv2.copyMakeBorder( + depth_img, 40, 40, 0, 0, cv2.BORDER_CONSTANT, value=0 + ) + # 机械臂数据 for arm_name, queue in self.sync_arm_queues.items(): - while queue[0].header.stamp.to_sec() < min_time: + while queue and queue[0].header.stamp.to_sec() < min_time - tolerance: queue.popleft() - frame_data['arms'][arm_name] = queue.popleft() - + if queue: + frame_data['arms'][arm_name] = queue.popleft() + # 基座数据 if self.use_robot_base and len(self.sync_base_queue) > 0: - while self.sync_base_queue[0].header.stamp.to_sec() < min_time: + while self.sync_base_queue and self.sync_base_queue[0].header.stamp.to_sec() < min_time - tolerance: self.sync_base_queue.popleft() - frame_data['base'] = self.sync_base_queue.popleft() + if self.sync_base_queue: + frame_data['base'] = self.sync_base_queue.popleft() return frame_data @@ -210,11 +212,11 @@ class AgilexRobot(Robot): if arm_states: obs_dict["observation.state"] = torch.tensor(np.concatenate(arm_states).reshape(-1)) # 先转Python列表 - if arm_velocity: - obs_dict["observation.velocity"] = torch.tensor(np.concatenate(arm_velocity).reshape(-1)) + # if arm_velocity: + # obs_dict["observation.velocity"] = torch.tensor(np.concatenate(arm_velocity).reshape(-1)) - if arm_effort: - obs_dict["observation.effort"] = torch.tensor(np.concatenate(arm_effort).reshape(-1)) + # if arm_effort: + # obs_dict["observation.effort"] = torch.tensor(np.concatenate(arm_effort).reshape(-1)) if actions: action_dict["action"] = torch.tensor(np.concatenate(actions).reshape(-1)) @@ -276,7 +278,7 @@ class AgilexRobot(Robot): # Log timing information # self.logs[f"read_arm_{arm_name}_pos_dt_s"] = time.perf_counter() - before_read_t - print(f"read_arm_{arm_name}_pos_dt_s is", time.perf_counter() - before_read_t) + # print(f"read_arm_{arm_name}_pos_dt_s is", time.perf_counter() - before_read_t) # Combine all arm states into single tensor if arm_states: @@ -299,7 +301,7 @@ class AgilexRobot(Robot): # Log timing information # self.logs[f"read_camera_{cam_name}_dt_s"] = time.perf_counter() - before_camread_t - print(f"read_camera_{cam_name}_dt_s is", time.perf_counter() - before_camread_t) + # print(f"read_camera_{cam_name}_dt_s is", time.perf_counter() - before_camread_t) # Process depth data if enabled if self.use_depth_image and 'depths' in frame_data: @@ -311,7 +313,7 @@ class AgilexRobot(Robot): obs_dict[f"observation.images.depth_{cam_name}"] = depth_tensor # self.logs[f"read_depth_{cam_name}_dt_s"] = time.perf_counter() - before_depthread_t - print(f"read_depth_{cam_name}_dt_s is", time.perf_counter() - before_depthread_t) + # print(f"read_depth_{cam_name}_dt_s is", time.perf_counter() - before_depthread_t) # Process base velocity if enabled if self.use_robot_base and 'base' in frame_data: @@ -341,8 +343,8 @@ class AgilexRobot(Robot): -0.03296661376953125, -0.010990142822265625, -0.010990142822265625, -0.03296661376953125, -0.03296661376953125] - last_effort = [-0.021978378295898438, 0.2417583465576172, 4.320878982543945, - 3.6527481079101562, -0.013187408447265625, -0.013187408447265625, + last_effort = [-0.021978378295898438, 0.2417583465576172, 0.320878982543945, + 0.6527481079101562, -0.013187408447265625, -0.013187408447265625, 0.0, -0.010990142822265625, -0.010990142822265625, -0.03296661376953125, -0.010990142822265625, -0.010990142822265625, -0.03296661376953125, -0.03296661376953125] @@ -369,22 +371,23 @@ class AgilexRobot(Robot): arm_velocity = last_velocity[from_idx:to_idx] arm_effort = last_effort[from_idx:to_idx] from_idx = to_idx + + # fix + arm_action[-1] = max(arm_action[-1]*15, 0) # Apply safety checks if configured - # if 'max_relative_target' in self.config: - # # Get current position from the queue - # if len(self.sync_arm_queues[arm_name]) > 0: - # current_state = self.sync_arm_queues[arm_name][-1] - # current_pos = np.array(current_state.position) - - # # Clip the action to stay within max relative target - # max_delta = self.config['max_relative_target'] - # clipped_action = np.clip(arm_action, - # current_pos - max_delta, - # current_pos + max_delta) - # arm_action = clipped_action + + # # Get current position from the queue + # if len(arm_action) > 0: + + # # Clip the action to stay within max relative target + # max_delta = 0.1 + # clipped_action = np.clip(arm_action, + # arm_action - max_delta, + # arm_action + max_delta) + # arm_action = clipped_action - action_sent.append(arm_action) + # action_sent.append(arm_action) # Create and publish JointState message joint_state = JointState() diff --git a/lerobot_aloha/common/robot_components.py b/lerobot_aloha/common/robot_components.py index 1d3957e..a7b18af 100644 --- a/lerobot_aloha/common/robot_components.py +++ b/lerobot_aloha/common/robot_components.py @@ -323,21 +323,21 @@ class RobotActuators: "names": {"motors": state.get('motors', "")} } - if self.config.get('velocity'): - velocity = self.config.get('velocity', "") - features["observation.velocity"] = { - "dtype": "float32", - "shape": (len(velocity.get('motors', "")),), - "names": {"motors": velocity.get('motors', "")} - } + # if self.config.get('velocity'): + # velocity = self.config.get('velocity', "") + # features["observation.velocity"] = { + # "dtype": "float32", + # "shape": (len(velocity.get('motors', "")),), + # "names": {"motors": velocity.get('motors', "")} + # } - if self.config.get('effort'): - effort = self.config.get('effort', "") - features["observation.effort"] = { - "dtype": "float32", - "shape": (len(effort.get('motors', "")),), - "names": {"motors": effort.get('motors', "")} - } + # if self.config.get('effort'): + # effort = self.config.get('effort', "") + # features["observation.effort"] = { + # "dtype": "float32", + # "shape": (len(effort.get('motors', "")),), + # "names": {"motors": effort.get('motors', "")} + # } def _init_action_features(self, features: Dict[str, Any]) -> None: """Initialize action features""" @@ -393,7 +393,7 @@ class RobotDataManager: # Check camera image queues rospy.loginfo(f"Nums of camera is {len(self.sensors.cameras)}") for cam_name in self.sensors.cameras: - if len(self.sensors.sync_img_queues[cam_name]) < 50: + if len(self.sensors.sync_img_queues[cam_name]) < 200: rospy.loginfo(f"Waiting for camera {cam_name} (current: {len(self.sensors.sync_img_queues[cam_name])}/50)") all_ready = False break @@ -401,7 +401,7 @@ class RobotDataManager: # Check depth queues if enabled if self.sensors.use_depth_image: for cam_name in self.sensors.sync_depth_queues: - if len(self.sensors.sync_depth_queues[cam_name]) < 50: + if len(self.sensors.sync_depth_queues[cam_name]) < 200: rospy.loginfo(f"Waiting for depth camera {cam_name} (current: {len(self.sensors.sync_depth_queues[cam_name])}/50)") all_ready = False break diff --git a/lerobot_aloha/common/utils/__pycache__/control_utils.cpython-310.pyc b/lerobot_aloha/common/utils/__pycache__/control_utils.cpython-310.pyc index b2af2e15a85ad1a1da25170ff7b513e5c1ddf816..1d84ccd4a7bfeac831db5aebb36973c25b268fdc 100644 GIT binary patch delta 32 mcmZ2xzRa9EpO=@50SNx5ec#Bvo`l} delta 32 mcmZ2xzRa9EpO=@50SMd|e%Z*qo`-D}BLhRG!{!4#&-eh4(g}G0 diff --git a/lerobot_aloha/common/utils/control_utils.py b/lerobot_aloha/common/utils/control_utils.py index 990d60b..d54ba8f 100644 --- a/lerobot_aloha/common/utils/control_utils.py +++ b/lerobot_aloha/common/utils/control_utils.py @@ -140,8 +140,8 @@ def control_loop( if num_images > 0: # 设置每个图像的显示尺寸 - display_width = 426 # 更小的宽度 - display_height = 320 # 更小的高度 + display_width = 640 # 更小的宽度 + display_height = 480 # 更小的高度 # 确定网格布局的行列数 (尽量接近正方形布局) grid_cols = int(np.ceil(np.sqrt(num_images))) diff --git a/lerobot_aloha/configs/agilex.yaml b/lerobot_aloha/configs/agilex.yaml index f156942..ff0e433 100644 --- a/lerobot_aloha/configs/agilex.yaml +++ b/lerobot_aloha/configs/agilex.yaml @@ -2,17 +2,19 @@ robot_type: aloha_agilex ros_node_name: record_episodes cameras: cam_high: - img_topic_name: /camera/color/image_raw - depth_topic_name: /camera/depth/image_rect_raw - rgb_shape: [480, 640, 3] - width: 480 - height: 640 - cam_front: + # img_topic_name: /camera/color/image_raw + # depth_topic_name: /camera/depth/image_rect_raw img_topic_name: /camera_f/color/image_raw depth_topic_name: /camera_f/depth/image_raw + rgb_shape: [480, 640, 3] width: 480 height: 640 - rgb_shape: [480, 640, 3] + # cam_front: + # img_topic_name: /camera_f/color/image_raw + # depth_topic_name: /camera_f/depth/image_raw + # width: 480 + # height: 640 + # rgb_shape: [480, 640, 3] cam_left: img_topic_name: /camera_l/color/image_raw depth_topic_name: /camera_l/depth/image_raw @@ -92,41 +94,41 @@ state: "right_none" ] -velocity: - motors: [ - "left_joint0", - "left_joint1", - "left_joint2", - "left_joint3", - "left_joint4", - "left_joint5", - "left_none", - "right_joint0", - "right_joint1", - "right_joint2", - "right_joint3", - "right_joint4", - "right_joint5", - "right_none" - ] +# velocity: +# motors: [ +# "left_joint0", +# "left_joint1", +# "left_joint2", +# "left_joint3", +# "left_joint4", +# "left_joint5", +# "left_none", +# "right_joint0", +# "right_joint1", +# "right_joint2", +# "right_joint3", +# "right_joint4", +# "right_joint5", +# "right_none" +# ] -effort: - motors: [ - "left_joint0", - "left_joint1", - "left_joint2", - "left_joint3", - "left_joint4", - "left_joint5", - "left_none", - "right_joint0", - "right_joint1", - "right_joint2", - "right_joint3", - "right_joint4", - "right_joint5", - "right_none" - ] +# effort: +# motors: [ +# "left_joint0", +# "left_joint1", +# "left_joint2", +# "left_joint3", +# "left_joint4", +# "left_joint5", +# "left_none", +# "right_joint0", +# "right_joint1", +# "right_joint2", +# "right_joint3", +# "right_joint4", +# "right_joint5", +# "right_none" +# ] action: motors: [ diff --git a/lerobot_aloha/replay_data.py b/lerobot_aloha/replay_data.py index 8b29a2c..c894396 100644 --- a/lerobot_aloha/replay_data.py +++ b/lerobot_aloha/replay_data.py @@ -1,36 +1,78 @@ -# coding=utf-8 +from lerobot.common.policies.act.modeling_act import ACTPolicy +from lerobot.common.robot_devices.utils import busy_wait +import time import argparse -import rospy -from common.rosrobot_factory import RobotFactory -from common.utils.replay_utils import replay +from common.agilex_robot import AgilexRobot +import torch +import cv2 +from lerobot.common.datasets.lerobot_dataset import LeRobotDataset +from lerobot.common.datasets.utils import cycle def get_arguments(): - """ - Parse command line arguments. - - Returns: - argparse.Namespace: Parsed arguments - """ parser = argparse.ArgumentParser() args = parser.parse_args() - args.repo_id = "tangger/test" - args.root = "/home/ubuntu/LYT/aloha_lerobot/data1" - args.episode = 1 # replay episode args.fps = 30 + args.resume = False + args.repo_id = "tangger/test" + # args.root = "/home/ubuntu/LYT/lerobot_aloha/datasets/move_a_tube_on_the_scale_without_front" + # args.root="/home/ubuntu/LYT/aloha_lerobot/data4" + args.root = "/home/ubuntu/LYT/lerobot_aloha/datasets/abcde" + args.num_image_writer_processes = 0 + args.num_image_writer_threads_per_camera = 4 + args.video = True + args.num_episodes = 50 + args.episode_time_s = 30000 + args.play_sounds = False + args.display_cameras = True + args.single_task = "test test" args.use_depth_image = False args.use_base = False + args.push_to_hub = False + args.policy= None + args.teleoprate = False return args -if __name__ == '__main__': - args = get_arguments() - - # Initialize ROS node - rospy.init_node("replay_node") - - # Create robot instance using factory pattern - robot = RobotFactory.create(config_file="/home/ubuntu/LYT/lerobot_aloha/lerobot_aloha/configs/agilex.yaml", args=args) - - # Replay the specified episode - replay(robot, args) +cfg = get_arguments() +robot = AgilexRobot(config_file="/home/ubuntu/LYT/lerobot_aloha/lerobot_aloha/configs/agilex.yaml", args=cfg) +inference_time_s = 360 +fps = 15 +device = "cuda" # TODO: On Mac, use "mps" or "cpu" + + +dataset = LeRobotDataset( + cfg.repo_id, + root=cfg.root, +) +shuffle = True +sampler = None +dataloader = torch.utils.data.DataLoader( + dataset, + num_workers=4, + batch_size=1, + shuffle=False, + sampler=sampler, + pin_memory=device != "cpu", + drop_last=False, +) +dl_iter = cycle(dataloader) + +# 控制播放速度fps=30 +for data in dl_iter: + start_time = time.perf_counter() + action = data["action"] + # cam_high = data["observation.images.cam_high"] + # cam_left = data["observation.images.cam_left"] + # cam_right = data["observation.images.cam_right"] + # print(data) + + # Remove batch dimension + action = action.squeeze(0) + # Move to cpu, if not already the case + action = action.to("cpu") + # Order the robot to move + robot.send_action(action) + print(action) + dt_s = time.perf_counter() - start_time + busy_wait(1 / fps - dt_s) \ No newline at end of file diff --git a/lerobot_aloha/test.py b/lerobot_aloha/test.py index 7aea744..dd52230 100644 --- a/lerobot_aloha/test.py +++ b/lerobot_aloha/test.py @@ -1,9 +1,12 @@ from lerobot.common.policies.act.modeling_act import ACTPolicy +from lerobot.common.policies.diffusion.modeling_diffusion import DiffusionPolicy from lerobot.common.robot_devices.utils import busy_wait import time import argparse from common.agilex_robot import AgilexRobot import torch +import cv2 + def get_arguments(): parser = argparse.ArgumentParser() @@ -34,8 +37,13 @@ inference_time_s = 360 fps = 30 device = "cuda" # TODO: On Mac, use "mps" or "cpu" -ckpt_path = "/home/ubuntu/LYT/lerobot_aloha/outputs/train/act_move_tube_on_scale/checkpoints/last/pretrained_model" -policy = ACTPolicy.from_pretrained(ckpt_path) +# ckpt_path = "/home/ubuntu/LYT/lerobot_aloha/outputs/train/act_move_bottle_on_scale_without_front/checkpoints/last/pretrained_model" +ckpt_path ="/home/ubuntu/LYT/lerobot_aloha/outputs/train/act_abcde/checkpoints/last/pretrained_model" +policy = ACTPolicy.from_pretrained(pretrained_name_or_path=ckpt_path) + +# ckpt_path ="/home/ubuntu/LYT/lerobot_aloha/outputs/train/diffusion_abcde/checkpoints/020000/pretrained_model" +# policy = DiffusionPolicy.from_pretrained(pretrained_name_or_path=ckpt_path) + policy.to(device) for _ in range(inference_time_s * fps): @@ -46,16 +54,23 @@ for _ in range(inference_time_s * fps): if observation is None: print("Observation is None, skipping...") continue + + # visualize the image in the obervation + # cv2.imshow("observation", observation["observation.image"]) # Convert to pytorch format: channel first and float32 in [0,1] # with batch dimension for name in observation: if "image" in name: + img = observation[name].numpy() + # cv2.imshow(name, cv2.cvtColor(img, cv2.COLOR_RGB2BGR)) + # cv2.imwrite(f"{name}.png", cv2.cvtColor(img, cv2.COLOR_RGB2BGR)) observation[name] = observation[name].type(torch.float32) / 255 observation[name] = observation[name].permute(2, 0, 1).contiguous() observation[name] = observation[name].unsqueeze(0) observation[name] = observation[name].to(device) + last_pos = observation["observation.state"] # Compute the next action with the policy # based on the current observation action = policy.select_action(observation) @@ -65,6 +80,8 @@ for _ in range(inference_time_s * fps): action = action.to("cpu") # Order the robot to move robot.send_action(action) + print("left pos:", action[:7]) + print("right pos:", action[7:]) dt_s = time.perf_counter() - start_time busy_wait(1 / fps - dt_s) \ No newline at end of file