diff --git a/.gitattributes b/.gitattributes index ec9880ee7650560081a7149f9aa14dd463e1bbd7..bb5c7a7bfa492ea450483ee85d5fa74137be29f6 100644 --- a/.gitattributes +++ b/.gitattributes @@ -286,3 +286,61 @@ real/assets/h1_inspire/meshes/right_hand_link.STL filter=lfs diff=lfs merge=lfs real/assets/h1_inspire/meshes/torso_link.STL filter=lfs diff=lfs merge=lfs -text real/assets/h1_inspire/meshes/Link19_R.STL filter=lfs diff=lfs merge=lfs -text real/assets/h1_inspire/meshes/Link13_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/h1_inspire/meshes/Link18_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/h1_inspire/meshes/left_hip_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/h1_inspire/meshes/left_elbow_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_hip_yaw_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_wrist_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_hip_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_knee_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_elbow_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link15_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link22_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link12_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link20_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link18_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_shoulder_yaw_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_ankle_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_shoulder_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link17_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_shoulder_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link21_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/L_hand_base_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/pelvis.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link14_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_shoulder_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link15_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link22_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link17_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link14_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link12_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_elbow_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/logo_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_knee_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link16_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_shoulder_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link19_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_elbow_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link20_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_ankle_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/R_hand_base_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_hip_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_hand_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_shoulder_yaw_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link13_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_hip_yaw_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/wrist_yaw_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link16_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_hand_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_wrist_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/torso_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link21_L.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/right_hip_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link19_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link13_R.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_hip_pitch_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/left_elbow_roll_link.STL filter=lfs diff=lfs merge=lfs -text +real/assets/inspire_hand/meshes/Link18_R.STL filter=lfs diff=lfs merge=lfs -text +assets/media/pico.png filter=lfs diff=lfs merge=lfs -text +assets/media/teaser.jpg filter=lfs diff=lfs merge=lfs -text +assets/media/arch.png filter=lfs diff=lfs merge=lfs -text diff --git a/assets/media/arch.png b/assets/media/arch.png new file mode 100644 index 0000000000000000000000000000000000000000..08afafdc1701fe99015f5d27e215727c00bb4b97 --- /dev/null +++ b/assets/media/arch.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dfe17b017427fb258bd744b60d23d46b3a3a29d518c9a7ced28b494474c7b441 +size 200530 diff --git a/assets/media/pico.png b/assets/media/pico.png new file mode 100644 index 0000000000000000000000000000000000000000..7563eef8e3fc45e4bf73a4419e5ced6bf21dab5e --- /dev/null +++ b/assets/media/pico.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5158c8dd30f2b5dcbd6670603e393ebd67d459f4ae696a51f9ba3844f1cbfc07 +size 735689 diff --git a/assets/media/teaser.jpg b/assets/media/teaser.jpg new file mode 100644 index 0000000000000000000000000000000000000000..c28f579afc775cd0e7364e0943a1920d19332619 --- /dev/null +++ b/assets/media/teaser.jpg @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:94d0e714d732ff40e2c22b144efbb6d55c33551a3237be155e66b8992a1483c6 +size 266354 diff --git a/real/assets/h1_inspire/meshes/Link18_R.STL b/real/assets/h1_inspire/meshes/Link18_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..3e6ff9f9b7bf00e90f71f5344223ca7a6a3551b9 --- /dev/null +++ b/real/assets/h1_inspire/meshes/Link18_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9711fac7df0c09a372e1529f74b9d506d37abf3ebc3241d976ad45e5ee7e2d69 +size 182984 diff --git a/real/assets/h1_inspire/meshes/left_elbow_roll_link.STL b/real/assets/h1_inspire/meshes/left_elbow_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..cbe5ffa6607f264beca4aa909d952c6d70a80833 --- /dev/null +++ b/real/assets/h1_inspire/meshes/left_elbow_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a9e5f7c5214f584a91543d0ddfac50331ac8d6316c133140cd1d2397ae23ec50 +size 1243084 diff --git a/real/assets/h1_inspire/meshes/left_hip_pitch_link.STL b/real/assets/h1_inspire/meshes/left_hip_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..e476201842930cc5155194c5806477f596e5be46 --- /dev/null +++ b/real/assets/h1_inspire/meshes/left_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa790c31ffc100cd182f2bf4480e7bfda675edc58ed8f361b6553a4975c9de3b +size 452384 diff --git a/real/assets/inspire_hand/meshes/L_hand_base_link.STL b/real/assets/inspire_hand/meshes/L_hand_base_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..88d668d9b03e3a74577668bb7c073d3a415f27bd --- /dev/null +++ b/real/assets/inspire_hand/meshes/L_hand_base_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:33ab4e97801bd5421903a78f5977a44e477ae8653eaf32358d35633e465411e0 +size 1448084 diff --git a/real/assets/inspire_hand/meshes/Link12_L.STL b/real/assets/inspire_hand/meshes/Link12_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..894e95880667713843b8e765dad9f87b3c0c786d --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link12_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9f35b78a9cab1007ac8b73023a5862ed055d33951b5742f9a783997d38e99551 +size 917284 diff --git a/real/assets/inspire_hand/meshes/Link12_R.STL b/real/assets/inspire_hand/meshes/Link12_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..930d8478e593f5c53e6b05e13d605bd3e1c4525e --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link12_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:347212bdb22d80fb364dd889de934d6fbb935e38d1f24c682caed84f7b6ea8cb +size 921984 diff --git a/real/assets/inspire_hand/meshes/Link13_L.STL b/real/assets/inspire_hand/meshes/Link13_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..33694b12879c6d18582cd72179e4d4a94adca851 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link13_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7e93b55f2ae73fe0f62cd27e81d8a4620211068cb1c9b25e6414e802a37541ee +size 488584 diff --git a/real/assets/inspire_hand/meshes/Link13_R.STL b/real/assets/inspire_hand/meshes/Link13_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..3e75d8d23377c4fb165150f534aa4875264149a2 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link13_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:c157dfe5b666efb1c3d51342e56f5caad2fc350930ac21ee1e02d956c1034ca3 +size 483284 diff --git a/real/assets/inspire_hand/meshes/Link14_L.STL b/real/assets/inspire_hand/meshes/Link14_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..29739dcb2b3f3d77f5b490ab1e99c0d29391b8be --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link14_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b677bcdcddcd90ff4b005f83daa9e50eb4003d5e67ea4eeefbf857718716c164 +size 155184 diff --git a/real/assets/inspire_hand/meshes/Link14_R.STL b/real/assets/inspire_hand/meshes/Link14_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..6a951090c6dc5f14febb0644223eea031f69140d --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link14_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:564aa8c3d2ff0e51b75736a8b01b4c309c7098582a424ea1391ccb75125519d8 +size 155584 diff --git a/real/assets/inspire_hand/meshes/Link15_L.STL b/real/assets/inspire_hand/meshes/Link15_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..a814265e0cdd56668ea507b4b1d1abb262e1284c --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link15_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8c601e7aa8e1c91310a75190dfb6b95a8dc981b707e5f104f9ca9ecbea118b34 +size 407184 diff --git a/real/assets/inspire_hand/meshes/Link15_R.STL b/real/assets/inspire_hand/meshes/Link15_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..d8528350adb18330616c68e4c9b912b1fb119afe --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link15_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:09c120f7fb7761aa97d89240cd8cb8e30c40ee60093cbb8b1c2e13a68a8e3845 +size 411884 diff --git a/real/assets/inspire_hand/meshes/Link16_L.STL b/real/assets/inspire_hand/meshes/Link16_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..c5e50bc9a8f303c143bd49608f635ddd251522f1 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link16_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7d8508c65db44e2349f37e8ab4c057d204a40093d4733897ae5b51075b00f5e9 +size 329384 diff --git a/real/assets/inspire_hand/meshes/Link16_R.STL b/real/assets/inspire_hand/meshes/Link16_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..9b829bfacd6208764ceedcf3f83027077de1f942 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link16_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:6942ada1189cdf442124c6b8935f3e3a3ae310092a3a1414fdeb0bd6d23578bb +size 335484 diff --git a/real/assets/inspire_hand/meshes/Link17_L.STL b/real/assets/inspire_hand/meshes/Link17_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..95ff58a479e108284ab82f283505171a5f9b24fd --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link17_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ca2a68fed635eca290b234fbdc649779036ba5bac753addcb7ce6a086ad95949 +size 407184 diff --git a/real/assets/inspire_hand/meshes/Link17_R.STL b/real/assets/inspire_hand/meshes/Link17_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..6ccd11de30e6d827ee8df3a5b751162908cbf5cb --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link17_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:36fc62c609800ce6d4521ae6f3c4c9b8a732be28654f32f04893696c17e6930c +size 411884 diff --git a/real/assets/inspire_hand/meshes/Link18_L.STL b/real/assets/inspire_hand/meshes/Link18_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..3a43b7cd7e76bc3f1ebc48292deda9de241e9215 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link18_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9425aea7ebbf6d1dfb2eb0714fb44dfaea6b6a81c71d6e96c8d3950f636dfa62 +size 182684 diff --git a/real/assets/inspire_hand/meshes/Link18_R.STL b/real/assets/inspire_hand/meshes/Link18_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..3e6ff9f9b7bf00e90f71f5344223ca7a6a3551b9 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link18_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:9711fac7df0c09a372e1529f74b9d506d37abf3ebc3241d976ad45e5ee7e2d69 +size 182984 diff --git a/real/assets/inspire_hand/meshes/Link19_L.STL b/real/assets/inspire_hand/meshes/Link19_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..fe16428f9fc90f760919907916e7c5fa575c972a --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link19_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:cbb256a70c17e21c995df66807f0755dc136d0312df245112d6d4bfcd29c43a4 +size 407184 diff --git a/real/assets/inspire_hand/meshes/Link19_R.STL b/real/assets/inspire_hand/meshes/Link19_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..cc8afb42e7ae816abb02e6f05db3b2d11651b8d1 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link19_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:46619bd6a887615d7dfe850d39a6f5d8f8c5ecfa9429c567338f3a659a5d4d75 +size 411884 diff --git a/real/assets/inspire_hand/meshes/Link20_L.STL b/real/assets/inspire_hand/meshes/Link20_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..1299f4bca88772e3b03aa7d28e8b8a1801504969 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link20_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e9145af7a515bce4c1c06d16b5be8900ec3f8375fc8e34d371315ce76f600877 +size 329484 diff --git a/real/assets/inspire_hand/meshes/Link20_R.STL b/real/assets/inspire_hand/meshes/Link20_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..cfe83e5a5b1a52a54557176e11b4d24be0b8cfa8 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link20_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a517debff6b924c22ae0f87dbb6aac199a1f130c7fef353a7d659e365f30b1f4 +size 334784 diff --git a/real/assets/inspire_hand/meshes/Link21_L.STL b/real/assets/inspire_hand/meshes/Link21_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..20bc1dd7faf8820072a205416c86218dffc33b1e --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link21_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:180ab4ac01dab60407d0b33819883eb0ec869e4d4f349f786972135c7114fc0d +size 407184 diff --git a/real/assets/inspire_hand/meshes/Link21_R.STL b/real/assets/inspire_hand/meshes/Link21_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..ae769cde24a23a79bfbbe06e4ac48783e68c5dcb --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link21_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ed0a29d2376e8874f724bc2c20384eeec93d8268e59bb8c45628335f7ab6742e +size 411884 diff --git a/real/assets/inspire_hand/meshes/Link22_L.STL b/real/assets/inspire_hand/meshes/Link22_L.STL new file mode 100644 index 0000000000000000000000000000000000000000..8ee6e22df6763466586e6c43b989b3f10fbd0f75 --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link22_L.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5e64ea976ecf08c46e8cb80a5dd56ee3e8c5f36a0573ab3c69160d9337f941e1 +size 413384 diff --git a/real/assets/inspire_hand/meshes/Link22_R.STL b/real/assets/inspire_hand/meshes/Link22_R.STL new file mode 100644 index 0000000000000000000000000000000000000000..9925767e505bfea1fa09a180b9b1e8d901f1699d --- /dev/null +++ b/real/assets/inspire_hand/meshes/Link22_R.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1aa3fb46f689251ee38f5262b71364c680f70d1ad1dd9c6f1cae8c098b0e8422 +size 416484 diff --git a/real/assets/inspire_hand/meshes/R_hand_base_link.STL b/real/assets/inspire_hand/meshes/R_hand_base_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..042016dbf302500a0cd544d3ff51f323a3a39c20 --- /dev/null +++ b/real/assets/inspire_hand/meshes/R_hand_base_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b4d84504242a99fa915c4ed5195ce4724c67cb94d9217cd4c254cfb5937ecf5c +size 1472384 diff --git a/real/assets/inspire_hand/meshes/left_ankle_roll_link.STL b/real/assets/inspire_hand/meshes/left_ankle_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..a753f845f2c7f903b08dc1f6b63b25684ff7ab86 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ead4b17518eaa0a9f137c6170be6151ccf58b06fc2894b6202511515d3ebaa2 +size 829884 diff --git a/real/assets/inspire_hand/meshes/left_elbow_pitch_link.STL b/real/assets/inspire_hand/meshes/left_elbow_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..2aed3f39f6af3c1032f42ee2e60682ef88624f75 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_elbow_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:1d9ed881d68aea86e7fe48b0430f3d48937481e702667f3484b0725bef22d3f0 +size 1025584 diff --git a/real/assets/inspire_hand/meshes/left_elbow_roll_link.STL b/real/assets/inspire_hand/meshes/left_elbow_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..cbe5ffa6607f264beca4aa909d952c6d70a80833 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_elbow_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:a9e5f7c5214f584a91543d0ddfac50331ac8d6316c133140cd1d2397ae23ec50 +size 1243084 diff --git a/real/assets/inspire_hand/meshes/left_hand_link.STL b/real/assets/inspire_hand/meshes/left_hand_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..775698f6c1dda44aeaab151eb7557a2df2422f0d --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_hand_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8bdf9e424ffcad03646a354fe5a4a08515e523c081070afabcadc9f5f03132e1 +size 147684 diff --git a/real/assets/inspire_hand/meshes/left_hip_pitch_link.STL b/real/assets/inspire_hand/meshes/left_hip_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..e476201842930cc5155194c5806477f596e5be46 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:aa790c31ffc100cd182f2bf4480e7bfda675edc58ed8f361b6553a4975c9de3b +size 452384 diff --git a/real/assets/inspire_hand/meshes/left_hip_roll_link.STL b/real/assets/inspire_hand/meshes/left_hip_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..75ecfa87cc1ae20f1aa6a7d18e07a18fd5d45936 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8ed880cf1171e048cfe50489fe2546c32790ac0c24a2b3b4c560495401013552 +size 769284 diff --git a/real/assets/inspire_hand/meshes/left_hip_yaw_link.STL b/real/assets/inspire_hand/meshes/left_hip_yaw_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..ca8f3e4777ce7ec6afcdebcc7fa03a0eca1208b2 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:103a854e684b84e1baba7d073822079069c64d0fa7b1592f29d38bd29f545bfb +size 885884 diff --git a/real/assets/inspire_hand/meshes/left_knee_link.STL b/real/assets/inspire_hand/meshes/left_knee_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..6d7faa015ee2efb10d119f9492f707221aaf07f4 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4ee2cd33688c2887f2aa281691e51eeb7c3ddd3900240fedd7bbd54fcfc9b5ce +size 451384 diff --git a/real/assets/inspire_hand/meshes/left_shoulder_pitch_link.STL b/real/assets/inspire_hand/meshes/left_shoulder_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..7b517f0fcc59b0a669f9910961d1684bcf1c5f49 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:42e1db49464ca0f44f933114ee426e7f6d6310d20cf104bfa38b5ed4794cfab7 +size 787484 diff --git a/real/assets/inspire_hand/meshes/left_shoulder_roll_link.STL b/real/assets/inspire_hand/meshes/left_shoulder_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..74a9f099a354a734ff2ad02712679539be3cfd0f --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8d3bcf767d72a9a5d7dae539eae79dc4132f0159b8803a6f6f2f9ad4b7e94d19 +size 903384 diff --git a/real/assets/inspire_hand/meshes/left_shoulder_yaw_link.STL b/real/assets/inspire_hand/meshes/left_shoulder_yaw_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..6f1af2d339aed8526eec8e4a425023c9c1e5e90d --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e22a5b53f8f307e6c5d6c17f9ceae4852c6468547a951cb39dfdec9ee414b515 +size 644184 diff --git a/real/assets/inspire_hand/meshes/left_wrist_pitch_link.STL b/real/assets/inspire_hand/meshes/left_wrist_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..fb3d7485e3d33da3e848ea465b782fdd1d5d5460 --- /dev/null +++ b/real/assets/inspire_hand/meshes/left_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b7bb3eaf1213e27d3ee7d4193a0f0974823ae61c821ff1ec34459d65b7e39583 +size 630184 diff --git a/real/assets/inspire_hand/meshes/logo_link.STL b/real/assets/inspire_hand/meshes/logo_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..0c9aa80cc5db8edd09377e4c8b83a8d73343977d --- /dev/null +++ b/real/assets/inspire_hand/meshes/logo_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ef9635c847ca7cd61aeaed86ad599935b648fb6f8688867c0448917b1811511b +size 145484 diff --git a/real/assets/inspire_hand/meshes/pelvis.STL b/real/assets/inspire_hand/meshes/pelvis.STL new file mode 100644 index 0000000000000000000000000000000000000000..67a467e437a2e01124ec5df1713138e4abfdd14e --- /dev/null +++ b/real/assets/inspire_hand/meshes/pelvis.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:dc2cbbeb07d3b63bce9fedeb8cf5252609dca1e89380734d22d5b3c8ded2f15c +size 672884 diff --git a/real/assets/inspire_hand/meshes/right_ankle_roll_link.STL b/real/assets/inspire_hand/meshes/right_ankle_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..a753f845f2c7f903b08dc1f6b63b25684ff7ab86 --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_ankle_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:5ead4b17518eaa0a9f137c6170be6151ccf58b06fc2894b6202511515d3ebaa2 +size 829884 diff --git a/real/assets/inspire_hand/meshes/right_elbow_pitch_link.STL b/real/assets/inspire_hand/meshes/right_elbow_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..ce7770dc8578b06f0099d2623a121234930cff55 --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_elbow_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ae09a5f8dcea82530824c88d9a5299e2b835543754e29ca952f75619c968a787 +size 1025384 diff --git a/real/assets/inspire_hand/meshes/right_elbow_roll_link.STL b/real/assets/inspire_hand/meshes/right_elbow_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..2d81779f04c23e417f4b55a4e687aa13ac01b50f --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_elbow_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d06e2e62f80ceef841a88055da884fade9235f21b66e272fc51331367a5ea174 +size 1242984 diff --git a/real/assets/inspire_hand/meshes/right_hand_link.STL b/real/assets/inspire_hand/meshes/right_hand_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..775698f6c1dda44aeaab151eb7557a2df2422f0d --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_hand_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8bdf9e424ffcad03646a354fe5a4a08515e523c081070afabcadc9f5f03132e1 +size 147684 diff --git a/real/assets/inspire_hand/meshes/right_hip_pitch_link.STL b/real/assets/inspire_hand/meshes/right_hip_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..f6e17009ea63f7327ef38a0d65c3bc2c36de0035 --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_hip_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3c0300dd96432f218857886007e9d4e8be038cd8b85e04e84a59df4de7f90476 +size 452384 diff --git a/real/assets/inspire_hand/meshes/right_hip_roll_link.STL b/real/assets/inspire_hand/meshes/right_hip_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..4eca1b5d2b26fef1328211a75304c3458fd0bbf3 --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_hip_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:3e95b01b0a215e1d134545d4073cf7a72ab38ca33e35c606c03f1706d63588c6 +size 768484 diff --git a/real/assets/inspire_hand/meshes/right_hip_yaw_link.STL b/real/assets/inspire_hand/meshes/right_hip_yaw_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..db4c44cd3a83632725ae96aec49505b89a7df20c --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_hip_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:10badbb0ac78e22fc220f421b1c919cfcf59406b412d78313e4b8b06cbc51e47 +size 884484 diff --git a/real/assets/inspire_hand/meshes/right_knee_link.STL b/real/assets/inspire_hand/meshes/right_knee_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..6d7faa015ee2efb10d119f9492f707221aaf07f4 --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_knee_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:4ee2cd33688c2887f2aa281691e51eeb7c3ddd3900240fedd7bbd54fcfc9b5ce +size 451384 diff --git a/real/assets/inspire_hand/meshes/right_shoulder_pitch_link.STL b/real/assets/inspire_hand/meshes/right_shoulder_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..00f104889d337a2204d857a09f1b88ba2dd7e7cc --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_shoulder_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:850ffa7321993bfadcdb8bab1d422239f6a20abe27daa5ef99622f55d4da3217 +size 788484 diff --git a/real/assets/inspire_hand/meshes/right_shoulder_roll_link.STL b/real/assets/inspire_hand/meshes/right_shoulder_roll_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..046b337bea91ed3a67ed97fbb6537ac04969b3ea --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_shoulder_roll_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b9e793e351e09027d27427c06df2c1cb6b37c0205d0f68686b0d73916c6821de +size 908284 diff --git a/real/assets/inspire_hand/meshes/right_shoulder_yaw_link.STL b/real/assets/inspire_hand/meshes/right_shoulder_yaw_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..799a41f6949066ee20acfacafb144043f38b19cd --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_shoulder_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0c9e4bec895f24c07fb2c4d132fc5e3cc7146155547267e9c270789af6567781 +size 646884 diff --git a/real/assets/inspire_hand/meshes/right_wrist_pitch_link.STL b/real/assets/inspire_hand/meshes/right_wrist_pitch_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..6ae199436cd4c481d8629637d02289852d4e294f --- /dev/null +++ b/real/assets/inspire_hand/meshes/right_wrist_pitch_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:11624373f44c3ac8fcccd09abbb2f5eb9490413a41fb0a9f3de71a01509c05ec +size 630184 diff --git a/real/assets/inspire_hand/meshes/torso_link.STL b/real/assets/inspire_hand/meshes/torso_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..f43b2d79894b4d1dded381f72a17d680556630fe --- /dev/null +++ b/real/assets/inspire_hand/meshes/torso_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:67eb7433ea97e20da48c39750095391ff4899e5d60d946bf4d700e2038e7d269 +size 2340384 diff --git a/real/assets/inspire_hand/meshes/wrist_yaw_link.STL b/real/assets/inspire_hand/meshes/wrist_yaw_link.STL new file mode 100644 index 0000000000000000000000000000000000000000..5183d1228bc1cb6cf0fa243c7f207fcc3d05f6f5 --- /dev/null +++ b/real/assets/inspire_hand/meshes/wrist_yaw_link.STL @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:270fb35db8c602a9495322e0be6924b8ed9e415f8e783434dea54596ca8558df +size 224484 diff --git a/scripts/deploy_real.tar.gz b/scripts/deploy_real.tar.gz new file mode 100644 index 0000000000000000000000000000000000000000..1b784d735ba670ab849cd267662036f971d5426c --- /dev/null +++ b/scripts/deploy_real.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:04d6dbd8f660fbdf5fb0f753a6a258c39b103142561a028f84dec4e505eb502c +size 68984 diff --git a/src/psi/config/data_lerobot.py b/src/psi/config/data_lerobot.py new file mode 100644 index 0000000000000000000000000000000000000000..d2cbad925116e533e0685660e622511689074fad --- /dev/null +++ b/src/psi/config/data_lerobot.py @@ -0,0 +1,50 @@ +from pydantic import BaseModel, Field, model_validator +from typing import Any, Optional, Dict, List, TYPE_CHECKING +from psi.config.config import DataConfig +from pathlib import Path +from psi.utils import resolve_data_path +import os +import json + +from psi.config.transform import ActionStateTransform +class LerobotDataConfig(DataConfig): + root_dir: str + train_repo_ids: List[str] = Field(default_factory=list) + val_repo_ids: List[str] = Field(default_factory=list) + + @model_validator(mode="after") + def check_repo_ids(self): + if len(self.train_repo_ids) == 0: + raise ValueError("train_repo_ids must be provided") + if len(self.val_repo_ids) == 0: + self.val_repo_ids = [self.train_repo_ids[0]] + return self + + @model_validator(mode="after") + def load_stats(self): + if not isinstance(self.transform.field, ActionStateTransform): + return self + if ( + not Path(self.transform.field.stat_path).is_absolute() and + self.transform.field.action_max is None + ): + fpath = resolve_data_path( + Path(self.root_dir) / self.train_repo_ids[0] / self.transform.field.stat_path + ) + if not os.path.exists(fpath): + return self + with open(fpath, "r") as f: + stats = json.load(f) + self.transform.field.populate_stats(stats) + return self + + def __call__(self, split: str = "train", transform_kwargs={}, **kwargs) -> Any: + from psi.data.lerobot import LeRobotDatasetWrapper + from psi.data.dataset import Dataset as MapStyleDataset + + train_dataset = LeRobotDatasetWrapper(self, split=split) + return MapStyleDataset(self, train_dataset, transform_kwargs=transform_kwargs) + + def mock(self, split: str = "train", transform_kwargs={}, **kwargs) -> Any: + dataset = self.__call__(split, transform_kwargs=transform_kwargs, **kwargs) + return dataset[0] diff --git a/src/psi/config/data_seed_video.py b/src/psi/config/data_seed_video.py new file mode 100644 index 0000000000000000000000000000000000000000..d9f85b5d2352b051aaa8de574d6a577635fe1fbf --- /dev/null +++ b/src/psi/config/data_seed_video.py @@ -0,0 +1,51 @@ +from __future__ import annotations + +from typing import Any + +from psi.config.config import DataConfig + + +class SeedVideoDataConfig(DataConfig): + root_dir: str + video_dir: str + val_ratio: float = 0.1 + split_seed: int = 42 + chunk_size: int = 30 + sample_stride: int = 1 + use_delta_actions: bool = True + motion_fps: int = 50 + video_sample_fps: float = 1.0 + video_size_pixels: int = 50176 + video_cache_size: int = 32 + video_embed_dir: str = "" + use_video_instruction: bool = False + + def __call__(self, split: str = "train", transform_kwargs={}, **kwargs) -> Any: + from psi.data.dataset import Dataset as MapStyleDataset + from psi.data.seed.seed_video_motion_dataset import SeedVideoMotionDataset + + repack = self.transform.repack + action_chunk_size = int(getattr(repack, "action_chunk_size", self.chunk_size)) + use_delta_actions = bool(getattr(repack, "use_delta_actions", self.use_delta_actions)) + num_past_frames = int(getattr(repack, "num_past_frames", 0)) + self.chunk_size = action_chunk_size + self.use_delta_actions = use_delta_actions + + dataset = SeedVideoMotionDataset( + root_dir=self.root_dir, + video_dir=self.video_dir, + split=split, + val_ratio=self.val_ratio, + split_seed=self.split_seed, + action_chunk_size=action_chunk_size, + sample_stride=self.sample_stride, + use_delta_actions=use_delta_actions, + num_past_frames=num_past_frames, + motion_fps=self.motion_fps, + video_sample_fps=self.video_sample_fps, + video_size_pixels=self.video_size_pixels, + video_cache_size=self.video_cache_size, + video_embed_dir=self.video_embed_dir, + use_video_instruction=self.use_video_instruction, + ) + return MapStyleDataset(self, dataset, transform_kwargs=transform_kwargs, **kwargs) diff --git a/src/psi/config/model_dp.py b/src/psi/config/model_dp.py new file mode 100644 index 0000000000000000000000000000000000000000..c6b9b2439adf663f148501e55752cd179d669904 --- /dev/null +++ b/src/psi/config/model_dp.py @@ -0,0 +1,14 @@ +from tyro.conf import subcommand as cmd +from typing import Union, Annotated, Optional, List +from psi.config.config import ModelConfig +from pydantic import Field + +class DiffusionPolicyModelConfig(ModelConfig): + num_diffusion_iters: int = 100 + action_chunk_size: int = 16 # Tp, pred_horizon + num_cameras: int = 1 + share_vision_encoder: bool = False + obs_horizon: int = 1 + action_exec_horizon: int = 6 + action_dim: int = 2 + obs_dim: int = 15 \ No newline at end of file diff --git a/src/psi/config/model_qwen3vl.py b/src/psi/config/model_qwen3vl.py new file mode 100644 index 0000000000000000000000000000000000000000..59b8bb22a861602d07b4548e15d21e8e1a66b320 --- /dev/null +++ b/src/psi/config/model_qwen3vl.py @@ -0,0 +1,42 @@ +from tyro.conf import subcommand as cmd +from typing import Union, Annotated +from psi.config.config import ModelConfig + +from .tokenizer import ( + FastActionTokenizerConfig, + VQActionTokenizerConfig, + TextActionTokenizerConfig +) + +class Qwen3VLModelConfig(ModelConfig): + # Training Schedule + weight_decay: float = 0.01 # L2 regularization strength + + # Core Arguments + model_name_or_path: str = "Qwen/Qwen3-VL-2B-Instruct" # [ModelArguments] Model identifier + tune_mm_llm: bool = True # [TrainingArguments] Train LLM or not + tune_mm_vision: bool = False # [TrainingArguments] Train VIT or not + tune_mm_mlp: bool = True # [TrainingArguments] Train MLP or not + # dataset_use: str = "egodex" # [DataArguments] Dataset specification + # output_dir: str = "./outputs/hfm_qwen3vl" # Output directory for checkpoints + # cache_dir: str = "./cache/models" # [TrainingArguments] Model cache location + gradient_checkpointing: bool = True # [TrainingArguments] Enable gradient checkpointing + + # Learning Rate Configuration + mm_projector_lr: float = 1e-5 # [TrainingArguments] Projector-specific LR + vision_tower_lr: float = 1e-6 # [TrainingArguments] Vision encoder LR + optim: str = "adamw_torch" # [TrainingArguments] Optimizer selection + + # Sequence Configuration + model_max_length: int = 8192 # [TrainingArguments] Max sequence length + + lora_r: int = 8 # [TrainingArguments] LoRA r + lora_alpha: int= 16 # [TrainingArguments] LoRA alpha + lora_dropout: float = 0.0 # [TrainingArguments] LoRA dropout + + action_tokenizer: Union[ + Annotated[FastActionTokenizerConfig, cmd("fast")], + Annotated[VQActionTokenizerConfig, cmd("vq")], + Annotated[TextActionTokenizerConfig, cmd("text")], + ] + deepspeed: str = "scripts/deepspeed/zero3.json" # DeepSpeed configuration name \ No newline at end of file diff --git a/src/psi/config/train/finetune_real_psi0_config.py b/src/psi/config/train/finetune_real_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..9fa87461fd0c4f978814937dc3a7ba39a776d483 --- /dev/null +++ b/src/psi/config/train/finetune_real_psi0_config.py @@ -0,0 +1,20 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator + +from psi.config.config import LaunchConfig +from psi.config.data_lerobot import LerobotDataConfig +from psi.config.model_psi0 import Psi0ModelConfig +from psi.config.transform import DataTransform +from psi.config import transform as pt + +class DynamicDataTransform(DataTransform): + repack: pt.RealRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + +class DynamicDataConfig(LerobotDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig \ No newline at end of file diff --git a/src/psi/config/train/finetune_simple_psi0_config.py b/src/psi/config/train/finetune_simple_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..bf8d88266afdf553786fa2f4ec1ea7e1a324d346 --- /dev/null +++ b/src/psi/config/train/finetune_simple_psi0_config.py @@ -0,0 +1,21 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator + +from psi.config.config import LaunchConfig +# from psi.config.data_simple import SimpleDataConfig +from psi.config.data_lerobot import LerobotDataConfig +from psi.config.model_psi0 import Psi0ModelConfig +from psi.config.transform import DataTransform +from psi.config import transform as pt + +class DynamicDataTransform(DataTransform): + repack: pt.SimpleRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + +class DynamicDataConfig(LerobotDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig \ No newline at end of file diff --git a/src/psi/config/train/posttrain_he_psi0_config.py b/src/psi/config/train/posttrain_he_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..44bd1812df768dd440d4aa3e1bc11e01344cc601 --- /dev/null +++ b/src/psi/config/train/posttrain_he_psi0_config.py @@ -0,0 +1,20 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator +# from tyro.conf import subcommand as cmd +from psi.config.config import LaunchConfig +from psi.config.model_psi0 import Psi0ModelConfig +from psi.config.data_he import HERawDataConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + +class DynamicDataTransform(DataTransform): + repack: pt.HEPosttrainRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + +class DynamicDataConfig(HERawDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig \ No newline at end of file diff --git a/src/psi/config/train/posttrain_mix_psi0_config.py b/src/psi/config/train/posttrain_mix_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..9cf26a28054477e85ac0b681097dd2095cf21dfe --- /dev/null +++ b/src/psi/config/train/posttrain_mix_psi0_config.py @@ -0,0 +1,21 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator +# from tyro.conf import subcommand as cmd +from psi.config.config import LaunchConfig +from psi.config.model_psi0 import Psi0ModelConfig +# from psi.config.data_he import HERawDataConfig +from psi.config.data_mix import MixedDataConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + +class DynamicDataTransform(DataTransform): + repack: pt.MixedRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + +class DynamicDataConfig(MixedDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig \ No newline at end of file diff --git a/src/psi/config/train/posttrain_v1_audio_psi0_config.py b/src/psi/config/train/posttrain_v1_audio_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..0ad8218bc841359fb5858dabc727b6d670e77cee --- /dev/null +++ b/src/psi/config/train/posttrain_v1_audio_psi0_config.py @@ -0,0 +1,20 @@ +from psi.config.config import LaunchConfig +from psi.config.data_seed_audio import SeedAudioDataConfig +from psi.config.model_psi0 import Psi0ModelConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + + +class DynamicDataTransform(DataTransform): + repack: pt.SeedPosttrainRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + + +class DynamicDataConfig(SeedAudioDataConfig): + transform: DynamicDataTransform + + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig diff --git a/src/psi/config/train/posttrain_v1_psi0_config.py b/src/psi/config/train/posttrain_v1_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..3321dcc1e8fb7fb323fe310ce8ff00f417f69442 --- /dev/null +++ b/src/psi/config/train/posttrain_v1_psi0_config.py @@ -0,0 +1,23 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator +# from tyro.conf import subcommand as cmd +from psi.config.config import LaunchConfig +from psi.config.model_psi0 import Psi0ModelConfig +# from psi.config.data_he import HERawDataConfig +# +from psi.config.data_seed import SeedDataConfig + +from psi.config import transform as pt +from psi.config.transform import DataTransform + +class DynamicDataTransform(DataTransform): + repack: pt.SeedPosttrainRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + +class DynamicDataConfig(SeedDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig \ No newline at end of file diff --git a/src/psi/config/train/posttrain_v1_seed_text_audio_mix_psi0_config.py b/src/psi/config/train/posttrain_v1_seed_text_audio_mix_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..66fafa1aac7376415fd79dbfa3334f313d549dcd --- /dev/null +++ b/src/psi/config/train/posttrain_v1_seed_text_audio_mix_psi0_config.py @@ -0,0 +1,20 @@ +from psi.config.config import LaunchConfig +from psi.config.data_seed_text_audio_mix import SeedTextAudioMixDataConfig +from psi.config.model_psi0 import Psi0ModelConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + + +class DynamicDataTransform(DataTransform): + repack: pt.SeedPosttrainRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + + +class DynamicDataConfig(SeedTextAudioMixDataConfig): + transform: DynamicDataTransform + + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig diff --git a/src/psi/config/train/posttrain_v1_seed_text_audio_video_mix_psi0_config.py b/src/psi/config/train/posttrain_v1_seed_text_audio_video_mix_psi0_config.py new file mode 100644 index 0000000000000000000000000000000000000000..6226752432b59643a2485dfa85927daa41dbf0e6 --- /dev/null +++ b/src/psi/config/train/posttrain_v1_seed_text_audio_video_mix_psi0_config.py @@ -0,0 +1,20 @@ +from psi.config.config import LaunchConfig +from psi.config.data_seed_text_audio_video_mix import SeedTextAudioVideoMixDataConfig +from psi.config.model_psi0 import Psi0ModelConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + + +class DynamicDataTransform(DataTransform): + repack: pt.SeedPosttrainRepackTransform + field: pt.ActionStateTransform + model: pt.Psi0ModelTransform + + +class DynamicDataConfig(SeedTextAudioVideoMixDataConfig): + transform: DynamicDataTransform + + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Psi0ModelConfig diff --git a/src/psi/config/train/posttrain_v1_seed_text_audio_video_mix_psi0_motion_distill_config.py b/src/psi/config/train/posttrain_v1_seed_text_audio_video_mix_psi0_motion_distill_config.py new file mode 100644 index 0000000000000000000000000000000000000000..a6d8d81fed7b65c65371a38828fe0c1420a571cc --- /dev/null +++ b/src/psi/config/train/posttrain_v1_seed_text_audio_video_mix_psi0_motion_distill_config.py @@ -0,0 +1,21 @@ +from psi.config.config import LaunchConfig +from psi.config.data_seed_text_audio_video_mix import SeedTextAudioVideoMixDataConfig +from psi.config.model_motion_distill import MotionDistillPsi0ModelConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform +from psi.config.transform_motion_distill import MotionDistillPsi0ModelTransform + + +class DynamicDataTransform(DataTransform): + repack: pt.SeedPosttrainRepackTransform + field: pt.ActionStateTransform + model: MotionDistillPsi0ModelTransform + + +class DynamicDataConfig(SeedTextAudioVideoMixDataConfig): + transform: DynamicDataTransform + + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: MotionDistillPsi0ModelConfig diff --git a/src/psi/config/train/pretrain_egodex_qwen3vl_config.py b/src/psi/config/train/pretrain_egodex_qwen3vl_config.py new file mode 100644 index 0000000000000000000000000000000000000000..0f78a2b86a0b4a8daad4c3bd2f777a8c787e4308 --- /dev/null +++ b/src/psi/config/train/pretrain_egodex_qwen3vl_config.py @@ -0,0 +1,21 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator +# from tyro.conf import subcommand as cmd +from psi.config.config import LaunchConfig +# from psi.config import model as pm +from psi.config.model_qwen3vl import Qwen3VLModelConfig +from psi.config.data_egodex import EgoDexDataConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + +class DynamicDataTransform(DataTransform): + repack: pt.EgodexRepackTransform + field: pt.ActionStateTransform + model: pt.Qwen3vlModelTransform + +class DynamicDataConfig(EgoDexDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Qwen3VLModelConfig diff --git a/src/psi/config/train/pretrain_he_qwen3vl_config.py b/src/psi/config/train/pretrain_he_qwen3vl_config.py new file mode 100644 index 0000000000000000000000000000000000000000..54813203c15d2a10acb8ccc96b11d2118e4b320c --- /dev/null +++ b/src/psi/config/train/pretrain_he_qwen3vl_config.py @@ -0,0 +1,21 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator +# from tyro.conf import subcommand as cmd +from psi.config.config import LaunchConfig +# from psi.config import model as pm +from psi.config.model_qwen3vl import Qwen3VLModelConfig +from psi.config.data_he import HERawDataConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + +class DynamicDataTransform(DataTransform): + repack: pt.HEPretrainRepackTransform + field: pt.ActionStateTransform + model: pt.Qwen3vlModelTransform + +class DynamicDataConfig(HERawDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Qwen3VLModelConfig diff --git a/src/psi/config/train/pretrain_mix_qwen3vl_config.py b/src/psi/config/train/pretrain_mix_qwen3vl_config.py new file mode 100644 index 0000000000000000000000000000000000000000..07e42e5292cc0383182100706d9591637e707b1e --- /dev/null +++ b/src/psi/config/train/pretrain_mix_qwen3vl_config.py @@ -0,0 +1,20 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator +# from tyro.conf import subcommand as cmd +from psi.config.config import LaunchConfig +from psi.config.model_qwen3vl import Qwen3VLModelConfig +from psi.config.data_mix import MixedDataConfig +from psi.config import transform as pt +from psi.config.transform import DataTransform + +class DynamicDataTransform(DataTransform): + repack: pt.MixedRepackTransform + field: pt.ActionStateTransform + model: pt.Qwen3vlModelTransform + +class DynamicDataConfig(MixedDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: Qwen3VLModelConfig \ No newline at end of file diff --git a/src/psi/config/train/real_act_config.py b/src/psi/config/train/real_act_config.py new file mode 100644 index 0000000000000000000000000000000000000000..be1672d280a8be6842f3652e1daed74e1d28173a --- /dev/null +++ b/src/psi/config/train/real_act_config.py @@ -0,0 +1,20 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator + +from psi.config.config import LaunchConfig +from psi.config.data_lerobot import LerobotDataConfig +from psi.config.model_act import ACTModelConfig +from psi.config.transform import DataTransform +from psi.config import transform as pt + +class DynamicDataTransform(DataTransform): + repack: pt.RealRepackTransform + field: pt.ActionStateTransform + model: pt.ACTModelTransform + +class DynamicDataConfig(LerobotDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: ACTModelConfig \ No newline at end of file diff --git a/src/psi/config/train/real_dp_config.py b/src/psi/config/train/real_dp_config.py new file mode 100644 index 0000000000000000000000000000000000000000..9e32b1ee89e227456d240449535a672923fcbe65 --- /dev/null +++ b/src/psi/config/train/real_dp_config.py @@ -0,0 +1,20 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator + +from psi.config.config import LaunchConfig +from psi.config.data_lerobot import LerobotDataConfig +from psi.config.model_dp import DiffusionPolicyModelConfig +from psi.config.transform import DataTransform +from psi.config import transform as pt + +class DynamicDataTransform(DataTransform): + repack: pt.RealRepackTransform + field: pt.ActionStateTransform + model: pt.DiffusionPolicyModelTransform + +class DynamicDataConfig(LerobotDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: DiffusionPolicyModelConfig \ No newline at end of file diff --git a/src/psi/config/train/simple_act_config.py b/src/psi/config/train/simple_act_config.py new file mode 100644 index 0000000000000000000000000000000000000000..40c45e8f420a47fa7897fc3b7cbd18e4d4f35672 --- /dev/null +++ b/src/psi/config/train/simple_act_config.py @@ -0,0 +1,20 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator + +from psi.config.config import LaunchConfig +from psi.config.data_lerobot import LerobotDataConfig +from psi.config.model_act import ACTModelConfig +from psi.config.transform import DataTransform +from psi.config import transform as pt + +class DynamicDataTransform(DataTransform): + repack: pt.SimpleRepackTransform + field: pt.ActionStateTransform + model: pt.ACTModelTransform + +class DynamicDataConfig(LerobotDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: ACTModelConfig \ No newline at end of file diff --git a/src/psi/config/train/simple_dp_config.py b/src/psi/config/train/simple_dp_config.py new file mode 100644 index 0000000000000000000000000000000000000000..78835f2467098ed0dbb09c5ca510309e284baf17 --- /dev/null +++ b/src/psi/config/train/simple_dp_config.py @@ -0,0 +1,20 @@ +from typing import Union, Annotated +from pydantic import BaseModel, Field, model_validator + +from psi.config.config import LaunchConfig +from psi.config.data_lerobot import LerobotDataConfig +from psi.config.model_dp import DiffusionPolicyModelConfig +from psi.config.transform import DataTransform +from psi.config import transform as pt + +class DynamicDataTransform(DataTransform): + repack: pt.SimpleRepackTransform + field: pt.ActionStateTransform + model: pt.DiffusionPolicyModelTransform + +class DynamicDataConfig(LerobotDataConfig): + transform: DynamicDataTransform + +class DynamicLaunchConfig(LaunchConfig): + data: DynamicDataConfig + model: DiffusionPolicyModelConfig \ No newline at end of file diff --git a/src/psi/config/transform.py b/src/psi/config/transform.py new file mode 100644 index 0000000000000000000000000000000000000000..ba2510ac2b7e94df1b1a0504d76ede80fb4cda3b --- /dev/null +++ b/src/psi/config/transform.py @@ -0,0 +1,1358 @@ +from __future__ import annotations +import os +import re +import base64 +import copy +import json +import math +import time +from io import BytesIO +from typing import (TYPE_CHECKING, Annotated, Any, Dict, List, Optional, Tuple, + Union) + +import numpy as np +import torch +import torch.nn.functional as F +from PIL import Image +from pydantic import BaseModel, Field, model_validator +from torchvision.transforms import v2 +from psi.config.augmentation import * +from psi.data.seed.motion_repr import build_seed_textop_action, build_seed_textop_state +from psi.utils import get_asset_dir, pt_to_pil, resolve_path, pad_to_len + +IGNORE_INDEX = -100 + +class RepackTransform(BaseModel): + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + return data + +class LerobotRepackTransform(RepackTransform): + + def delta_timestamps(self, fps: int) -> dict[str, list[float]]: + ... + +class ModelTransform(BaseModel): + def __call__(self, data: dict[str, Any], no_aug: bool = False, **kwargs) -> dict[str, Any]: + return data + +class FieldTransform(BaseModel): + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + return data + +class DataTransform(BaseModel): + repack: RepackTransform + model: ModelTransform + field: FieldTransform + + def __call__(self, data:dict[str, Any], **kwargs) -> dict[str, Any]: + data = self.repack(data, **kwargs) + data = self.field(data, **kwargs) + data = self.model(data, **kwargs) + return data + +class EgodexRepackTransform(BaseModel): + dataset_name: str = "egodex" + stage:str = "pretrain" # "pretrain" or "postpre" + pad_action_dim: int | None = None + pad_state_dim: int | None = None + + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + states, _ = pad_to_len(data["states"], self.pad_state_dim) if self.pad_state_dim is not None else (data["states"], None) + if self.pad_action_dim is not None: + actions, mask = pad_to_len(data["actions"], self.pad_action_dim) + else: + actions = data["actions"] + mask = np.ones_like(actions, dtype=bool) + + return dict( + observations = [Image.fromarray(img[0]) for img in data["current_images"]], # list of PIL Image + states=states, # (To, Da) + actions=actions, # (Tp, Da) + instruction=data["instruction"].lower(), + dataset=data.get("dataset_name", self.dataset_name), + actions_mask=mask, #(Tp, Da) + ) + +class HEPretrainRepackTransform(RepackTransform): + dataset_name: str = "humanoid-everyday" + num_past_frames: int = 0 + action_chunk_size: int = 1 + use_delta_actions: bool = True + robot_type: str = "g1" + # pad_action_dim: int | None = None + # pad_state_dim: int | None = None + + def _to_numpy(self, value, dtype=np.float32): + if isinstance(value, torch.Tensor): + value = value.detach().cpu().numpy() + return np.asarray(value, dtype=dtype) + + def _ensure_2d(self, value, dim: int, name: str): + arr = self._to_numpy(value) + if arr.ndim == 1: + if arr.shape[0] != dim: + raise ValueError(f"{name} expected shape ({dim},), got {arr.shape}") + return arr.reshape(1, dim) + if arr.ndim == 2: + if arr.shape[1] != dim: + raise ValueError(f"{name} expected shape (T,{dim}), got {arr.shape}") + return arr + raise ValueError(f"{name} expected 1D or 2D, got {arr.shape}") + + def _rpy_to_rot6d(self, rpy): + from scipy.spatial.transform import Rotation as R + rot = R.from_euler("xyz", rpy).as_matrix() + return rot[:, :2].reshape(6) + + def _delta_rpy(self, rpy_seq): + from scipy.spatial.transform import Rotation as R + rot = R.from_euler("xyz", rpy_seq).as_matrix() + rel = rot[1:] @ np.transpose(rot[:-1], (0, 2, 1)) + return R.from_matrix(rel).as_euler("xyz", degrees=False).astype(np.float32) + + def _build_actions(self, data: dict[str, Any]) -> np.ndarray: + left_wrist_xyz = self._ensure_2d( + data["action.wrists.left.xyz"], 3, "action.wrists.left.xyz" + ) + left_wrist_rpy = self._ensure_2d( + data["action.wrists.left.rpy"], 3, "action.wrists.left.rpy" + ) + right_wrist_xyz = self._ensure_2d( + data["action.wrists.right.xyz"], 3, "action.wrists.right.xyz" + ) + right_wrist_rpy = self._ensure_2d( + data["action.wrists.right.rpy"], 3, "action.wrists.right.rpy" + ) + + def tip_xyz(keys): + for key in keys: + if key in data: + return self._ensure_2d(data[key], 3, key) + return np.zeros_like(left_wrist_xyz) + + left_thumb = tip_xyz(["action.hands.left_thumb.xyz"]) + left_index = tip_xyz(["action.hands.left_index.xyz"]) + left_middle = tip_xyz( + ["action.hands.left_middle.xyz", "action.hands.left_middle_finger.xyz"] + ) + left_ring = tip_xyz(["action.hands.left_ring_finger.xyz"]) + left_little = tip_xyz(["action.hands.left_little_finger.xyz"]) + + right_thumb = tip_xyz(["action.hands.right_thumb.xyz"]) + right_index = tip_xyz(["action.hands.right_index.xyz"]) + right_middle = tip_xyz( + ["action.hands.right_middle.xyz", "action.hands.right_middle_finger.xyz"] + ) + right_ring = tip_xyz(["action.hands.right_ring_finger.xyz"]) + right_little = tip_xyz(["action.hands.right_little_finger.xyz"]) + + if self.use_delta_actions: + if left_wrist_xyz.shape[0] < 2: + raise ValueError( + "use_delta_actions requires at least 2 action frames; " + "increase action_chunk_size or disable delta." + ) + left_wrist_xyz = left_wrist_xyz[1:] - left_wrist_xyz[:-1] + right_wrist_xyz = right_wrist_xyz[1:] - right_wrist_xyz[:-1] + left_wrist_rpy = self._delta_rpy(left_wrist_rpy) + right_wrist_rpy = self._delta_rpy(right_wrist_rpy) + + left_thumb = left_thumb[1:] - left_thumb[:-1] + left_index = left_index[1:] - left_index[:-1] + left_middle = left_middle[1:] - left_middle[:-1] + left_ring = left_ring[1:] - left_ring[:-1] + left_little = left_little[1:] - left_little[:-1] + + right_thumb = right_thumb[1:] - right_thumb[:-1] + right_index = right_index[1:] - right_index[:-1] + right_middle = right_middle[1:] - right_middle[:-1] + right_ring = right_ring[1:] - right_ring[:-1] + right_little = right_little[1:] - right_little[:-1] + + left_rot_pad = np.zeros_like(left_wrist_rpy) + right_rot_pad = np.zeros_like(right_wrist_rpy) + else: + left_rot6d = np.stack( + [self._rpy_to_rot6d(rpy) for rpy in left_wrist_rpy], axis=0 + ) + right_rot6d = np.stack( + [self._rpy_to_rot6d(rpy) for rpy in right_wrist_rpy], axis=0 + ) + left_wrist_rpy = left_rot6d + right_wrist_rpy = right_rot6d + left_rot_pad = None + right_rot_pad = None + + left_wrist = ( + np.concatenate([left_wrist_xyz, left_wrist_rpy, left_rot_pad], axis=1) #type:ignore + if self.use_delta_actions + else np.concatenate([left_wrist_xyz, left_wrist_rpy], axis=1) + ) + right_wrist = ( + np.concatenate([right_wrist_xyz, right_wrist_rpy, right_rot_pad], axis=1) #type:ignore + if self.use_delta_actions + else np.concatenate([right_wrist_xyz, right_wrist_rpy], axis=1) + ) + + actions = np.concatenate( + [ + left_wrist, + left_thumb, + left_index, + left_middle, + left_ring, + left_little, + right_wrist, + right_thumb, + right_index, + right_middle, + right_ring, + right_little, + ], + axis=1, + ).astype(np.float32) + return actions + + def __call__( + self, + data: dict[str, Any], + metadata=None, + **kwargs, + ) -> dict[str, Any]: + hand_states = self._to_numpy(data["observation.hand_joints"]) + arm_states = self._to_numpy(data["observation.arm_joints"]) + if hand_states.ndim == 2: + hand_states = hand_states[-1] + if arm_states.ndim == 2: + arm_states = arm_states[-1] + states = np.concatenate((hand_states, arm_states), axis=0).astype(np.float32) + + try: + actions = self._build_actions(data) + except Exception as e: + print("Error building actions:") + print(data) + + import traceback + traceback.print_exc() + raise e + + instruction = "" + if metadata is not None and "episode_index" in data: + episode_idx = int(self._to_numpy(data["episode_index"]).reshape(-1)[0]) + if ( + hasattr(metadata, "episodes") + and metadata.episodes is not None + and episode_idx < len(metadata.episodes) + ): + instruction = metadata.episodes[episode_idx].get("instruction", "") + if not instruction and "task" in data: + instruction = data["task"] + if isinstance(instruction, bytes): + instruction = instruction.decode("utf-8") + instruction = self.nice_instruction(instruction.split("/")[1]) + + img = data["observation.images.egocentric"] + if hasattr(img, "dim") and img.dim() == 4: + img = img[0] + elif isinstance(img, np.ndarray) and img.ndim == 4: + img = img[0] + + return dict( + observations=[pt_to_pil(img, normalized=False)], # list of PIL Image + states=states, + actions=actions, + instruction=str(instruction).lower(), + dataset=data.get("dataset_name", self.dataset_name), + ) + + def nice_instruction(self, instruction: str) -> str: + instruction = instruction.replace("_", " ").replace("-", " ") + instruction = re.sub(r"\s+", " ", instruction) + return instruction.strip() + + def delta_timestamps(self, fps: int): + delta = {} + delta["observation.hand_joints"] = [ + -t / fps for t in range(self.num_past_frames, -1, -1) + ] + delta["observation.arm_joints"] = [ + -t / fps for t in range(self.num_past_frames, -1, -1) + ] + + action_keys = [ + "action.joint_angles", + "action.wrists.left.xyz", + "action.wrists.left.rpy", + "action.wrists.right.xyz", + "action.wrists.right.rpy", + "action.hands.left_thumb.xyz", + "action.hands.left_index.xyz", + "action.hands.right_thumb.xyz", + "action.hands.right_index.xyz", + ] + if self.robot_type == "h1": + action_keys += [ + "action.hands.left_middle_finger.xyz", + "action.hands.left_ring_finger.xyz", + "action.hands.left_little_finger.xyz", + "action.hands.right_middle_finger.xyz", + "action.hands.right_ring_finger.xyz", + "action.hands.right_little_finger.xyz", + ] + else: + action_keys += [ + "action.hands.left_middle.xyz", + "action.hands.right_middle.xyz", + ] + action_len = ( + self.action_chunk_size + 1 if self.use_delta_actions else self.action_chunk_size + ) + for key in action_keys: + delta[key] = [t / fps for t in range(action_len)] + return delta + +class RealRepackTransform(LerobotRepackTransform): + dataset_name: str = "real" + + conditions: list[str] = Field(default_factory=lambda: []) + image_keys: List[str] = Field(default_factory=lambda: ["observation.images.egocentric"]) + state_key: str = "states" + action_key: str = "action" + instruction_key: str = "task" + + num_past_frames: int = 0 + action_chunk_size: int = 30 + + pad_action_dim: int | None = None + pad_state_dim: int | None = None + + def delta_timestamps(self, fps): + delta = {} + for image_key in self.image_keys: + delta[image_key] = [-t/fps for t in range(self.num_past_frames, -1, -1)] + + delta[self.state_key] = [-t/fps for t in range(self.num_past_frames, -1, -1)] + delta[self.action_key] = [t/fps for t in range(self.action_chunk_size)] + return delta + + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + states, _ = pad_to_len(data[self.state_key], self.pad_state_dim) if self.pad_state_dim is not None else (data[self.state_key], None) + + if self.pad_action_dim is not None: + actions, mask = pad_to_len(data[self.action_key], self.pad_action_dim) + else: + actions = data[self.action_key] + mask = np.ones_like(actions, dtype=bool) + + result = { + "observations": [ + pt_to_pil(data[key], normalized=False) + for key in self.image_keys + ], # list of PIL Image + "states": np.array(states, dtype=np.float32), # (To, Da) + "actions": np.array(actions, dtype=np.float32), # (Tp, Da) + "instruction": data[self.instruction_key].lower(), + "actions_mask": mask, #(Tp, Da) + } + return result + +class SimpleRepackTransform(LerobotRepackTransform): + dataset_name: str = "simple" + + num_past_frames: int = 0 # single current frame + action_chunk_size: int = 30 + + pad_action_dim: int | None = None + pad_state_dim: int | None = None + + def delta_timestamps(self, fps: int): + return { + "observation.images.egocentric": [-t/fps for t in range(self.num_past_frames, -1, -1)], + "states": [-t/fps for t in range(self.num_past_frames, -1, -1)], + "action": [t/fps for t in range(self.action_chunk_size)] + } + + + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + image_key = "observation.images.egocentric" + states = data["states"].numpy() # (To, Ds) + actions = data["action"].numpy() # (Tp, Da) + action_is_pad = data["action_is_pad"].numpy() # (Ta,) + pad_mask = np.ones_like(actions) * (1. - action_is_pad[...,None].astype(np.float32)) + + states, _ = pad_to_len(states, self.pad_state_dim) if self.pad_state_dim is not None else (states, None) + if self.pad_action_dim is not None: + actions, _ = pad_to_len(actions, self.pad_action_dim) + mask,_ = pad_to_len(pad_mask, self.pad_action_dim) + else: + mask = np.ones_like(actions, dtype=np.float32) + + return { + "observations": [pt_to_pil(data[image_key],normalized=False)], # single view + "states": states.astype(np.float32), # (To, Do) + "actions": actions.astype(np.float32), # (Tp, Da) + # "action_is_pad": np.array(data["action_is_pad"], dtype=bool), + "instruction": data["task"].lower(), + "actions_mask": mask + } + +class IdentityTransform(BaseModel): + # boilerplate transform config + # define primitive data types only! eg., int, float, str + # container types are allowed eg., list[int], dict[str, float] + # default no-op transform + # passing external dependency through **kwargs, eg., tokenizer, processor + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + return data + +class ActionStateTransform(FieldTransform): + + stat_path: str + action_norm_type: str = "bounds" # "bounds_q99" + stat_action_key: str = "action" + stat_state_key: str = "states" + use_norm_mask: bool = False + action_norm_masks: List[bool] = [ + True, + True, + True, + True, + True, + True, + False, + ] # delta-eef + gripper + action_min: Optional[List[float]] = None + action_max: Optional[List[float]] = None + state_min: Optional[List[float]] = None + state_max: Optional[List[float]] = None + normalize_state: bool = False # whether to normalize states + + pad_action_dim: int | None = None + pad_state_dim: int | None = None + + def model_post_init(self, __context: Any) -> None: + if not os.path.exists(resolve_path(self.stat_path)): + return + with open(resolve_path(self.stat_path), "r") as f: + stats = json.load(f) + self.populate_stats(stats) + + def populate_stats(self, stats: dict[str, Any]): + if self.action_norm_type == "bounds_q99": + self.action_min = stats[self.stat_action_key]["q01"] + self.action_max = stats[self.stat_action_key]["q99"] + if self.normalize_state: + self.state_min = stats[self.stat_state_key]["q01"] + self.state_max = stats[self.stat_state_key]["q99"] + elif self.action_norm_type == "bounds": + self.action_min = stats[self.stat_action_key]["min"] + self.action_max = stats[self.stat_action_key]["max"] + if self.normalize_state: + self.state_min = stats[self.stat_state_key]["min"] + self.state_max = stats[self.stat_state_key]["max"] + else: + raise ValueError(f"Unsupported action normalization type: {self.action_norm_type}") + + if self.pad_action_dim is not None: + self.action_min= pad_to_len(np.array(self.action_min, dtype=np.float32), self.pad_action_dim, dim=0)[0].tolist() + self.action_max = pad_to_len(np.array(self.action_max, dtype=np.float32), self.pad_action_dim, dim=0)[0].tolist() + + if self.pad_state_dim is not None and self.normalize_state: + self.state_min = pad_to_len(np.array(self.state_min, dtype=np.float32), self.pad_state_dim, dim=0)[0].tolist() + self.state_max = pad_to_len(np.array(self.state_max, dtype=np.float32), self.pad_state_dim, dim=0)[0].tolist() + + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + assert self.action_min is not None and self.action_max is not None, \ + f"{self.stat_path} is not loaded properly. Probably {resolve_path(self.stat_path)} does not exist." + action_min = np.array(self.action_min, dtype=np.float32) + action_max = np.array(self.action_max, dtype=np.float32) + if self.normalize_state: + data["states"] = self.normalize_state_func(data["states"]) + + # tolerate near-zero, not just exact zero + ill_mask = np.abs(action_max - action_min) < 1e-4 * (np.abs(action_max) + np.abs(action_min) + 1e-8) + action_max[ill_mask] = 1.0 # prevent division by zero + actions_normalized = np.where( + ill_mask, data["actions"], (data["actions"] - action_min) / (action_max - action_min) * 2 - 1 + ) + + if self.use_norm_mask: + actions = np.where(self.action_norm_masks, actions_normalized, data["actions"]) + else: + actions = actions_normalized + + # if (np.abs(actions) > 10.0).any(): + # print("Error: values outside quantile range during normalization.") + + actions = np.clip(actions, -1, 1).astype(np.float32) + # print(data["dataset"], actions.max(), actions.min(), actions.mean(), actions.std()) + data["raw_actions"] = data["actions"] + data["actions"] = actions + + return data + + def normalize(self, action, **kwargs): + data = {"actions": action} + return self.__call__(data)["actions"] + + def normalize_state_func(self, states, **kwargs): + state_min = np.array(self.state_min, dtype=np.float32) + state_max = np.array(self.state_max, dtype=np.float32) + # Normalize states, tolerate near-zero, not just exact zero + ill_mask = np.abs(state_max - state_min) < 1e-4 * (np.abs(state_max) + np.abs(state_min) + 1e-8) + state_max[ill_mask] = 1.0 # prevent division by zero + current_state = np.where( + ill_mask, 0, + (states - state_min) / (state_max - state_min) * 2 - 1 + ) + current_state = np.clip(current_state, -1, 1).astype(np.float32) + if np.isnan(current_state).any(): + current_state = np.nan_to_num(current_state, nan=0.0) + return current_state + + def reverse_call(self, array: Any, **kwargs) -> Any: + assert self.action_min is not None and self.action_max is not None + action_min = np.array(self.action_min) + action_max = np.array(self.action_max) + reversed_array = (array + 1) / 2 * (action_max - action_min) + action_min + return reversed_array + + def denormalize_L1_action_err(self, L1_err): + """return denormalized L1 err loss""" + + array_class = torch.tensor if torch.is_tensor(L1_err) else np.array + where = torch.where if torch.is_tensor(L1_err) else np.where + data_type = L1_err.dtype + + if self.action_norm_type == "bounds" or \ + self.action_norm_type == "bounds_q99": + low = array_class(self.action_min, dtype=data_type) # type: ignore + high = array_class(self.action_max, dtype=data_type) # type: ignore + + if self.use_norm_mask: + if torch.is_tensor(L1_err): + low = low.to(L1_err.device) + high = high.to(L1_err.device) + action_norm_masks = torch.tensor( + self.action_norm_masks, device=L1_err.device + ) + else: + action_norm_masks = self.action_norm_masks + result = where(action_norm_masks, 0.5 * L1_err * (high - low), L1_err) # type:ignore + else: + result = 0.5 * L1_err * (high - low) + return result + else: + raise NotImplementedError + + + def denormalize(self, normalized: np.ndarray|torch.Tensor) -> np.ndarray|torch.Tensor: + """ Denormalize the action. """ + array_class = torch.tensor if torch.is_tensor(normalized) else np.array + where = torch.where if torch.is_tensor(normalized) else np.where + data_type = normalized.dtype + + if self.action_norm_type == "bounds" or \ + self.action_norm_type == "bounds_q99": + if self.action_norm_type == "bounds": + low = array_class(self.action_min, dtype=data_type) # type: ignore + high = array_class(self.action_max, dtype=data_type) # type: ignore + elif self.action_norm_type == "bounds_q99": + low = array_class(self.action_min, dtype=data_type) # type: ignore + high = array_class(self.action_max, dtype=data_type) # type: ignore + + if not self.use_norm_mask: + assert self.action_min is not None + action_norm_masks = [True] * len(self.action_min) + else: + action_norm_masks = self.action_norm_masks + + if torch.is_tensor(normalized): + low = low.to(normalized.device) # type: ignore + high = high.to(normalized.device) # type: ignore + action_norm_masks = torch.tensor(action_norm_masks, device=normalized.device) + else: + action_norm_masks = np.array(action_norm_masks) + + action = where(action_norm_masks, 0.5 * (normalized + 1) * (high - low) + low, normalized) # type: ignore + return action # type: ignore + + +class Qwen3vlModelTransform(ModelTransform): + resize: ResizeImage = Field(default_factory=lambda: ResizeImage(size=(270, 480))) + color_jitter: ColorJitter + img_aug: bool = False + # anchor_size: tuple[int, int] = (256, 256) # H, W + + # for mixed dataset with different image sizes + adaptive_resize: bool = False + img_sizes: dict[str, Any] = Field(default_factory=lambda: { + "egodex": [270, 480], + "he" : [240, 320], + }) + + def __call__( + self, + data: dict[str, Any], + no_aug: bool = False, + vlm_processor = None, + action_tokenizer = None, + **kwargs + ) -> dict[str, Any]: + if vlm_processor is None or action_tokenizer is None: + return {"raw_actions": data["actions"], **data} + """ + data: dict with keys: + - instruction: str + - observations: List[Image] (To x PIL) + - states: (To, Da) torch.Tensor + - action: (Tp, Da) torch.Tensor + """ + do_img_aug = False if no_aug else self.img_aug + if self.adaptive_resize: + assert data["dataset"] is not None + match data["dataset"]: + case "egodex": + target_size = self.img_sizes["egodex"] + case "humanoid-everyday": + target_size = self.img_sizes["he"] + case _: + target_size = (256, 256) + resizer = ResizeImage(size=tuple(target_size))() # type: ignore + else: + resizer = self.resize() + t1 = v2.Compose([ + resizer, + self.color_jitter() if do_img_aug else v2.Identity(), + ]) + + images = [t1(img) for img in data["observations"]] + instruction = data["instruction"] + state = data["states"] + action = data["actions"] + inputs, num_answer_tokens_list = self.build_qwenvl_inputs( + vlm_processor, action_tokenizer, [images], [instruction], [state], [action] + ) + labels = copy.deepcopy(inputs["input_ids"]) + # keep loss on the answer + EOS + formatting tokens + labels[:, : -(num_answer_tokens_list[0] + 2)] = IGNORE_INDEX + inputs["labels"] = labels + inputs["dataset_name"] = data.get("dataset", "unknown") + inputs["raw_actions"] = action + # inputs["raw_instruction"] = instruction + inputs["raw_images"] = np.stack([np.array(img) for img in images]) + return inputs + + def build_qwenvl_inputs( + self, + vlm_processor, + action_tokenizer, + images, + instructions, + states, + actions, + **kwargs, + ): + """adapted from Qwen_VL_Interface.build_qwenvl_inputs""" + # Create messages: one message per sample + messages = [] + num_answer_tokens_list = [] + assert len(images) == len( + instructions + ), "Images and instructions must have the same length" + + for imgs, instruction, action in zip(images, instructions, actions): + tokenized_action = action_tokenizer(action) + raw_action_tokens = vlm_processor.tokenizer(tokenized_action)["input_ids"] + num_answer_tokens = len(raw_action_tokens) + num_answer_tokens_list.append(num_answer_tokens) + + # print(action[0].shape) + content = [{"type": "image", "image": img} for img in imgs] + content.append({"type": "text", "text": instruction}) + user_msg = {"role": "user", "content": content} + assistant_msg = { + "role": "assistant", + "content": [ + {"type": "text", "text": tokenized_action} + ], # squeeze batch dim + } + messages.append([user_msg, assistant_msg]) + + # Prepare text prompts using processor + # default process is json --> message --> texts --> input_ids + texts = [ + vlm_processor.apply_chat_template( + m, tokenize=False, add_generation_prompt=False + ) + for m in messages + ] + + try: + from qwen_vl_utils import process_vision_info + except: + raise ImportError("qwen_vl_utils not found, make sure to install it if using Qwen-VL model!") + image_inputs, video_inputs = process_vision_info(messages, image_patch_size=16) # type: ignore + inputs = vlm_processor( + text=texts, + images=image_inputs, + videos=video_inputs, + padding=True, + return_tensors="pt", + ) + return inputs, num_answer_tokens_list + +class HEPosttrainRepackTransform(RepackTransform): + dataset_name: str = "humanoid-everyday" + num_past_frames: int = 0 + action_chunk_size: int = 16 + use_delta_actions: bool = True + pad_action_dim: int | None = None + pad_state_dim: int | None = None + + def __call__( + self, + data: dict[str, Any], + **kwargs, + ) -> dict[str, Any]: + # HACK: different action format determines robot type + if data["action.joint_angles"].shape[1] == 26: + # H1 robot + raw = data["action.joint_angles"] + actions = np.concatenate([ + raw[:, 6:12][::-1], # from left thumb to little + np.zeros((raw.shape[0], 1), dtype=np.float32), # pad finger to 7 dof + raw[:, 0:6][::-1], # from right thumb to little + np.zeros((raw.shape[0], 1), dtype=np.float32), # pad finger to 7 dof + raw[:, 12:], # left arm + right arm (each from should to wrist) + ], axis=1).astype(np.float32) + hand = data["observation.hand_joints"] + states = np.concatenate([ + hand[:, 6:12], + np.zeros((hand.shape[0], 1), dtype=np.float32), # pad finger to 7 dof + hand[:, 0:6], + np.zeros((hand.shape[0], 1), dtype=np.float32), # pad finger to 7 dof + data["observation.arm_joints"] + ], axis=1).astype(np.float32) + else: + actions = data["action.joint_angles"].astype(np.float32) + states = np.concatenate((data["observation.hand_joints"], data["observation.arm_joints"]), axis=1).astype(np.float32) + + + if self.use_delta_actions: + actions = actions[1:] - actions[:-1] + action_mask = data["action_mask"][1:] + else: + action_mask = data["action_mask"] + + # Expand action_mask to (T, Da) by repeating along last dimension + if action_mask.ndim == 1: + action_mask = np.repeat(action_mask[:, None], actions.shape[1], axis=1) + + states, _ = pad_to_len(states, self.pad_state_dim) if self.pad_state_dim is not None else (states, None) + if self.pad_action_dim is not None: + actions, _ = pad_to_len(actions, self.pad_action_dim) + mask, _ = pad_to_len(action_mask, self.pad_action_dim) + else: + mask = np.ones_like(actions, dtype=bool) + # mask = action_mask + return { + "observations": [pt_to_pil(img, normalized=False) for img in data["observation.images.egocentric"]], + "states": states, + "actions": actions, + "instruction": str(data["task"]).lower(), + "dataset": data.get("dataset_name", self.dataset_name), + "actions_mask": mask,# torch.from_numpy(mask), + "obs_mask": data["obs_mask"], #torch.from_numpy(data["obs_mask"]), + } + +class MixedRepackTransform(RepackTransform): + dataset_name: str = "mixed" + + num_past_frames: int = 0 + action_chunk_size: int = 1 + use_delta_actions: bool = True + robot_type: str = "mixed" + stage: str = "pretrain" # "pretrain" or "postpre" + pad_action_dim: int | None = None + pad_state_dim: int | None = None + + def model_post_init(self, __context: Any) -> None: + if self.stage == "pretrain": + self._he_repacker = HEPretrainRepackTransform.model_validate(self.model_dump()) + else: + self._he_repacker = HEPosttrainRepackTransform.model_validate(self.model_dump()) + self._egodex_repacker = EgodexRepackTransform.model_validate(self.model_dump()) + + def __call__(self, data: dict[str, Any], **kwargs) -> dict[str, Any]: + dataset_name = data.get("dataset_name", "default") + match(dataset_name): + case "egodex": + return self._egodex_repacker(data, **kwargs) + case "humanoid-everyday": + return self._he_repacker(data, **kwargs) + return data + +# +class SeedPosttrainRepackTransform(ModelTransform): + dataset_name: str = "seed-g1" + action_chunk_size: int = 16 + use_delta_actions: bool = False + # TextOp 39D: joint_abs(29)+root_delta_xy(2)+root_rot5(5)+root_height(1)+foot_contact(2). + textop_actions: bool = False + # 简单hybrid baseline:关节action保持绝对值,root xyz/quat使用相邻帧delta。 + root_delta_actions: bool = False + # 用于驱动SEED数据集返回过去几帧state;repack本身保持透传。 + num_past_frames: int = 0 + pad_action_dim: int | None = None + pad_state_dim: int | None = None + + def __call__( + self, + data: dict[str, Any], + **kwargs, + ) -> dict[str, Any]: + actions = np.asarray(data["actions"], dtype=np.float32) + action_mask = np.asarray( + data.get("action_mask", np.ones(actions.shape[0], dtype=np.float32)), + dtype=np.float32, + ) + # root delta第一帧需要用当前state作为起点,后续用上一帧action。 + states = data.get("states", np.zeros((1, actions.shape[1]), dtype=np.float32)) + states = np.asarray(states, dtype=np.float32) + if states.ndim == 1: + states = states[None, :] + + # 三种动作模式只能开一个,避免训练目标和推理decode语义混在一起。 + enabled_action_modes = int(self.textop_actions) + int(self.use_delta_actions) + int(self.root_delta_actions) + if enabled_action_modes > 1: + raise ValueError("Only one of textop_actions/use_delta_actions/root_delta_actions can be enabled") + + # TextOp模式下action/state都转到同一套39D motion token,保证训推输入语义一致。 + if self.textop_actions: + if actions.shape[1] < 36 or states.shape[1] < 36: + raise ValueError("textop_actions requires 36D SEED action/state") + actions = build_seed_textop_action(actions[:, :36], states[:, :36]) + states = build_seed_textop_state(states[:, :36]) + if action_mask.ndim == 1: + action_mask = np.repeat(action_mask[:, None], actions.shape[1], axis=1) + elif action_mask.shape[1] != actions.shape[1]: + action_mask = np.ones_like(actions, dtype=np.float32) + elif self.use_delta_actions: + if actions.shape[0] < 2: + raise ValueError("SeedPosttrainRepackTransform requires at least 2 action steps for delta mode") + actions = actions[1:] - actions[:-1] + action_mask = action_mask[1:] + if "future_progress" in data: + data = dict(data) + data["future_progress"] = np.asarray(data["future_progress"], dtype=np.float32)[1:] + elif self.root_delta_actions: + if actions.shape[1] < 36 or states.shape[1] < 36: + raise ValueError("root_delta_actions requires 36D SEED action/state") + # 原逻辑:root xyz/quat 7维都做分量相减,保留为旧checkpoint兼容分支。 + prev_root = np.concatenate([states[-1:, 29:36], actions[:-1, 29:36]], axis=0) + actions = actions.copy() + actions[:, 29:36] = actions[:, 29:36] - prev_root + + if action_mask.ndim == 1: + action_mask = np.repeat(action_mask[:, None], actions.shape[1], axis=1) + + states, _ = ( + pad_to_len(states, self.pad_state_dim) + if self.pad_state_dim is not None + else (states, None) + ) + if self.pad_action_dim is not None: + actions, _ = pad_to_len(actions, self.pad_action_dim) + action_mask, _ = pad_to_len(action_mask, self.pad_action_dim) + + instruction = str(data.get("instruction", "")).strip() + output = { + "states": states.astype(np.float32), + "actions": actions.astype(np.float32), + "instruction": instruction, + "dataset": data.get("dataset_name", self.dataset_name), + "actions_mask": action_mask.astype(np.float32), + } + if "progress" in data: + output["progress"] = np.asarray(data["progress"], dtype=np.float32) + if "future_progress" in data: + output["future_progress"] = np.asarray(data["future_progress"], dtype=np.float32) + # 音频训练数据需要把已切好的audio片段和对齐信息一路透传到model transform。 + # video->motion训练同样透传整段抽帧视频,保证train/infer输入一致。 + for key in ( + "row_id", + "move_name", + "motion_name", + "motion_dir", + "frame_id", + "audio", + "audio_path", + "audio_start_sec", + "audio_duration_sec", + "full_audio", + "video", + "video_path", + "video_sample_fps", + "video_size_pixels", + "source_video_fps", + "video_duration_sec", + "full_video", + "video_embed_path", + "pixel_values_videos", + "video_grid_thw", + "video_second_per_grid", + "total_frames", + ): + if key in data: + output[key] = data[key] + return output + + +class Psi0ModelTransform(ModelTransform): + resize: ResizeImage = Field(default_factory=lambda: ResizeImage(size=(224))) + center_crop: CenterCrop = Field(default_factory=lambda: CenterCrop(size=224)) + color_jitter: ColorJitter = Field(default_factory=lambda: ColorJitter()) + gaussian_noise: GaussianNoise = Field(default_factory=lambda: GaussianNoise(mean=0, std=3, prob_skip=0.1)) + img_aug: bool = False + text_only: bool = False # + # 音频输入分支:可在同一个Qwen-Omni user message中同时放audio和text。 + audio_only: bool = False + # 纯视频输入分支:只使用video token,可选携带简短文本提示。 + video_only: bool = False + video_size_pixels: int = 50176 + + # for mixed dataset with different image sizes + adaptive_resize: bool = False + img_sizes: dict[str, Any] = Field(default_factory=lambda: { + "egodex": [270, 480], + "he" : [240, 320], + }) + + def __call__( + self, data: dict[str, Any], vlm_processor=None, no_aug=False, **kwargs + ) -> dict[str, Any]: + if self.video_only and ("video" in data or "pixel_values_videos" in data): + instruction = str(data.get("instruction", "")).strip() + inputs = self.build_qwenvl_inputs( + vlm_processor, + imgs=None, + instruction=instruction, + video=data.get("video", None), + video_path=data.get("video_path", "video.mp4"), + video_size_pixels=data.get("video_size_pixels", self.video_size_pixels), + video_sample_fps=data.get("video_sample_fps", 1.0), + pixel_values_videos=data.get("pixel_values_videos", None), + video_grid_thw=data.get("video_grid_thw", None), + video_second_per_grid=data.get("video_second_per_grid", None), + ) + inputs["dataset_name"] = data.get("dataset", "unknown") + inputs["raw_actions"] = data["raw_actions"] + if "actions_mask" in data: + inputs["actions_mask"] = data["actions_mask"] + inputs["actions"] = data["actions"] + inputs["states"] = data["states"] + inputs["instruction"] = instruction + if "progress" in data: + inputs["progress"] = data["progress"] + if "future_progress" in data: + inputs["future_progress"] = data["future_progress"] + for key in ( + "row_id", + "move_name", + "motion_name", + "motion_dir", + "frame_id", + "total_frames", + "video_path", + "video_sample_fps", + "video_size_pixels", + "source_video_fps", + "video_duration_sec", + "full_video", + "video_embed_path", + ): + if key in data: + inputs[key] = data[key] + return inputs + + # 音频分支,使用Qwen2.5-Omni processor把音频片段转成input_features。 + if self.audio_only and "audio" in data: + instruction = str(data.get("instruction", "")) + inputs = self.build_qwenvl_inputs( + vlm_processor, + imgs=None, + instruction=instruction, + audio=data["audio"], + audio_path=data.get("audio_path", "audio.wav"), + ) + inputs["dataset_name"] = data.get("dataset", "unknown") + inputs["raw_actions"] = data["raw_actions"] + if "actions_mask" in data: + inputs["actions_mask"] = data["actions_mask"] + inputs["actions"] = data["actions"] + inputs["states"] = data["states"] + inputs["instruction"] = instruction + if "progress" in data: + inputs["progress"] = data["progress"] + if "future_progress" in data: + inputs["future_progress"] = data["future_progress"] + for key in ( + "row_id", + "move_name", + "motion_name", + "motion_dir", + "frame_id", + "total_frames", + "audio_path", + "audio_start_sec", + "audio_duration_sec", + "full_audio", + ): + if key in data: + inputs[key] = data[key] + return inputs + + # 纯文本分支,去掉image处理和读取 + if self.text_only: + instruction = data["instruction"] + inputs = self.build_qwenvl_inputs(vlm_processor, imgs=None, instruction=instruction) + inputs["dataset_name"] = data.get("dataset", "unknown") + inputs["raw_actions"] = data["raw_actions"] + if "actions_mask" in data: + inputs["actions_mask"] = data["actions_mask"] + inputs["actions"] = data["actions"] + inputs["states"] = data["states"] + inputs["instruction"] = instruction + if "progress" in data: + inputs["progress"] = data["progress"] + if "future_progress" in data: + inputs["future_progress"] = data["future_progress"] + # 临时透传样本来源信息,方便定位大梯度batch。 + for key in ("row_id", "move_name", "motion_dir", "frame_id", "total_frames"): + if key in data: + inputs[key] = data[key] + return inputs + + # 原VL分支 + do_img_aug = False if no_aug else self.img_aug + if self.adaptive_resize: + assert data["dataset"] is not None + match data["dataset"]: + case "egodex": + target_size = self.img_sizes["egodex"] + case "humanoid-everyday": + target_size = self.img_sizes["he"] + case _: + target_size = (256, 256) + resizer = ResizeImage(size=tuple(target_size))() # type: ignore + center_crop = CenterCrop(size=tuple(target_size))() # type: ignore + else: + resizer = self.resize() + center_crop = self.center_crop() + + t1 = v2.Compose([ + resizer, + center_crop, + self.color_jitter() if do_img_aug else v2.Identity(), + ]) + + images = [t1(img) for img in data["observations"]] + instruction = data["instruction"] + + inputs: dict = self.build_qwenvl_inputs( + vlm_processor, images, instruction, + ) # type: ignore + inputs["dataset_name"] = data.get("dataset", "unknown") + inputs["raw_actions"] = data["raw_actions"] + if "actions_mask" in data: + inputs["actions_mask"] = data["actions_mask"] + + inputs["raw_images"] = images + inputs['actions'] = data["actions"] + inputs['states'] = data["states"] + inputs['instruction'] = data["instruction"] + if "progress" in data: + inputs["progress"] = data["progress"] + if "future_progress" in data: + inputs["future_progress"] = data["future_progress"] + # 临时透传样本来源信息,方便定位大梯度batch。 + for key in ("row_id", "move_name", "motion_dir", "frame_id", "total_frames"): + if key in data: + inputs[key] = data[key] + return inputs + + def build_qwenvl_inputs( + self, + vlm_processor, + imgs, + instruction, + audio=None, + audio_path=None, + video=None, + video_path=None, + video_size_pixels=None, + video_sample_fps=1.0, + pixel_values_videos=None, + video_grid_thw=None, + video_second_per_grid=None, + # state, + # action, + **kwargs, + ) -> tuple[dict, int]: + # + omni_default_system = ( + "You are Qwen, a virtual human developed by the Qwen Team, Alibaba Group, " + "capable of perceiving auditory and visual inputs, as well as generating text and speech." + ) + + # 音频训练:真实waveform通过processor的audio参数传入; + # user content按Qwen-Omni混合媒体格式排列为audio后接可选text。 + if audio is not None: + content = [{"type": "audio", "audio": str(audio_path or "audio.wav")}] + if str(instruction).strip(): + content.append({"type": "text", "text": str(instruction).strip()}) + messages = [[ + {"role": "system", "content": [{"type": "text", "text": omni_default_system}]}, + {"role": "user", "content": content}, + ]] + texts = [ + vlm_processor.apply_chat_template(m, tokenize=False, add_generation_prompt=True) + for m in messages + ] + inputs = vlm_processor( + text=texts, + audio=[np.asarray(audio, dtype=np.float32)], + padding=True, + return_tensors="pt", + ) + return inputs + + if video is not None or pixel_values_videos is not None: + size_pixels = int(video_size_pixels or self.video_size_pixels) + content = [{"type": "video", "video": str(video_path or "video.mp4")}] + if str(instruction).strip(): + content.append({"type": "text", "text": str(instruction).strip()}) + messages = [[ + {"role": "system", "content": [{"type": "text", "text": omni_default_system}]}, + {"role": "user", "content": content}, + ]] + texts = [ + vlm_processor.apply_chat_template(m, tokenize=False, add_generation_prompt=True) + for m in messages + ] + if pixel_values_videos is not None and video_grid_thw is not None: + video_grid_arr = np.asarray(video_grid_thw) + video_grid_for_text = video_grid_arr[0] if video_grid_arr.ndim == 2 else video_grid_arr + if video_second_per_grid is None: + second_per_grid = float(vlm_processor.video_processor.temporal_patch_size) / float(video_sample_fps) + video_second_per_grid = np.asarray([second_per_grid], dtype=np.float32) + texts = vlm_processor.replace_multimodal_special_tokens( + texts, + iter([]), + iter([]), + iter([video_grid_for_text]), + iter([float(np.asarray(video_second_per_grid).reshape(-1)[0])]), + False, + 25, + 2.0, + ) + inputs = vlm_processor.tokenizer( + texts, + padding=True, + return_tensors="pt", + ) + inputs = {k: v for k, v in inputs.items()} + inputs["pixel_values_videos"] = torch.as_tensor(pixel_values_videos, dtype=torch.float32) + inputs["video_grid_thw"] = torch.as_tensor(video_grid_thw, dtype=torch.long) + inputs["video_second_per_grid"] = torch.as_tensor( + video_second_per_grid, + dtype=torch.float32, + ).reshape(-1) + return inputs + + return vlm_processor( + text=texts, + videos=[np.asarray(video, dtype=np.uint8)], + padding=True, + return_tensors="pt", + use_audio_in_video=False, + videos_kwargs={ + "size": { + "shortest_edge": size_pixels, + "longest_edge": size_pixels, + }, + "fps": float(video_sample_fps), + }, + ) + + # + if imgs is None: + messages = [[ + {"role": "system", "content": [{"type": "text", "text": omni_default_system}]}, + {"role": "user", "content": [{"type": "text", "text": instruction}]}, + ]] + texts = [ + vlm_processor.apply_chat_template(m, tokenize=False, add_generation_prompt=True) # 把人类语言转换成模型喜欢的特定格式,未tokenize + for m in messages + ] + # 查字典,把字符串变成token,找到对应id + inputs = vlm_processor( + text=texts, + padding=True, + return_tensors="pt", + ) + return inputs + + + from qwen_vl_utils import process_vision_info + + """adapted from Qwen_VL_Interface.build_qwenvl_inputs""" + messages = [] + + content = [{"type": "image", "image": img} for img in imgs] + content.append({"type": "text", "text": instruction}) + user_msg = {"role": "user", "content": content} + # + system_msg = {"role": "system", "content": [{"type": "text", "text": omni_default_system}]} + messages.append([system_msg, user_msg]) + # messages.append([user_msg]) + + # Prepare text prompts using processor + # default process is json --> message --> texts --> input_ids + texts = [ + vlm_processor.apply_chat_template( + m, tokenize=False, add_generation_prompt=True + ) + for m in messages + ] + image_inputs, video_inputs = process_vision_info(messages, image_patch_size=16) # type: ignore + inputs = vlm_processor( + text=texts, + images=image_inputs, + videos=video_inputs, + padding=True, + return_tensors="pt", + ) + return inputs + +class ACTModelTransform(ModelTransform): + resize: ResizeImage + center_crop: CenterCrop + color_jitter: ColorJitter + normalize: Normalize + img_aug: bool = False + + def __call__( + self, + data: dict[str, Any], + no_aug: bool = False, + **kwargs, + ) -> dict[str, Any]: + do_img_aug = False if no_aug else self.img_aug + + # Image transform pipeline + t = v2.Compose( + [ + v2.ToImage(), + v2.ToDtype(torch.float32, scale=True), + self.resize(), + self.center_crop(), + self.color_jitter() if do_img_aug else v2.Identity(), + self.normalize(), + ] + ) + + processed_images = torch.stack([t(img) for img in data["observations"]]) # (N*To, C, H, W) + + if "action_is_pad" in data: + # LerobotRepackTransform format: (chunk_size,), True=padded + action_is_pad = data["action_is_pad"] + elif "actions_mask" in data: + # WeGR00TRepackTransform format: (chunk_size, Da), True=original, False=padded + actions_mask = data["actions_mask"] + if isinstance(actions_mask, np.ndarray): + actions_mask = actions_mask.astype(bool) + # (chunk_size, Da) --> (chunk_size,) + if actions_mask.ndim == 2: + action_is_pad = ~actions_mask[:, 0] # Invert: True->False (original), False->True (padded) + else: + action_is_pad = ~actions_mask + else: + actions_mask = actions_mask.bool() if hasattr(actions_mask, 'bool') else actions_mask + action_is_pad = ~actions_mask[:, 0] if len(actions_mask.shape) == 2 else ~actions_mask + else: + action_is_pad = np.zeros(data["actions"].shape[0], dtype=bool) + + return { + "observation.images": processed_images, # (N*To, C, H, W) + "observation.state": data["states"], # (state_dim,) or (obs_horizon, state_dim) + "action": data["actions"], # (chunk_size, action_dim) + "action_is_pad": action_is_pad, # (chunk_size,), standardized: True=padded, False=original + } + +class DiffusionPolicyModelTransform(ModelTransform): + resize: ResizeImage + color_jitter: ColorJitter + center_crop: CenterCrop + normalize: Normalize + img_aug: bool = False + + def __call__( + self, + data: dict[str, Any], + no_aug: bool = False, + **kwargs, + ) -> dict[str, Any]: + + # import torch + # try: + # from torchvision.transforms import v2 + # except: + # from torchvision import transforms as v2 + do_img_aug = False if no_aug else self.img_aug + t = v2.Compose( + [ + v2.ToImage(), + v2.ToDtype(torch.float32, scale=True), + self.resize(), + self.center_crop(), + self.color_jitter() if do_img_aug else v2.Identity(), + self.normalize(), + ] + ) + processed_images = torch.stack([t(img) for img in data["observations"]]) # (To, 3, H, W) + + if "action_is_pad" in data: + # LerobotRepackTransform format: (chunk_size,), True=padded + action_is_pad = data["action_is_pad"] + elif "actions_mask" in data: + # WeGR00TRepackTransform format: (chunk_size, Da), True=original, False=padded + actions_mask = data["actions_mask"] + if isinstance(actions_mask, np.ndarray): + actions_mask = actions_mask.astype(bool) + # (chunk_size, Da) --> (chunk_size,) + if actions_mask.ndim == 2: + action_is_pad = ~actions_mask[:, 0] # Invert: True->False (original), False->True (padded) + else: + action_is_pad = ~actions_mask + else: + actions_mask = actions_mask.bool() if hasattr(actions_mask, 'bool') else actions_mask + action_is_pad = ~actions_mask[:, 0] if len(actions_mask.shape) == 2 else ~actions_mask + else: + action_is_pad = np.zeros(data["actions"].shape[0], dtype=bool) + + + return dict( + image=processed_images, # (To, 3, H, W) + agent_pos=data["states"], # (To, Da) + action=data["actions"], #(Tp, Da) + action_is_pad=action_is_pad, # (Tp,), standardized: True=padded, False=original + ) + + # return { + # "observation.images": processed_images, # (To, C, H, W) + # "observation.state": data["states"], # (To, state_dim) + # "action": data["actions"], # (Tp, Da) + # } diff --git a/src/psi/data/__init__.py b/src/psi/data/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..f7ce572ff90bcc6e96a71edb286ddfbe3196f3ce --- /dev/null +++ b/src/psi/data/__init__.py @@ -0,0 +1,3 @@ + +from .dataset import Dataset, IterableDataset +# from .collator import PaddedCollatorForActionPrediction diff --git a/src/psi/data/dataset.py b/src/psi/data/dataset.py new file mode 100644 index 0000000000000000000000000000000000000000..35bcce6818ae39062a9bf0e6cc62183172873ba4 --- /dev/null +++ b/src/psi/data/dataset.py @@ -0,0 +1,88 @@ +from __future__ import annotations +from typing import Any, Dict, TYPE_CHECKING + +if TYPE_CHECKING: + from psi.config.config import DataConfig + from psi.data.sampler import DatasetSpec + + +from collections.abc import Iterator, Sized +import torch +from torch.utils.data import Dataset as TorchDataset +from torch.utils.data import IterableDataset as TorchIterableDataset + +class Dataset(torch.utils.data.Dataset): + def __init__(self, dataCfg: DataConfig, dataset: TorchDataset|Any, **kwargs) -> None: + self.raw_dataset = dataset + self.transform = dataCfg.transform + self.transform_kwargs = kwargs.get("transform_kwargs", {}) + + def __len__(self): + return len(self.raw_dataset) # type: ignore + + def __getitem__(self, idx: int) -> dict[str, Any]: + raw = self.raw_dataset[idx] + data = self.transform(raw, **self.transform_kwargs) + if isinstance(raw, dict): + for key in ("task_name", "task_family"): + if key in raw and key not in data: + data[key] = raw[key] + return data + + @property + def dataset_length(self) -> int: + return len(self.raw_dataset) # type: ignore + + @property + def dataset_statistics(self) -> Dict[str, Any]: + return getattr(self.raw_dataset, "dataset_statistics", {}) + +class IterableDataset(torch.utils.data.IterableDataset): + + def __init__( + self, dataCfg: DataConfig, dataset: TorchIterableDataset|Any, **kwargs + ) -> None: + self.raw_dataset = dataset + self.transform = dataCfg.transform + self.transform_kwargs = kwargs.get("transform_kwargs", {}) + + def __iter__(self) -> Iterator[Any]: + for rlds_batch in self.raw_dataset: + try: + data = self.transform(rlds_batch, **self.transform_kwargs) + if isinstance(rlds_batch, dict): + for key in ("task_name", "task_family"): + if key in rlds_batch and key not in data: + data[key] = rlds_batch[key] + yield data + except Exception as e: + print(f"Error processing batch: {e}") + raise e + + @property + def dataset_length(self) -> int: + return len(self.raw_dataset) # type: ignore + + @property + def dataset_statistics(self) -> Dict[str, Any]: + return getattr(self.raw_dataset, "dataset_statistics", {}) + + +class MixtureDataset(TorchDataset): + + specs: list[DatasetSpec] + + def __init__(self, datasets: dict[Dataset|IterableDataset, float], num_samples_per_epoch: int) -> None: + self.datasets = list(datasets.keys()) + self.ratios = list(datasets.values()) + self.num_samples_per_epoch = num_samples_per_epoch + + def __getitem__(self, index): + assert isinstance(index, tuple) and len(index) == 2, "Maybe forget to use batch sampler? see ../data/sampler.py" + dataset_id, sample_id = index + return self.datasets[dataset_id][sample_id] + + def __len__(self): + # raise RuntimeError("Length is defined by the sampler") + # return sum(d.dataset_length for d in self.datasets) + return self.num_samples_per_epoch diff --git a/src/psi/data/egodex/egodex_dataset.py b/src/psi/data/egodex/egodex_dataset.py new file mode 100644 index 0000000000000000000000000000000000000000..adfbc6390bf4d4e7c1d4d305fe8eff4b2c2d0e44 --- /dev/null +++ b/src/psi/data/egodex/egodex_dataset.py @@ -0,0 +1,585 @@ +#!/usr/bin/env python3 +""" +EgoDex dataset loader +Implements 48-dimensional hand action representation, single-view images and language embedding data loading + +-- adapted from H-RDT +""" + +import h5py +import numpy as np +import torch +import os +import cv2 +from pathlib import Path +import warnings +import random +import json +from torchcodec.decoders import VideoDecoder +import time +import pickle +from scipy.spatial.transform import Rotation as R +from psi.data.egodex.utils.data_utils import convert_to_camera_frame + +warnings.filterwarnings("ignore") + +def d9_to_mat44(nine_d): + """ + Convert 9D representation back to 4x4 transformation matrix + + Args: + nine_d: 9D vector [position(3) + rotation_6d(6)] + rotation_6d is first two columns of rotation matrix + + Returns: + mat44: 4x4 transformation matrix + """ + position = nine_d[:3] + rot_col0 = nine_d[3:6] + rot_col1 = nine_d[6:9] + + # Gram-Schmidt orthogonalization to ensure valid rotation matrix + col0 = rot_col0 / (np.linalg.norm(rot_col0) + 1e-8) + col1 = rot_col1 - np.dot(rot_col1, col0) * col0 + col1 = col1 / (np.linalg.norm(col1) + 1e-8) + col2 = np.cross(col0, col1) + + mat44 = np.eye(4, dtype=nine_d.dtype) + mat44[:3, 0] = col0 + mat44[:3, 1] = col1 + mat44[:3, 2] = col2 + mat44[:3, 3] = position + return mat44 + +def delta_rpy_from_tfs(tfs): + """ + Compute delta roll, pitch, yaw between each consecutive transformation matrix. + + Args: + tfs: np.ndarray of shape (N, 4, 4) + + Returns: + delta_rpy: np.ndarray of shape (N-1, 3), where each row is (d_roll, d_pitch, d_yaw) + """ + N = tfs.shape[0] + delta_rpy = np.zeros((N-1, 3), dtype=np.float32) + for i in range(N-1): + # Get rotation matrices + R1 = tfs[i][:3, :3] + R2 = tfs[i+1][:3, :3] + # Relative rotation: R_rel = R2 * R1.T + R_rel = R2 @ R1.T + # Convert to euler angles (roll, pitch, yaw) + rpy = R.from_matrix(R_rel).as_euler('xyz', degrees=False) + delta_rpy[i] = rpy + return delta_rpy + +def points_to_camera(points_3d, cam_ext): + # Convert to homogeneous coordinates + points_3d_homo = np.hstack([points_3d, np.ones((points_3d.shape[0], 1))]) + # Transform to camera coordinates using extrinsics + points_cam = (np.linalg.inv(cam_ext) @ points_3d_homo.T).T + # Get 3D point sin camera frame + points_cam_3d = points_cam[:, :3] + return points_cam_3d + +def convert_to_delta_actions(actions, chunk_size, cam_ext): + left_wrist = np.empty((chunk_size,4,4), dtype=np.float32) + right_wrist = np.empty((chunk_size,4,4), dtype=np.float32) + + left_hand_finger_tips = { + "leftThumbTip": np.empty((chunk_size, 3), dtype=np.float32), + "leftIndexFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + "leftMiddleFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + "leftRingFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + "leftLittleFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + } + right_hand_finger_tips = { + "rightThumbTip": np.empty((chunk_size, 3), dtype=np.float32), + "rightIndexFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + "rightMiddleFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + "rightRingFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + "rightLittleFingerTip": np.empty((chunk_size, 3), dtype=np.float32), + } + + for i in range(actions.shape[0]): + # action = actions[i] + left_wrist[i] = d9_to_mat44(actions[i, :9]) + left_hand_finger_tips["leftThumbTip"][i] = actions[i, 9:12] + left_hand_finger_tips["leftIndexFingerTip"][i] = actions[i, 12:15] + left_hand_finger_tips["leftMiddleFingerTip"][i] = actions[i, 15:18] + left_hand_finger_tips["leftRingFingerTip"][i] = actions[i, 18:21] + left_hand_finger_tips["leftLittleFingerTip"][i] = actions[i, 21:24] + + right_wrist[i] = d9_to_mat44(actions[i, 24:33]) + right_hand_finger_tips["rightThumbTip"][i] = actions[i, 33:36] + right_hand_finger_tips["rightIndexFingerTip"][i] = actions[i, 36:39] + right_hand_finger_tips["rightMiddleFingerTip"][i] = actions[i, 39:42] + right_hand_finger_tips["rightRingFingerTip"][i] = actions[i, 42:45] + right_hand_finger_tips["rightLittleFingerTip"][i] = actions[i, 45:48] + + left_wrist_tfs_in_cam = convert_to_camera_frame(left_wrist, cam_ext) # (N,4,4) + right_wrist_tfs_in_cam = convert_to_camera_frame(right_wrist, cam_ext) + + # convert to deleta action representations + delta_left_wrist_xyz = left_wrist_tfs_in_cam[1:, :3, 3] - left_wrist_tfs_in_cam[:-1, :3, 3] + delta_left_wrist_rpy = delta_rpy_from_tfs(left_wrist_tfs_in_cam) + + delta_right_wrist_xyz = right_wrist_tfs_in_cam[1:, :3, 3] - right_wrist_tfs_in_cam[:-1, :3, 3] + delta_right_wrist_rpy = delta_rpy_from_tfs(right_wrist_tfs_in_cam) + + leftThumbTip = points_to_camera(left_hand_finger_tips["leftThumbTip"], cam_ext) + delta_left_thumbtip = leftThumbTip[1:] - leftThumbTip[:-1] + leftIndexFingerTip = points_to_camera(left_hand_finger_tips["leftIndexFingerTip"], cam_ext) + delta_left_indextip = leftIndexFingerTip[1:] - leftIndexFingerTip[:-1] + leftMiddleFingerTip = points_to_camera(left_hand_finger_tips["leftMiddleFingerTip"], cam_ext) + delta_left_middletip = leftMiddleFingerTip[1:] - leftMiddleFingerTip[:-1] + leftRingFingerTip = points_to_camera(left_hand_finger_tips["leftRingFingerTip"], cam_ext) + delta_left_ringtip = leftRingFingerTip[1:] - leftRingFingerTip[:-1] + leftLittleFingerTip = points_to_camera(left_hand_finger_tips["leftLittleFingerTip"], cam_ext) + delta_left_littletip = leftLittleFingerTip[1:] - leftLittleFingerTip[:-1] + + rightThumbTip = points_to_camera(right_hand_finger_tips["rightThumbTip"], cam_ext) + delta_right_thumbtip = rightThumbTip[1:] - rightThumbTip[:-1] + rightIndexFingerTip = points_to_camera(right_hand_finger_tips["rightIndexFingerTip"], cam_ext) + delta_right_indextip = rightIndexFingerTip[1:] - rightIndexFingerTip[:-1] + rightMiddleFingerTip = points_to_camera(right_hand_finger_tips["rightMiddleFingerTip"], cam_ext) + delta_right_middletip = rightMiddleFingerTip[1:] - rightMiddleFingerTip[:-1] + rightRingFingerTip = points_to_camera(right_hand_finger_tips["rightRingFingerTip"], cam_ext) + delta_right_ringtip = rightRingFingerTip[1:] - rightRingFingerTip[:-1] + rightLittleFingerTip = points_to_camera(right_hand_finger_tips["rightLittleFingerTip"], cam_ext) + delta_right_littletip = rightLittleFingerTip[1:] - rightLittleFingerTip[:-1] + + # re-construct the delta actions + delta_actions = np.concatenate([ + delta_left_wrist_xyz, + delta_left_wrist_rpy, + np.zeros_like(delta_left_wrist_rpy), + delta_left_thumbtip, + delta_left_indextip, + delta_left_middletip, + delta_left_ringtip, + delta_left_littletip, + delta_right_wrist_xyz, + delta_right_wrist_rpy, + np.zeros_like(delta_right_wrist_rpy), + delta_right_thumbtip, + delta_right_indextip, + delta_right_middletip, + delta_right_ringtip, + delta_right_littletip, + ], axis=1) # (N-1, 48) + return delta_actions + +class EgoDexDataset: + """EgoDex dataset loader""" + + def __init__( + self, + data_root=None, + upsample_rate=3, + val=False, + # use_precomp_lang_embed=True, + chunk_size=1, + img_history_size=1, + # require_image=True, + viz=False, + use_delta_actions=False, + load_retarget=False, + data_downsample=1, + ): + """ + Args: + data_root: Data root directory (e.g., "/share/hongzhe/datasets/egodex") + upsample_rate: Temporal data upsampling rate (frame sampling interval) + val: Whether it's validation set (True for test, False for train) + use_precomp_lang_embed: Whether to use precomputed language embeddings + """ + self.DATASET_NAME = "egodex" + self.data_root = Path(data_root) + self.upsample_rate = upsample_rate + self.val = val + self.viz = viz + self.use_delta_actions = use_delta_actions + self.load_retarget = load_retarget + # self.use_precomp_lang_embed = use_precomp_lang_embed + # self.require_image = require_image + + self.chunk_size = chunk_size + # self.state_dim = 48 + self.img_history_size = img_history_size + self.data_downsample = data_downsample + + # Load data file list + self.data_files = self._load_file_list() + split_name = "test" if self.val else "train" + print(f"Loaded {len(self.data_files)} {split_name} data files") + # if len(self.data_files) == 0: + # raise ValueError(f"No data files found in {self.data_root} for split {split_name}") + + def get_dataset_name(self): + """Return dataset name""" + return self.DATASET_NAME + + def _load_file_list(self): + """Load data file list, using cached file lists if available.""" + data_files = [] + + def cache_path(part_dir): + if self.load_retarget: + part = part_dir.name + return part_dir.parent.parent / "egodex_retargeting" / f"egodex_{part}_filelist.pkl" + else: + return part_dir / "filelist_cache.pkl" + + if not self.val: + # Training set: part1-part5 + extra + if self.load_retarget: + parts = ["part1", "part2", "part3", "part4", "part5"] + else: + parts = ["part1", "part2", "part3", "part4", "part5", "extra"] + for part in parts: + part_dir = self.data_root / part + if part_dir.exists(): + cache_file = cache_path(part_dir) + if cache_file.exists(): + try: + with open(cache_file, "rb") as f: + part_files = pickle.load(f) + except Exception as e: + print(f"Warning: Failed to load cache for {part_dir}: {e}, regenerating.") + part_files = self._scan_directory(part_dir) + with open(cache_file, "wb") as f: + pickle.dump(part_files, f) + else: + part_files = self._scan_directory(part_dir) + with open(cache_file, "wb") as f: + pickle.dump(part_files, f) + data_files.extend(part_files[::self.data_downsample]) # ablation: use 10% of egodex data for pretraining + else: + # Test set: test + test_dir = self.data_root / "test" + if test_dir.exists(): + cache_file = cache_path(test_dir) + if cache_file.exists(): + try: + with open(cache_file, "rb") as f: + test_files = pickle.load(f) + except Exception as e: + print(f"Warning: Failed to load cache for {test_dir}: {e}, regenerating.") + test_files = self._scan_directory(test_dir) + with open(cache_file, "wb") as f: + pickle.dump(test_files, f) + else: + test_files = self._scan_directory(test_dir) + with open(cache_file, "wb") as f: + pickle.dump(test_files, f) + data_files.extend(test_files[::self.data_downsample]) # ablation: use 10% of egodex data for pretraining + + return data_files + + def _scan_directory(self, directory): + """Scan files in directory""" + files = [] + for task_dir in directory.iterdir(): + if task_dir.is_dir(): + # Collect all triplets: hdf5, mp4, pt + hdf5_files = list(task_dir.glob("*.hdf5")) + for hdf5_file in hdf5_files: + file_index = hdf5_file.stem # Get filename without extension + mp4_file = task_dir / f"{file_index}.mp4" + # pt_file = task_dir / f"{file_index}.pt" + + # Ensure all required files exist + if hdf5_file.exists() and mp4_file.exists(): # and pt_file.exists(): + files.append( + { + "hdf5": hdf5_file, + "mp4": mp4_file, + # "pt": pt_file, + "task": task_dir.name, + "file_index": file_index, + } + ) + + # break # FIXME for debugging + return files + + def construct_48d_action(self, hdf5_file, frame_indices): + """ + Directly extract precomputed 48-dimensional hand action representation + + Args: + hdf5_file: HDF5 file object + frame_indices: List of frame indices to extract + + Returns: + actions: (T, 57) action sequence + """ + if "actions_48d" not in hdf5_file: + raise ValueError( + "Missing precomputed actions_48d data in HDF5 file, please run precompute_48d_actions.py first" + ) + + # Directly read precomputed 48-dimensional action data + precomputed_actions = hdf5_file["actions_48d"][:] + + # Extract actions for specified frames + selected_actions = precomputed_actions[frame_indices] + + return selected_actions.astype(np.float32) + + def parse_img_data(self, mp4_path, idx): + """ + Args: + mp4_path: MP4 file path + idx: Current frame index + + Returns: + frames: (img_history_size, H, W, 3) image frames + """ + # cap = cv2.VideoCapture(str(mp4_path)) + # total_frames = int(cap.get(cv2.CAP_PROP_FRAME_COUNT)) + decoder = VideoDecoder(mp4_path, device='cpu', dimension_order='NHWC') + total_frames = len(decoder) + + # Calculate sampling range following cvpr_real_dataset.py logic + start_i = max(idx - self.img_history_size * self.upsample_rate + 1, 0) + num_frames = (idx - start_i) // self.upsample_rate + 1 + + frames = [] + + try: + for i, frame_idx in enumerate(range(start_i, idx + 1, self.upsample_rate)): + if frame_idx < total_frames: + frame = decoder[frame_idx] + if frame is not None: + # BGR to RGB + frame = frame.cpu().numpy() + frames.append(frame) + else: + print(f"Warning: Not enough frames in {mp4_path}") + break + else: + # If frame index exceeds total frames, use last valid frame + print(f"Warning: Frame index exceeds total frames in {mp4_path}") + break + except Exception as e: + raise ValueError(f"Error loading image frames: {e}") + + + # Convert to numpy array + frames = np.array(frames) # type: ignore + + # Pad if necessary (following cvpr_real_dataset.py logic) + if frames.shape[0] < self.img_history_size: + pad_frames = np.repeat( + frames[:1], self.img_history_size - frames.shape[0], axis=0 + ) + frames = np.concatenate([pad_frames, frames], axis=0) + + return frames + + + def __len__(self): + return len(self.data_files) + + def get_item(self, idx=None): + """ + Get a data sample + + Returns: + Data dictionary containing all required fields + """ + if idx is None: + idx = random.randint(0, len(self.data_files) - 1) + + file_info = self.data_files[idx % len(self.data_files)] + + def full_path(path): + if os.path.isabs(path): + return path + else: + return self.data_root / path + + def reorder(x): + # reorder 28d actions to align with he raw dataset + # [left hand, right hand, left arm, right arm] + return np.concatenate([ + x[:, 7:10], # left hand + x[:, 12:14], + x[:, 10:12], + x[:, 21:24], # right hand + x[:, 26:28], + x[:, 24:26], + x[:, 0:7], # left arm + x[:, 14:21], # right arm + ], axis=1) + + try: + # Load HDF5 data + with h5py.File(full_path(file_info["hdf5"]), "r") as root: + # Get language instruction + if root.attrs['llm_type'] == 'reversible': + direction = root.attrs['which_llm_description'] + instruction = root.attrs['llm_description' if direction == '1' else 'llm_description2'] + if isinstance(instruction, bytes): + instruction = instruction.decode("utf-8") + instruction = instruction.strip() + else: + instruction = root.attrs['llm_description'] + if isinstance(instruction, bytes): + instruction = instruction.decode("utf-8") + instruction = instruction.strip() + assert instruction is not None + + # Get total number of frames + transforms_group = root["transforms"] + total_frames = list(transforms_group.values())[0].shape[0] + + # Calculate random index following cvpr_real_dataset.py logic + max_index = total_frames - 2 + if max_index < 0: + print(f"Warning: Not enough frames in {file_info['hdf5']}") + return None + + # Random index for sampling + index = random.randint(0, max_index) + + if self.load_retarget: + # load retargeted 28 actions + if "retarget_npz" not in file_info: + raise ValueError("Missing retarget_npz info in file list") + npz_file = self.data_root.parent / "egodex_retargeting" / file_info["retarget_npz"] + retarget_npz = np.load(npz_file) + pseudo_qpos = retarget_npz["g1_qpos"] + pseudo_qpos[:, 7:14] = retarget_npz["left_hand_qpos"] + pseudo_qpos[:, 21:28] = retarget_npz["right_hand_qpos"] + assert pseudo_qpos.shape[0] == total_frames + current_action = reorder(pseudo_qpos[index:index+1]) # (1, 28) + else: + # Construct 48-dimensional actions using current index + current_action = self.construct_48d_action(root, [index]) # (1, 48) + + chunk_size = self.chunk_size + 1 if self.use_delta_actions else self.chunk_size + # Future action sequence + action_end = min( + index + chunk_size * self.upsample_rate, + max_index + 1 + ) + action_indices = list( + range(index if self.use_delta_actions else index + self.upsample_rate, + action_end, + self.upsample_rate) + ) + + # If not enough action frames, repeat the last one + while len(action_indices) < chunk_size: + action_indices.append( + action_indices[-1] if action_indices else index + 1 + ) + + if self.load_retarget: + # load retargeted 28 actions + actions = reorder(pseudo_qpos[action_indices[:chunk_size]]) # (chunk_size, 28) + else: + # Extract action sequence + actions = self.construct_48d_action( + root, action_indices[: chunk_size] + ) + + if self.viz: + # for visualizer hands + from psi.data.egodex.utils.skeleton_tfs import RIGHT_FINGERS, RIGHT_INDEX, RIGHT_THUMB, RIGHT_RING, RIGHT_MIDDLE, RIGHT_LITTLE + from psi.data.egodex.utils.skeleton_tfs import LEFT_FINGERS, LEFT_INDEX, LEFT_THUMB, LEFT_RING, LEFT_MIDDLE, LEFT_LITTLE + query_tfs = RIGHT_FINGERS + ['rightHand', 'rightForearm'] + LEFT_FINGERS + ['leftHand', 'leftForearm'] + tfdtype = root['/transforms/camera'][0].dtype # type: ignore + tfs = np.zeros([len(query_tfs), 4, 4], dtype=tfdtype) + for i, tf_name in enumerate(query_tfs): + tfs[i] = root['/transforms/' + tf_name][index] # type: ignore + + cam_ext = np.array(root['/transforms/camera'][index]) # type: ignore , extrinsics + cam_int = root['/camera/intrinsic'][:] # # type: ignore , intrinsics + + if self.use_delta_actions: + if self.load_retarget: + actions = actions[1:] - actions[:-1] + else: + actions = convert_to_delta_actions(actions, chunk_size, cam_ext) + + # Load single-view image frames using new sampling logic + image_frames = self.parse_img_data(full_path(file_info['mp4']), index) + + # Take only the required history size + image_frames = image_frames[-self.img_history_size:] + + # Load language embedding + # lang_embed_path = file_info["pt"] + + assert actions.shape[0] == self.chunk_size + result = { + "states": current_action, # (1, 48) + "actions": actions, # (chunk_size, 48) + "action_norm": np.ones_like(actions), # Action indicator + "current_images": [ + image_frames + ], # [(img_history_size, H, W, 3)] single view + "current_images_mask": [ + np.ones(self.img_history_size, dtype=bool) + ], # Image mask + "instruction": instruction, # str(lang_embed_path), # Language embedding file path + "dataset_name": self.DATASET_NAME, + "task": file_info["task"], + "file_info": { + "hdf5_path": str(file_info["hdf5"]), + "mp4_path": str(file_info["mp4"]), + # "pt_path": str(file_info["pt"]), + "total_frames": total_frames, + "selected_index": index, + "action_indices": action_indices, + }, + "dataset_name": "egodex" + } + if self.viz: + result['viz'] = { + 'tfs': tfs, # (num_tfs, 4, 4) # type: ignore + 'cam_ext': cam_ext, # (4, 4) # type: ignore + 'cam_int': cam_int, # (3, 3) # type: ignore + } + return result + + except Exception as e: + import traceback, sys + traceback.print_exc() + sys.stderr.flush() + raise e + # print(f"Error loading data {file_info['hdf5']}: {e}") + return None + + def __getitem__(self, idx): + """PyTorch Dataset interface""" + """ print(f"egodex dataset __getitem__ called with idx={idx}") + if not os.path.exists(f"tmp/{idx}.pkl"): + print(f"missing {idx}.pkl, regenerating") + data = self.get_item(idx) + pickle.dump(data, open(f"tmp/{idx}.pkl", "wb")) + else: + data = pickle.load(open(f"tmp/{idx}.pkl", "rb")) + return data """ + return self.get_item(idx) + + +if __name__ == "__main__": + # Test dataset + dataset = EgoDexDataset(data_root="./data/egodex", val=True, upsample_rate=3) + + print(f"Dataset size: {len(dataset)}") + + # Test loading samples + sample = dataset.get_item(0) + print("Sample data structure:") + for key, value in sample.items(): + if isinstance(value, np.ndarray): + print(f" {key}: {value.shape}") + else: + print(f" {key}: {type(value)}") diff --git a/src/psi/data/humanoid/he_raw_dataset.py b/src/psi/data/humanoid/he_raw_dataset.py new file mode 100644 index 0000000000000000000000000000000000000000..205a105e0d2aa2284463921b2ee6f6bf2bf990bf --- /dev/null +++ b/src/psi/data/humanoid/he_raw_dataset.py @@ -0,0 +1,237 @@ +from __future__ import annotations + +import json +from functools import lru_cache +from pathlib import Path +from typing import Any, Dict, List, Optional, Tuple +import os +import cv2 +import numpy as np +import torch +from tqdm import tqdm +import re + +class HERawDataset(torch.utils.data.Dataset): + def __init__( + self, + data_root: str, + num_past_frames: int = 0, + action_chunk_size: int = 1, + upsample_rate: int = 1, + use_delta_actions: bool = True, + robot_type: str = "both", + episodes: Optional[List[int]] = None, + force_rewrite_cache: bool = False, + read_mp4: bool = False, + ) -> None: + self.data_root = Path(data_root) + self.num_past_frames = num_past_frames + self.action_chunk_size = action_chunk_size + self.upsample_rate = upsample_rate + self.use_delta_actions = use_delta_actions + self.robot_type = robot_type + self.episodes_filter = set(episodes) if episodes is not None else None + self.read_mp4 = read_mp4 + + with open(f"{data_root}/task_description_dict.json", "r") as f: + self.task_description_dict = json.load(f) + + file_suffix = f"_{self.robot_type}" if self.robot_type != "both" else "" + if force_rewrite_cache or not os.path.exists(self.data_root / f"episode_cache{file_suffix}.json"): + self.episode_list, self.episodes_lens = self._load_episode_list() + + def to_rel_path(eps_list: list[Dict[str, Any]]) -> list[Dict[str, Any]]: + import os + root_dir_with_slash = str(self.data_root) + os.sep + for ep in eps_list: + ep["episode_path"] = str(ep["episode_path"]).replace(root_dir_with_slash, "") + ep["data_path"] = str(ep["data_path"]).replace(root_dir_with_slash, "") + return eps_list + + cache_data = { + "episode_list": to_rel_path(self.episode_list), + "episodes_lens": self.episodes_lens, + } + with open(self.data_root / f"episode_cache{file_suffix}.json", "w") as f: + json.dump(cache_data, f, indent=2, ensure_ascii=False) + + else: + with open(self.data_root / f"episode_cache{file_suffix}.json", "r") as f: + cache_data = json.load(f) + self.episode_list = cache_data["episode_list"] + self.episodes_lens = cache_data["episodes_lens"] + + self.cumsum_lens = np.cumsum(self.episodes_lens) + + def _load_episode_list(self) -> Tuple[List[Dict[str, Any]], List[int]]: + episode_list: List[Dict[str, Any]] = [] + episode_idx = 0 + for cat_dir in tqdm(sorted([p for p in self.data_root.iterdir() if p.is_dir()], key=lambda p: p.name.lower())): + for task_dir in tqdm(sorted([p for p in cat_dir.iterdir() if p.is_dir()], key=lambda p: p.name.lower()), leave=False): + task_desc = self.task_description_dict.get(task_dir.name, "") + ep_dirs = tqdm(sorted([p for p in task_dir.iterdir() if p.is_dir() and p.name.startswith("episode_")]), leave=False) + for ep_dir in ep_dirs: + if self.episodes_filter is not None and episode_idx not in self.episodes_filter: + episode_idx += 1 + continue + data_path = ep_dir / "data.json" + if not data_path.exists(): + episode_idx += 1 + continue + data_list = self._load_data_json(data_path) + if not data_list: + episode_idx += 1 + continue + ep_robot_type = self._detect_robot_type(data_list) + if self.robot_type != "both" and ep_robot_type != self.robot_type: + episode_idx += 1 + continue + episode_list.append( + { + "episode_index": episode_idx, + "episode_path": ep_dir, + "data_path": data_path, + "len_episode": len(data_list), + "instruction": task_desc, + } + ) + episode_idx += 1 + episodes_lens = [e["len_episode"] for e in episode_list] + return episode_list, episodes_lens + + @staticmethod + def _detect_robot_type(data_list: List[Dict[str, Any]]) -> str: + if not data_list: + return "h1" + for frame in data_list: + st = frame.get("states", {}) + if isinstance(st, dict) and "robot_type" in st: + try: + return str(st["robot_type"]).lower() + except Exception: + return "h1" + if "robot_type" in frame: + try: + return str(frame["robot_type"]).lower() + except Exception: + return "h1" + return "h1" + + # @staticmethod + # @lru_cache(maxsize=128) + def _load_data_json(self, path: Path) -> List[Dict[str, Any]]: + with open(os.path.join(self.data_root, path), "r") as f: + data = json.load(f) + if not isinstance(data, list): + raise ValueError(f"Expected list in {path}") + return data + + def _locate_frame(self, idx: int) -> Tuple[int, int]: + episode_id = int(np.searchsorted(self.cumsum_lens, idx, side="right")) + if episode_id == 0: + frame_id = idx + else: + frame_id = idx - int(self.cumsum_lens[episode_id - 1]) + return episode_id, frame_id + + def __len__(self) -> int: + return int(np.sum(self.episodes_lens)) + + def _load_image(self, episode_path: Path, rel_path: str) -> torch.Tensor: + img_path = self.data_root / episode_path / rel_path + if self.read_mp4: + match = re.search(r'frame_(\d+)', rel_path) + assert match is not None + frame_idx = int(match.group(1)) + ... # TODO + + image = cv2.imread(str(img_path)) + if image is None: + raise FileNotFoundError(f"Missing image: {img_path}") + image = cv2.cvtColor(image, cv2.COLOR_BGR2RGB) + image = torch.from_numpy(image).permute(2, 0, 1).float() / 255.0 + return image # (3, H, W) + + def _flatten_action(self, action_dict: Dict[str, Any]) -> Dict[str, np.ndarray]: + flat: Dict[str, np.ndarray] = {} + + def walk(prefix: str, node: Any) -> None: + if isinstance(node, dict): + for key, value in node.items(): + walk(f"{prefix}.{key}" if prefix else key, value) + return + arr = np.asarray(node, dtype=np.float32) + flat[f"action.{prefix}"] = arr + + walk("", action_dict) + return flat + + def _pad_index(self, idx: int, length: int) -> int: + if idx < 0: + return 0 + if idx >= length: + return length - 1 + return idx + + def __getitem__(self, idx: int) -> Dict[str, Any]: + episode_id, frame_id = self._locate_frame(idx) + ep_info = self.episode_list[episode_id] + data_list = self._load_data_json(ep_info["data_path"]) + length = ep_info["len_episode"] + + obs_indices = [ + self._pad_index(frame_id - (self.num_past_frames + t) * self.upsample_rate, length) + for t in range(self.num_past_frames + 1) + ] + action_len = self.action_chunk_size + (1 if self.use_delta_actions else 0) + action_indices = [ + self._pad_index(frame_id + t * self.upsample_rate, length) + for t in range(action_len) + ] + + # Compute masks for padding (1 = real, 0 = padded) + obs_mask = [ + int(0 <= frame_id - (self.num_past_frames + t) * self.upsample_rate < length) + for t in range(self.num_past_frames + 1) + ] + action_mask = [ + int(0 <= frame_id + t * self.upsample_rate < length) + for t in range(action_len) + ] + + images = [] + hand_states = [] + arm_states = [] + + for i in obs_indices: + frame = data_list[i] + images.append(self._load_image(ep_info["episode_path"], frame["image"])) + states = frame.get("states", {}) or {} + hand_states.append(np.asarray(states.get("hand_state", []), dtype=np.float32)) + arm_states.append(np.asarray(states.get("arm_state", []), dtype=np.float32)) + + action_sequences: Dict[str, List[np.ndarray]] = {} + for i in action_indices: + frame = data_list[i] + action_dict = frame.get("action", {}) or {} + flat = self._flatten_action(action_dict) + for key, value in flat.items(): + action_sequences.setdefault(key, []).append(value) + + action_arrays = {key: np.stack(vals, axis=0) for key, vals in action_sequences.items()} + instruction = ep_info["instruction"] + if not instruction: + # Fallback to episode directory name + instruction = ep_info["episode_path"].split("/")[1].replace("_", " ").lower() + return { + "observation.images.egocentric": torch.stack(images, dim=0), + "observation.hand_joints": np.stack(hand_states, axis=0), + "observation.arm_joints": np.stack(arm_states, axis=0), + **action_arrays, + "episode_index": ep_info["episode_index"], + "frame_id": frame_id, + "task": instruction, + "dataset_name": "humanoid-everyday", + "obs_mask": np.array(obs_mask, dtype=np.float32), + "action_mask": np.array(action_mask, dtype=np.float32), + } diff --git a/src/psi/data/lerobot/__init__.py b/src/psi/data/lerobot/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..4fbb8f02a48f7f11bb30d9d1ef693515374db895 --- /dev/null +++ b/src/psi/data/lerobot/__init__.py @@ -0,0 +1 @@ +from .lerobot_ext import LeRobotDatasetWrapper \ No newline at end of file diff --git a/src/psi/data/lerobot/compat.py b/src/psi/data/lerobot/compat.py new file mode 100644 index 0000000000000000000000000000000000000000..cd74728ef90067a20dad0d6377fbc033d2402a6c --- /dev/null +++ b/src/psi/data/lerobot/compat.py @@ -0,0 +1,25 @@ +from __future__ import annotations + +try: + from lerobot.common.datasets.lerobot_dataset import ( # type: ignore + LeRobotDataset, + LeRobotDatasetMetadata, + MultiLeRobotDataset, + ) + + LEROBOT_LAYOUT = "common" +except ModuleNotFoundError: + from lerobot.datasets.lerobot_dataset import ( # type: ignore + LeRobotDataset, + LeRobotDatasetMetadata, + MultiLeRobotDataset, + ) + + LEROBOT_LAYOUT = "datasets" + +__all__ = [ + "LEROBOT_LAYOUT", + "LeRobotDataset", + "LeRobotDatasetMetadata", + "MultiLeRobotDataset", +] diff --git a/src/psi/data/lerobot/lerobot_ext.py b/src/psi/data/lerobot/lerobot_ext.py new file mode 100644 index 0000000000000000000000000000000000000000..47dd6cfb8d7eb99afc66605bdfd0a2236065241f --- /dev/null +++ b/src/psi/data/lerobot/lerobot_ext.py @@ -0,0 +1,71 @@ +from __future__ import annotations +from typing import Any, Dict, TYPE_CHECKING +if TYPE_CHECKING: + from psi.config.data_lerobot import LerobotDataConfig + # from psi.config.data_simple import SimpleDataConfig + +import torch +from psi.data.lerobot.compat import ( + LeRobotDataset, + LeRobotDatasetMetadata, + MultiLeRobotDataset, +) +from psi.utils import resolve_path +from psi.config.transform import LerobotRepackTransform + +class LeRobotDatasetWrapper(torch.utils.data.Dataset): + """ A wrapper around LeRobotDataset to support multiple datasets. + """ + + def __init__( + self, + data_cfg: LerobotDataConfig, + split: str = "train" + ): + repo_ids = data_cfg.train_repo_ids if split == "train" else data_cfg.val_repo_ids + first_repo = repo_ids[0] if isinstance(repo_ids, list) else repo_ids + dataset_meta = LeRobotDatasetMetadata(first_repo, resolve_path(f"{data_cfg.root_dir}/{first_repo}")) + assert isinstance(data_cfg.transform.repack, LerobotRepackTransform) + delta_timestamps = data_cfg.transform.repack.delta_timestamps(dataset_meta.fps) + + if len(repo_ids) > 1: + root_dir = data_cfg.root_dir + lerobot_dataset_class = MultiLeRobotDataset + else: + repo_ids = first_repo + root_dir = resolve_path(f"{data_cfg.root_dir}/{first_repo}") + lerobot_dataset_class = LeRobotDataset + + self.base_dataset = lerobot_dataset_class( + repo_ids,# type: ignore + root=root_dir, + delta_timestamps=delta_timestamps, # type: ignore + image_transforms=None, + ) + self._cache = {} + + def __getitem__(self, idx) -> dict: + return self.base_dataset[idx] + + def __len__(self): + return len(self.base_dataset) + + @property + def episode_data_index(self): + return self.base_dataset.episode_data_index # type: ignore + + @property + def num_episodes(self): + return self.base_dataset.num_episodes + + @property + def num_frames(self): + return self.base_dataset.num_frames + + @property + def meta(self): + return self.base_dataset.meta # type: ignore + + @property + def stats(self): + return self.base_dataset.stats if type(self.base_dataset) == MultiLeRobotDataset else self.base_dataset.meta.stats # type: ignore diff --git a/src/psi/data/sampler.py b/src/psi/data/sampler.py new file mode 100644 index 0000000000000000000000000000000000000000..ea3ffbdcac35c6b351ab10599b8aaca660e9c61d --- /dev/null +++ b/src/psi/data/sampler.py @@ -0,0 +1,237 @@ +import torch +from torch.utils.data import Sampler +from accelerate import Accelerator +import math +import os +import random +from dataclasses import dataclass +from psi.utils import initialize_overwatch +overwatch = initialize_overwatch(__name__) + + +def _normalize_positive_weights(mixture_ratios): + weights = torch.tensor(mixture_ratios, dtype=torch.double) + if weights.numel() == 0: + raise ValueError("Sampler received empty mixture ratios") + if torch.any(weights < 0): + raise ValueError(f"Sampler received negative mixture ratios: {mixture_ratios}") + weight_sum = weights.sum() + if weight_sum.item() <= 0: + raise ValueError("Sampler received no positive mixture ratios") + weights /= weight_sum + return weights + +class BatchMixtureSampler(Sampler): + """ + Multi-node safe dataset mixture sampler. + Example: datasets = [ds1, ds2], ratio = [4, 1] + """ + def __init__(self, dataset_lens, mixture_ratios, num_samples_per_epoch, batch_size, seed=42): + self.dataset_lens = dataset_lens + self.weights = _normalize_positive_weights(mixture_ratios) + self.num_samples = num_samples_per_epoch + self.batch_size = batch_size + self.seed = seed + + # Distributed settings + self.rank = int(os.environ.get("RANK", 0)) + self.world_size = int(os.environ.get("WORLD_SIZE", 1)) + + # each rank gets this many samples + self.num_samples_rank = math.ceil(self.num_samples / self.world_size) + self.epoch = 0 + self.batch_counts = self._build_batch_counts() + + def _build_batch_counts(self): + positive_ids = [i for i, weight in enumerate(self.weights.tolist()) if weight > 0] + if not positive_ids: + raise ValueError("BatchMixtureSampler received no positive mixture ratios") + + counts = torch.zeros(len(self.dataset_lens), dtype=torch.long) + if self.batch_size >= len(positive_ids): + counts[positive_ids] = 1 + remaining = self.batch_size - len(positive_ids) + if remaining > 0: + positive_weights = self.weights[positive_ids] + positive_weights = positive_weights / positive_weights.sum() + raw_extra = positive_weights * remaining + extra_counts = torch.floor(raw_extra).long() + counts[positive_ids] += extra_counts + extra_left = remaining - int(extra_counts.sum().item()) + if extra_left > 0: + frac = raw_extra - extra_counts.double() + for frac_index in torch.argsort(frac, descending=True)[:extra_left].tolist(): + counts[positive_ids[frac_index]] += 1 + else: + for dataset_id in torch.argsort(self.weights, descending=True)[:self.batch_size].tolist(): + counts[dataset_id] += 1 + return counts.tolist() + + def set_epoch(self, epoch): + self.epoch = epoch + + def __iter__(self): + num_batches_rank = len(self) + total_batches = num_batches_rank * self.world_size + + for batch_idx in range(total_batches): + if batch_idx % self.world_size != self.rank: + continue + + g = torch.Generator() + g.manual_seed(self.seed + self.epoch * total_batches + batch_idx) + + batch = [] + for dataset_id, count in enumerate(self.batch_counts): + if count <= 0: + continue + dataset_len = int(self.dataset_lens[dataset_id]) + rand_indices = torch.randint( + low=0, + high=dataset_len, + size=(count,), + generator=g, + ).tolist() + batch.extend((dataset_id, sample_id) for sample_id in rand_indices) + + shuffle_order = torch.randperm(len(batch), generator=g).tolist() + yield [batch[i] for i in shuffle_order] + + def __len__(self): + return math.ceil(self.num_samples_rank / self.batch_size) + + +class TaskMixtureSampler(Sampler): + """ + Multi-node safe dataset mixture sampler that assigns exactly one dataset to + each global batch, then samples a full homogeneous batch from that dataset. + """ + + def __init__(self, dataset_lens, mixture_ratios, num_samples_per_epoch, batch_size, seed=42): + self.dataset_lens = dataset_lens + self.weights = _normalize_positive_weights(mixture_ratios) + self.num_samples = num_samples_per_epoch + self.batch_size = batch_size + self.seed = seed + + self.rank = int(os.environ.get("RANK", 0)) + self.world_size = int(os.environ.get("WORLD_SIZE", 1)) + + self.num_samples_rank = math.ceil(self.num_samples / self.world_size) + self.epoch = 0 + + def set_epoch(self, epoch): + self.epoch = epoch + + def _build_batch_schedule(self, total_batches): + positive_ids = [i for i, weight in enumerate(self.weights.tolist()) if weight > 0] + if not positive_ids: + raise ValueError("TaskMixtureSampler received no positive mixture ratios") + + positive_weights = self.weights[positive_ids] + positive_weights = positive_weights / positive_weights.sum() + raw_counts = positive_weights * total_batches + batch_counts = torch.floor(raw_counts).long() + batches_left = total_batches - int(batch_counts.sum().item()) + if batches_left > 0: + frac = raw_counts - batch_counts.double() + for frac_index in torch.argsort(frac, descending=True)[:batches_left].tolist(): + batch_counts[frac_index] += 1 + + schedule = [] + for local_idx, dataset_id in enumerate(positive_ids): + schedule.extend([dataset_id] * int(batch_counts[local_idx].item())) + + if len(schedule) != total_batches: + raise RuntimeError( + f"TaskMixtureSampler built invalid schedule: expected {total_batches} batches, got {len(schedule)}" + ) + + g = torch.Generator() + g.manual_seed(self.seed + self.epoch) + shuffle_order = torch.randperm(len(schedule), generator=g).tolist() + return [schedule[i] for i in shuffle_order] + + def __iter__(self): + num_batches_rank = len(self) + total_batches = num_batches_rank * self.world_size + batch_schedule = self._build_batch_schedule(total_batches) + + for batch_idx, dataset_id in enumerate(batch_schedule): + if batch_idx % self.world_size != self.rank: + continue + + dataset_len = int(self.dataset_lens[dataset_id]) + g = torch.Generator() + g.manual_seed(self.seed + self.epoch * total_batches + batch_idx) + rand_indices = torch.randint( + low=0, + high=dataset_len, + size=(self.batch_size,), + generator=g, + ).tolist() + yield [(dataset_id, sample_id) for sample_id in rand_indices] + + def __len__(self): + return math.ceil(self.num_samples_rank / self.batch_size) + +@dataclass +class DatasetSpec: + # dataset: torch.utils.data.Dataset + dataset_length: int + prob: float # mixture ratio + image_size: tuple[int, int] # e.g. 224, 448 + tokens_per_image: int # (image_size / patch)^2 + +class TokenMixtureSampler(Sampler): + """ + Multi-node safe dataset mixture sampler that samples from datasets with different image resolutoion while + keeping the total number of tokens per batch (approximately) fixed. + """ + def __init__( + self, + specs: list[DatasetSpec], + tokens_per_batch: int, # per rank + num_batches_per_rank: int, + seed: int = 0, + ): + self.specs = specs + self.tokens_per_batch = tokens_per_batch + self.num_batches_per_rank = num_batches_per_rank + self.seed = seed + + self.probs = [s.prob for s in specs] + self.epoch = 0 + + # Distributed settings + self.rank = int(os.environ.get("RANK", 0)) + self.world_size = int(os.environ.get("WORLD_SIZE", 1)) + + def set_epoch(self, epoch): + self.epoch = epoch + # overwatch.info(f"TokenMixtureSampler set to epoch {epoch}") + + def __iter__(self): + # assert self.epoch is not None, "TokenMixtureSampler epoch not set. Please call set_epoch(epoch) before iterating." + + # For each global batch index, synchronize dataset_id selection across all ranks + for batch_idx in range(self.num_batches_per_rank * self.world_size): + if batch_idx % self.world_size != self.rank: + continue + # Use a deterministic RNG for this global! batch to pick dataset_id and indices + batch_seed = self.seed + self.epoch + batch_idx # // self.world_size + local_rng = random.Random(batch_seed) + dataset_id = local_rng.choices(range(len(self.specs)), weights=self.probs, k=1)[0] + # print(f"[TokenMixtureSampler] rank={self.rank} seed={batch_seed} dataset_id={dataset_id}") + spec = self.specs[dataset_id] + batch_size = max(1, self.tokens_per_batch // spec.tokens_per_image) + local_batch_rng = random.Random(self.seed + self.epoch + batch_idx) + indices = [ + (dataset_id, local_batch_rng.randrange(spec.dataset_length)) + for _ in range(batch_size) + ] + # overwatch.info(f"rank {overwatch.rank()}: {self.epoch} batch {batch_idx} dataset_id {dataset_id} batch_size {batch_size}") + yield indices + + def __len__(self): + return self.num_batches_per_rank diff --git a/src/psi/data/sampler_test.py b/src/psi/data/sampler_test.py new file mode 100644 index 0000000000000000000000000000000000000000..50da06d83acf9db1dc0f4e01256962675a571c5c --- /dev/null +++ b/src/psi/data/sampler_test.py @@ -0,0 +1,76 @@ +from collections import Counter + +from psi.data.sampler import TaskMixtureSampler + + +def _dataset_sequence(sampler): + sequence = [] + for batch in sampler: + dataset_ids = {dataset_id for dataset_id, _ in batch} + assert len(dataset_ids) == 1 + sequence.append(next(iter(dataset_ids))) + return sequence + + +def test_task_mixture_sampler_batches_are_homogeneous(monkeypatch): + monkeypatch.setenv("RANK", "0") + monkeypatch.setenv("WORLD_SIZE", "1") + + sampler = TaskMixtureSampler( + dataset_lens=[100, 50, 30], + mixture_ratios=[3, 1, 0], + num_samples_per_epoch=64, + batch_size=4, + seed=7, + ) + + batches = list(sampler) + assert len(batches) == 16 + for batch in batches: + assert len(batch) == 4 + assert len({dataset_id for dataset_id, _ in batch}) == 1 + + +def test_task_mixture_sampler_respects_batch_ratios(monkeypatch): + monkeypatch.setenv("RANK", "0") + monkeypatch.setenv("WORLD_SIZE", "1") + + sampler = TaskMixtureSampler( + dataset_lens=[100, 50], + mixture_ratios=[3, 1], + num_samples_per_epoch=64, + batch_size=4, + seed=11, + ) + + counts = Counter(_dataset_sequence(sampler)) + assert counts == Counter({0: 12, 1: 4}) + + +def test_task_mixture_sampler_distributed_schedule_matches_global(monkeypatch): + common_kwargs = dict( + dataset_lens=[100, 50, 30], + mixture_ratios=[5, 3, 2], + num_samples_per_epoch=64, + batch_size=4, + seed=19, + ) + + monkeypatch.setenv("RANK", "0") + monkeypatch.setenv("WORLD_SIZE", "1") + global_sequence = _dataset_sequence(TaskMixtureSampler(**common_kwargs)) + + monkeypatch.setenv("RANK", "0") + monkeypatch.setenv("WORLD_SIZE", "2") + rank0_sequence = _dataset_sequence(TaskMixtureSampler(**common_kwargs)) + + monkeypatch.setenv("RANK", "1") + monkeypatch.setenv("WORLD_SIZE", "2") + rank1_sequence = _dataset_sequence(TaskMixtureSampler(**common_kwargs)) + + merged_sequence = [] + for batch_idx in range(len(global_sequence)): + local_idx = batch_idx // 2 + merged_sequence.append(rank0_sequence[local_idx] if batch_idx % 2 == 0 else rank1_sequence[local_idx]) + + assert merged_sequence == global_sequence diff --git a/src/psi/data/seed/motion_repr.py b/src/psi/data/seed/motion_repr.py new file mode 100644 index 0000000000000000000000000000000000000000..7043cc03e248be1f0f896db46e111146a3bb3d26 --- /dev/null +++ b/src/psi/data/seed/motion_repr.py @@ -0,0 +1,311 @@ +from __future__ import annotations + +import numpy as np + +# SEED absolute pose keeps the old layout: +# joints(29), root xyz(3), root quat wxyz(4). +SEED_JOINT_DIM = 29 +SEED_ABS_DIM = 36 +SEED_TEXTOP_ACTION_DIM = 39 + +TEXTOP_JOINT_ABS = slice(0, 29) +TEXTOP_ROOT_DELTA_LOCAL_XY = slice(29, 31) +TEXTOP_ROOT_ROT5_DELTA = slice(31, 36) +TEXTOP_ROOT_HEIGHT = slice(36, 37) +TEXTOP_FOOT_CONTACT = slice(37, 39) + +# 兼容旧推理脚本的import名称;语义已变为2D/5D,joint_delta不再存在。 +TEXTOP_JOINT_DELTA = slice(0, 0) +TEXTOP_ROOT_DELTA_LOCAL = TEXTOP_ROOT_DELTA_LOCAL_XY +TEXTOP_ROOT_ROT6D_DELTA = TEXTOP_ROOT_ROT5_DELTA + + +def normalize_quat_wxyz(quat: np.ndarray) -> np.ndarray: + quat = np.asarray(quat, dtype=np.float32) + norm = np.linalg.norm(quat, axis=-1, keepdims=True) + norm = np.where(norm < 1e-8, 1.0, norm) + out = quat / norm + if out.shape[-1] == 4: + out = np.where(out[..., :1] < 0.0, -out, out) + return out.astype(np.float32) + + +def quat_mul_wxyz(a: np.ndarray, b: np.ndarray) -> np.ndarray: + aw, ax, ay, az = np.moveaxis(a, -1, 0) + bw, bx, by, bz = np.moveaxis(b, -1, 0) + out = np.stack( + [ + aw * bw - ax * bx - ay * by - az * bz, + aw * bx + ax * bw + ay * bz - az * by, + aw * by - ax * bz + ay * bw + az * bx, + aw * bz + ax * by - ay * bx + az * bw, + ], + axis=-1, + ) + return normalize_quat_wxyz(out) + + +def quat_inv_wxyz(quat: np.ndarray) -> np.ndarray: + quat = normalize_quat_wxyz(quat) + out = quat.copy() + out[..., 1:] *= -1.0 + return out.astype(np.float32) + + +def quat_to_yaw_wxyz(quat: np.ndarray) -> np.ndarray: + quat = normalize_quat_wxyz(quat) + w, x, y, z = np.moveaxis(quat, -1, 0) + return np.arctan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)).astype(np.float32) + + +def wrap_angle(angle: np.ndarray) -> np.ndarray: + return ((angle + np.pi) % (2.0 * np.pi) - np.pi).astype(np.float32) + + +def quat_to_rpy_wxyz(quat: np.ndarray) -> np.ndarray: + quat = normalize_quat_wxyz(quat) + w, x, y, z = np.moveaxis(quat, -1, 0) + roll = np.arctan2(2.0 * (w * x + y * z), 1.0 - 2.0 * (x * x + y * y)) + pitch_arg = np.clip(2.0 * (w * y - z * x), -1.0, 1.0) + pitch = np.arcsin(pitch_arg) + yaw = quat_to_yaw_wxyz(quat) + return np.stack([roll, pitch, yaw], axis=-1).astype(np.float32) + + +def rpy_to_quat_wxyz(roll: float, pitch: float, yaw: float) -> np.ndarray: + cr = np.cos(roll * 0.5) + sr = np.sin(roll * 0.5) + cp = np.cos(pitch * 0.5) + sp = np.sin(pitch * 0.5) + cy = np.cos(yaw * 0.5) + sy = np.sin(yaw * 0.5) + quat = np.array( + [ + cr * cp * cy + sr * sp * sy, + sr * cp * cy - cr * sp * sy, + cr * sp * cy + sr * cp * sy, + cr * cp * sy - sr * sp * cy, + ], + dtype=np.float32, + ) + return normalize_quat_wxyz(quat) + + +def quat_delta_to_textop_rot5(prev_quat: np.ndarray, cur_quat: np.ndarray) -> np.ndarray: + prev_rpy = quat_to_rpy_wxyz(prev_quat) + cur_rpy = quat_to_rpy_wxyz(cur_quat) + roll = cur_rpy[..., 0] + pitch = cur_rpy[..., 1] + yaw_delta = wrap_angle(cur_rpy[..., 2] - prev_rpy[..., 2]) + return np.stack( + [np.sin(roll), np.cos(roll), np.sin(pitch), np.cos(pitch), yaw_delta], + axis=-1, + ).astype(np.float32) + + +def local_delta_from_world(prev_pos: np.ndarray, cur_pos: np.ndarray, prev_quat: np.ndarray) -> np.ndarray: + delta = np.asarray(cur_pos, dtype=np.float32) - np.asarray(prev_pos, dtype=np.float32) + yaw = quat_to_yaw_wxyz(prev_quat) + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + out = np.empty_like(delta, dtype=np.float32) + out[..., 0] = cos_yaw * delta[..., 0] + sin_yaw * delta[..., 1] + out[..., 1] = -sin_yaw * delta[..., 0] + cos_yaw * delta[..., 1] + out[..., 2] = delta[..., 2] + return out + + +def world_delta_from_local(prev_quat: np.ndarray, local_delta: np.ndarray) -> np.ndarray: + local_delta = np.asarray(local_delta, dtype=np.float32) + yaw = quat_to_yaw_wxyz(prev_quat) + cos_yaw = np.cos(yaw) + sin_yaw = np.sin(yaw) + out = np.empty_like(local_delta, dtype=np.float32) + out[..., 0] = cos_yaw * local_delta[..., 0] - sin_yaw * local_delta[..., 1] + out[..., 1] = sin_yaw * local_delta[..., 0] + cos_yaw * local_delta[..., 1] + out[..., 2] = local_delta[..., 2] + return out + + +def quat_to_matrix_wxyz(quat: np.ndarray) -> np.ndarray: + quat = normalize_quat_wxyz(quat) + w, x, y, z = np.moveaxis(quat, -1, 0) + matrix = np.empty(quat.shape[:-1] + (3, 3), dtype=np.float32) + matrix[..., 0, 0] = 1.0 - 2.0 * (y * y + z * z) + matrix[..., 0, 1] = 2.0 * (x * y - z * w) + matrix[..., 0, 2] = 2.0 * (x * z + y * w) + matrix[..., 1, 0] = 2.0 * (x * y + z * w) + matrix[..., 1, 1] = 1.0 - 2.0 * (x * x + z * z) + matrix[..., 1, 2] = 2.0 * (y * z - x * w) + matrix[..., 2, 0] = 2.0 * (x * z - y * w) + matrix[..., 2, 1] = 2.0 * (y * z + x * w) + matrix[..., 2, 2] = 1.0 - 2.0 * (x * x + y * y) + return matrix + + +def matrix_to_quat_wxyz(matrix: np.ndarray) -> np.ndarray: + matrix = np.asarray(matrix, dtype=np.float32) + flat = matrix.reshape(-1, 3, 3) + quats = [] + for rot in flat: + trace = float(np.trace(rot)) + if trace > 0.0: + s = np.sqrt(trace + 1.0) * 2.0 + quat = np.array( + [ + 0.25 * s, + (rot[2, 1] - rot[1, 2]) / s, + (rot[0, 2] - rot[2, 0]) / s, + (rot[1, 0] - rot[0, 1]) / s, + ], + dtype=np.float32, + ) + else: + idx = int(np.argmax(np.diag(rot))) + if idx == 0: + s = np.sqrt(max(1.0 + rot[0, 0] - rot[1, 1] - rot[2, 2], 1e-8)) * 2.0 + quat = np.array( + [ + (rot[2, 1] - rot[1, 2]) / s, + 0.25 * s, + (rot[0, 1] + rot[1, 0]) / s, + (rot[0, 2] + rot[2, 0]) / s, + ], + dtype=np.float32, + ) + elif idx == 1: + s = np.sqrt(max(1.0 + rot[1, 1] - rot[0, 0] - rot[2, 2], 1e-8)) * 2.0 + quat = np.array( + [ + (rot[0, 2] - rot[2, 0]) / s, + (rot[0, 1] + rot[1, 0]) / s, + 0.25 * s, + (rot[1, 2] + rot[2, 1]) / s, + ], + dtype=np.float32, + ) + else: + s = np.sqrt(max(1.0 + rot[2, 2] - rot[0, 0] - rot[1, 1], 1e-8)) * 2.0 + quat = np.array( + [ + (rot[1, 0] - rot[0, 1]) / s, + (rot[0, 2] + rot[2, 0]) / s, + (rot[1, 2] + rot[2, 1]) / s, + 0.25 * s, + ], + dtype=np.float32, + ) + quats.append(quat) + out = np.stack(quats, axis=0).reshape(matrix.shape[:-2] + (4,)) + return normalize_quat_wxyz(out) + + +def matrix_to_rot6d(matrix: np.ndarray) -> np.ndarray: + matrix = np.asarray(matrix, dtype=np.float32) + return np.concatenate([matrix[..., :, 0], matrix[..., :, 1]], axis=-1).astype(np.float32) + + +def rot6d_to_matrix(rot6d: np.ndarray) -> np.ndarray: + rot6d = np.asarray(rot6d, dtype=np.float32) + a1 = rot6d[..., 0:3] + a2 = rot6d[..., 3:6] + b1 = a1 / np.maximum(np.linalg.norm(a1, axis=-1, keepdims=True), 1e-8) + a2_orth = a2 - np.sum(b1 * a2, axis=-1, keepdims=True) * b1 + b2 = a2_orth / np.maximum(np.linalg.norm(a2_orth, axis=-1, keepdims=True), 1e-8) + b3 = np.cross(b1, b2) + return np.stack([b1, b2, b3], axis=-1).astype(np.float32) + + +def quat_delta_to_rot6d(prev_quat: np.ndarray, cur_quat: np.ndarray) -> np.ndarray: + prev_quat = normalize_quat_wxyz(prev_quat) + cur_quat = normalize_quat_wxyz(cur_quat) + cur_quat = np.where(np.sum(prev_quat * cur_quat, axis=-1, keepdims=True) < 0.0, -cur_quat, cur_quat) + rel_quat = quat_mul_wxyz(quat_inv_wxyz(prev_quat), cur_quat) + return matrix_to_rot6d(quat_to_matrix_wxyz(rel_quat)) + + +def rot6d_delta_to_quat(rot6d: np.ndarray) -> np.ndarray: + return matrix_to_quat_wxyz(rot6d_to_matrix(rot6d)) + + +def estimate_two_foot_contacts(joint_delta: np.ndarray, root_delta_local: np.ndarray) -> np.ndarray: + left_leg_motion = np.linalg.norm(joint_delta[..., 0:6], axis=-1) + right_leg_motion = np.linalg.norm(joint_delta[..., 6:12], axis=-1) + root_z_motion = np.abs(root_delta_local[..., 2]) + left = np.logical_and(left_leg_motion < 0.035, root_z_motion < 0.025) + right = np.logical_and(right_leg_motion < 0.035, root_z_motion < 0.025) + return np.stack([left, right], axis=-1).astype(np.float32) + + +def build_seed_textop_action(actions_abs: np.ndarray, states_abs: np.ndarray) -> np.ndarray: + actions_abs = np.asarray(actions_abs, dtype=np.float32) + states_abs = np.asarray(states_abs, dtype=np.float32) + if actions_abs.shape[-1] != SEED_ABS_DIM or states_abs.shape[-1] != SEED_ABS_DIM: + raise ValueError("TextOp SEED representation requires 36D absolute action/state.") + if states_abs.ndim == 1: + states_abs = states_abs[None, :] + + prev_abs = np.concatenate([states_abs[-1:, :], actions_abs[:-1, :]], axis=0) + joint_abs = actions_abs[:, :SEED_JOINT_DIM] + joint_delta = joint_abs - prev_abs[:, :SEED_JOINT_DIM] + root_delta_local = local_delta_from_world(prev_abs[:, 29:32], actions_abs[:, 29:32], prev_abs[:, 32:36]) + root_delta_local_xy = root_delta_local[:, :2] + root_rot5_delta = quat_delta_to_textop_rot5(prev_abs[:, 32:36], actions_abs[:, 32:36]) + root_height = actions_abs[:, 31:32] + foot_contact = estimate_two_foot_contacts(joint_delta, root_delta_local) + + return np.concatenate( + [joint_abs, root_delta_local_xy, root_rot5_delta, root_height, foot_contact], + axis=-1, + ).astype(np.float32) + + +def build_seed_textop_state(states_abs: np.ndarray) -> np.ndarray: + states_abs = np.asarray(states_abs, dtype=np.float32) + if states_abs.shape[-1] != SEED_ABS_DIM: + raise ValueError("TextOp SEED state representation requires 36D absolute state.") + if states_abs.ndim == 1: + states_abs = states_abs[None, :] + + # state也使用同一套39D motion token;首帧没有更早上下文,delta置0。 + prev_abs = np.concatenate([states_abs[:1, :], states_abs[:-1, :]], axis=0) + joint_abs = states_abs[:, :SEED_JOINT_DIM] + joint_delta = joint_abs - prev_abs[:, :SEED_JOINT_DIM] + root_delta_local = local_delta_from_world(prev_abs[:, 29:32], states_abs[:, 29:32], prev_abs[:, 32:36]) + root_delta_local_xy = root_delta_local[:, :2] + root_rot5 = quat_delta_to_textop_rot5(prev_abs[:, 32:36], states_abs[:, 32:36]) + root_height = states_abs[:, 31:32] + foot_contact = estimate_two_foot_contacts(joint_delta, root_delta_local) + return np.concatenate([joint_abs, root_delta_local_xy, root_rot5, root_height, foot_contact], axis=-1).astype(np.float32) + + +def recover_seed_abs_from_textop_action(features: np.ndarray, current_abs: np.ndarray) -> np.ndarray: + features = np.asarray(features, dtype=np.float32) + current_abs = np.asarray(current_abs, dtype=np.float32).copy() + if features.shape[-1] != SEED_TEXTOP_ACTION_DIM: + raise ValueError(f"Expected textop action dim {SEED_TEXTOP_ACTION_DIM}, got {features.shape[-1]}") + if current_abs.shape[-1] != SEED_ABS_DIM: + raise ValueError(f"Expected current_abs dim {SEED_ABS_DIM}, got {current_abs.shape[-1]}") + + frames = [] + prev_abs = current_abs.copy() + for feat in features: + joint_abs = feat[TEXTOP_JOINT_ABS] + root_delta_local_xy = feat[TEXTOP_ROOT_DELTA_LOCAL_XY] + root_rot_delta = feat[TEXTOP_ROOT_ROT5_DELTA] + root_height = feat[TEXTOP_ROOT_HEIGHT][0] + + root_delta_local = np.array([root_delta_local_xy[0], root_delta_local_xy[1], 0.0], dtype=np.float32) + root_pos = prev_abs[29:32] + world_delta_from_local(prev_abs[32:36], root_delta_local) + root_pos[2] = root_height + roll = float(np.arctan2(root_rot_delta[0], root_rot_delta[1])) + pitch = float(np.arctan2(root_rot_delta[2], root_rot_delta[3])) + yaw = float(quat_to_yaw_wxyz(prev_abs[32:36]) + root_rot_delta[4]) + root_quat = rpy_to_quat_wxyz(roll, pitch, yaw) + + frame = np.concatenate([joint_abs, root_pos.astype(np.float32), root_quat.astype(np.float32)], axis=0) + frame = frame.astype(np.float32) + frames.append(frame) + prev_abs = frame + + return np.stack(frames, axis=0).astype(np.float32) diff --git a/src/psi/data/seed/progress.py b/src/psi/data/seed/progress.py new file mode 100644 index 0000000000000000000000000000000000000000..f952421cc663f9a5bbf3db78d29156061a5ea1a6 --- /dev/null +++ b/src/psi/data/seed/progress.py @@ -0,0 +1,62 @@ +from __future__ import annotations + +import math +from typing import Tuple + +import numpy as np + + +def build_progress_conditions( + *, + frame_id: int, + total_frames: int, + action_horizon: int, + step_stride: int = 1, + use_delta_actions: bool = False, +) -> Tuple[np.ndarray, np.ndarray]: + """Build train/infer progress conditions for a chunk. + + progress: [start, end, remaining, sin(start), cos(start)] + future_progress[t]: [future, remaining, start, sin(future), cos(future)] + """ + total_frames = int(total_frames) + action_horizon = int(action_horizon) + step_stride = int(step_stride) + progress_den = float(max(total_frames - 1, 1)) + max_frame = max(total_frames - 1, 0) + + start_frame = min(int(frame_id), max_frame) + start_progress = float(start_frame) / progress_den + end_frame = min(int(frame_id) + action_horizon * step_stride, max_frame) + end_progress = float(end_frame) / progress_den + remaining_progress = max(0.0, 1.0 - start_progress) + progress = np.asarray( + [ + start_progress, + end_progress, + remaining_progress, + math.sin(2.0 * math.pi * start_progress), + math.cos(2.0 * math.pi * start_progress), + ], + dtype=np.float32, + ) + + rows = [] + for step in range(action_horizon): + if use_delta_actions: + future_frame = int(frame_id) + step * step_stride + else: + future_frame = int(frame_id) + (step + 1) * step_stride + future_frame = min(future_frame, max_frame) + future_progress = float(future_frame) / progress_den + future_remaining = max(0.0, 1.0 - future_progress) + rows.append( + [ + future_progress, + future_remaining, + start_progress, + math.sin(2.0 * math.pi * future_progress), + math.cos(2.0 * math.pi * future_progress), + ] + ) + return progress, np.asarray(rows, dtype=np.float32) diff --git a/src/psi/data/seed/seed_audio_motion_dataset.py b/src/psi/data/seed/seed_audio_motion_dataset.py new file mode 100644 index 0000000000000000000000000000000000000000..8f955e86ed4437b2fda8a056b166e533cedd49f5 --- /dev/null +++ b/src/psi/data/seed/seed_audio_motion_dataset.py @@ -0,0 +1,872 @@ +from __future__ import annotations + +import hashlib +import json +import os +import pickle +import random +import re +import subprocess +import time +from pathlib import Path +from typing import Any, Dict, List + +import numpy as np +import pandas as pd +import torch +from tqdm import tqdm + +from psi.data.seed.progress import build_progress_conditions + + +_AUDIO_DANCE_PROMPT_POOLS: Dict[str, List[str]] = { + "system_first": [ + "Generate a dance motion that follows the rhythm and energy of this music.", + "Create a full-body dance sequence that matches the beat, tempo, and mood of the audio.", + "Synthesize a natural dance motion conditioned on the provided music.", + "Generate motion that stays synchronized with the musical rhythm and dynamics.", + "Produce a coherent dance trajectory that fits this audio performance.", + ], + "audio_first": [ + "Based on the music above, generate a rhythm-matched dance motion.", + "Using the audio above, synthesize a smooth full-body dance sequence.", + "Listen to the music above and produce a dance motion that follows its groove.", + "Generate stable, expressive motion from the musical audio above.", + "Turn the rhythm and mood of the audio above into a continuous dance motion.", + ], + "neutral": [ + "Generate a dance sequence that matches the provided music.", + "Create a motion sequence aligned with the audio rhythm.", + "Synthesize full-body dance motion from the music.", + "Produce a natural dance performance for this audio.", + "Generate a coherent motion trajectory driven by the musical beat.", + ], +} + + +_AUDIO_COMMAND_PROMPT_POOLS: Dict[str, List[str]] = { + "speech_understanding": [ + "Listen for the spoken task in the audio, then carry out that task as body motion.", + "The audio contains a verbal command; execute the action it asks for.", + "Use the speech content, not the melody, to decide which movement to perform.", + "Parse the utterance in the audio and move according to that instruction.", + "Act on the command spoken by the speaker.", + ], + "spoken_examples": [ + "If the speaker says to salute, perform a salute.", + "If the speaker asks for walking, walk in the requested manner.", + "If the audio asks for a turn, rotate the body accordingly.", + "If the command mentions raising a hand, make that arm gesture.", + "If the voice asks for a bow, bend the upper body and recover.", + ], + "contrast_music": [ + "Ignore musical rhythm here; the important signal is the spoken instruction.", + "This is command following from speech, not dancing to the soundtrack.", + "Choose the action from the words in the audio rather than the beat.", + "Treat the audio like an oral text prompt for motion control.", + "Respond to the voice command with the corresponding robot movement.", + ], +} + + +def ffmpeg_bin() -> str: + return os.environ.get("PSI_FFMPEG_BIN", "ffmpeg") + + +def ffprobe_bin() -> str: + return os.environ.get("PSI_FFPROBE_BIN", "ffprobe") + + +def _rank0_tqdm(iterable, **kwargs): + rank = int(os.environ.get("RANK", "0")) + return tqdm(iterable, disable=(rank != 0), **kwargs) + + +class SeedAudioMotionDataset(torch.utils.data.Dataset): + def __init__( + self, + root_dir: str = "", + video_dir: str = "", + split: str = "train", + val_ratio: float = 0.1, + split_seed: int = 42, + action_chunk_size: int = 30, + sample_stride: int = 1, + use_delta_actions: bool = True, + num_past_frames: int = 0, + motion_fps: int = 50, + audio_sampling_rate: int = 16000, + audio_window_seconds: float = 1.0, + audio_left_context_seconds: float = 0.0, + full_audio: bool = False, + align_start_hold: bool = True, + use_audio_instruction: bool = False, + dataset_root: str = "", + metadata_jsonl: str = "", + manifest_path: str = "", + motion_dir_key: str = "converted_path", + audio_path_key: str = "video_path", + instruction_key: str = "audio_caption", + name_key: str = "name", + id_key: str = "id", + motion_metadata_csv: str = "", + motion_metadata_key: str = "filename", + motion_metadata_path_key: str = "move_g1_path", + audio_file_extensions: str = ".wav,.flac,.mp3,.m4a,.aac,.ogg,.npy", + motion_cache_size: int = 8, + audio_cache_size: int = 8, + dataset_name: str = "seed-audio-g1", + source_name: str = "", + require_audio: bool = True, + audio_task: str = "dance", + records_cache_dir: str = "", + ) -> None: + self.root_dir = Path(root_dir) if str(root_dir).strip() else None + self.video_dir = Path(video_dir) if str(video_dir).strip() else None + self.split = split + self.val_ratio = float(val_ratio) + self.split_seed = int(split_seed) + self.action_chunk_size = int(action_chunk_size) + self.sample_stride = int(sample_stride) + self.use_delta_actions = bool(use_delta_actions) + self.num_past_frames = int(num_past_frames) + self.motion_fps = int(motion_fps) + self.audio_sampling_rate = int(audio_sampling_rate) + self.audio_window_seconds = float(audio_window_seconds) + self.audio_left_context_seconds = float(audio_left_context_seconds) + self.full_audio = bool(full_audio) + self.align_start_hold = bool(align_start_hold) + self.use_audio_instruction = bool(use_audio_instruction) + self.dataset_root = Path(dataset_root) if str(dataset_root).strip() else None + self.metadata_jsonl = str(metadata_jsonl or manifest_path).strip() + self.motion_dir_key = motion_dir_key + self.audio_path_key = audio_path_key + self.instruction_key = instruction_key + self.name_key = name_key + self.id_key = id_key + self.motion_metadata_csv = str(motion_metadata_csv).strip() + self.motion_metadata_key = motion_metadata_key + self.motion_metadata_path_key = motion_metadata_path_key + self.audio_file_extensions = tuple( + ext if ext.startswith(".") else f".{ext}" + for ext in [part.strip().lower() for part in str(audio_file_extensions).split(",")] + if ext + ) + self.motion_cache_size = int(motion_cache_size) + self.audio_cache_size = int(audio_cache_size) + self.dataset_name = dataset_name + self.source_name = source_name or dataset_name + self.require_audio = bool(require_audio) + self.audio_task = str(audio_task or "dance").strip().lower() + self.records_cache_dir = str(records_cache_dir).strip() + self._motion_metadata_index: Dict[str, Path] | None = None + self._motion_metadata_frames_index: Dict[str, int] | None = None + + if self.action_chunk_size <= 0: + raise ValueError(f"action_chunk_size must be positive, got {self.action_chunk_size}") + if self.sample_stride <= 0: + raise ValueError(f"sample_stride must be positive, got {self.sample_stride}") + if self.motion_fps <= 0: + raise ValueError(f"motion_fps must be positive, got {self.motion_fps}") + if self.audio_sampling_rate <= 0: + raise ValueError(f"audio_sampling_rate must be positive, got {self.audio_sampling_rate}") + if self.audio_window_seconds <= 0 and not self.full_audio: + raise ValueError(f"audio_window_seconds must be positive, got {self.audio_window_seconds}") + if self.audio_task not in ("dance", "command"): + raise ValueError(f"Unsupported audio_task={self.audio_task!r}, expected dance/command") + + self.skipped_no_audio = 0 + if self.metadata_jsonl: + self.motion_records = self._load_or_build_manifest_records() + else: + self.motion_records = self._load_or_build_legacy_records() + + if not self.motion_records: + raise RuntimeError("No valid audio motion records found after filtering.") + # 记录过滤数量,方便确认本次训练实际用了多少带音频样本。 + if self.skipped_no_audio > 0: + print( + f"SeedAudioMotionDataset split={self.split}: " + f"skipped {self.skipped_no_audio} videos without audio stream." + ) + + sample_counts = np.fromiter( + ( + max(1, (int(record["n_frames"]) + self.sample_stride - 1) // self.sample_stride) + for record in _rank0_tqdm( + self.motion_records, + desc=f"Build audio index {self.source_name}/{self.split}", + unit="motion", + ) + ), + dtype=np.int64, + count=len(self.motion_records), + ) + self.sample_offsets = np.zeros(len(self.motion_records) + 1, dtype=np.int64) + self.sample_offsets[1:] = np.cumsum(sample_counts, dtype=np.int64) + self.num_samples = int(self.sample_offsets[-1]) + + self._motion_cache: Dict[str, np.ndarray] = {} + self._audio_cache: Dict[str, np.ndarray] = {} + + def _cache_array(self, cache: Dict[str, np.ndarray], key: str, value: np.ndarray, max_size: int) -> None: + if max_size <= 0: + return + if len(cache) >= max_size: + cache.pop(next(iter(cache))) + cache[key] = value + + def _resolve_path(self, path_value: Any, base_dir: Path | None = None) -> Path: + p = Path(str(path_value).strip()) + if p.is_absolute(): + return p + if base_dir is not None: + return base_dir / p + return p + + def _first_non_empty(self, row: dict[str, Any], keys: list[str]) -> Any: + for key in keys: + if key in row and row[key] not in (None, ""): + return row[key] + return None + + def _manifest_path(self) -> Path: + return self._resolve_path(self.metadata_jsonl, self.dataset_root) + + def _default_records_cache_dir(self, manifest_path: Path) -> Path: + if self.records_cache_dir: + return Path(self.records_cache_dir) + base_dir = self.dataset_root or manifest_path.parent + return base_dir / ".index_cache_multimodal_yth" + + def _default_legacy_records_cache_dir(self) -> Path: + if self.records_cache_dir: + return Path(self.records_cache_dir) + if self.root_dir is None: + raise FileNotFoundError("root_dir is required for legacy audio cache") + return self.root_dir / ".index_cache_multimodal_yth" + + def _records_cache_path(self) -> Path | None: + if not self.metadata_jsonl: + return None + manifest_path = self._manifest_path() + if not manifest_path.exists(): + return None + stat = manifest_path.stat() + key_data = { + "version": 2, + "kind": "audio_records", + "manifest_path": str(manifest_path.resolve()), + "manifest_size": stat.st_size, + "manifest_mtime_ns": stat.st_mtime_ns, + "dataset_root": str(self.dataset_root.resolve()) if self.dataset_root is not None else "", + "split": self.split, + "val_ratio": self.val_ratio, + "split_seed": self.split_seed, + "motion_dir_key": self.motion_dir_key, + "audio_path_key": self.audio_path_key, + "instruction_key": self.instruction_key, + "name_key": self.name_key, + "id_key": self.id_key, + "motion_metadata_csv": self.motion_metadata_csv, + "motion_metadata_key": self.motion_metadata_key, + "motion_metadata_path_key": self.motion_metadata_path_key, + "dataset_name": self.dataset_name, + "source_name": self.source_name, + "require_audio": self.require_audio, + "audio_task": self.audio_task, + "align_start_hold": self.align_start_hold, + } + digest = hashlib.sha1(json.dumps(key_data, sort_keys=True).encode("utf-8")).hexdigest()[:20] + return self._default_records_cache_dir(manifest_path) / f"records.{self.source_name}.{self.split}.{digest}.pkl" + + def _legacy_records_cache_path(self) -> Path | None: + if self.root_dir is None or self.video_dir is None: + return None + if not self.root_dir.exists() or not self.video_dir.exists(): + return None + key_data = { + "version": 2, + "kind": "audio_legacy_records", + "root_dir": str(self.root_dir.resolve()), + "video_dir": str(self.video_dir.resolve()), + "split": self.split, + "val_ratio": self.val_ratio, + "split_seed": self.split_seed, + "dataset_name": self.dataset_name, + "source_name": self.source_name, + "require_audio": self.require_audio, + "audio_task": self.audio_task, + "align_start_hold": self.align_start_hold, + } + digest = hashlib.sha1(json.dumps(key_data, sort_keys=True).encode("utf-8")).hexdigest()[:20] + return self._default_legacy_records_cache_dir() / f"records.{self.source_name}.{self.split}.{digest}.pkl" + + def _list_legacy_motion_dirs(self) -> List[Path]: + if self.root_dir is None: + return [] + motion_dirs: List[Path] = [] + for path in sorted(self.root_dir.iterdir()): + if not path.is_dir(): + continue + if path.name.startswith("."): + continue + if not (path / "joint_pos.csv").exists(): + continue + motion_dirs.append(path) + return motion_dirs + + def _load_or_build_manifest_records(self) -> List[Dict[str, Any]]: + cache_path = self._records_cache_path() + rank = int(os.environ.get("RANK", "0")) + if cache_path is not None: + cached = self._try_load_pickle(cache_path) + if cached is not None: + return cached + if rank != 0: + waited = self._wait_for_pickle(cache_path) + if waited is not None: + return waited + + records = self._build_manifest_records() + if cache_path is not None: + self._atomic_save_pickle(cache_path, records) + return records + + def _load_or_build_legacy_records(self) -> List[Dict[str, Any]]: + cache_path = self._legacy_records_cache_path() + rank = int(os.environ.get("RANK", "0")) + if cache_path is not None: + cached = self._try_load_pickle(cache_path) + if cached is not None: + return cached + if rank != 0: + waited = self._wait_for_pickle(cache_path) + if waited is not None: + return waited + + records = self._build_legacy_records() + if cache_path is not None: + self._atomic_save_pickle(cache_path, records) + return records + + def _try_load_pickle(self, path: Path) -> Any | None: + if not path.exists(): + return None + try: + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + size_mb = path.stat().st_size / (1024 ** 2) + print(f"[AUDIO CACHE] loading {path} ({size_mb:.1f} MB)", flush=True) + start = time.time() + with path.open("rb") as f: + obj = pickle.load(f) + if isinstance(obj, dict) and "data" in obj: + obj = obj["data"] + if rank == 0: + print(f"[AUDIO CACHE] loaded {path} in {time.time() - start:.1f}s", flush=True) + return obj + except Exception: + return None + + def _wait_for_pickle(self, path: Path, timeout_sec: int = 7200) -> Any | None: + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + print(f"[AUDIO CACHE] waiting for {path}", flush=True) + start = time.time() + while time.time() - start < timeout_sec: + obj = self._try_load_pickle(path) + if obj is not None: + return obj + time.sleep(2.0) + return None + + def _atomic_save_pickle(self, path: Path, data: Any) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + tmp_path = path.with_name(f".{path.name}.{os.getpid()}.tmp") + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + print(f"[AUDIO CACHE] saving {path}", flush=True) + start = time.time() + with tmp_path.open("wb") as f: + pickle.dump({"data": data}, f, protocol=pickle.HIGHEST_PROTOCOL) + os.replace(tmp_path, path) + if rank == 0: + size_mb = path.stat().st_size / (1024 ** 2) + print(f"[AUDIO CACHE] saved {path} ({size_mb:.1f} MB) in {time.time() - start:.1f}s", flush=True) + + def _build_legacy_records(self) -> List[Dict[str, Any]]: + if self.root_dir is None or not self.root_dir.exists(): + raise FileNotFoundError(f"Motion root dir does not exist: {self.root_dir}") + if self.video_dir is None or not self.video_dir.exists(): + raise FileNotFoundError(f"Video dir does not exist: {self.video_dir}") + + motion_dirs = self._list_legacy_motion_dirs() + selected_ids = self._select_ids(len(motion_dirs), self.split) + records: List[Dict[str, Any]] = [] + for motion_id in _rank0_tqdm( + selected_ids, + desc=f"Build audio records {self.source_name}/{self.split}", + unit="motion", + ): + motion_dir = motion_dirs[int(motion_id)] + audio_path = self.video_dir / f"{motion_dir.name}.mp4" + if not audio_path.exists(): + raise FileNotFoundError(f"Matched audio/video file does not exist: {audio_path}") + if self.require_audio and not self._has_audio_stream(audio_path): + self.skipped_no_audio += 1 + continue + + n_frames = self._metadata_num_frames({}, motion_dir.name) + if n_frames <= 0: + n_frames = self._peek_num_frames(motion_dir) + if n_frames <= 0: + continue + + records.append( + { + "motion_dir": motion_dir, + "audio_path": audio_path, + "audio_paths": [audio_path], + "motion_name": motion_dir.name, + "n_frames": n_frames, + "start_hold_seconds": self._read_start_hold_seconds(motion_dir), + "source_name": self.source_name, + } + ) + return records + + def _build_manifest_records(self) -> List[Dict[str, Any]]: + manifest_path = self._manifest_path() + if not manifest_path.exists(): + raise FileNotFoundError(f"Audio manifest does not exist: {manifest_path}") + + base_dir = self.dataset_root or manifest_path.parent + if self.dataset_root is not None and not self.dataset_root.exists(): + raise FileNotFoundError(f"Dataset root does not exist: {self.dataset_root}") + + rows: List[dict[str, Any]] = [] + with manifest_path.open("r", encoding="utf-8") as f: + for line_no, line in enumerate( + _rank0_tqdm( + f, + desc=f"Read audio manifest {self.source_name}/{self.split}", + unit="line", + ), + start=1, + ): + line = line.strip() + if not line: + continue + try: + rows.append(json.loads(line)) + except json.JSONDecodeError as exc: + raise ValueError(f"Bad jsonl at {manifest_path}:{line_no}") from exc + + selected_ids = self._select_ids(len(rows), self.split) + records: List[Dict[str, Any]] = [] + for row_id in _rank0_tqdm( + selected_ids, + desc=f"Build audio records {self.source_name}/{self.split}", + total=len(selected_ids), + unit="row", + ): + row = rows[int(row_id)] + motion_value = self._first_non_empty( + row, + [self.motion_dir_key, "converted_path", "motion_dir", "motion_path"], + ) + audio_value = self._first_non_empty( + row, + [self.audio_path_key, "audio_path", "video_path", "real_path"], + ) + if motion_value is None: + raise ValueError(f"Missing motion path in {manifest_path} row {row_id}") + if audio_value is None: + raise ValueError(f"Missing audio path in {manifest_path} row {row_id}") + + motion_dir = self._resolve_manifest_motion_dir(motion_value, base_dir) + audio_source_path, audio_paths = self._resolve_audio_paths(audio_value, base_dir) + if not motion_dir.exists(): + raise FileNotFoundError(f"Manifest motion dir does not exist: {motion_dir}") + if ( + self.require_audio + and audio_source_path.is_file() + and audio_source_path.suffix.lower() not in self.audio_file_extensions + (".npy",) + and not self._has_audio_stream(audio_source_path) + ): + self.skipped_no_audio += 1 + continue + + n_frames = self._metadata_num_frames(row, motion_value) + if n_frames <= 0: + n_frames = self._peek_num_frames(motion_dir) + if n_frames <= 0: + continue + + motion_name = self._first_non_empty(row, [self.name_key, "name"]) or motion_dir.name + records.append( + { + "motion_dir": motion_dir, + "audio_path": audio_paths[0], + "audio_paths": audio_paths, + "motion_name": str(motion_name), + "n_frames": n_frames, + "start_hold_seconds": self._read_start_hold_seconds(motion_dir), + "instruction": "", + "manifest_id": str(self._first_non_empty(row, [self.id_key, "id"]) or row_id), + "source_name": self.source_name, + "motion_type": str(row.get("motion_type", "")), + } + ) + return records + + def _map_seed_motion_dir(self, move_g1_path: str) -> Path: + rel = str(move_g1_path).strip().replace("\\", "/") + rel = rel.replace("g1/csv/", "g1/csv_sonic/") + rel = rel.replace("/csv/", "/csv_sonic/") + if rel.endswith(".csv"): + rel = rel[:-4] + + if rel.startswith("dataset/seed/"): + rel = rel[len("dataset/seed/") :] + if rel.startswith("seed/"): + rel = rel[len("seed/") :] + + p = Path(rel) + if p.is_absolute(): + return p + if self.root_dir is not None: + return self.root_dir / p + return p + + def _load_motion_metadata_index(self) -> Dict[str, Path]: + if self._motion_metadata_index is not None: + return self._motion_metadata_index + if not self.motion_metadata_csv: + self._motion_metadata_index = {} + self._motion_metadata_frames_index = {} + return self._motion_metadata_index + + metadata_path = self._resolve_path(self.motion_metadata_csv, self.root_dir or self.dataset_root) + if not metadata_path.exists(): + raise FileNotFoundError(f"Motion metadata csv does not exist: {metadata_path}") + + preview = pd.read_csv(metadata_path, nrows=1) + required_cols = [self.motion_metadata_key, self.motion_metadata_path_key] + has_duration_frames = "move_duration_frames" in preview.columns + if has_duration_frames: + required_cols.append("move_duration_frames") + metadata = pd.read_csv(metadata_path, usecols=required_cols) + index: Dict[str, Path] = {} + frame_index: Dict[str, int] = {} + for _, row in metadata.iterrows(): + key_value = row[self.motion_metadata_key] + path_value = row[self.motion_metadata_path_key] + if pd.isna(key_value) or pd.isna(path_value): + continue + key = str(key_value).strip() + if not key: + continue + index[key] = self._map_seed_motion_dir(str(path_value)) + if has_duration_frames and pd.notna(row.get("move_duration_frames")): + frame_index[key] = int(row["move_duration_frames"]) + self._motion_metadata_index = index + self._motion_metadata_frames_index = frame_index + return index + + def _motion_metadata_num_frames(self, motion_value: Any) -> int: + if self._motion_metadata_frames_index is None: + self._load_motion_metadata_index() + frame_index = self._motion_metadata_frames_index or {} + key = str(motion_value).strip() + if key in frame_index: + return int(frame_index[key]) + stem = Path(key).stem + if stem in frame_index: + return int(frame_index[stem]) + return 0 + + def _metadata_num_frames(self, row: dict[str, Any], motion_value: Any) -> int: + for key in ("clip_frames", "motion_chunk_frames", "n_frames", "move_duration_frames"): + value = row.get(key) + if value not in (None, ""): + try: + n_frames = int(value) + except (TypeError, ValueError): + n_frames = 0 + if n_frames > 0: + return n_frames + + metadata_frames = self._motion_metadata_num_frames(motion_value) + if metadata_frames > 0: + return metadata_frames + + duration = row.get("duration") + if duration not in (None, ""): + try: + duration_sec = float(duration) + except (TypeError, ValueError): + duration_sec = 0.0 + if duration_sec > 0: + estimated = max(1, int(round(duration_sec * float(self.motion_fps))) - 3) + return estimated + return 0 + + def _resolve_manifest_motion_dir(self, motion_value: Any, base_dir: Path) -> Path: + motion_dir = self._resolve_path(motion_value, base_dir) + if motion_dir.exists(): + return motion_dir + + motion_index = self._load_motion_metadata_index() + key = str(motion_value).strip() + if key in motion_index: + return motion_index[key] + + stem = Path(key).stem + if stem in motion_index: + return motion_index[stem] + + return motion_dir + + def _resolve_audio_paths(self, audio_value: Any, base_dir: Path) -> tuple[Path, List[Path]]: + audio_path = self._resolve_path(audio_value, base_dir) + if not audio_path.exists(): + raise FileNotFoundError(f"Manifest audio path does not exist: {audio_path}") + if audio_path.is_dir(): + audio_paths = sorted( + p + for p in audio_path.iterdir() + if p.is_file() and p.suffix.lower() in self.audio_file_extensions + ) + if not audio_paths: + raise FileNotFoundError(f"No audio files found in manifest audio dir: {audio_path}") + return audio_path, audio_paths + return audio_path, [audio_path] + + def _compose_audio_instruction(self) -> str: + prompt_pools = _AUDIO_COMMAND_PROMPT_POOLS if self.audio_task == "command" else _AUDIO_DANCE_PROMPT_POOLS + layout = random.choice(list(prompt_pools.keys())) + return random.choice(prompt_pools[layout]).strip() + + def _record_instruction(self, record: Dict[str, Any]) -> str: + if not self.use_audio_instruction: + return "" + return self._compose_audio_instruction() + + def _select_ids(self, n: int, split: str) -> np.ndarray: + if n == 0: + return np.array([], dtype=np.int64) + + rng = np.random.RandomState(self.split_seed) + perm = rng.permutation(n) + n_val = int(round(n * self.val_ratio)) + if n > 1 and self.val_ratio > 0 and n_val == 0: + n_val = 1 + if n > 1 and n_val >= n: + n_val = n - 1 + + split_norm = (split or "train").lower() + if split_norm == "train": + return perm[n_val:] + if split_norm in ("val", "valid", "validation", "test"): + return perm[:n_val] + if split_norm == "all": + return perm + raise ValueError(f"Unsupported split='{split}', expected train/val/all") + + def _peek_num_frames(self, motion_dir: Path) -> int: + joint_path = motion_dir / "joint_pos.csv" + if not joint_path.exists(): + raise FileNotFoundError(f"joint_pos.csv not found: {joint_path}") + return int(len(pd.read_csv(joint_path))) + + def _read_start_hold_seconds(self, motion_dir: Path) -> float: + if not self.align_start_hold: + return 0.0 + + info_path = motion_dir / "info.txt" + if not info_path.exists(): + return 0.0 + + text = info_path.read_text(encoding="utf-8", errors="ignore") + match = re.search(r"start_hold_seconds:\s*([0-9.]+)", text) + return float(match.group(1)) if match else 0.0 + + def _has_audio_stream(self, video_path: Path) -> bool: + # 用 ffprobe 判断是否存在第一路音频,避免无声 mp4 进入训练集。 + cmd = [ + ffprobe_bin(), + "-v", + "error", + "-select_streams", + "a:0", + "-show_entries", + "stream=index", + "-of", + "csv=p=0", + str(video_path), + ] + proc = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, check=False) + return proc.returncode == 0 and bool(proc.stdout.strip()) + + def _load_motion_array(self, motion_dir: Path) -> np.ndarray: + key = str(motion_dir) + if key in self._motion_cache: + return self._motion_cache[key] + + joint = pd.read_csv(motion_dir / "joint_pos.csv").to_numpy(dtype=np.float32) + body_pos = pd.read_csv(motion_dir / "body_pos.csv").to_numpy(dtype=np.float32) + body_quat = pd.read_csv(motion_dir / "body_quat.csv").to_numpy(dtype=np.float32) + lengths = {joint.shape[0], body_pos.shape[0], body_quat.shape[0]} + if len(lengths) != 1: + raise ValueError( + f"Frame length mismatch at {motion_dir}: " + f"joint={joint.shape[0]}, body_pos={body_pos.shape[0]}, body_quat={body_quat.shape[0]}" + ) + if joint.shape[1] != 29 or body_pos.shape[1] != 3 or body_quat.shape[1] != 4: + raise ValueError(f"Unexpected G1 csv shape at {motion_dir}") + + motion = np.concatenate([joint, body_pos, body_quat], axis=1).astype(np.float32) + self._cache_array(self._motion_cache, key, motion, self.motion_cache_size) + return motion + + def _load_audio_array(self, video_path: Path) -> np.ndarray: + key = str(video_path) + if key in self._audio_cache: + return self._audio_cache[key] + + if video_path.suffix.lower() == ".npy": + audio = np.load(video_path, allow_pickle=False).astype(np.float32, copy=False).reshape(-1).copy() + if audio.size == 0: + raise RuntimeError(f"Empty audio npy: {video_path}") + self._cache_array(self._audio_cache, key, audio, self.audio_cache_size) + return audio + + cmd = [ + ffmpeg_bin(), + "-v", + "error", + "-threads", + "1", + "-i", + str(video_path), + "-ac", + "1", + "-ar", + str(self.audio_sampling_rate), + "-f", + "f32le", + "-", + ] + proc = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, check=False) + if proc.returncode != 0: + msg = proc.stderr.decode("utf-8", errors="ignore") + raise RuntimeError(f"ffmpeg failed to decode audio from {video_path}: {msg}") + + audio = np.frombuffer(proc.stdout, dtype=np.float32).copy() + if audio.size == 0: + raise RuntimeError(f"No audio samples decoded from {video_path}") + self._cache_array(self._audio_cache, key, audio, self.audio_cache_size) + return audio + + def _audio_segment(self, audio: np.ndarray, start_sec: float, duration_sec: float) -> np.ndarray: + num_samples = max(1, int(round(duration_sec * self.audio_sampling_rate))) + start_sample = int(round(start_sec * self.audio_sampling_rate)) + end_sample = start_sample + num_samples + + segment = np.zeros((num_samples,), dtype=np.float32) + src_start = max(0, start_sample) + src_end = min(audio.shape[0], end_sample) + if src_end > src_start: + dst_start = src_start - start_sample + segment[dst_start : dst_start + (src_end - src_start)] = audio[src_start:src_end] + return segment + + def __len__(self) -> int: + return self.num_samples + + def _pad_index(self, index: int, length: int) -> int: + if index < 0: + return 0 + if index >= length: + return length - 1 + return index + + def __getitem__(self, idx: int) -> Dict[str, Any]: + if idx < 0 or idx >= self.num_samples: + raise IndexError(f"Index out of range: {idx}") + motion_id = int(np.searchsorted(self.sample_offsets, idx, side="right") - 1) + local_index = int(idx - self.sample_offsets[motion_id]) + frame_id = int(local_index * self.sample_stride) + record = self.motion_records[motion_id] + motion_dir: Path = record["motion_dir"] + audio_paths: List[Path] = record.get("audio_paths", [record["audio_path"]]) + audio_path: Path = random.choice(audio_paths) if len(audio_paths) > 1 else audio_paths[0] + + motion = self._load_motion_array(motion_dir) + audio = self._load_audio_array(audio_path) + total_frames = motion.shape[0] + action_len = self.action_chunk_size + (1 if self.use_delta_actions else 0) + + actions = [] + action_mask = [] + for step in range(action_len): + if self.use_delta_actions: + raw_index = frame_id + step * self.sample_stride + else: + raw_index = frame_id + (step + 1) * self.sample_stride + valid = raw_index < total_frames + actions.append(motion[min(raw_index, total_frames - 1)]) + action_mask.append(1.0 if valid else 0.0) + action_arr = np.stack(actions, axis=0).astype(np.float32) + progress, future_progress = build_progress_conditions( + frame_id=frame_id, + total_frames=total_frames, + action_horizon=action_arr.shape[0], + step_stride=self.sample_stride, + use_delta_actions=self.use_delta_actions, + ) + + obs_indices = [ + self._pad_index(frame_id - (self.num_past_frames - t) * self.sample_stride, total_frames) + for t in range(self.num_past_frames + 1) + ] + audio_start_sec = ( + frame_id / float(self.motion_fps) + - float(record["start_hold_seconds"]) + - self.audio_left_context_seconds + ) + # 本实验每个样本直接把整段视频音频送给Qwen-Omni,不再按当前frame切局部窗口。 + if self.full_audio: + audio_input = audio.astype(np.float32) + audio_start_sec = 0.0 + audio_duration_sec = audio.shape[0] / float(self.audio_sampling_rate) + else: + audio_input = self._audio_segment(audio, audio_start_sec, self.audio_window_seconds) + audio_duration_sec = self.audio_window_seconds + + output = { + "actions": action_arr, + "action_mask": np.asarray(action_mask, dtype=np.float32), + "states": motion[obs_indices].astype(np.float32), + "progress": progress, + "future_progress": future_progress, + "instruction": self._record_instruction(record), + "dataset_name": self.dataset_name, + "audio": audio_input, + "audio_path": str(audio_path), + "audio_start_sec": float(audio_start_sec), + "audio_duration_sec": float(audio_duration_sec), + "full_audio": bool(self.full_audio), + "motion_name": str(record["motion_name"]), + "motion_dir": str(motion_dir), + "frame_id": int(frame_id), + "total_frames": int(total_frames), + "source_name": str(record.get("source_name", self.source_name)), + "audio_task": self.audio_task, + } + for key in ("manifest_id", "motion_type"): + if key in record: + output[key] = record[key] + return output diff --git a/src/psi/data/seed/seed_video_motion_dataset.py b/src/psi/data/seed/seed_video_motion_dataset.py new file mode 100644 index 0000000000000000000000000000000000000000..ba96031a12551e5f34bc745888d735b1e66ffafa --- /dev/null +++ b/src/psi/data/seed/seed_video_motion_dataset.py @@ -0,0 +1,1585 @@ +from __future__ import annotations + +import json +import hashlib +import multiprocessing as mp +import os +import pickle +import random +import subprocess +import time +from pathlib import Path +from typing import Any, Dict, List + +import cv2 +cv2.setNumThreads(0) +cv2.ocl.setUseOpenCL(False) +import numpy as np +import pandas as pd +import torch +from tqdm import tqdm + +from psi.data.seed.progress import build_progress_conditions + + +_VIDEO_MOTION_PROMPT_POOLS: Dict[str, List[str]] = { + "visual_reference": [ + "The clip is the reference. Imitate the person's body movement as closely as possible.", + "Watch the person in the video and copy the visible action with the robot body.", + "Use the video as a demonstration, preserving the order of poses and transitions.", + "Treat the human motion in this clip as the target trajectory.", + "Mirror the action shown by the person, including weight shifts and arm gestures.", + "Read the motion from the frames and reproduce the demonstrated movement.", + "The answer should follow what the person does on screen.", + "Track the visible body configuration through the clip and convert it to robot motion.", + ], + "short_commands": [ + "Copy the motion in the video.", + "Act out the movement shown here.", + "Reproduce the person's action.", + "Follow the visual demonstration.", + "Imitate the on-screen body motion.", + "Convert this video demonstration into motion.", + "Move like the person in the clip.", + "Use the clip as the action instruction.", + ], + "daily_actions": [ + "If the person salutes, raise the hand to the head and return naturally.", + "If the clip shows walking, preserve the stepping direction and cadence.", + "If the person waves, keep the arm gesture and torso balance from the video.", + "For a turn-around clip, rotate the body in the same direction and timing.", + "For a crouch or stand-up action, keep the height change visible in the footage.", + "For a pointing gesture, follow the arm path and final pointing pose.", + "For a step-in-place action, keep the alternating foot rhythm seen in the frames.", + "For a pose transition, move from the initial stance to the final stance shown.", + ], + "visual_details": [ + "Focus on the person's pose changes rather than describing the scene background.", + "Use the body orientation, limb placement, and motion pace visible in the video.", + "The output should reflect the action dynamics seen in the frames, not an audio rhythm.", + "Keep the start pose, main gesture, and ending posture aligned with the clip.", + "When the video contains a dance, follow it; otherwise follow the ordinary action shown.", + "Respect pauses, holds, and sudden direction changes visible in the demonstration.", + "Base the movement on the image sequence: arms, legs, torso, and facing direction.", + "Let the person's visible motion, not a text caption, decide the robot action.", + ], +} + + +def ffmpeg_bin() -> str: + return os.environ.get("PSI_FFMPEG_BIN", "ffmpeg") + + +def ffprobe_bin() -> str: + return os.environ.get("PSI_FFPROBE_BIN", "ffprobe") + + +def _rank0_tqdm(iterable=None, **kwargs): + rank = int(os.environ.get("RANK", "0")) + return tqdm(iterable, disable=(rank != 0), **kwargs) + + +def _safe_int(value: Any, default: int = 0) -> int: + try: + if value in (None, ""): + return default + return int(value) + except (TypeError, ValueError): + return default + + +def _safe_float(value: Any, default: float = 0.0) -> float: + try: + if value in (None, ""): + return default + return float(value) + except (TypeError, ValueError): + return default + + +def _resolve_worker_path(path_value: Any, base_dir: Path) -> Path: + p = Path(str(path_value).strip()) + if p.is_absolute(): + return p + return base_dir / p + + +def _first_non_empty_in_row(row: dict[str, Any], keys: list[str]) -> Any: + for key in keys: + if key in row and row[key] not in (None, ""): + return row[key] + return None + + +def _peek_num_frames_worker(motion_dir: Path) -> int: + joint_path = motion_dir / "joint_pos.csv" + if not joint_path.exists(): + raise FileNotFoundError(f"joint_pos.csv not found: {joint_path}") + return int(len(pd.read_csv(joint_path))) + + +_LAZY_RECORDS_VERSION = 1 +_LAZY_RECORD_PATH_KEYS = ("motion_dir", "video_path", "source_motion_dir", "source_video_path") + + +def _lazy_records_cache_paths(cache_path: Path) -> tuple[Path, Path, Path]: + return ( + cache_path.with_suffix(".lazy.jsonl"), + cache_path.with_suffix(".lazy.offsets.npy"), + cache_path.with_suffix(".lazy.meta.json"), + ) + + +def _json_ready_record(record: Dict[str, Any]) -> Dict[str, Any]: + json_record: Dict[str, Any] = {} + for key, value in record.items(): + if isinstance(value, Path): + json_record[key] = str(value) + elif isinstance(value, np.integer): + json_record[key] = int(value) + elif isinstance(value, np.floating): + json_record[key] = float(value) + elif isinstance(value, np.bool_): + json_record[key] = bool(value) + else: + json_record[key] = value + return json_record + + +def _restore_record_paths(record: Dict[str, Any]) -> Dict[str, Any]: + for key in _LAZY_RECORD_PATH_KEYS: + value = record.get(key) + if value not in (None, ""): + record[key] = Path(value) + return record + + +class LazyVideoRecordStore: + def __init__(self, records_path: Path, offsets_path: Path, count: int) -> None: + self.records_path = Path(records_path) + self.offsets_path = Path(offsets_path) + self.count = int(count) + self.offsets = np.load(self.offsets_path, mmap_mode="r") + if len(self.offsets) != self.count: + raise ValueError( + f"Lazy record offset count mismatch: count={self.count} offsets={len(self.offsets)}" + ) + self._fh = None + + def __len__(self) -> int: + return self.count + + def __bool__(self) -> bool: + return self.count > 0 + + def __iter__(self): + for index in range(self.count): + yield self[index] + + def __getitem__(self, index: int) -> Dict[str, Any]: + index = int(index) + if index < 0: + index += self.count + if index < 0 or index >= self.count: + raise IndexError(f"Lazy record index out of range: {index}") + if self._fh is None: + self._fh = self.records_path.open("rb") + self._fh.seek(int(self.offsets[index])) + line = self._fh.readline() + if not line: + raise RuntimeError(f"Empty lazy record line at index={index}: {self.records_path}") + return _restore_record_paths(json.loads(line)) + + def close(self) -> None: + if self._fh is not None: + self._fh.close() + self._fh = None + + def __getstate__(self): + state = dict(self.__dict__) + state["_fh"] = None + return state + + def __del__(self) -> None: + self.close() + + +def _build_video_records_chunk( + chunk: list[tuple[int, str]], + worker_cfg: dict[str, Any], +) -> list[dict[str, Any]]: + base_dir = Path(worker_cfg["base_dir"]) + trust_metadata = bool(worker_cfg["trust_metadata"]) + motion_dir_key = str(worker_cfg["motion_dir_key"]) + video_path_key = str(worker_cfg["video_path_key"]) + instruction_key = str(worker_cfg["instruction_key"]) + name_key = str(worker_cfg["name_key"]) + id_key = str(worker_cfg["id_key"]) + source_name = str(worker_cfg["source_name"]) + + records: list[dict[str, Any]] = [] + for row_id, line in chunk: + try: + row = json.loads(line) + except json.JSONDecodeError as exc: + raise ValueError(f"Bad jsonl row at selected row {row_id}") from exc + + motion_value = _first_non_empty_in_row( + row, + [motion_dir_key, "converted_path", "motion_dir", "motion_path"], + ) + video_value = _first_non_empty_in_row( + row, + [video_path_key, "video_path", "original_video", "real_path"], + ) + if motion_value is None: + raise ValueError(f"Missing motion path in row {row_id}") + if video_value is None: + raise ValueError(f"Missing video path in row {row_id}") + + motion_dir = _resolve_worker_path(motion_value, base_dir) + video_path = _resolve_worker_path(video_value, base_dir) + source_motion_value = _first_non_empty_in_row(row, ["source_motion_dir", "source_motion_path"]) + source_video_value = _first_non_empty_in_row(row, ["source_video_path"]) + source_motion_dir = _resolve_worker_path(source_motion_value, base_dir) if source_motion_value else None + source_video_path = _resolve_worker_path(source_video_value, base_dir) if source_video_value else None + metadata_only = bool(row.get("metadata_only", False)) + + if not trust_metadata: + if not motion_dir.exists() and source_motion_dir is None: + raise FileNotFoundError(f"Manifest motion dir does not exist: {motion_dir}") + if source_motion_dir is not None and not source_motion_dir.exists(): + raise FileNotFoundError(f"Manifest source motion dir does not exist: {source_motion_dir}") + if not video_path.exists() and not metadata_only and source_video_path is None: + raise FileNotFoundError(f"Manifest video file does not exist: {video_path}") + if source_video_path is not None and not source_video_path.exists(): + raise FileNotFoundError(f"Manifest source video file does not exist: {source_video_path}") + + metadata_n_frames = _safe_int( + row.get("clip_frames") or row.get("motion_chunk_frames") or row.get("n_frames"), + default=0, + ) + if trust_metadata and metadata_n_frames > 0: + n_frames = metadata_n_frames + elif motion_dir.exists(): + n_frames = _peek_num_frames_worker(motion_dir) + else: + n_frames = metadata_n_frames + if n_frames <= 0: + continue + + motion_name = _first_non_empty_in_row(row, [name_key, "name"]) or motion_dir.name + record = { + "motion_dir": motion_dir, + "video_path": video_path, + "motion_name": str(motion_name), + "n_frames": int(n_frames), + "instruction": "", + "manifest_id": str(_first_non_empty_in_row(row, [id_key, "id"]) or row_id), + "source_name": source_name, + "motion_type": str(row.get("motion_type", "")), + "motion_fps_hint": _safe_float(row.get("motion_fps") or row.get("motion_fps_hint"), default=0.0), + "video_duration_hint_sec": _safe_float( + row.get("duration") or row.get("video_duration_sec"), + default=0.0, + ), + "metadata_only": metadata_only, + } + if source_motion_dir is not None: + record["source_motion_dir"] = source_motion_dir + record["source_start_frame"] = _safe_int( + row.get("chunk_start_frame") or row.get("context_start_frame"), + default=0, + ) + if source_video_path is not None: + record["source_video_path"] = source_video_path + record["source_video_start_sec"] = _safe_float(row.get("chunk_start_sec"), default=0.0) + records.append(record) + return records + + +def _build_video_records_chunk_star( + args: tuple[list[tuple[int, str]], dict[str, Any]] +) -> tuple[int, list[dict[str, Any]]]: + chunk, worker_cfg = args + return len(chunk), _build_video_records_chunk(chunk, worker_cfg) + + +def load_video_array( + video_path: Path, + video_sample_fps: float = 1.0, + video_size_pixels: int = 50176, + video_decoder: str = "opencv", +) -> tuple[np.ndarray, float, float]: + if str(video_decoder).strip().lower() == "ffmpeg": + return load_video_array_ffmpeg(video_path, video_sample_fps, video_size_pixels) + + cap = cv2.VideoCapture(str(video_path)) + src_fps = float(cap.get(cv2.CAP_PROP_FPS) or 0.0) + frame_count = int(cap.get(cv2.CAP_PROP_FRAME_COUNT) or 0) + if src_fps <= 0 or frame_count <= 0: + cap.release() + raise RuntimeError(f"Bad video metadata at {video_path}: fps={src_fps}, frames={frame_count}") + + duration_sec = frame_count / src_fps + sample_fps = float(video_sample_fps) + if sample_fps <= 0: + raise ValueError(f"video_sample_fps must be positive, got {video_sample_fps}") + num_frames = max(1, int(np.ceil(duration_sec * sample_fps))) + frame_times = np.arange(num_frames, dtype=np.float32) / sample_fps + frame_indices = np.clip(np.round(frame_times * src_fps).astype(np.int64), 0, frame_count - 1) + + frames: List[np.ndarray] = [] + for frame_index in frame_indices.tolist(): + cap.set(cv2.CAP_PROP_POS_FRAMES, int(frame_index)) + ok, frame = cap.read() + if not ok: + continue + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + if video_size_pixels > 0: + h, w = frame.shape[:2] + scale = float(np.sqrt(float(video_size_pixels) / float(max(1, h * w)))) + new_w = max(2, int(round(w * scale / 2.0)) * 2) + new_h = max(2, int(round(h * scale / 2.0)) * 2) + frame = cv2.resize(frame, (new_w, new_h), interpolation=cv2.INTER_AREA) + frames.append(frame) + cap.release() + + if not frames: + raise RuntimeError(f"No frames decoded from {video_path}") + return np.stack(frames, axis=0).astype(np.uint8), src_fps, duration_sec + + +def _parse_rate(value: str) -> float: + if not value or value == "0/0": + return 0.0 + if "/" in value: + num, den = value.split("/", 1) + den_f = float(den) + return float(num) / den_f if den_f else 0.0 + return float(value) + + +def _probe_video(video_path: Path) -> tuple[int, int, float, float]: + cmd = [ + ffprobe_bin(), + "-v", + "error", + "-select_streams", + "v:0", + "-show_entries", + "stream=width,height,avg_frame_rate,r_frame_rate,duration:format=duration", + "-of", + "json", + str(video_path), + ] + proc = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, check=False) + if proc.returncode != 0: + msg = proc.stderr.decode("utf-8", errors="ignore") + raise RuntimeError(f"ffprobe failed for {video_path}: {msg}") + + info = json.loads(proc.stdout.decode("utf-8")) + streams = info.get("streams") or [] + if not streams: + raise RuntimeError(f"No video stream found by ffprobe: {video_path}") + stream = streams[0] + width = int(stream.get("width") or 0) + height = int(stream.get("height") or 0) + src_fps = _parse_rate(str(stream.get("avg_frame_rate") or stream.get("r_frame_rate") or "0/0")) + duration_sec = float(stream.get("duration") or info.get("format", {}).get("duration") or 0.0) + if width <= 0 or height <= 0 or src_fps <= 0 or duration_sec <= 0: + raise RuntimeError( + f"Bad video metadata at {video_path}: width={width}, height={height}, " + f"fps={src_fps}, duration={duration_sec}" + ) + return width, height, src_fps, duration_sec + + +def load_video_array_ffmpeg( + video_path: Path, + video_sample_fps: float = 1.0, + video_size_pixels: int = 50176, +) -> tuple[np.ndarray, float, float]: + width, height, src_fps, duration_sec = _probe_video(video_path) + sample_fps = float(video_sample_fps) + if sample_fps <= 0: + raise ValueError(f"video_sample_fps must be positive, got {video_sample_fps}") + + if video_size_pixels > 0: + scale = float(np.sqrt(float(video_size_pixels) / float(max(1, width * height)))) + new_w = max(2, int(round(width * scale / 2.0)) * 2) + new_h = max(2, int(round(height * scale / 2.0)) * 2) + else: + new_w, new_h = width, height + + cmd = [ + ffmpeg_bin(), + "-v", + "error", + "-threads", + "1", + "-i", + str(video_path), + "-vf", + f"fps={sample_fps},scale={new_w}:{new_h}", + "-pix_fmt", + "rgb24", + "-f", + "rawvideo", + "-", + ] + proc = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, check=False) + if proc.returncode != 0: + msg = proc.stderr.decode("utf-8", errors="ignore") + raise RuntimeError(f"ffmpeg failed to decode video from {video_path}: {msg}") + + frame_size = new_h * new_w * 3 + n_frames = len(proc.stdout) // frame_size + if n_frames <= 0: + raise RuntimeError(f"No frames decoded from {video_path}") + frames = np.frombuffer(proc.stdout[: n_frames * frame_size], dtype=np.uint8) + return frames.reshape(n_frames, new_h, new_w, 3).copy(), src_fps, duration_sec + + +def load_video_clip_array( + video_path: Path, + start_sec: float, + duration_sec: float, + video_sample_fps: float = 1.0, + video_size_pixels: int = 50176, + video_decoder: str = "opencv", +) -> tuple[np.ndarray, float, float]: + if str(video_decoder).strip().lower() == "ffmpeg": + return load_video_clip_array_ffmpeg( + video_path, + start_sec=start_sec, + duration_sec=duration_sec, + video_sample_fps=video_sample_fps, + video_size_pixels=video_size_pixels, + ) + + cap = cv2.VideoCapture(str(video_path)) + src_fps = float(cap.get(cv2.CAP_PROP_FPS) or 0.0) + frame_count = int(cap.get(cv2.CAP_PROP_FRAME_COUNT) or 0) + if src_fps <= 0 or frame_count <= 0: + cap.release() + raise RuntimeError(f"Bad video metadata at {video_path}: fps={src_fps}, frames={frame_count}") + + sample_fps = float(video_sample_fps) + if sample_fps <= 0: + cap.release() + raise ValueError(f"video_sample_fps must be positive, got {video_sample_fps}") + + duration_sec_total = frame_count / src_fps + clip_start = max(0.0, float(start_sec)) + clip_duration = max(1.0 / sample_fps, float(duration_sec)) + if duration_sec_total > 0 and clip_start + clip_duration > duration_sec_total: + clip_start = max(0.0, duration_sec_total - clip_duration) + num_frames = max(1, int(np.ceil(clip_duration * sample_fps))) + frame_times = clip_start + np.arange(num_frames, dtype=np.float32) / sample_fps + frame_indices = np.clip(np.round(frame_times * src_fps).astype(np.int64), 0, frame_count - 1) + + frames: List[np.ndarray] = [] + for frame_index in frame_indices.tolist(): + cap.set(cv2.CAP_PROP_POS_FRAMES, int(frame_index)) + ok, frame = cap.read() + if not ok: + continue + frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) + if video_size_pixels > 0: + h, w = frame.shape[:2] + scale = float(np.sqrt(float(video_size_pixels) / float(max(1, h * w)))) + new_w = max(2, int(round(w * scale / 2.0)) * 2) + new_h = max(2, int(round(h * scale / 2.0)) * 2) + frame = cv2.resize(frame, (new_w, new_h), interpolation=cv2.INTER_AREA) + frames.append(frame) + cap.release() + + if not frames: + raise RuntimeError(f"No frames decoded from {video_path}") + while len(frames) < num_frames: + frames.append(frames[-1].copy()) + return np.stack(frames[:num_frames], axis=0).astype(np.uint8), src_fps, clip_duration + + +def load_video_clip_array_ffmpeg( + video_path: Path, + start_sec: float, + duration_sec: float, + video_sample_fps: float = 1.0, + video_size_pixels: int = 50176, +) -> tuple[np.ndarray, float, float]: + width, height, src_fps, source_duration_sec = _probe_video(video_path) + sample_fps = float(video_sample_fps) + if sample_fps <= 0: + raise ValueError(f"video_sample_fps must be positive, got {video_sample_fps}") + + clip_start = max(0.0, float(start_sec)) + clip_duration = max(1.0 / sample_fps, float(duration_sec)) + if source_duration_sec > 0 and clip_start + clip_duration > source_duration_sec: + clip_start = max(0.0, source_duration_sec - clip_duration) + num_frames = max(1, int(np.ceil(clip_duration * sample_fps))) + + if video_size_pixels > 0: + scale = float(np.sqrt(float(video_size_pixels) / float(max(1, width * height)))) + new_w = max(2, int(round(width * scale / 2.0)) * 2) + new_h = max(2, int(round(height * scale / 2.0)) * 2) + else: + new_w, new_h = width, height + + cmd = [ + ffmpeg_bin(), + "-v", + "error", + "-threads", + "1", + "-i", + str(video_path), + "-ss", + f"{clip_start:.6f}", + "-t", + f"{clip_duration:.6f}", + "-vf", + f"setpts=PTS-STARTPTS,fps={sample_fps},scale={new_w}:{new_h}", + "-pix_fmt", + "rgb24", + "-f", + "rawvideo", + "-", + ] + proc = subprocess.run(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, check=False) + if proc.returncode != 0: + msg = proc.stderr.decode("utf-8", errors="ignore") + raise RuntimeError(f"ffmpeg failed to decode video clip from {video_path}: {msg}") + + frame_size = new_h * new_w * 3 + n_frames = len(proc.stdout) // frame_size + if n_frames <= 0: + raise RuntimeError(f"No frames decoded from {video_path}") + frames = np.frombuffer(proc.stdout[: n_frames * frame_size], dtype=np.uint8) + frames = frames.reshape(n_frames, new_h, new_w, 3).copy() + if frames.shape[0] < num_frames: + pad = np.repeat(frames[-1:], num_frames - frames.shape[0], axis=0) + frames = np.concatenate([frames, pad], axis=0) + return frames[:num_frames], src_fps, clip_duration + + +def video_embed_cache_path( + video_path: Path, + video_embed_dir: str | Path, + video_sample_fps: float = 1.0, + video_size_pixels: int = 50176, +) -> Path: + fps_tag = str(float(video_sample_fps)).replace(".", "p") + cache_name = f"{video_path.stem}.fps{fps_tag}.size{int(video_size_pixels)}.npz" + return Path(video_embed_dir) / cache_name + + +def load_video_embed_cache(cache_path: Path) -> Dict[str, Any]: + with np.load(cache_path, allow_pickle=False) as cache: + return { + "pixel_values_videos": cache["pixel_values_videos"], + "video_grid_thw": cache["video_grid_thw"], + "video_second_per_grid": cache["video_second_per_grid"], + "video_sample_fps": float(cache["video_sample_fps"][0]), + "video_size_pixels": int(cache["video_size_pixels"][0]), + "source_video_fps": float(cache["source_video_fps"][0]), + "video_duration_sec": float(cache["video_duration_sec"][0]), + } + + +class SeedVideoMotionDataset(torch.utils.data.Dataset): + def __init__( + self, + root_dir: str = "", + video_dir: str = "", + split: str = "train", + val_ratio: float = 0.1, + split_seed: int = 42, + action_chunk_size: int = 30, + sample_stride: int = 1, + use_delta_actions: bool = True, + num_past_frames: int = 0, + motion_fps: int = 50, + video_sample_fps: float = 1.0, + video_size_pixels: int = 50176, + video_decoder: str = "opencv", + video_cache_size: int = 32, + motion_cache_size: int = 8, + video_embed_dir: str = "", + use_video_instruction: bool = False, + dataset_root: str = "", + metadata_jsonl: str = "", + manifest_path: str = "", + motion_dir_key: str = "converted_path", + video_path_key: str = "video_path", + instruction_key: str = "video_caption", + name_key: str = "name", + id_key: str = "id", + dataset_name: str = "seed-video-g1", + source_name: str = "", + trust_metadata: bool = False, + records_cache_dir: str = "", + record_build_workers: int = 0, + record_build_chunk_size: int = 4096, + build_base_sample_index: bool = True, + ) -> None: + self.root_dir = Path(root_dir) if str(root_dir).strip() else None + self.video_dir = Path(video_dir) if str(video_dir).strip() else None + self.split = split + self.val_ratio = float(val_ratio) + self.split_seed = int(split_seed) + self.action_chunk_size = int(action_chunk_size) + self.sample_stride = int(sample_stride) + self.use_delta_actions = bool(use_delta_actions) + self.num_past_frames = int(num_past_frames) + self.motion_fps = int(motion_fps) + self.video_sample_fps = float(video_sample_fps) + self.video_size_pixels = int(video_size_pixels) + self.video_decoder = str(video_decoder or "opencv").strip().lower() + self.video_cache_size = int(video_cache_size) + self.motion_cache_size = int(motion_cache_size) + self.video_embed_dir = Path(video_embed_dir) if str(video_embed_dir).strip() else None + self.use_video_instruction = bool(use_video_instruction) + self.dataset_root = Path(dataset_root) if str(dataset_root).strip() else None + self.metadata_jsonl = str(metadata_jsonl or manifest_path).strip() + self.motion_dir_key = motion_dir_key + self.video_path_key = video_path_key + self.instruction_key = instruction_key + self.name_key = name_key + self.id_key = id_key + self.dataset_name = dataset_name + self.source_name = source_name or dataset_name + self.trust_metadata = bool(trust_metadata) + self.records_cache_dir = str(records_cache_dir).strip() + self.record_build_workers = int(record_build_workers) + self.record_build_chunk_size = max(128, int(record_build_chunk_size)) + self.build_base_sample_index = bool(build_base_sample_index) + + if self.action_chunk_size <= 0: + raise ValueError(f"action_chunk_size must be positive, got {self.action_chunk_size}") + if self.sample_stride <= 0: + raise ValueError(f"sample_stride must be positive, got {self.sample_stride}") + if self.motion_fps <= 0: + raise ValueError(f"motion_fps must be positive, got {self.motion_fps}") + if self.video_sample_fps <= 0: + raise ValueError(f"video_sample_fps must be positive, got {self.video_sample_fps}") + if self.video_decoder not in ("opencv", "ffmpeg"): + raise ValueError(f"Unsupported video_decoder={self.video_decoder!r}, expected opencv/ffmpeg") + + if self.metadata_jsonl: + self.motion_records = self._load_or_build_manifest_records() + else: + self.motion_records = self._load_or_build_legacy_records() + + if not self.motion_records: + raise RuntimeError("No valid video motion records found after filtering.") + + self.sample_index: List[tuple[int, int]] = [] + if self.build_base_sample_index: + for motion_id, record in enumerate(self.motion_records): + n_frames = int(record["n_frames"]) + for frame_id in range(0, n_frames, self.sample_stride): + self.sample_index.append((motion_id, frame_id)) + if hasattr(self.motion_records, "close"): + self.motion_records.close() + + self._motion_cache: Dict[str, np.ndarray] = {} + self._video_cache: Dict[str, tuple[np.ndarray, float, float]] = {} + self._video_embed_cache: Dict[str, Dict[str, Any]] = {} + + def _resolve_path(self, path_value: Any, base_dir: Path | None = None) -> Path: + p = Path(str(path_value).strip()) + if p.is_absolute(): + return p + if base_dir is not None: + return base_dir / p + return p + + def _manifest_path(self) -> Path: + return self._resolve_path(self.metadata_jsonl, self.dataset_root) + + def _default_records_cache_dir(self, manifest_path: Path) -> Path: + if self.records_cache_dir: + return Path(self.records_cache_dir) + base_dir = self.dataset_root or manifest_path.parent + return base_dir / ".index_cache_multimodal_yth" + + def _default_legacy_records_cache_dir(self) -> Path: + if self.records_cache_dir: + return Path(self.records_cache_dir) + if self.root_dir is None: + raise FileNotFoundError("root_dir is required for legacy video cache") + return self.root_dir / ".index_cache_multimodal_yth" + + def _records_cache_path(self) -> Path | None: + if not self.metadata_jsonl: + return None + manifest_path = self._manifest_path() + if not manifest_path.exists(): + return None + stat = manifest_path.stat() + key_data = { + "version": 2, + "kind": "records", + "manifest_path": str(manifest_path.resolve()), + "manifest_size": stat.st_size, + "manifest_mtime_ns": stat.st_mtime_ns, + "dataset_root": str(self.dataset_root.resolve()) if self.dataset_root is not None else "", + "split": self.split, + "val_ratio": self.val_ratio, + "split_seed": self.split_seed, + "motion_dir_key": self.motion_dir_key, + "video_path_key": self.video_path_key, + "instruction_key": self.instruction_key, + "name_key": self.name_key, + "id_key": self.id_key, + "dataset_name": self.dataset_name, + "source_name": self.source_name, + "trust_metadata": self.trust_metadata, + } + digest = hashlib.sha1(json.dumps(key_data, sort_keys=True).encode("utf-8")).hexdigest()[:20] + return self._default_records_cache_dir(manifest_path) / f"records.{self.source_name}.{self.split}.{digest}.pkl" + + def _legacy_records_cache_path(self) -> Path | None: + if self.root_dir is None or self.video_dir is None: + return None + if not self.root_dir.exists() or not self.video_dir.exists(): + return None + key_data = { + "version": 2, + "kind": "video_legacy_records", + "root_dir": str(self.root_dir.resolve()), + "video_dir": str(self.video_dir.resolve()), + "split": self.split, + "val_ratio": self.val_ratio, + "split_seed": self.split_seed, + "dataset_name": self.dataset_name, + "source_name": self.source_name, + } + digest = hashlib.sha1(json.dumps(key_data, sort_keys=True).encode("utf-8")).hexdigest()[:20] + return self._default_legacy_records_cache_dir() / f"records.{self.source_name}.{self.split}.{digest}.pkl" + + def _list_legacy_motion_dirs(self) -> List[Path]: + if self.root_dir is None: + return [] + motion_dirs: List[Path] = [] + for path in sorted(self.root_dir.iterdir()): + if not path.is_dir(): + continue + if path.name.startswith("."): + continue + if not (path / "joint_pos.csv").exists(): + continue + motion_dirs.append(path) + return motion_dirs + + def _load_or_build_manifest_records(self) -> Any: + cache_path = self._records_cache_path() + rank = int(os.environ.get("RANK", "0")) + if cache_path is not None: + cached_lazy = self._try_load_lazy_records(cache_path) + if cached_lazy is not None: + return cached_lazy + if rank != 0: + waited = self._wait_for_lazy_records(cache_path) + if waited is not None: + return waited + + cached = self._try_load_pickle(cache_path) + if cached is not None: + self._atomic_save_lazy_records(cache_path, cached) + cached_lazy = self._try_load_lazy_records(cache_path) + return cached_lazy if cached_lazy is not None else cached + if rank != 0: + waited = self._wait_for_pickle(cache_path) + if waited is not None: + return waited + + records = self._build_manifest_records() + if cache_path is not None: + self._atomic_save_lazy_records(cache_path, records) + cached_lazy = self._try_load_lazy_records(cache_path) + if cached_lazy is not None: + return cached_lazy + return records + + def _load_or_build_legacy_records(self) -> Any: + cache_path = self._legacy_records_cache_path() + rank = int(os.environ.get("RANK", "0")) + if cache_path is not None: + cached_lazy = self._try_load_lazy_records(cache_path) + if cached_lazy is not None: + return cached_lazy + if rank != 0: + waited = self._wait_for_lazy_records(cache_path) + if waited is not None: + return waited + + cached = self._try_load_pickle(cache_path) + if cached is not None: + self._atomic_save_lazy_records(cache_path, cached) + cached_lazy = self._try_load_lazy_records(cache_path) + return cached_lazy if cached_lazy is not None else cached + if rank != 0: + waited = self._wait_for_pickle(cache_path) + if waited is not None: + return waited + + records = self._build_legacy_records() + if cache_path is not None: + self._atomic_save_lazy_records(cache_path, records) + cached_lazy = self._try_load_lazy_records(cache_path) + if cached_lazy is not None: + return cached_lazy + return records + + def _try_load_lazy_records(self, cache_path: Path) -> LazyVideoRecordStore | None: + records_path, offsets_path, meta_path = _lazy_records_cache_paths(cache_path) + if not (records_path.exists() and offsets_path.exists() and meta_path.exists()): + return None + try: + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + size_gb = records_path.stat().st_size / (1024 ** 3) + print(f"[INDEX CACHE] lazy loading {records_path} ({size_gb:.2f} GB)", flush=True) + start = time.time() + with meta_path.open("r", encoding="utf-8") as f: + meta = json.load(f) + if int(meta.get("version", 0)) != _LAZY_RECORDS_VERSION: + return None + store = LazyVideoRecordStore( + records_path=records_path, + offsets_path=offsets_path, + count=int(meta["count"]), + ) + if rank == 0: + print(f"[INDEX CACHE] lazy loaded {records_path} in {time.time() - start:.1f}s", flush=True) + return store + except Exception as exc: + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + print(f"[INDEX CACHE] lazy cache unavailable for {cache_path}: {exc}", flush=True) + return None + + def _wait_for_lazy_records(self, cache_path: Path, timeout_sec: int = 7200) -> LazyVideoRecordStore | None: + rank = int(os.environ.get("RANK", "0")) + records_path, _, _ = _lazy_records_cache_paths(cache_path) + if rank == 0: + print(f"[INDEX CACHE] waiting for lazy records {records_path}", flush=True) + start = time.time() + while time.time() - start < timeout_sec: + obj = self._try_load_lazy_records(cache_path) + if obj is not None: + return obj + time.sleep(2.0) + return None + + def _try_load_pickle(self, path: Path) -> Any | None: + if not path.exists(): + return None + try: + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + size_gb = path.stat().st_size / (1024 ** 3) + print(f"[INDEX CACHE] loading {path} ({size_gb:.2f} GB)", flush=True) + start = time.time() + with path.open("rb") as f: + obj = pickle.load(f) + if isinstance(obj, dict) and "data" in obj: + obj = obj["data"] + if rank == 0: + print(f"[INDEX CACHE] loaded {path} in {time.time() - start:.1f}s", flush=True) + return obj + except Exception: + return None + + def _wait_for_pickle(self, path: Path, timeout_sec: int = 7200) -> Any | None: + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + print(f"[INDEX CACHE] waiting for {path}", flush=True) + start = time.time() + while time.time() - start < timeout_sec: + obj = self._try_load_pickle(path) + if obj is not None: + return obj + time.sleep(2.0) + return None + + def _atomic_save_pickle(self, path: Path, data: Any) -> None: + path.parent.mkdir(parents=True, exist_ok=True) + tmp_path = path.with_name(f".{path.name}.{os.getpid()}.tmp") + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + print(f"[INDEX CACHE] saving {path}", flush=True) + start = time.time() + with tmp_path.open("wb") as f: + pickle.dump({"data": data}, f, protocol=pickle.HIGHEST_PROTOCOL) + os.replace(tmp_path, path) + if rank == 0: + size_gb = path.stat().st_size / (1024 ** 3) + print(f"[INDEX CACHE] saved {path} ({size_gb:.2f} GB) in {time.time() - start:.1f}s", flush=True) + + def _atomic_save_lazy_records(self, cache_path: Path, records: Any) -> None: + records_path, offsets_path, meta_path = _lazy_records_cache_paths(cache_path) + records_path.parent.mkdir(parents=True, exist_ok=True) + pid = os.getpid() + tmp_records_path = records_path.with_name(f".{records_path.name}.{pid}.tmp") + tmp_offsets_path = offsets_path.with_name(f".{offsets_path.name}.{pid}.tmp") + tmp_meta_path = meta_path.with_name(f".{meta_path.name}.{pid}.tmp") + + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + print(f"[INDEX CACHE] saving lazy records {records_path}", flush=True) + start = time.time() + offsets: list[int] = [] + count = 0 + try: + with tmp_records_path.open("wb") as f: + for record in _rank0_tqdm( + records, + desc=f"Write lazy records {self.source_name}/{self.split}", + total=len(records) if hasattr(records, "__len__") else None, + unit="record", + ): + offsets.append(f.tell()) + line = json.dumps( + _json_ready_record(record), + ensure_ascii=False, + separators=(",", ":"), + ).encode("utf-8") + f.write(line) + f.write(b"\n") + count += 1 + + with tmp_offsets_path.open("wb") as f: + np.save(f, np.asarray(offsets, dtype=np.int64)) + with tmp_meta_path.open("w", encoding="utf-8") as f: + json.dump( + { + "version": _LAZY_RECORDS_VERSION, + "count": count, + "source_name": self.source_name, + "split": self.split, + "records_cache": cache_path.name, + }, + f, + indent=2, + ) + + os.replace(tmp_records_path, records_path) + os.replace(tmp_offsets_path, offsets_path) + os.replace(tmp_meta_path, meta_path) + finally: + for tmp_path in (tmp_records_path, tmp_offsets_path, tmp_meta_path): + if tmp_path.exists(): + try: + tmp_path.unlink() + except OSError: + pass + + if rank == 0: + size_gb = records_path.stat().st_size / (1024 ** 3) + print( + f"[INDEX CACHE] saved lazy records {records_path} " + f"({size_gb:.2f} GB, {count:,} records) in {time.time() - start:.1f}s", + flush=True, + ) + + def _first_non_empty(self, row: dict[str, Any], keys: list[str]) -> Any: + for key in keys: + if key in row and row[key] not in (None, ""): + return row[key] + return None + + def _build_legacy_records(self) -> List[Dict[str, Any]]: + if self.root_dir is None or not self.root_dir.exists(): + raise FileNotFoundError(f"Motion root dir does not exist: {self.root_dir}") + if self.video_dir is None or not self.video_dir.exists(): + raise FileNotFoundError(f"Video dir does not exist: {self.video_dir}") + + motion_dirs = self._list_legacy_motion_dirs() + selected_ids = self._select_ids(len(motion_dirs), self.split) + + records: List[Dict[str, Any]] = [] + for motion_id in _rank0_tqdm( + selected_ids, + desc=f"Build video records {self.source_name}/{self.split}", + unit="motion", + ): + motion_dir = motion_dirs[int(motion_id)] + video_path = self.video_dir / f"{motion_dir.name}.mp4" + if not video_path.exists(): + raise FileNotFoundError(f"Matched video file does not exist: {video_path}") + + n_frames = self._peek_num_frames(motion_dir) + if n_frames <= 0: + continue + + records.append( + { + "motion_dir": motion_dir, + "video_path": video_path, + "motion_name": motion_dir.name, + "n_frames": n_frames, + "source_name": self.source_name, + } + ) + return records + + def _count_manifest_entries(self, manifest_path: Path) -> int: + count = 0 + with manifest_path.open("r", encoding="utf-8") as f: + for line in _rank0_tqdm( + f, + desc=f"Count video manifest {self.source_name}/{self.split}", + unit="line", + ): + if line.strip(): + count += 1 + return count + + def _selected_manifest_mask(self, total_rows: int) -> np.ndarray: + selected_ids = self._select_ids(total_rows, self.split) + selected_mask = np.zeros(total_rows, dtype=np.bool_) + selected_mask[selected_ids] = True + return selected_mask + + def _iter_selected_manifest_chunks( + self, + manifest_path: Path, + selected_mask: np.ndarray, + ): + chunk: list[tuple[int, str]] = [] + data_row_id = 0 + with manifest_path.open("r", encoding="utf-8") as f: + for line in f: + line = line.strip() + if not line: + continue + if selected_mask[data_row_id]: + chunk.append((data_row_id, line)) + if len(chunk) >= self.record_build_chunk_size: + yield chunk + chunk = [] + data_row_id += 1 + if chunk: + yield chunk + + def _manifest_worker_count(self, selected_rows: int) -> int: + if self.record_build_workers <= 1: + return 0 + if selected_rows < 200_000: + return 0 + cpu_cap = os.cpu_count() or 1 + return max(0, min(self.record_build_workers, cpu_cap)) + + def _manifest_worker_cfg(self, base_dir: Path) -> dict[str, Any]: + return { + "base_dir": str(base_dir), + "trust_metadata": bool(self.trust_metadata), + "motion_dir_key": self.motion_dir_key, + "video_path_key": self.video_path_key, + "instruction_key": self.instruction_key, + "name_key": self.name_key, + "id_key": self.id_key, + "source_name": self.source_name, + } + + def _build_manifest_records(self) -> List[Dict[str, Any]]: + manifest_path = self._manifest_path() + if not manifest_path.exists(): + raise FileNotFoundError(f"Video manifest does not exist: {manifest_path}") + + base_dir = self.dataset_root or manifest_path.parent + if self.dataset_root is not None and not self.dataset_root.exists(): + raise FileNotFoundError(f"Dataset root does not exist: {self.dataset_root}") + + total_rows = self._count_manifest_entries(manifest_path) + selected_mask = self._selected_manifest_mask(total_rows) + selected_rows = int(selected_mask.sum()) + worker_count = self._manifest_worker_count(selected_rows) + + rank = int(os.environ.get("RANK", "0")) + if rank == 0: + print( + f"[INDEX BUILD] {self.source_name}/{self.split}: total_rows={total_rows:,} " + f"selected_rows={selected_rows:,} workers={worker_count or 1} " + f"chunk_size={self.record_build_chunk_size}", + flush=True, + ) + + if worker_count <= 0: + records: List[Dict[str, Any]] = [] + progress = _rank0_tqdm( + total=selected_rows, + desc=f"Build video records {self.source_name}/{self.split}", + unit="row", + ) + try: + for chunk in self._iter_selected_manifest_chunks(manifest_path, selected_mask): + records.extend(_build_video_records_chunk(chunk, self._manifest_worker_cfg(base_dir))) + progress.update(len(chunk)) + finally: + progress.close() + return records + + records: List[Dict[str, Any]] = [] + worker_cfg = self._manifest_worker_cfg(base_dir) + ctx = mp.get_context("spawn") + chunk_iter = self._iter_selected_manifest_chunks(manifest_path, selected_mask) + with ctx.Pool(processes=worker_count) as pool: + progress = _rank0_tqdm( + total=selected_rows, + desc=f"Build video records {self.source_name}/{self.split}", + unit="row", + ) + try: + task_iter = ((chunk, worker_cfg) for chunk in chunk_iter) + for chunk_size, chunk_records in pool.imap( + _build_video_records_chunk_star, + task_iter, + chunksize=1, + ): + records.extend(chunk_records) + progress.update(chunk_size) + finally: + progress.close() + return records + + def _compose_video_instruction(self) -> str: + layout = random.choice(list(_VIDEO_MOTION_PROMPT_POOLS.keys())) + return random.choice(_VIDEO_MOTION_PROMPT_POOLS[layout]).strip() + + def _record_instruction(self, record: Dict[str, Any]) -> str: + if not self.use_video_instruction: + return "" + return self._compose_video_instruction() + + def _select_ids(self, n: int, split: str) -> np.ndarray: + if n == 0: + return np.array([], dtype=np.int64) + + rng = np.random.RandomState(self.split_seed) + perm = rng.permutation(n) + n_val = int(round(n * self.val_ratio)) + if n > 1 and self.val_ratio > 0 and n_val == 0: + n_val = 1 + if n > 1 and n_val >= n: + n_val = n - 1 + + split_norm = (split or "train").lower() + if split_norm == "train": + return perm[n_val:] + if split_norm in ("val", "valid", "validation", "test"): + return perm[:n_val] + if split_norm == "all": + return perm + raise ValueError(f"Unsupported split='{split}', expected train/val/all") + + def _peek_num_frames(self, motion_dir: Path) -> int: + joint_path = motion_dir / "joint_pos.csv" + if not joint_path.exists(): + raise FileNotFoundError(f"joint_pos.csv not found: {joint_path}") + return int(len(pd.read_csv(joint_path))) + + def _load_motion_array(self, motion_dir: Path) -> np.ndarray: + key = str(motion_dir) + if key in self._motion_cache: + return self._motion_cache[key] + + joint = pd.read_csv(motion_dir / "joint_pos.csv").to_numpy(dtype=np.float32) + body_pos = pd.read_csv(motion_dir / "body_pos.csv").to_numpy(dtype=np.float32) + body_quat = pd.read_csv(motion_dir / "body_quat.csv").to_numpy(dtype=np.float32) + lengths = {joint.shape[0], body_pos.shape[0], body_quat.shape[0]} + if len(lengths) != 1: + raise ValueError( + f"Frame length mismatch at {motion_dir}: " + f"joint={joint.shape[0]}, body_pos={body_pos.shape[0]}, body_quat={body_quat.shape[0]}" + ) + if joint.shape[1] != 29 or body_pos.shape[1] != 3 or body_quat.shape[1] != 4: + raise ValueError(f"Unexpected G1 csv shape at {motion_dir}") + + motion = np.concatenate([joint, body_pos, body_quat], axis=1).astype(np.float32) + if self.motion_cache_size > 0: + if len(self._motion_cache) >= self.motion_cache_size: + self._motion_cache.pop(next(iter(self._motion_cache))) + self._motion_cache[key] = motion + return motion + + def _load_record_motion_array(self, record: Dict[str, Any]) -> np.ndarray: + source_motion_dir = record.get("source_motion_dir") + if source_motion_dir is None: + return self._load_motion_array(record["motion_dir"]) + + full_motion = self._load_motion_array(Path(source_motion_dir)) + start = int(record.get("source_start_frame") or 0) + n_frames = int(record["n_frames"]) + end = start + n_frames + if start < 0 or end > full_motion.shape[0]: + raise ValueError( + f"Source motion slice out of range at {source_motion_dir}: " + f"start={start}, n_frames={n_frames}, total={full_motion.shape[0]}" + ) + return full_motion[start:end].copy() + + def _load_video_array(self, video_path: Path) -> tuple[np.ndarray, float, float]: + key = str(video_path) + if key in self._video_cache: + return self._video_cache[key] + + video_item = load_video_array( + video_path, + video_sample_fps=self.video_sample_fps, + video_size_pixels=self.video_size_pixels, + video_decoder=self.video_decoder, + ) + if self.video_cache_size > 0: + if len(self._video_cache) >= self.video_cache_size: + self._video_cache.pop(next(iter(self._video_cache))) + self._video_cache[key] = video_item + return video_item + + def _load_video_embed_cache(self, video_path: Path) -> tuple[Path, Dict[str, Any]] | None: + if self.video_embed_dir is None: + return None + + cache_path = video_embed_cache_path( + video_path, + self.video_embed_dir, + video_sample_fps=self.video_sample_fps, + video_size_pixels=self.video_size_pixels, + ) + if not cache_path.exists(): + return None + + key = str(cache_path) + if key in self._video_embed_cache: + return cache_path, self._video_embed_cache[key] + + video_embed = load_video_embed_cache(cache_path) + if self.video_cache_size > 0: + if len(self._video_embed_cache) >= self.video_cache_size: + self._video_embed_cache.pop(next(iter(self._video_embed_cache))) + self._video_embed_cache[key] = video_embed + return cache_path, self._video_embed_cache[key] + + def __len__(self) -> int: + return len(self.sample_index) + + def _pad_index(self, index: int, length: int) -> int: + if index < 0: + return 0 + if index >= length: + return length - 1 + return index + + def __getitem__(self, idx: int) -> Dict[str, Any]: + motion_id, frame_id = self.sample_index[idx] + record = self.motion_records[motion_id] + motion_dir: Path = record["motion_dir"] + video_path: Path = record["video_path"] + + motion = self._load_record_motion_array(record) + cached_video = self._load_video_embed_cache(video_path) + if cached_video is None: + video, source_video_fps, video_duration_sec = self._load_video_array(video_path) + video_embed_path = "" + video_embed: Dict[str, Any] = {} + else: + video_embed_path_obj, video_embed = cached_video + video = None + video_embed_path = str(video_embed_path_obj) + source_video_fps = float(video_embed["source_video_fps"]) + video_duration_sec = float(video_embed["video_duration_sec"]) + total_frames = motion.shape[0] + action_len = self.action_chunk_size + (1 if self.use_delta_actions else 0) + + actions = [] + action_mask = [] + for step in range(action_len): + if self.use_delta_actions: + raw_index = frame_id + step * self.sample_stride + else: + raw_index = frame_id + (step + 1) * self.sample_stride + valid = raw_index < total_frames + actions.append(motion[min(raw_index, total_frames - 1)]) + action_mask.append(1.0 if valid else 0.0) + action_arr = np.stack(actions, axis=0).astype(np.float32) + progress, future_progress = build_progress_conditions( + frame_id=frame_id, + total_frames=total_frames, + action_horizon=action_arr.shape[0], + step_stride=self.sample_stride, + use_delta_actions=self.use_delta_actions, + ) + + obs_indices = [ + self._pad_index(frame_id - (self.num_past_frames - t) * self.sample_stride, total_frames) + for t in range(self.num_past_frames + 1) + ] + + output = { + "actions": action_arr, + "action_mask": np.asarray(action_mask, dtype=np.float32), + "states": motion[obs_indices].astype(np.float32), + "progress": progress, + "future_progress": future_progress, + "instruction": self._record_instruction(record), + "dataset_name": self.dataset_name, + "video_path": str(video_path), + "video_sample_fps": float(self.video_sample_fps), + "video_size_pixels": int(self.video_size_pixels), + "source_video_fps": float(source_video_fps), + "video_duration_sec": float(video_duration_sec), + "full_video": True, + "video_embed_path": video_embed_path, + "motion_name": str(record["motion_name"]), + "motion_dir": str(motion_dir), + "frame_id": int(frame_id), + "total_frames": int(total_frames), + "source_name": str(record.get("source_name", self.source_name)), + } + for key in ("manifest_id", "motion_type"): + if key in record: + output[key] = record[key] + if video is not None: + output["video"] = video + for key in ("pixel_values_videos", "video_grid_thw", "video_second_per_grid"): + if key in video_embed: + output[key] = video_embed[key] + return output + + +class SeedVideoClipMotionDataset(SeedVideoMotionDataset): + def __init__( + self, + *args, + action_exec_horizon: int = 0, + clip_stride_frames: int = 0, + **kwargs, + ) -> None: + kwargs["build_base_sample_index"] = False + super().__init__(*args, **kwargs) + + self.action_exec_horizon = int(action_exec_horizon) + stride_frames = int(clip_stride_frames) + if stride_frames <= 0 and self.action_exec_horizon > 0: + stride_frames = self.action_exec_horizon * self.sample_stride + if stride_frames <= 0: + stride_frames = self.action_chunk_size * self.sample_stride + if stride_frames <= 0: + raise ValueError(f"clip_stride_frames must be positive, got {stride_frames}") + self.clip_stride_frames = stride_frames + + cached_sample_index = self._load_or_build_clip_sample_index() + if cached_sample_index is not None: + self.sample_index = cached_sample_index + return + + self.sample_index = self._build_clip_sample_index() + cache_path = self._clip_sample_index_cache_path() + if cache_path is not None: + self._atomic_save_pickle(cache_path, self.sample_index) + + def _clip_sample_index_cache_path(self) -> Path | None: + records_cache_path = self._records_cache_path() + if records_cache_path is None: + return None + key_data = { + "version": 2, + "kind": "video_clip_sample_index", + "records_cache": records_cache_path.name, + "action_exec_horizon": self.action_exec_horizon, + "clip_stride_frames": self.clip_stride_frames, + "action_chunk_size": self.action_chunk_size, + "sample_stride": self.sample_stride, + "use_delta_actions": self.use_delta_actions, + } + digest = hashlib.sha1(json.dumps(key_data, sort_keys=True).encode("utf-8")).hexdigest()[:20] + return records_cache_path.parent / f"sample_index.{self.source_name}.{self.split}.{digest}.pkl" + + def _load_or_build_clip_sample_index(self) -> List[tuple[int, int]] | None: + cache_path = self._clip_sample_index_cache_path() + if cache_path is None: + return None + cached = self._try_load_pickle(cache_path) + if cached is not None: + return cached + rank = int(os.environ.get("RANK", "0")) + if rank != 0: + return self._wait_for_pickle(cache_path) + return None + + def _build_clip_sample_index(self) -> List[tuple[int, int]]: + sample_index: List[tuple[int, int]] = [] + try: + for motion_id, record in _rank0_tqdm( + enumerate(self.motion_records), + total=len(self.motion_records), + desc=f"Build video clip index {self.source_name}/{self.split}", + unit="motion", + ): + n_frames = int(record["n_frames"]) + max_start_frame = self._max_clip_start_frame(record) + if max_start_frame < 0 and n_frames > 0: + max_start_frame = 0 + for frame_id in range(0, max_start_frame + 1, self.clip_stride_frames): + sample_index.append((motion_id, frame_id)) + finally: + if hasattr(self.motion_records, "close"): + self.motion_records.close() + return sample_index + + def _record_motion_fps(self, record: Dict[str, Any]) -> float: + motion_fps_hint = float(record.get("motion_fps_hint") or 0.0) + if motion_fps_hint > 0: + return motion_fps_hint + n_frames = int(record["n_frames"]) + duration_sec = float(record.get("video_duration_hint_sec") or 0.0) + if duration_sec > 0 and n_frames > 0: + return float(n_frames) / duration_sec + return float(self.motion_fps) + + def _max_clip_start_frame(self, record: Dict[str, Any]) -> int: + n_frames = int(record["n_frames"]) + max_action_start = n_frames - self.action_chunk_size * self.sample_stride - 1 + duration_sec = float(record.get("video_duration_hint_sec") or 0.0) + if duration_sec <= 0: + return min(n_frames - 1, max_action_start) + + motion_fps = self._record_motion_fps(record) + clip_duration_sec = float(self.action_chunk_size * self.sample_stride) / motion_fps + max_video_start = int(np.floor(max(0.0, duration_sec - clip_duration_sec) * motion_fps + 1e-6)) + return min(n_frames - 1, max_action_start, max_video_start) + + def _clip_window(self, record: Dict[str, Any], frame_id: int) -> tuple[float, float]: + motion_fps = self._record_motion_fps(record) + start_sec = float(frame_id) / motion_fps + duration_sec = float(self.action_chunk_size * self.sample_stride) / motion_fps + return start_sec, duration_sec + + def _source_clip_window(self, record: Dict[str, Any], frame_id: int) -> tuple[Path, float, float] | None: + source_video_path = record.get("source_video_path") + if source_video_path is None: + return None + local_start_sec, duration_sec = self._clip_window(record, frame_id) + source_start_sec = float(record.get("source_video_start_sec") or 0.0) + local_start_sec + return Path(source_video_path), source_start_sec, duration_sec + + def _load_video_clip_array( + self, + video_path: Path, + start_sec: float, + duration_sec: float, + ) -> tuple[np.ndarray, float, float]: + key = ( + f"{video_path}|start={start_sec:.6f}|duration={duration_sec:.6f}|" + f"fps={self.video_sample_fps}|size={self.video_size_pixels}|decoder={self.video_decoder}" + ) + if key in self._video_cache: + return self._video_cache[key] + + video_item = load_video_clip_array( + video_path, + start_sec=start_sec, + duration_sec=duration_sec, + video_sample_fps=self.video_sample_fps, + video_size_pixels=self.video_size_pixels, + video_decoder=self.video_decoder, + ) + if self.video_cache_size > 0: + if len(self._video_cache) >= self.video_cache_size: + self._video_cache.pop(next(iter(self._video_cache))) + self._video_cache[key] = video_item + return video_item + + def __getitem__(self, idx: int) -> Dict[str, Any]: + motion_id, frame_id = self.sample_index[idx] + record = self.motion_records[motion_id] + motion_dir: Path = record["motion_dir"] + video_path: Path = record["video_path"] + + motion = self._load_record_motion_array(record) + video_clip_start_sec, video_clip_duration_sec = self._clip_window(record, frame_id) + cached_video = self._load_video_embed_cache(video_path) + if cached_video is None: + source_clip = self._source_clip_window(record, frame_id) + decode_video_path = video_path + decode_start_sec = video_clip_start_sec + if source_clip is not None and not video_path.exists(): + decode_video_path, decode_start_sec, video_clip_duration_sec = source_clip + video, source_video_fps, video_duration_sec = self._load_video_clip_array( + decode_video_path, + start_sec=decode_start_sec, + duration_sec=video_clip_duration_sec, + ) + video_embed_path = "" + video_embed: Dict[str, Any] = {} + else: + video_embed_path_obj, video_embed = cached_video + video = None + video_embed_path = str(video_embed_path_obj) + source_video_fps = float(video_embed["source_video_fps"]) + video_duration_sec = float(video_embed["video_duration_sec"]) + total_frames = motion.shape[0] + action_len = self.action_chunk_size + (1 if self.use_delta_actions else 0) + + actions = [] + action_mask = [] + for step in range(action_len): + if self.use_delta_actions: + raw_index = frame_id + step * self.sample_stride + else: + raw_index = frame_id + (step + 1) * self.sample_stride + valid = raw_index < total_frames + actions.append(motion[min(raw_index, total_frames - 1)]) + action_mask.append(1.0 if valid else 0.0) + action_arr = np.stack(actions, axis=0).astype(np.float32) + progress, future_progress = build_progress_conditions( + frame_id=frame_id, + total_frames=total_frames, + action_horizon=action_arr.shape[0], + step_stride=self.sample_stride, + use_delta_actions=self.use_delta_actions, + ) + + obs_indices = [ + self._pad_index(frame_id - (self.num_past_frames - t) * self.sample_stride, total_frames) + for t in range(self.num_past_frames + 1) + ] + + output = { + "actions": action_arr, + "action_mask": np.asarray(action_mask, dtype=np.float32), + "states": motion[obs_indices].astype(np.float32), + "progress": progress, + "future_progress": future_progress, + "instruction": self._record_instruction(record), + "dataset_name": self.dataset_name, + "video_path": str(video_path), + "video_sample_fps": float(self.video_sample_fps), + "video_size_pixels": int(self.video_size_pixels), + "source_video_fps": float(source_video_fps), + "video_duration_sec": float(video_duration_sec), + "full_video": False, + "video_embed_path": video_embed_path, + "motion_name": str(record["motion_name"]), + "motion_dir": str(motion_dir), + "frame_id": int(frame_id), + "total_frames": int(total_frames), + "source_name": str(record.get("source_name", self.source_name)), + "video_clip_start_sec": float(video_clip_start_sec), + "video_clip_duration_sec": float(video_clip_duration_sec), + "video_clip_stride_frames": int(self.clip_stride_frames), + "video_clip_action_chunk_frames": int(self.action_chunk_size * self.sample_stride), + } + if video is not None: + output["video"] = video + for key in ("pixel_values_videos", "video_grid_thw", "video_second_per_grid"): + if key in video_embed: + output[key] = video_embed[key] + for key in ("manifest_id", "motion_type"): + if key in record: + output[key] = record[key] + return output diff --git a/src/psi/data/seed/text_prompt.py b/src/psi/data/seed/text_prompt.py new file mode 100644 index 0000000000000000000000000000000000000000..8fdea3f69c2e24213c6893149d0aa163dc4e813b --- /dev/null +++ b/src/psi/data/seed/text_prompt.py @@ -0,0 +1,144 @@ +from __future__ import annotations + +import random +from collections.abc import Mapping, Sequence +from typing import Any, List + +import pandas as pd + + +DEFAULT_SEED_TEXT_FIELDS: List[str] = [ + "content_short_description", + "content_short_description_2", + "content_technical_description", + "content_natural_desc_1", + "content_natural_desc_2", + "content_natural_desc_3", + "content_natural_desc_4", +] + +SYSTEM_PROMPT_POOLS: dict[str, list[str]] = { + "system_first": [ + "Generate a motion sequence that matches the following description.", + "Convert the following language instruction into an executable motion sequence.", + "From a motion-control perspective, realize the following behavior description.", + "Read the following description and generate a matching motion.", + "Complete the motion generation task based on the description below.", + "Generate a trackable motion sequence centered on the following description.", + "Given the description below, produce a continuous full-body motion clip.", + "Use the text instruction below to synthesize a coherent motion trajectory.", + "Please generate motion conditioned on the following user request.", + "Generate physically plausible motion according to the description that follows.", + ], + "desc_first": [ + "Based on the description above, generate a matching motion sequence.", + "Using the user request above, synthesize a smooth full-body motion trajectory.", + "Take the instruction above and convert it into an executable motion sequence.", + "Generate stable and semantically consistent motion from the description above.", + "Ground the behavior described above into time-series motion output.", + ], + "neutral": [ + "You are a motion generation assistant. Produce a natural and coherent full-body motion from the provided text.", + "Synthesize a full-body motion trajectory from the text description.", + "Generate a realistic motion performance consistent with the text prompt.", + "Output a complete motion sequence from the requirement description without semantic drift.", + "Generate a motion trajectory that satisfies the given text instruction.", + ], +} + +PROMPT_COMPOSITION_TEMPLATES_BY_LAYOUT: dict[str, list[str]] = { + "system_first": [ + "{system}\nDescription: {desc}", + "{system}\nUser request: {desc}", + "{system} {desc}", + "{system}\nInput text: \"{desc}\"", + "{system}\nBehavior description: {desc}", + "{system} (Description: {desc})", + ], + "desc_first": [ + "Description: {desc}\n{system}", + "User request: {desc}\n{system}", + "{desc}\n{system}", + "Input text: \"{desc}\"\n{system}", + "Behavior description: {desc}\n{system}", + ], + "neutral": [ + "{system}\nDescription: {desc}", + "Description: {desc}\n{system}", + "{system} {desc}", + "{desc}\n{system}", + "{system}\nInput text: {desc}", + ], +} + + +def _row_has_key(row: Any, key: str) -> bool: + if isinstance(row, Mapping): + return key in row + index = getattr(row, "index", None) + return key in index if index is not None else hasattr(row, key) + + +def _row_get(row: Any, key: str, default: Any = None) -> Any: + if isinstance(row, Mapping): + return row.get(key, default) + if _row_has_key(row, key): + return row[key] + return getattr(row, key, default) + + +def collect_seed_text_candidates(row: Any, text_fields: Sequence[str] | None = None) -> list[str]: + candidates: list[str] = [] + seen = set() + for key in text_fields or DEFAULT_SEED_TEXT_FIELDS: + if not _row_has_key(row, key): + continue + value = _row_get(row, key) + if pd.isna(value): + continue + text = str(value).strip() + if (not text) or text.lower() == "nan": + continue + signature = text.lower() + if signature in seen: + continue + seen.add(signature) + candidates.append(text) + return candidates + + +def compose_seed_motion_prompt(desc: str, *, rng: Any = random) -> str: + desc = str(desc).strip() + if not desc: + desc = "seed motion" + layout = rng.choice(list(SYSTEM_PROMPT_POOLS.keys())) + system_prompt = rng.choice(SYSTEM_PROMPT_POOLS[layout]) + template = rng.choice(PROMPT_COMPOSITION_TEMPLATES_BY_LAYOUT[layout]) + return template.format(system=system_prompt, desc=desc).strip() + + +def compose_seed_motion_prompt_from_candidates( + candidates: Sequence[str], + *, + fallback: str = "seed motion", + rng: Any = random, +) -> str: + pool = [str(c).strip() for c in candidates if str(c).strip()] + if not pool: + pool = [str(fallback).strip() or "seed motion"] + return compose_seed_motion_prompt(rng.choice(pool), rng=rng) + + +def compose_seed_motion_prompt_from_row( + row: Any, + text_fields: Sequence[str] | None = None, + *, + fallback_key: str = "move_name", + rng: Any = random, +) -> str: + fallback = str(_row_get(row, fallback_key, "seed motion")).strip() or "seed motion" + return compose_seed_motion_prompt_from_candidates( + collect_seed_text_candidates(row, text_fields), + fallback=fallback, + rng=rng, + ) diff --git a/src/psi/deploy/helpers.py b/src/psi/deploy/helpers.py new file mode 100644 index 0000000000000000000000000000000000000000..849f68484f7ec1e72f7f2930e73d8836f3d50ef0 --- /dev/null +++ b/src/psi/deploy/helpers.py @@ -0,0 +1,123 @@ +import json +import torch +import numpy as np + +from base64 import b64decode, b64encode +from numpy.lib.format import descr_to_dtype, dtype_to_descr +from fastapi.responses import JSONResponse +from dataclasses import dataclass +from typing import Union, Dict, Any, List +from pathlib import Path + +def numpy_serialize(o): + if isinstance(o, (np.ndarray, np.generic)): + data = o.data if o.flags["C_CONTIGUOUS"] else o.tobytes() + return { + "__numpy__": b64encode(data).decode(), + "dtype": dtype_to_descr(o.dtype), + "shape": o.shape, + } + + msg = f"Object of type {o.__class__.__name__} is not JSON serializable" + raise TypeError(msg) + +def numpy_deserialize(dct): + if "__numpy__" in dct: + np_obj = np.frombuffer(b64decode(dct["__numpy__"]), descr_to_dtype(dct["dtype"])) + return np_obj.reshape(shape) if (shape := dct["shape"]) else np_obj[0] + return dct + + +def convert_numpy_in_dict(data, func): + """ + Recursively processes a JSON-like dictionary, converting any NumPy arrays + or lists of NumPy arrays into a serializable format using the provided function. + + Args: + data: The JSON-like dictionary or object to process. + func: A function to apply to each NumPy array to make it serializable. + + Returns: + The processed dictionary or object with all NumPy arrays converted. + """ + if isinstance(data, dict): + if "__numpy__" in data: + return func(data) + return {key: convert_numpy_in_dict(value, func) for key, value in data.items()} + elif isinstance(data, list): + return [convert_numpy_in_dict(item, func) for item in data] + elif isinstance(data, (np.ndarray, np.generic)): + return func(data) + else: + return data + +class Message(object): + def __init__(self): + pass + + def serialize(self): + raise NotImplementedError + + @classmethod + def deserialize(cls, response: Dict[str, Any]): + raise NotImplementedError + +class RequestMessage(Message): + def __init__(self, image: Dict[str, Any], + instruction: str, + history: Dict[str, Any], + state: Dict[str, Any], + condition: Dict[str, Any], + gt_action: Union[np.ndarray, List], + dataset_name: str, + timestamp: str): + self.image, self.instruction, self.history, self.state, self.gt_action, self.dataset_name, self.timestamp = \ + image, instruction, history, state, gt_action, dataset_name, timestamp + self.condition = condition + + def serialize(self): + msg = { + "image": self.image, + "instruction": self.instruction, + "history": self.history, + "state": self.state, + "condition": self.condition, + "gt_action": self.gt_action, + "dataset_name": self.dataset_name, + "timestamp": self.timestamp + } + return convert_numpy_in_dict(msg, numpy_serialize) + + @classmethod + def deserialize(cls, response: Dict[str, Any]): + response = convert_numpy_in_dict(response, numpy_deserialize) # type: ignore + return cls( + image=response["image"], + instruction=response["instruction"], + history=response["history"], + state=response["state"], + condition=response["condition"], + gt_action=response["gt_action"], + dataset_name=response["dataset_name"], + timestamp=response["timestamp"] + ) + +class ResponseMessage(Message): + def __init__(self, action: np.ndarray, err: float, traj_image: np.ndarray = np.zeros((1,1,3), dtype=np.uint8)): + self.action = action + self.err = err + self.traj_image = traj_image + + def serialize(self): + msg = { + "action": self.action, + "err": self.err, + "traj_image": self.traj_image + } + return convert_numpy_in_dict(msg, numpy_serialize) + + @classmethod + def deserialize(cls, response: Dict[str, Any]): + response = convert_numpy_in_dict(response, numpy_deserialize) # type: ignore + return cls(action=response["action"], err=response["err"], traj_image=response["traj_image"]) + diff --git a/src/psi/deploy/psi0_serve_simple.py b/src/psi/deploy/psi0_serve_simple.py new file mode 100644 index 0000000000000000000000000000000000000000..338d143e1f88e5c1c3883ae56cce8815a171b706 --- /dev/null +++ b/src/psi/deploy/psi0_serve_simple.py @@ -0,0 +1,211 @@ +import os +import sys +import tyro +import torch +import time +import numpy as np +import os.path as osp +from pathlib import Path +import uvicorn +from fastapi import FastAPI +from PIL import Image +from typing import Union, Dict, Any, List +from base64 import b64decode, b64encode +from fastapi.responses import JSONResponse +from numpy.lib.format import descr_to_dtype, dtype_to_descr +from torchvision.transforms import v2 + +from psi.deploy.helpers import * +from psi.config.config import LaunchConfig, ServerConfig +from psi.config.transform import SimpleRepackTransform, Psi0ModelTransform, ActionStateTransform +from psi.utils import parse_args_to_tyro_config, pad_to_len, seed_everything +from psi.utils.overwatch import initialize_overwatch + +overwatch = initialize_overwatch(__name__) + +class Server: + + def __init__( + self, + policy:str, + run_dir: Path, + ckpt_step: int | str = "latest", + device: str = "cuda:0", + enable_rtc: bool = False, + action_exec_horizon: int | None = None + ): + if not torch.cuda.is_available(): + raise RuntimeError("CUDA is not available. Please check your CUDA installation.") + + self.device = torch.device(device) + overwatch.info(f"Using device: {self.device}") + overwatch.info(f"Serving {policy}") + + assert osp.exists(run_dir), f"run_dir {run_dir} does not exist!" + assert osp.exists(run_dir / "checkpoints" / f"ckpt_{ckpt_step}"), f"ckpt {ckpt_step} does not exist!" + assert osp.exists(run_dir / "run_config.json"), f"run config does not exist!" + + # load launch config + config_: LaunchConfig = parse_args_to_tyro_config(run_dir / "argv.txt") # type: ignore + conf = (run_dir / "run_config.json").open("r").read() + launch_config = config_.model_validate_json(conf) + seed_everything(launch_config.seed or 42) + + from psi.models.psi0 import Psi0Model + self.model = Psi0Model.from_pretrained(run_dir, ckpt_step, launch_config, device=device) + self.model.to(device) + self.model.eval() + + self.maxmin:ActionStateTransform = launch_config.data.transform.field # type:ignore + self.repack_transform:SimpleRepackTransform = launch_config.data.transform.repack # type:ignore + self.model_transform:Psi0ModelTransform = launch_config.data.transform.model # type:ignore + + # Print number of total/trainable model parameters + num_params = sum(p.numel() for p in self.model.parameters()) + overwatch.info(f"Parameters (in millions): {num_params*1e-6:.3f} Total", ctx_level=1) + + # self.previous_rpy = np.array([0.0, 0.0, 0.0], dtype=np.float32) # FIXME + # self.previous_height = np.array([0.74], dtype=np.float32) + + self.Da = launch_config.model.action_dim # type:ignore + self.Tp = launch_config.model.action_chunk_size # type:ignore + self.Ta = action_exec_horizon or launch_config.model.action_exec_horizon # type:ignore + assert self.Ta <= self.Tp, "action_exec_horizon is too big" + self.launch_config = launch_config + self.count = 0 + + self.enable_rtc = enable_rtc + if enable_rtc: + assert launch_config.model.rtc, "rtc is not supported for this model" #type:ignore + self.rtc_max_delay = launch_config.model.max_delay # type:ignore + assert self.Tp - self.Ta <= self.rtc_max_delay, "action_exec_horizon is too big for the given rtc_max_delay and action_chunk_size" + self.previous_action = None #np.zeros((self.Tp, self.Da), dtype=np.float32) + overwatch.info(f"RTC enabled with max_delay={self.rtc_max_delay}, \n" + f"action_dim={self.Da}, \n" + f"action_chunk_size={self.Tp}, \n" + f"action_exec_horizon={self.Ta}") + self.last_serve_time = time.monotonic() + + + def predict_action(self, payload: Dict[str, Any]) -> JSONResponse: + # overwatch.info(f"Received request with payload: {payload}") + try: + request = RequestMessage.deserialize(payload) + image_dict, instruction, history_dict, state_dict, gt_action, dataset_name = \ + request.image, request.instruction, request.history, request.state, request.gt_action, request.dataset_name + + overwatch.info(f"Instruction: {instruction}") + overwatch.info(f"history_dict: {history_dict}") + + transforms = [self.model_transform.resize(), self.model_transform.center_crop()] + t = v2.Compose(transforms) + + states = torch.from_numpy(state_dict["states"].copy()) + # self.repack_transform.to_psi0_state_format( + # torch.from_numpy(state_dict["proprio_joint_positions"].copy()), + # torch.from_numpy(state_dict["amo_policy_command"].copy()), + # ) + + if self.maxmin.normalize_state: # type:ignore + states = torch.from_numpy( + self.maxmin.normalize_state_func( + pad_to_len(states.numpy(), self.maxmin.pad_state_dim, dim=1)[0] + ) + ).to(self.device) + + if not self.enable_rtc: + raw_pred_actions = self.model.predict_action( + observations=[[t(Image.fromarray(img)) for img in image_dict.values()]], + states=states.unsqueeze(0), # B, To, Ds + instructions=[instruction], # [Task] * B + num_inference_steps=10, + traj2ds=None + ) + else: # rtc + current_time = time.monotonic() + if self.previous_action is None or "reset" in history_dict: # or (current_time - self.last_serve_time) > 30 #if idle more than 60s, reset previous action + overwatch.info("===Reset or first step, without condition===") + raw_pred_actions = self.model.predict_action( + observations=[[t(Image.fromarray(img)) for img in image_dict.values()]], + states=states.unsqueeze(0), # B, To, Ds + instructions=[instruction], # [Task] * B + num_inference_steps=10, + traj2ds=None + ) + else: + overwatch.info("RTC enabled, using RTC inference") + overwatch.info("Last chunk execution loop time: {:.2f}s ago".format(current_time - self.last_serve_time)) + prev_actions = np.concatenate([ + self.previous_action[None, self.Ta:, :], + np.zeros((1, self.Ta, self.Da), dtype=np.float32) + ], axis=1) # (1, Tp, Da) + prev_actions = torch.from_numpy(prev_actions).to(self.device) + + raw_pred_actions = self.model.predict_action_with_training_rtc_flow( + observations=[[t(Image.fromarray(img)) for img in image_dict.values()]], + states=states.unsqueeze(0), # B, To, Ds + instructions=[instruction], # [Task] * B + num_inference_steps=10, + traj2ds=None, + prev_actions=prev_actions, + inference_delay=(self.Tp - self.Ta), + max_delay=self.rtc_max_delay + ) + + raw_pred_actions = raw_pred_actions.reshape(-1, self.Da).cpu().numpy() # (Tp, Da) + pred_actions = self.maxmin.denormalize(raw_pred_actions) # (Ta, Da) + self.previous_action = raw_pred_actions.copy().astype(np.float32) # for rtc + pred_actions = pred_actions[:self.Ta] # type:ignore + overwatch.info(f"Return Action ({pred_actions.shape})") # : {pred_actions} + + self.last_serve_time = time.monotonic() + response = ResponseMessage(pred_actions, 0.0) # type:ignore + return JSONResponse(content=response.serialize()) + + except Exception as e: + import traceback + overwatch.warning(traceback.format_exc()) + return JSONResponse(content=f'{{"status": "{e}"}}') + + + def run(self, host: str = "0.0.0.0", port: int = 8000) -> None: + self.app = FastAPI() + self.app.post("/act")(self.predict_action) + self.app.get("/health")(lambda: JSONResponse(content={"status": "ok"})) + overwatch.info(f"Server listens on {host}:{port}") + try: + uvicorn.run(self.app, host=host, port=port) + except Exception as e: + overwatch.warning(f"Server crashed, {e}") + finally: + overwatch.info("Server stopped.") + exit(1) + +def serve(cfg: ServerConfig) -> None: + overwatch.info("Server :: Initializing Psi0") + assert cfg.policy is not None, "which policy to serve?" + server = Server( + cfg.policy, + Path(cfg.run_dir), + cfg.ckpt_step, + cfg.device, + cfg.rtc, + cfg.action_exec_horizon + ) + + overwatch.info("Server :: Spinning Up") + server.run(cfg.host, cfg.port) + +def main(): + overwatch.info("Start Serving from uv") + overwatch.info(f"Args: {sys.argv}") + from dotenv import load_dotenv + assert load_dotenv() + config = tyro.cli(ServerConfig, config=(tyro.conf.ConsolidateSubcommandArgs,), args=sys.argv[1:]) + serve(config) + +if __name__ == "__main__": + from dotenv import load_dotenv + assert load_dotenv() + config = tyro.cli(ServerConfig, config=(tyro.conf.ConsolidateSubcommandArgs,)) + serve(config) \ No newline at end of file diff --git a/src/psi/deploy/psi_serve_rtc-dexmate.py b/src/psi/deploy/psi_serve_rtc-dexmate.py new file mode 100644 index 0000000000000000000000000000000000000000..014123708dc50d5a680b7d1c39f39e781e07a6ec --- /dev/null +++ b/src/psi/deploy/psi_serve_rtc-dexmate.py @@ -0,0 +1,538 @@ +import asyncio +from fastapi import FastAPI, WebSocket, WebSocketDisconnect + +from pydantic import BaseModel +from collections import deque +import threading +import time +import copy + +import os +import sys +import json +import tyro +# import draccus +import uvicorn +import numpy as np +from PIL import Image +from pathlib import Path +from typing import Dict, Any, List +import os.path as osp + +from psi.models.psi0 import Psi0Model + +# os.environ["TRANSFORMERS_OFFLINE"] = "1" + +# Ensure imports work regardless of current working directory +repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) +if repo_root not in sys.path: + sys.path.insert(0, repo_root) + +from psi.config.config import LaunchConfig, ServerConfig +from psi.deploy.helpers import * + +# from pipelines import ActionPipeline +# from misc import move_to_device +from psi.utils import parse_args_to_tyro_config, pad_to_len, seed_everything + +from psi.utils.overwatch import initialize_overwatch +overwatch = initialize_overwatch(__name__) + + + +PREDICT_HORIZON = 30 # == H +MIN_EXEC_HORIZON = 15 # == s_min # TODO: should match D_INIT, ideally s_min >= d_real +DELAY_BUFFER_SIZE = 6 # == delay_buffer_size +D_INIT = 6 # == d_init # TODO: placeholder, needs calculation +CTRL_PERIOD_SEC = 1. / 30 # 30Hz + + + +class RealTimeChunkController: + def __init__(self, + policy: Psi0Model, + prediction_horizon: int = PREDICT_HORIZON, + min_exec_horizon: int = MIN_EXEC_HORIZON, + delay_buf_size: int = DELAY_BUFFER_SIZE, + d_init: int = D_INIT, + o_first: np.ndarray | None = None): # type: ignore + + self.policy : Psi0Model = policy + self.device = self.policy.device + self.H = prediction_horizon + + self.s_min = min_exec_horizon + + self.t: int = 0 + assert o_first != None, "please provide o_first" + + A_first = self._predict_action(o_first) # (H, D) + + # warmup the model + for i in range (2): + _ = self._predict_action_rtc(copy.deepcopy(o_first), np.concatenate([copy.deepcopy(A_first[self.s_min:, :]), np.zeros((self.s_min, A_first.shape[1]), dtype=A_first.dtype)], axis=0), d_init) + print("Model warmed up") + + self.A_cur = A_first # (H, D) + self.o_cur: Dict[str, Any] | None = None + + self.Q = deque([d_init], maxlen=delay_buf_size) + + self.M = threading.Lock() + self.C = threading.Condition(self.M) + + self._infer_th = threading.Thread(target=self._inference_loop, daemon=True) + self._infer_th.start() + + + def step(self, obs_next: Dict[str, Any]): # consume a_(t-1) and provide o_t + with self.C: + self.t += 1 + self.o_cur = obs_next + self.C.notify() + if self.t-1 >= len(self.A_cur): + single_action = self.A_cur[-1] + print("failed") + else: + single_action = self.A_cur[self.t - 1] + return single_action[np.newaxis, :] # (1, D) + + def _inference_loop(self): + while True: + with self.C: + try: + while self.t < self.s_min: + self.C.wait() # wait until notified and get the lock + s = self.t + + # FIXME: + # 1. maybe bug at "s-2" + # 2. inputs should be : normalize_states(denormalize_action(A_cur[s-2])) + # but in our current data, the stats for rpy and height are nearly the same, + # so "normalize_states(denormalize_action())" equals to doing nothing. + + assert (s-2) >= 0 + # self.o_cur = self.replace_prev_actions_to_obs(self.o_cur, copy.deepcopy(self.A_cur[s-2, 28:31]), copy.deepcopy(self.A_cur[s-2, 31:32])) + # + + o = copy.deepcopy(self.o_cur) + d = max(self.Q) + # A_prev = copy.deepcopy(torch.cat([self.A_cur[s:, :], torch.zeros((s, self.A_cur.shape[1]), device=self.A_cur.device, dtype=self.A_cur.dtype)], dim=0)) # (H, D) + A_prev = np.concatenate([copy.deepcopy(self.A_cur[s:, :]), np.zeros((s, self.A_cur.shape[1]), dtype=self.A_cur.dtype)], axis=0) # (H, D) + + inference_start = time.perf_counter() + self.C.release() + A_new = self._predict_action_rtc(o, A_prev, d) + self.C.acquire() + + self.A_cur = A_new + self.t = self.t - s + self.Q.append(self.t) + # self.C.notify_all() + print(f"[inference] latency={time.perf_counter()-inference_start:.4f}s s={s} d={d} self.t={self.t}") + except Exception as e: + print(f"\n[ERROR] Inference loop crashed!") + print(f"Error: {e}") + import traceback + traceback.print_exc() + print("\n[FATAL] Stopping program...") + os._exit(1) # 强制退出整个程序 + + def _predict_action_rtc(self, o, A_prev, d): + A_new = self.policy.predict_action_with_training_rtc_flow( + observations=o['imgs'], + states=torch.from_numpy(o['obs']).to(self.device), + traj2ds=None, + instructions=o['text_instructions'], + num_inference_steps = 8, + prev_actions=torch.from_numpy(A_prev[np.newaxis, :, :]).to(self.device), # (H, D) -> (1, H, D) + inference_delay=d, + max_delay=8 + )[0].float().detach().cpu().numpy() # (1, H, D) -> (H, D) + return A_new + + def _predict_action(self, o): + normalized_actions = self.policy.predict_action( + observations=o['imgs'], + states=torch.from_numpy(o['obs']).to(self.device), + traj2ds=None, + instructions=o['text_instructions'], + num_inference_steps = 8, + )[0].float().detach().cpu().numpy() # (1, H, D) -> (H, D) + + return normalized_actions + + +class Server: + + def __init__( + self, + policy:str, + run_dir: Path, + ckpt_step: int | str = "latest", + device: str = "cuda:0", + enable_rtc: bool = False, + action_exec_horizon: int | None = None + ): + if not torch.cuda.is_available(): + raise RuntimeError("CUDA is not available. Please check your CUDA installation.") + + self.device = torch.device(device) + overwatch.info(f"Using device: {self.device}") + overwatch.info(f"Serving {policy}") + + overwatch.info(f"Using device: {self.device}") + overwatch.info(f"Serving {policy}") + + assert osp.exists(run_dir), f"run_dir {run_dir} does not exist!" + assert osp.exists(run_dir / "checkpoints" / f"ckpt_{ckpt_step}"), f"ckpt {ckpt_step} does not exist!" + assert osp.exists(run_dir / "run_config.json"), f"run config does not exist!" + + # first build dynamic config + config_: LaunchConfig = parse_args_to_tyro_config(run_dir / "argv.txt") # type: ignore + # then load it from previsously saved json + conf = (run_dir / "run_config.json").open("r").read() + launch_config = config_.model_validate_json(conf) + seed_everything(launch_config.seed or 42) + + + overwatch.info("loading action model...") + from psi.models.psi0 import Psi0Model + self.model = Psi0Model.from_pretrained(run_dir, ckpt_step, launch_config, device=device) + self.model.to(device) + self.model.eval() + + from psi.config.transform import SimpleRepackTransform, Psi0ModelTransform, ActionStateTransform + self.maxmin:ActionStateTransform = launch_config.data.transform.field # type:ignore + self.model_transform:Psi0ModelTransform = launch_config.data.transform.model # type:ignore + + + self.Da = launch_config.model.action_dim # type:ignore + self.Tp = launch_config.model.action_chunk_size # type:ignore + self.Ta = action_exec_horizon or launch_config.model.action_exec_horizon # type:ignore + assert self.Ta <= self.Tp, "action_exec_horizon is too big" + self.launch_cfg = launch_config + self.count = 0 + + + # control - shared state with locks + self.latest_obs = None + self.latest_action = None + self.action_version = 0 # Used by client to check if there's a new action + + self.obs_lock = threading.Lock() + self.action_lock = threading.Lock() + + self.controller = None + self._control_loop_started = False + + # WebSocket: asyncio event to notify when new action is ready + self.app = FastAPI() + self._setup_routes() + + self._action_ready_event: asyncio.Event = None # Will be created in async context + self._active_websocket: WebSocket = None + self._loop = None # asyncio event loop reference for thread-safe notification + self.start_time = time.time() + self.start_time_obs = time.time() + + def _init_controller(self, o_first): + controller = RealTimeChunkController(policy=self.model, o_first=o_first) + return controller + + def _postprocess_action(self, action): + # return self.launch_cfg.data.data_transforms.denormalize_action(action) + return self.maxmin.denormalize(action) # denormalization is done in the pipeline + + + + def preprocess_image(self, image_dict: Dict[str, Any]) -> Dict[str, Any]: + imgs = {} + # # FIXME + # image_key_to_cam_idx = {'front_stereo_left': 0, 'front_stereo_right': 1, 'left_future_traj_2d': 4, 'right_future_traj_2d': 5, 'side': 3, 'side_future_traj_2d': 7, 'wrist': 2, 'wrist_future_traj_2d': 6} + # for img_key in self.launch_config.data.transform.repack.image_keys: + # cam_idx = image_key_to_cam_idx[img_key] #self.launch_config.data.transform.repack.image_key_to_cam_idx[img_key] + # imgs[f"cam{cam_idx}"] = self._process_img(image_dict[f"{img_key}".replace("image_", "")])#[None, ...] + + for k in image_dict.keys(): + imgs[k] = self._process_img(image_dict[k]) + + + return imgs + + def _process_img(self, img): + from torchvision.transforms import v2 + + transforms = [self.model_transform.resize(), self.model_transform.center_crop()] + t = v2.Compose(transforms) + + return [t(img)] + + def _parse_obs_payload(self, payload: Dict[str, Any]) -> Dict[str, Any]: + """Parse observation payload and return processed obs dict""" + request = RequestMessage.deserialize(payload) + image_dict, instruction, history_dict, state_dict, gt_action, dataset_name = \ + request.image, request.instruction, request.history, request.state, request.gt_action, request.dataset_name + condition_dict = request.condition + overwatch.info(f"Instruction: {instruction}") + + print("image dict keys:", image_dict.keys()) + + # parts = instruction.split(".") + # if len(parts) > 1 and parts[-1].isdigit(): + # instruction = parts[0].lower() + "." + # img_id = int(parts[-1]) + # assert False + # else: + instruction = instruction.lower() + # img_id = -1 + + + # TODO support image history + # img dict: {"video": np.array(...).shape(480, 640, 3)} + imgs = {} + print("image keys:", self.launch_cfg.data.transform.repack.image_keys) + for cam_idx, img_key in enumerate(self.launch_cfg.data.transform.repack.image_keys): + imgs[f"cam{cam_idx}"] = Image.fromarray(np.clip(image_dict[img_key], 0, 255).astype(np.uint8)) + + hand_joints = state_dict["hand_joints"].copy() # shape (12,) + arm_joints = state_dict["arm_joints"].copy() # shape (14,) + head_joints = state_dict["head_joints"].copy() # shape (3,) + torso_joints = state_dict["torso_joints"].copy() # shape (3,) + + obs = np.concatenate([hand_joints, arm_joints, head_joints, torso_joints], axis=-1) # (32,) + + # normalize states + assert self.maxmin.normalize_state, "check" + if self.maxmin.pad_state_dim != len(obs): + obs = pad_to_len(obs, self.maxmin.pad_state_dim, dim=0)[0] + obs = self.maxmin.normalize_state_func(obs) # shape (32,) + obs = obs[np.newaxis, np.newaxis, :] # (32,) -> (1, 1, 32) + + + image_input = self.preprocess_image(imgs) + batch_images = [image_input['cam0']] # batch size == 1 + + + conditions = {} + + text_instructions = [instruction] # len == 1 + + return {'imgs': batch_images, 'text_instructions': text_instructions, 'obs': obs, 'conditions': conditions} + + async def websocket_handler(self, websocket: WebSocket): + """ + WebSocket handler for bidirectional communication: + - Receive obs from client at high frequency + - Send action to client immediately when new action is ready + """ + await websocket.accept() + self._active_websocket = websocket + + # Create asyncio event for action notification + self._action_ready_event = asyncio.Event() + + print("[WebSocket] Client connected") + async def receive_obs(): + """Continuously receive obs from client""" + try: + while True: + # Receive obs from client + data = await websocket.receive_text() + payload = json.loads(data) + interval = time.time() - self.start_time_obs + self.start_time_obs = time.time() + print(f"[WebSocket] receive_obs interval: {interval} seconds") + # Parse and update latest_obs + this_o = self._parse_obs_payload(payload) + with self.obs_lock: + self.latest_obs = this_o + + # If control loop hasn't started, start it + if not self._control_loop_started and self.latest_obs is not None: + self._start_control_loop() + + # # 清空缓冲区,只保留最新的 + # latest_data = None + # while True: + # try: + # # 非阻塞地读取所有可用消息 + # data = await asyncio.wait_for( + # websocket.receive_text(), + # timeout=0.001 # 1ms超时 + # ) + # latest_data = data # 保留最新的 + # except asyncio.TimeoutError: + # break # 没有更多消息了 + + # if latest_data: + # payload = json.loads(latest_data) + # interval = time.time() - self.start_time_obs + # self.start_time_obs = time.time() + # print(f"[WebSocket] receive_obs interval: {interval} seconds") + + # this_o = self._parse_obs_payload(payload) + # with self.obs_lock: + # self.latest_obs = this_o + + # if not self._control_loop_started and self.latest_obs is not None: + # self._start_control_loop() + + except WebSocketDisconnect: + print("[WebSocket] Client disconnected (receive)") + except Exception as e: + import traceback + traceback.print_exc() + print(f"[WebSocket] Receive error: {e}") + + async def send_action(): + """Send action to client when new action is ready""" + try: + while True: + # Wait for new action to be ready + await self._action_ready_event.wait() + self._action_ready_event.clear() + + interval = time.time() - self.start_time + self.start_time = time.time() + print(f"[WebSocket] send_action interval: {interval} seconds") + + # Get the action + with self.action_lock: + action = self.latest_action + version = self.action_version + self.latest_action = None # Reset after sending + + if action is not None: + # Send action to client + response = ResponseMessage(action, err=0.0) + resp_dict = response.serialize() + resp_dict["version"] = version + await websocket.send_text(json.dumps(resp_dict)) + print(f"[WebSocket] Sent action, version={version}") + else: + assert False, "action is None" + + except WebSocketDisconnect: + print("[WebSocket] Client disconnected (send)") + except Exception as e: + print(f"[WebSocket] Send error: {e}") + + try: + # Run both tasks concurrently + await asyncio.gather(receive_obs(), send_action()) + except Exception as e: + print(f"[WebSocket] Connection closed: {e}") + finally: + self._active_websocket = None + print("[WebSocket] Handler finished") + + def _start_control_loop(self): + """Start control loop thread""" + if self._control_loop_started: + return + self._control_loop_started = True + + # Initialize controller with first obs + with self.obs_lock: + o_first = copy.deepcopy(self.latest_obs) + + self.controller = self._init_controller(o_first) # wait for model warm up + + # Start control loop thread + self._control_thread = threading.Thread(target=self._control_loop, daemon=True) + self._control_thread.start() + print("[control loop] started") + + def _control_loop(self): + """ + Control loop: Execute controller.step strictly every CTRL_PERIOD_SEC + And expect at next time, the obs_next sent from client is the one after executing the action + """ + next_tick = time.perf_counter() + prev_tick = time.perf_counter() + + while True: + # loop_start = time.time() + + # 1. Get latest obs + with self.obs_lock: + obs_next = copy.deepcopy(self.latest_obs) + + # 2. Execute step + action = self.controller.step(obs_next) # (1, D) + pred_action = self._postprocess_action(action) # (1, D) + + # 3. Update latest_action + with self.action_lock: + self.latest_action = pred_action + self.action_version += 1 + + # 4. Notify WebSocket that new action is ready + if self._action_ready_event is not None: + # Thread-safe way to set asyncio event from another thread + try: + self._loop.call_soon_threadsafe(self._action_ready_event.set) + except Exception as e: + print(f"[control loop] Failed to notify WebSocket: {e}") + + # elapsed = (time.time() - loop_start) * 1000 + # print(f"[control loop] step took {elapsed:.1f}ms, version={self.action_version}") + + # 5. Wait until next ctrl period + next_tick += CTRL_PERIOD_SEC + sleep_time = next_tick - time.perf_counter() + now = time.perf_counter() + interval = now - prev_tick + prev_tick = now + print(f"[control loop] interval: {interval} seconds") + if sleep_time > 0: + time.sleep(sleep_time) + # delay_ms(sleep_time*1000) + else: + print(f"[control loop] WARNING: missed tick by {-sleep_time*1000:.1f}ms") + next_tick = time.perf_counter() + + + def _setup_routes(self): + """设置所有路由""" + @self.app.websocket("/ws") + async def websocket_endpoint(websocket: WebSocket): + self._loop = asyncio.get_event_loop() + await self.websocket_handler(websocket) + + @self.app.get("/health") + async def health_check(): + return {"status": "ok"} + + def run(self, host: str = "0.0.0.0", port: int = 8000) -> None: + print(f"Server listens on {host}:{port}") + print(f"WebSocket endpoint: ws://{host}:{port}/ws") + try: + uvicorn.run(self.app, host=host, port=port) + except Exception as e: + print(f"Server crashed, {e}") + finally: + print("Server stopped.") + exit(1) + +def serve(cfg: ServerConfig) -> None: + overwatch.info("Server :: Initializing Policy") + assert cfg.policy is not None, "which policy to serve?" + assert cfg.rtc, "this server is for rtc" + server = Server( + cfg.policy, + Path(cfg.run_dir), + cfg.ckpt_step, + cfg.device, + cfg.rtc, + cfg.action_exec_horizon) + + print("Server :: Spinning Up") + server.run(cfg.host, cfg.port) + +if __name__ == "__main__": + from dotenv import load_dotenv + load_dotenv() # take environment variables from .env file + config = tyro.cli(ServerConfig, config=(tyro.conf.ConsolidateSubcommandArgs,)) + serve(config) \ No newline at end of file diff --git a/src/psi/deploy/psi_serve_rtc-trainingtimertc-sonic.py b/src/psi/deploy/psi_serve_rtc-trainingtimertc-sonic.py new file mode 100644 index 0000000000000000000000000000000000000000..226d0335d45725661bdc05b2309a5555ce0cb9dc --- /dev/null +++ b/src/psi/deploy/psi_serve_rtc-trainingtimertc-sonic.py @@ -0,0 +1,1170 @@ +import asyncio +from fastapi import FastAPI, WebSocket, WebSocketDisconnect + +from pydantic import BaseModel +from collections import OrderedDict, deque +import threading +import time +import copy + +import os +import sys +import json +import math +import tyro +# import draccus +import uvicorn +import numpy as np +from PIL import Image +from pathlib import Path +from typing import Dict, Any, List +import os.path as osp + +from psi.models.psi0 import Psi0Model +from psi.data.seed.motion_repr import ( + SEED_ABS_DIM, + build_seed_textop_state, + normalize_quat_wxyz, +) +from psi.data.seed.seed_video_motion_dataset import load_video_array, load_video_clip_array + +# os.environ["TRANSFORMERS_OFFLINE"] = "1" + +# Ensure imports work regardless of current working directory +repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) +if repo_root not in sys.path: + sys.path.insert(0, repo_root) + +from psi.config.config import LaunchConfig, ServerConfig +from psi.deploy.helpers import * + +# from pipelines import ActionPipeline +# from misc import move_to_device +from psi.utils import parse_args_to_tyro_config, pad_to_len, seed_everything + +from psi.utils.overwatch import initialize_overwatch +overwatch = initialize_overwatch(__name__) + + + +PREDICT_HORIZON = 30 # == H +# 训练配置是预测30帧、执行20帧;SONIC在线stream也按20帧有效窗口取,不再默认15。 +# 原逻辑: +# MIN_EXEC_HORIZON = 15 # == s_min # TODO: should match D_INIT, ideally s_min >= d_real +MIN_EXEC_HORIZON = 20 # == action_exec_horizon +DELAY_BUFFER_SIZE = 6 # == delay_buffer_size +D_INIT = 6 # == d_init # TODO: placeholder, needs calculation +# SONIC motion tracking/control runs at 50Hz; keep the websocket sonic loop on the same clock. +# 原逻辑: +# CTRL_PERIOD_SEC = 1. / 30 # 30Hz +CTRL_HZ = 50 +CTRL_PERIOD_SEC = 1. / CTRL_HZ + +VIDEO_CLIP_CACHE: OrderedDict[tuple, np.ndarray] = OrderedDict() +VIDEO_FULL_CACHE: OrderedDict[tuple, tuple[np.ndarray, float, float]] = OrderedDict() + + +def build_progress_condition(generated_frames: int, target_frames: int, action_horizon: int) -> np.ndarray: + # v13 infer同款5D progress条件:[start, end, remaining, sin, cos]。 + progress_den = float(max(target_frames - 1, 1)) + start_frame = min(int(generated_frames), max(target_frames - 1, 0)) + start_progress = float(start_frame) / progress_den + end_frame = min(generated_frames + action_horizon, max(target_frames - 1, 0)) + end_progress = float(end_frame) / progress_den + remaining_progress = max(0.0, 1.0 - start_progress) + return np.asarray( + [ + start_progress, + end_progress, + remaining_progress, + math.sin(2.0 * math.pi * start_progress), + math.cos(2.0 * math.pi * start_progress), + ], + dtype=np.float32, + ) + + +def build_future_progress_condition(generated_frames: int, target_frames: int, action_horizon: int) -> np.ndarray: + # v13 infer同款逐action-token future progress条件。 + progress_den = float(max(target_frames - 1, 1)) + start_frame = min(int(generated_frames), max(target_frames - 1, 0)) + start_progress = float(start_frame) / progress_den + rows = [] + for step in range(action_horizon): + future_frame = min(generated_frames + step + 1, max(target_frames - 1, 0)) + future_progress = float(future_frame) / progress_den + future_remaining = max(0.0, 1.0 - future_progress) + rows.append( + [ + future_progress, + future_remaining, + start_progress, + math.sin(2.0 * math.pi * future_progress), + math.cos(2.0 * math.pi * future_progress), + ] + ) + return np.asarray(rows, dtype=np.float32) + + +def build_local_future_progress_condition(action_horizon: int) -> np.ndarray: + return build_future_progress_condition(0, int(action_horizon) + 1, int(action_horizon)) + + +def build_model_states_from_abs(state_history_abs: List[np.ndarray], textop_actions: bool, odim: int) -> np.ndarray: + # v13 TextOp训练时state/action都是39D;server端不能把36D真实姿态直接喂给模型。 + states_abs = np.stack(state_history_abs, axis=0).astype(np.float32) + if textop_actions: + states = build_seed_textop_state(states_abs[:, :SEED_ABS_DIM]) + else: + states = states_abs + if states.shape[-1] < odim: + pad = np.zeros((states.shape[0], odim - states.shape[-1]), dtype=np.float32) + states = np.concatenate([states, pad], axis=-1) + if states.shape[-1] != odim: + raise ValueError(f"Server state dim mismatch: built {states.shape[-1]}D, model odim={odim}") + return states.astype(np.float32) + + + +class RealTimeChunkController: + def __init__(self, + policy: Psi0Model, + maxmin, + text_only: bool, + textop_actions: bool, + odim: int, + normalize_state: bool, + num_past_frames: int, + state_history_stride: int, + use_progress_token: bool, + use_future_progress_embedding: bool, + progress_target_frames: int, + num_inference_steps: int, + rtc_max_delay: int, + segment_switch_mode: str, + prediction_horizon: int = PREDICT_HORIZON, + min_exec_horizon: int = MIN_EXEC_HORIZON, + delay_buf_size: int = DELAY_BUFFER_SIZE, + d_init: int = D_INIT, + o_first: Dict[str, Any] | None = None): # type: ignore + + self.policy : Psi0Model = policy + self.device = self.policy.device + self.maxmin = maxmin + self.text_only = bool(text_only) + self.textop_actions = bool(textop_actions) + self.odim = int(odim) + self.normalize_state = bool(normalize_state) + self.num_past_frames = int(num_past_frames) + self.state_history_stride = int(state_history_stride) + self.use_progress_token = bool(use_progress_token) + self.use_future_progress_embedding = bool(use_future_progress_embedding) + self.progress_target_frames = int(progress_target_frames) + self.num_inference_steps = int(num_inference_steps) + # 多段planner切换策略可切换:reset保留当前做法;rtc_tail对齐infer_render_plan的RTC承接。 + self.segment_switch_mode = str(segment_switch_mode).strip() + self.H = prediction_horizon + + self.s_min = min_exec_horizon + self.rtc_delay = int(self.H - self.s_min) + self.max_delay = int(rtc_max_delay) + if self.rtc_delay <= 0: + self.rtc_delay = 1 + if self.rtc_delay >= self.max_delay: + print(f"[rtc] clamp fixed delay from {self.rtc_delay} to {self.max_delay - 1}") + self.rtc_delay = self.max_delay - 1 + + self.t: int = 0 + self.generated_frames: int = 0 + # online planner多段执行时,段号变化只重置progress计数,不重启RTC controller。 + self.current_segment_index: int = 0 + self.current_video_key: tuple | None = None + self.current_video: np.ndarray | None = None + self.video_clip_cache = VIDEO_CLIP_CACHE + self.video_clip_cache_max_items = int(os.environ.get("PSI0_VIDEO_CLIP_CACHE_SIZE", "64")) + self.video_full_cache = VIDEO_FULL_CACHE + self.video_full_cache_max_items = int(os.environ.get("PSI0_VIDEO_FULL_CACHE_SIZE", "2")) + self.use_full_video_cache = os.environ.get("PSI0_VIDEO_FULL_CACHE", "1") != "0" + # 段切换时需要用新prompt重新采样A_cur,否则RTC prefix会把上一段动作惯性带到下一段。 + self.pending_segment_reset: bool = False + self.state_history_abs: List[np.ndarray] = [] + assert o_first is not None, "please provide o_first" + + first_obs = self._prepare_model_obs(o_first, append_history=True) + A_first = self._predict_action(first_obs) # (H, D) + + # warmup the model + for i in range (2): + _ = self._predict_action_rtc(copy.deepcopy(first_obs), self._build_rtc_prefix(A_first), self.rtc_delay) + print("Model warmed up") + + self.A_cur = np.clip(A_first, -1.0, 1.0).astype(np.float32) # (H, D) + # 第一段初始化已经用当前instruction采样过A_cur,不需要在第一帧step里再重采样一次。 + self.pending_segment_reset = False + # chunk_version只在产生新的预测窗口时递增;避免50Hz control loop重复发送同一个A_cur窗口。 + # 原逻辑:每个control tick都把A_cur[:s_min]当成一个新action发给bridge。 + self.chunk_version = 1 + self.last_returned_chunk_version = 0 + self.o_cur: Dict[str, Any] | None = first_obs + + # 保存d_init给后续segment reset复用;之前这里只初始化Q,没有保存self.d_init, + # 切到下一段时 self.Q.append(self.d_init) 会抛 AttributeError,server直接退出,bridge就会报连接丢失。 + # 原逻辑: + # self.Q = deque([d_init], maxlen=delay_buf_size) + self.d_init = int(d_init) + self.Q = deque([self.d_init], maxlen=delay_buf_size) + + self.M = threading.Lock() + self.C = threading.Condition(self.M) + # websocket结束后需要停掉RTC后台线程,否则server会残留空跑线程。 + self.closed = False + + self._infer_th = threading.Thread(target=self._inference_loop, daemon=True) + self._infer_th.start() + + def close(self) -> None: + # 关闭当前controller的RTC inference线程;重跑online脚本时避免旧controller继续占GPU/CPU。 + with self.C: + self.closed = True + self.C.notify_all() + if self._infer_th.is_alive(): + self._infer_th.join(timeout=1.0) + + # def replace_prev_actions_to_obs(self, o, previous_rpy, previous_height): + # o['obs'] = np.concatenate([o['obs'][:, :, :28], previous_rpy[np.newaxis, np.newaxis, :], previous_height[np.newaxis, np.newaxis, :], o['obs'][:, :, 32:]], axis=-1) # (1, 1, 28) -> (1, 1, 32) + # return o + + def _append_abs_state(self, abs_state: np.ndarray) -> np.ndarray: + abs_state = np.asarray(abs_state, dtype=np.float32).reshape(-1) + if abs_state.shape[0] < SEED_ABS_DIM: + raise ValueError(f"Expected at least {SEED_ABS_DIM}D abs state, got {abs_state.shape}") + abs_state = abs_state[:SEED_ABS_DIM].copy() + abs_state[32:36] = normalize_quat_wxyz(abs_state[32:36]) + self.state_history_abs.append(abs_state) + keep = max(2, self.num_past_frames * self.state_history_stride + 2) + if len(self.state_history_abs) > keep: + self.state_history_abs = self.state_history_abs[-keep:] + return abs_state + + def _apply_segment_condition(self, obs_payload: Dict[str, Any]) -> None: + conditions = obs_payload.get("conditions", {}) + if not isinstance(conditions, dict): + return + segment_index = int(conditions.get("segment_index", 0)) + target_frames = int(conditions.get("progress_target_frames", 0)) + if target_frames > 0: + self.progress_target_frames = target_frames + if segment_index <= 0: + return + if segment_index != self.current_segment_index: + # 保留state_history_abs连续性,但下一次_prepare_model_obs后用新prompt重采样A_cur。 + # 原逻辑: + # 保留state_history_abs和A_cur,让RTC prefix继续承接上一段尾部,只重置段内progress。 + segment_elapsed = int(conditions.get("segment_elapsed_frames", 0)) + self.current_segment_index = segment_index + self.generated_frames = max(0, segment_elapsed) + self.pending_segment_reset = True + print( + f"[rtc] switch segment={segment_index} " + f"progress={self.generated_frames}/{self.progress_target_frames}", + flush=True, + ) + + def _prepare_model_obs(self, obs_payload: Dict[str, Any], append_history: bool) -> Dict[str, Any]: + self._apply_segment_condition(obs_payload) + abs_state = self._append_abs_state(obs_payload["abs_state"]) if append_history else obs_payload["abs_state"] + state_rows_abs: List[np.ndarray] = [] + for past_i in range(self.num_past_frames, 0, -1): + hist_index = len(self.state_history_abs) - 1 - past_i * self.state_history_stride + hist_index = max(0, hist_index) + state_rows_abs.append(self.state_history_abs[hist_index]) + state_rows_abs.append(abs_state) + states = build_model_states_from_abs(state_rows_abs, self.textop_actions, self.odim) + if self.normalize_state: + states = self.maxmin.normalize_state_func(states) + + model_obs = dict(obs_payload) + model_obs["obs"] = states[np.newaxis, :, :].astype(np.float32) + model_obs["imgs"] = None if self.text_only else obs_payload["imgs"] + self._prepare_video_clip(model_obs) + if self.use_progress_token: + model_obs["progress"] = build_progress_condition( + self.generated_frames, + self.progress_target_frames, + self.H, + )[np.newaxis, :] + else: + model_obs["progress"] = None + if self.use_future_progress_embedding: + conditions = model_obs.get("conditions", {}) + if isinstance(conditions, dict) and conditions.get("input_mode") == "video_clip": + future_progress = build_local_future_progress_condition(self.H) + else: + future_progress = build_future_progress_condition( + self.generated_frames, + self.progress_target_frames, + self.H, + ) + model_obs["future_progress"] = future_progress[np.newaxis, :, :] + else: + model_obs["future_progress"] = None + # planner段切换后,用当前live state + 新instruction立即重采样当前chunk,避免第二段继承上一段RTC尾巴后变静止。 + # 原逻辑:段切换只更新progress,A_cur等到后台RTC inference慢慢刷新。 + if self.pending_segment_reset and hasattr(self, "A_cur"): + self.pending_segment_reset = False + if self.segment_switch_mode == "rtc_tail": + # 可选对齐infer_render_plan:切段首包用上一段未执行tail作为RTC prefix,但instruction/progress已换成新段。 + # 原逻辑: + # self.A_cur = np.clip(self._predict_action(model_obs), -1.0, 1.0).astype(np.float32) + A_prev = self._build_rtc_prefix(self.A_cur) + self.A_cur = np.clip(self._predict_action_rtc(model_obs, A_prev, self.rtc_delay), -1.0, 1.0).astype(np.float32) + print("[rtc] segment reset: refreshed A_cur with RTC tail and new instruction", flush=True) + else: + self.A_cur = np.clip(self._predict_action(model_obs), -1.0, 1.0).astype(np.float32) + print("[rtc] segment reset: refreshed A_cur with new instruction", flush=True) + self.t = 0 + self.Q.clear() + self.Q.append(self.d_init) + self.chunk_version += 1 + return model_obs + + def _prepare_video_clip(self, model_obs: Dict[str, Any]) -> None: + conditions = model_obs.get("conditions", {}) + if not isinstance(conditions, dict) or conditions.get("input_mode") != "video_clip": + return + video_path = str(conditions.get("video_path", "")).strip() + if not video_path: + raise ValueError("video_clip input requires condition.video_path") + start_sec = float(conditions.get("video_clip_start_sec", 0.0)) + duration_sec = float(conditions.get("video_clip_duration_sec", float(self.H) / 50.0)) + sample_fps = float(conditions.get("video_sample_fps", 8.0)) + size_pixels = int(conditions.get("video_size_pixels", 50176)) + decoder = str(conditions.get("video_decoder", "ffmpeg")) + video_key = (video_path, round(start_sec, 6), round(duration_sec, 6), round(sample_fps, 6), size_pixels, decoder) + if self.current_video_key != video_key: + cached_video = self.video_clip_cache.get(video_key) + if cached_video is not None: + self.video_clip_cache.move_to_end(video_key) + self.current_video = cached_video + print( + "[video] cache hit " + f"start={start_sec:.3f}s duration={duration_sec:.3f}s shape={self.current_video.shape}", + flush=True, + ) + else: + load_start = time.perf_counter() + full_key = (video_path, round(sample_fps, 6), size_pixels, decoder) + if self.use_full_video_cache: + full_cached = self.video_full_cache.get(full_key) + if full_cached is None: + full_load_start = time.perf_counter() + full_video, _, full_duration = load_video_array( + Path(video_path), + video_sample_fps=sample_fps, + video_size_pixels=size_pixels, + video_decoder=decoder, + ) + full_cached = (np.asarray(full_video, dtype=np.uint8), sample_fps, full_duration) + if self.video_full_cache_max_items > 0: + self.video_full_cache[full_key] = full_cached + self.video_full_cache.move_to_end(full_key) + while len(self.video_full_cache) > self.video_full_cache_max_items: + self.video_full_cache.popitem(last=False) + print( + "[video] decoded full video " + f"path={video_path} shape={full_cached[0].shape} " + f"latency={time.perf_counter() - full_load_start:.4f}s " + f"cache_items={len(self.video_full_cache)}", + flush=True, + ) + else: + self.video_full_cache.move_to_end(full_key) + + full_video, cached_sample_fps, full_duration = full_cached + clip_duration = max(1.0 / cached_sample_fps, duration_sec) + clip_start = max(0.0, start_sec) + if full_duration > 0 and clip_start + clip_duration > full_duration: + clip_start = max(0.0, full_duration - clip_duration) + num_frames = max(1, int(np.ceil(clip_duration * cached_sample_fps))) + start_index = int(round(clip_start * cached_sample_fps)) + end_index = min(start_index + num_frames, full_video.shape[0]) + video = full_video[start_index:end_index].copy() + if video.shape[0] <= 0: + video = full_video[-1:].copy() + while video.shape[0] < num_frames: + video = np.concatenate([video, video[-1:].copy()], axis=0) + video = video[:num_frames] + else: + video, _, _ = load_video_clip_array( + Path(video_path), + start_sec=start_sec, + duration_sec=duration_sec, + video_sample_fps=sample_fps, + video_size_pixels=size_pixels, + video_decoder=decoder, + ) + self.current_video = np.asarray(video, dtype=np.uint8) + if self.video_clip_cache_max_items > 0: + self.video_clip_cache[video_key] = self.current_video + self.video_clip_cache.move_to_end(video_key) + while len(self.video_clip_cache) > self.video_clip_cache_max_items: + self.video_clip_cache.popitem(last=False) + print( + "[video] decoded clip " + f"start={start_sec:.3f}s duration={duration_sec:.3f}s " + f"shape={self.current_video.shape} " + f"latency={time.perf_counter() - load_start:.4f}s " + f"cache_items={len(self.video_clip_cache)}", + flush=True, + ) + self.current_video_key = video_key + model_obs["videos"] = [self.current_video] + model_obs["video_paths"] = [video_path] + model_obs["video_size_pixels"] = size_pixels + model_obs["video_sample_fps"] = sample_fps + + def _to_tensor(self, value: np.ndarray | None): + if value is None: + return None + return torch.from_numpy(np.asarray(value, dtype=np.float32)).to(self.device) + + def _build_rtc_prefix(self, actions: np.ndarray) -> np.ndarray: + # 与v13离线RTC一致:保留上一chunk未执行尾部,后接Ta帧0。 + # 原逻辑: + # A_prev = np.concatenate([copy.deepcopy(self.A_cur[s:, :]), np.zeros((s, self.A_cur.shape[1]), dtype=self.A_cur.dtype)], axis=0) + actions = np.asarray(actions, dtype=np.float32) + return np.concatenate( + [ + actions[self.s_min :, :], + np.zeros((self.s_min, actions.shape[1]), dtype=actions.dtype), + ], + axis=0, + ) + + def step(self, obs_next: Dict[str, Any], return_chunk: bool = False): # consume a_(t-1) and provide o_t + with self.C: + self.t += 1 + # o_t对应已经推进的在线帧数;下一轮RTC推理的progress用推进后的帧号。 + # 原逻辑:在函数返回前才self.generated_frames += 1。 + self.generated_frames += 1 + self.o_cur = self._prepare_model_obs(obs_next, append_history=True) + self.C.notify() + if self.t-1 >= len(self.A_cur): + single_action = self.A_cur[-1] + print("failed") + else: + single_action = self.A_cur[self.t - 1] + # SONIC streamed-motion需要未来一段连续reference,而不是单步action。 + if return_chunk: + # 只在新的A_cur预测窗口可用时返回chunk;chunk长度用完整H=30,bridge按20帧推进、10帧overlap。 + # 原逻辑: + # 每个50Hz step都返回self.A_cur[:self.s_min],bridge会把重复窗口标成新的frame range发给WBC。 + action_chunk = None + chunk_info = None + if self.chunk_version != self.last_returned_chunk_version: + action_chunk = self.A_cur[: self.H] + chunk_info = { + "chunk_version": int(self.chunk_version), + "controller_generated_frames": int(self.generated_frames), + "controller_t": int(self.t), + } + self.last_returned_chunk_version = self.chunk_version + # 旧逻辑: + # 原逻辑: + # start = min(max(self.t - 1, 0), len(self.A_cur) - 1) + # end = min(start + self.s_min, len(self.A_cur)) + # action_chunk = self.A_cur[start:end] + # action_chunk = self.A_cur[: self.s_min] + # 旧padding逻辑也不能用于70D TextOp,因为重复最后一帧会重复root delta。 + # 原逻辑: + # action_chunk = self.A_cur[start:] + # if action_chunk.shape[0] < self.H: + # pad = np.repeat(action_chunk[-1:], self.H - action_chunk.shape[0], axis=0) + # action_chunk = np.concatenate([action_chunk, pad], axis=0) + # else: + # action_chunk = action_chunk[:self.H] + out = (single_action[np.newaxis, :], action_chunk, chunk_info) + else: + out = single_action[np.newaxis, :] # (1, D) + return out + + def _inference_loop(self): + while True: + with self.C: + try: + while self.t < self.s_min and not self.closed: + self.C.wait(timeout=0.1) # wait until notified and get the lock + if self.closed: + break + # v13 RTC按固定action_exec_horizon滚动,prefix也固定从Ta之后取。 + # 原逻辑: + # s = self.t + s = self.s_min + + # FIXME: + # 1. maybe bug at "s-2" + # 2. inputs should be : normalize_states(denormalize_action(A_cur[s-2])) + # but in our current data, the stats for rpy and height are nearly the same, + # so "normalize_states(denormalize_action())" equals to doing nothing. + + assert (s-2) >= 0 + # self.o_cur = self.replace_prev_actions_to_obs(self.o_cur, copy.deepcopy(self.A_cur[s-2, 28:31]), copy.deepcopy(self.A_cur[s-2, 31:32])) + + # + + o = copy.deepcopy(self.o_cur) + d = self.rtc_delay + A_prev = self._build_rtc_prefix(self.A_cur) + + inference_start = time.perf_counter() + self.C.release() + A_new = self._predict_action_rtc(o, A_prev, d) + self.C.acquire() + + self.A_cur = np.clip(A_new, -1.0, 1.0).astype(np.float32) + self.t = max(0, self.t - s) + self.chunk_version += 1 + self.Q.append(self.t) + # self.C.notify_all() + print(f"[inference] latency={time.perf_counter()-inference_start:.4f}s s={s} d={d} self.t={self.t}") + except Exception as e: + print(f"\n[ERROR] Inference loop crashed!") + print(f"Error: {e}") + import traceback + traceback.print_exc() + print("\n[FATAL] Stopping program...") + os._exit(1) # 强制退出整个程序 + + def _predict_action_rtc(self, o, A_prev, d): + # 部署时真实推理延迟偶尔会超过RTC训练接口max_delay,这里用run_config的max_delay裁剪。 + # 原逻辑:直接把d传给predict_action_with_training_rtc_flow。 + max_delay = self.max_delay + if d >= max_delay: + print(f"[rtc] clamp inference_delay from {d} to {max_delay - 1}") + d = max_delay - 1 + A_new = self.policy.predict_action_with_training_rtc_flow( + observations=None if self.text_only else o['imgs'], + states=torch.from_numpy(o['obs']).to(self.device), + traj2ds=None, + instructions=o['text_instructions'], + num_inference_steps = self.num_inference_steps, + prev_actions=torch.from_numpy(A_prev[np.newaxis, :, :]).to(self.device), # (H, D) -> (1, H, D) + inference_delay=d, + max_delay=max_delay, + progress=self._to_tensor(o.get("progress")), + future_progress=self._to_tensor(o.get("future_progress")), + videos=o.get("videos"), + video_paths=o.get("video_paths"), + video_size_pixels=o.get("video_size_pixels", getattr(self.policy, "video_size_pixels", 50176)), + video_sample_fps=o.get("video_sample_fps", 1.0), + )[0].float().detach().cpu().numpy() # (1, H, D) -> (H, D) + return np.clip(A_new, -1.0, 1.0).astype(np.float32) + + def _predict_action(self, o): + normalized_actions = self.policy.predict_action( + observations=None if self.text_only else o['imgs'], + states=torch.from_numpy(o['obs']).to(self.device), + traj2ds=None, + instructions=o['text_instructions'], + num_inference_steps = self.num_inference_steps, + progress=self._to_tensor(o.get("progress")), + future_progress=self._to_tensor(o.get("future_progress")), + videos=o.get("videos"), + video_paths=o.get("video_paths"), + video_size_pixels=o.get("video_size_pixels", getattr(self.policy, "video_size_pixels", 50176)), + video_sample_fps=o.get("video_sample_fps", 1.0), + )[0].float().detach().cpu().numpy() # (1, H, D) -> (H, D) + + return np.clip(normalized_actions, -1.0, 1.0).astype(np.float32) + + +class Server: + + def __init__( + self, + policy:str, + run_dir: Path, + ckpt_step: int | str = "latest", + device: str = "cuda:0", + enable_rtc: bool = False, + action_exec_horizon: int | None = None, + ): + if not torch.cuda.is_available(): + raise RuntimeError("CUDA is not available. Please check your CUDA installation.") + + self.device = torch.device(device) + overwatch.info(f"Using device: {self.device}") + overwatch.info(f"Serving {policy}") + + overwatch.info(f"Using device: {self.device}") + overwatch.info(f"Serving {policy}") + + assert osp.exists(run_dir), f"run_dir {run_dir} does not exist!" + # 兼容ckpt_8000这类保存在run根目录下的中间checkpoint。 + # 原逻辑: + # assert osp.exists(run_dir / "checkpoints" / f"ckpt_{ckpt_step}"), f"ckpt {ckpt_step} does not exist!" + if str(ckpt_step) in {"best", "best_ckpt"}: + ckpt_exists = osp.exists(run_dir / "checkpoints" / "best_ckpt") + else: + ckpt_exists = ( + osp.exists(run_dir / "checkpoints" / f"ckpt_{ckpt_step}") + or osp.exists(run_dir / f"ckpt_{ckpt_step}") + ) + assert ckpt_exists, f"ckpt {ckpt_step} does not exist!" + assert osp.exists(run_dir / "run_config.json"), f"run config does not exist!" + + # first build dynamic config + config_: LaunchConfig = parse_args_to_tyro_config(run_dir / "argv.txt") # type: ignore + # then load it from previsously saved json + conf = (run_dir / "run_config.json").open("r").read() + launch_config = config_.model_validate_json(conf) + seed_everything(launch_config.seed or 42) + + + overwatch.info("loading action model...") + from psi.models.psi0 import Psi0Model + self.model = Psi0Model.from_pretrained(run_dir, ckpt_step, launch_config, device=device) + input_mode = os.environ.get("PSI0_DEPLOY_INPUT_MODE", "").strip().lower() + if input_mode in {"video", "video_clip"}: + self.model.text_only = False + self.model.audio_only = False + self.model.video_only = True + self.model.to(device) + self.model.eval() + + from psi.config.transform import SimpleRepackTransform, Psi0ModelTransform, ActionStateTransform + self.maxmin:ActionStateTransform = launch_config.data.transform.field # type:ignore + self.model_transform:Psi0ModelTransform = launch_config.data.transform.model # type:ignore + + + self.Da = launch_config.model.action_dim # type:ignore + self.Tp = launch_config.model.action_chunk_size # type:ignore + self.Ta = action_exec_horizon or launch_config.model.action_exec_horizon # type:ignore + assert self.Ta <= self.Tp, "action_exec_horizon is too big" + # server端读取v13 infer相关run_config,保证TextOp/state/progress语义和离线推理一致。 + # 原逻辑:只读取action_dim/action_chunk_size/action_exec_horizon。 + self.odim = int(launch_config.model.odim) # type:ignore + self.text_only = bool(getattr(self.model, "text_only", getattr(launch_config.data.transform.model, "text_only", False))) + self.textop_actions = bool(getattr(launch_config.data.transform.repack, "textop_actions", False)) + self.normalize_state = bool(getattr(self.maxmin, "normalize_state", False)) + self.num_past_frames = max( + int(getattr(launch_config.data.transform.repack, "num_past_frames", 0)), + int(getattr(launch_config.data, "num_past_frames", 0)), + ) + self.state_history_stride = int(getattr(launch_config.data, "state_history_stride", 30)) + self.use_progress_token = bool(getattr(launch_config.model, "use_progress_token", False)) + self.use_future_progress_embedding = bool(getattr(launch_config.model, "use_future_progress_embedding", False)) + self.rtc_max_delay = int(getattr(launch_config.model, "max_delay", 8)) + self.num_inference_steps = int(os.environ.get("PSI0_NUM_INFERENCE_STEPS", "8")) + self.progress_target_frames = int(os.environ.get("PSI0_PROGRESS_TARGET_FRAMES", str(CTRL_HZ * 120))) + # deploy侧环境变量控制planner切段策略,避免改全局ServerConfig。 + # 在scripts/deploy/serve_psi0-rtc-sonic.sh里设置PSI0_SEGMENT_SWITCH_MODE。 + segment_switch_mode = os.environ.get("PSI0_SEGMENT_SWITCH_MODE", "reset").strip() + if segment_switch_mode not in {"reset", "rtc_tail"}: + raise ValueError("PSI0_SEGMENT_SWITCH_MODE must be 'reset' or 'rtc_tail'") + self.segment_switch_mode = segment_switch_mode + self.launch_cfg = launch_config + self.count = 0 + print( + "[server] v13 infer config " + f"action_dim={self.Da} odim={self.odim} textop={self.textop_actions} " + f"text_only={self.text_only} future_progress={self.use_future_progress_embedding} " + f"progress_target_frames={self.progress_target_frames} " + f"segment_switch_mode={self.segment_switch_mode}" + ) + + + # control - shared state with locks + self.latest_obs = None + self.latest_action = None + self.latest_action_info = {} + self.action_version = 0 # Used by client to check if there's a new action + + self.obs_lock = threading.Lock() + self.action_lock = threading.Lock() + + self.controller = None + self._control_loop_started = False + # WebSocket断开时停止control loop,避免客户端退出后server还在后台50Hz推理。 + self._stop_control_event = threading.Event() + + # WebSocket: asyncio event to notify when new action is ready + self.app = FastAPI() + self._setup_routes() + + self._action_ready_event: asyncio.Event = None # Will be created in async context + self._active_websocket: WebSocket = None + self._loop = None # asyncio event loop reference for thread-safe notification + self.start_time = time.time() + self.start_time_obs = time.time() + # server侧若把50Hz receive/send/instruction全量打印到终端,会看起来像“卡死刷屏”。 + # 原逻辑: + # 无这些调试日志的限频状态 + self.last_instruction_log_time = 0.0 + self.last_instruction_log_text = "" + self.last_receive_log_time = 0.0 + self.last_send_log_time = 0.0 + self.last_sent_action_log_time = 0.0 + + def _init_controller(self, o_first): + progress_target_frames = self.progress_target_frames + # 优先使用bridge在request.condition里传入的目标动作总帧数,和v13离线infer的progress_target_frames一致。 + conditions = o_first.get("conditions", {}) if isinstance(o_first, dict) else {} + if isinstance(conditions, dict) and int(conditions.get("progress_target_frames", 0)) > 0: + progress_target_frames = int(conditions["progress_target_frames"]) + print(f"[server] controller progress_target_frames={progress_target_frames}") + # 使用run_config/启动参数里的预测长度和执行长度,保持训练/部署一致。 + # 原逻辑:controller = RealTimeChunkController(policy=self.model, o_first=o_first) + controller = RealTimeChunkController( + policy=self.model, + maxmin=self.maxmin, + text_only=self.text_only, + textop_actions=self.textop_actions, + odim=self.odim, + normalize_state=self.normalize_state, + num_past_frames=self.num_past_frames, + state_history_stride=self.state_history_stride, + use_progress_token=self.use_progress_token, + use_future_progress_embedding=self.use_future_progress_embedding, + progress_target_frames=progress_target_frames, + num_inference_steps=self.num_inference_steps, + rtc_max_delay=self.rtc_max_delay, + segment_switch_mode=self.segment_switch_mode, + prediction_horizon=self.Tp, + min_exec_horizon=self.Ta, + o_first=o_first, + ) + return controller + + def _postprocess_action(self, action): + # 与v13离线infer一致:normalized action先裁剪到[-1,1],再反归一化。 + # 原逻辑: + # return self.maxmin.denormalize(action) # denormalization is done in the pipeline + action = np.clip(np.asarray(action, dtype=np.float32), -1.0, 1.0) + return self.maxmin.denormalize(action) # denormalization is done in the pipeline + + + + def preprocess_image(self, image_dict: Dict[str, Any]) -> Dict[str, Any]: + imgs = {} + # # FIXME + # image_key_to_cam_idx = {'front_stereo_left': 0, 'front_stereo_right': 1, 'left_future_traj_2d': 4, 'right_future_traj_2d': 5, 'side': 3, 'side_future_traj_2d': 7, 'wrist': 2, 'wrist_future_traj_2d': 6} + # for img_key in self.launch_config.data.transform.repack.image_keys: + # cam_idx = image_key_to_cam_idx[img_key] #self.launch_config.data.transform.repack.image_key_to_cam_idx[img_key] + # imgs[f"cam{cam_idx}"] = self._process_img(image_dict[f"{img_key}".replace("image_", "")])#[None, ...] + + for k in image_dict.keys(): + imgs[k] = self._process_img(image_dict[k]) + + + return imgs + + def _process_img(self, img): + from torchvision.transforms import v2 + + transforms = [self.model_transform.resize(), self.model_transform.center_crop()] + t = v2.Compose(transforms) + + return [t(img)] + + def _parse_obs_payload(self, payload: Dict[str, Any]) -> Dict[str, Any]: + """Parse observation payload and return processed obs dict""" + request = RequestMessage.deserialize(payload) + image_dict, instruction, history_dict, state_dict, gt_action, dataset_name = \ + request.image, request.instruction, request.history, request.state, request.gt_action, request.dataset_name + condition_dict = request.condition + # Instruction在50Hz receive路径里;只在文本变化或至少隔1秒时打印一次,避免server终端刷满。 + # 原逻辑: + # overwatch.info(f"Instruction: {instruction}") + now = time.time() + if instruction != self.last_instruction_log_text or now - self.last_instruction_log_time >= 1.0: + print(f"[server] Instruction: {instruction}") + self.last_instruction_log_text = str(instruction) + self.last_instruction_log_time = now + + # parts = instruction.split(".") + # if len(parts) > 1 and parts[-1].isdigit(): + # instruction = parts[0].lower() + "." + # img_id = int(parts[-1]) + # assert False + # else: + # v13训练/离线infer会保留模板大小写;server端只清理首尾空白,不再强制lower。 + # 原逻辑: + # instruction = instruction.lower() + instruction = instruction.strip() + # img_id = -1 + + + # v13 TextOp checkpoint要求模型输入是39D state;这里保留36D abs,交给controller构造历史/TextOp/归一化。 + # 原逻辑: + # obs = state_dict['states'].copy() + # if self.maxmin.pad_state_dim != len(obs): + # obs = pad_to_len(obs, self.maxmin.pad_state_dim, dim=0)[0] + # obs = self.maxmin.normalize_state_func(obs) + # obs = obs[np.newaxis, np.newaxis, :] + abs_state = np.asarray(state_dict['states'], dtype=np.float32).reshape(-1) + if abs_state.shape[0] < SEED_ABS_DIM: + raise ValueError(f"Expected at least {SEED_ABS_DIM}D state from client, got {abs_state.shape}") + abs_state = abs_state[:SEED_ABS_DIM].copy() + abs_state[32:36] = normalize_quat_wxyz(abs_state[32:36]) + + batch_images = None + if not self.text_only: + # TODO support image history + # img dict: {"video": np.array(...).shape(480, 640, 3)} + imgs = {} + for cam_idx, img in enumerate(image_dict.values()): + imgs[f"cam{cam_idx}"] = Image.fromarray(np.clip(img, 0, 255).astype(np.uint8)) + image_input = self.preprocess_image(imgs) + batch_images = [image_input['cam0']] # batch size == 1 + + + # 保留client传来的condition,用于目标帧数/progress等在线推理条件。 + # 原逻辑: + # conditions = {} + conditions = condition_dict if isinstance(condition_dict, dict) else {} + + text_instructions = [instruction] # len == 1 + + return { + 'imgs': batch_images, + 'text_instructions': text_instructions, + 'abs_state': abs_state, + 'conditions': conditions, + } + + async def websocket_handler(self, websocket: WebSocket): + """ + WebSocket handler for bidirectional communication: + - Receive obs from client at high frequency + - Send action to client immediately when new action is ready + """ + # online脚本可能被重复source;新连接进来时先停止上一轮control loop,避免旧连接残留后只产action不发送。 + # 原逻辑:所有WebSocket连接共享self.controller/self._action_ready_event,但新连接不会强制清理旧control loop。 + if self._control_loop_started: + print("[WebSocket] stopping previous control loop before accepting new client", flush=True) + self._stop_control_event.set() + old_thread = getattr(self, "_control_thread", None) + if old_thread is not None and old_thread.is_alive(): + old_thread.join(timeout=2.0) + if self.controller is not None: + # 新连接前关闭旧controller的后台RTC线程。 + self.controller.close() + self._control_loop_started = False + self.controller = None + with self.obs_lock: + self.latest_obs = None + with self.action_lock: + self.latest_action = None + self.latest_action_info = {} + self.action_version = 0 + + await websocket.accept() + self._active_websocket = websocket + + # Create asyncio event for action notification + self._action_ready_event = asyncio.Event() + + print("[WebSocket] Client connected") + async def receive_obs(): + """Continuously receive obs from client""" + try: + while True: + # Receive obs from client + data = await websocket.receive_text() + payload = json.loads(data) + interval = time.time() - self.start_time_obs + self.start_time_obs = time.time() + # receive_obs是50Hz热路径;这里改成最多每秒打印一次。 + # 原逻辑: + # print(f"[WebSocket] receive_obs interval: {interval} seconds") + now = time.time() + if now - self.last_receive_log_time >= 1.0: + print(f"[WebSocket] receive_obs interval: {interval:.6f} seconds") + self.last_receive_log_time = now + # Parse and update latest_obs + this_o = self._parse_obs_payload(payload) + with self.obs_lock: + self.latest_obs = this_o + + # If control loop hasn't started, start it + if not self._control_loop_started and self.latest_obs is not None: + self._start_control_loop() + + # # 清空缓冲区,只保留最新的 + # latest_data = None + # while True: + # try: + # # 非阻塞地读取所有可用消息 + # data = await asyncio.wait_for( + # websocket.receive_text(), + # timeout=0.001 # 1ms超时 + # ) + # latest_data = data # 保留最新的 + # except asyncio.TimeoutError: + # break # 没有更多消息了 + + # if latest_data: + # payload = json.loads(latest_data) + # interval = time.time() - self.start_time_obs + # self.start_time_obs = time.time() + # print(f"[WebSocket] receive_obs interval: {interval} seconds") + + # this_o = self._parse_obs_payload(payload) + # with self.obs_lock: + # self.latest_obs = this_o + + # if not self._control_loop_started and self.latest_obs is not None: + # self._start_control_loop() + + except WebSocketDisconnect: + print("[WebSocket] Client disconnected (receive)") + except Exception as e: + import traceback + traceback.print_exc() + print(f"[WebSocket] Receive error: {e}") + finally: + # receive侧结束就停掉control loop,避免客户端sender线程死后server继续空跑。 + self._stop_control_event.set() + + async def send_action(): + """Send action to client when new action is ready""" + last_sent_version = 0 + try: + while True: + # Wait for new action to be ready + await self._action_ready_event.wait() + self._action_ready_event.clear() + + interval = time.time() - self.start_time + self.start_time = time.time() + # send_action也是50Hz热路径;这里改成最多每秒打印一次。 + # 原逻辑: + # print(f"[WebSocket] send_action interval: {interval} seconds") + now = time.time() + if now - self.last_send_log_time >= 1.0: + print(f"[WebSocket] send_action interval: {interval:.6f} seconds") + self.last_send_log_time = now + + # Get the action + with self.action_lock: + action = self.latest_action + action_info = dict(self.latest_action_info) + version = self.action_version + # 不再把latest_action清空;asyncio.Event可能有延迟/重复set,清空会导致下一轮拿到None后退出。 + # 原逻辑: + # self.latest_action = None # Reset after sending + + if action is None or version == last_sent_version: + # 忽略过期event或重复version,不让send_action退出导致bridge等不到后续action。 + continue + + if action is not None: + # Send action to client + response = ResponseMessage(action, err=0.0) + resp_dict = response.serialize() + resp_dict["version"] = version + # 把server侧chunk元信息传给bridge日志,用于确认是否重复/跳帧。 + for key, value in action_info.items(): + resp_dict[key] = value + await websocket.send_text(json.dumps(resp_dict)) + last_sent_version = version + # 发送成功日志也限频;保留版本号方便确认server仍在出包。 + # 原逻辑: + # print(f"[WebSocket] Sent action, version={version}") + now = time.time() + if now - self.last_sent_action_log_time >= 1.0: + print(f"[WebSocket] Sent action, version={version}") + self.last_sent_action_log_time = now + else: + # 原逻辑: + # assert False, "action is None" + continue + + except WebSocketDisconnect: + print("[WebSocket] Client disconnected (send)") + except Exception as e: + print(f"[WebSocket] Send error: {e}") + finally: + # send侧异常后也停止control loop;否则bridge收不到action,WBC会一直等下一包motion。 + self._stop_control_event.set() + + try: + # Run both tasks concurrently + await asyncio.gather(receive_obs(), send_action()) + except Exception as e: + print(f"[WebSocket] Connection closed: {e}") + finally: + # 客户端断开后停止本次在线控制循环,下一次连接再重新初始化controller。 + self._stop_control_event.set() + if self.controller is not None: + self.controller.close() + self.controller = None + self._control_loop_started = False + self._active_websocket = None + print("[WebSocket] Handler finished") + + def _start_control_loop(self): + """Start control loop thread""" + if self._control_loop_started: + return + self._control_loop_started = True + self._stop_control_event.clear() + + # Initialize controller with first obs + with self.obs_lock: + o_first = copy.deepcopy(self.latest_obs) + + self.controller = self._init_controller(o_first) # wait for model warm up + + # Start control loop thread + self._control_thread = threading.Thread(target=self._control_loop, daemon=True) + self._control_thread.start() + print("[control loop] started") + + def _close_active_websocket_from_thread(self) -> None: + # control loop线程异常退出时,主动关闭当前websocket,让bridge立刻感知而不是永远卡在等action。 + # 原逻辑: + # control loop退出后不主动关闭websocket + if self._active_websocket is None or self._loop is None: + return + + websocket = self._active_websocket + loop = self._loop + + async def _close_ws(): + try: + await websocket.close() + except Exception as exc: + print(f"[control loop] websocket close failed: {exc}") + + try: + asyncio.run_coroutine_threadsafe(_close_ws(), loop) + except Exception as exc: + print(f"[control loop] failed to schedule websocket close: {exc}") + + def _control_loop(self): + """ + Control loop: Execute controller.step strictly every CTRL_PERIOD_SEC + And expect at next time, the obs_next sent from client is the one after executing the action + """ + next_tick = time.perf_counter() + prev_tick = time.perf_counter() + last_interval_log_time = 0.0 + + try: + while not self._stop_control_event.is_set(): + # loop_start = time.time() + + # 1. Get latest obs + with self.obs_lock: + obs_next = copy.deepcopy(self.latest_obs) + + # latest_obs异常丢失时显式报错,让bridge收到断连而不是一直空等。 + # 原逻辑: + # 默认假设latest_obs总是存在 + if obs_next is None: + raise RuntimeError("latest_obs is None in control loop") + + # 2. Execute step + # 返回当前未来有效执行窗口action chunk给SONIC bridge,训练配置下是20帧。 + # 原逻辑: + # action = self.controller.step(obs_next) # (1, D) + # pred_action = self._postprocess_action(action) # (1, D) + action, action_chunk, chunk_info = self.controller.step(obs_next, return_chunk=True) # (1, D), (H, D), metadata + if action_chunk is not None: + pred_action = self._postprocess_action(action_chunk) # (H, D) + + # 3. Update latest_action + with self.action_lock: + self.latest_action = pred_action + self.latest_action_info = dict(chunk_info or {}) + self.action_version += 1 + + # 4. Notify WebSocket that new action is ready + if self._action_ready_event is not None: + # Thread-safe way to set asyncio event from another thread + try: + self._loop.call_soon_threadsafe(self._action_ready_event.set) + except Exception as e: + print(f"[control loop] Failed to notify WebSocket: {e}") + + # elapsed = (time.time() - loop_start) * 1000 + # print(f"[control loop] step took {elapsed:.1f}ms, version={self.action_version}") + + # 5. Wait until next ctrl period + next_tick += CTRL_PERIOD_SEC + sleep_time = next_tick - time.perf_counter() + now = time.perf_counter() + interval = now - prev_tick + prev_tick = now + # 原来每20ms打印一次interval,会在server终端疯狂刷屏;这里改成最多每秒打印一次。 + # 原逻辑: + # print(f"[control loop] interval: {interval} seconds") + wall_time = time.time() + if wall_time - last_interval_log_time >= 1.0: + print(f"[control loop] interval: {interval:.6f} seconds") + last_interval_log_time = wall_time + if sleep_time > 0: + time.sleep(sleep_time) + # delay_ms(sleep_time*1000) + else: + print(f"[control loop] WARNING: missed tick by {-sleep_time*1000:.1f}ms") + next_tick = time.perf_counter() + except Exception as e: + # control loop线程异常时显式打印并主动断开websocket,避免bridge/WBC无限刷waiting。 + # 原逻辑: + # control loop线程异常后仅依赖Python线程默认报错,不会主动关闭websocket + import traceback + + print(f"[control loop] crashed: {e}") + traceback.print_exc() + self._stop_control_event.set() + self._close_active_websocket_from_thread() + finally: + self._control_loop_started = False + print("[control loop] stopped") + + + def _setup_routes(self): + """设置所有路由""" + @self.app.websocket("/ws") + async def websocket_endpoint(websocket: WebSocket): + self._loop = asyncio.get_event_loop() + await self.websocket_handler(websocket) + + @self.app.get("/health") + async def health_check(): + return {"status": "ok"} + + def run(self, host: str = "0.0.0.0", port: int = 8000) -> None: + print(f"Server listens on {host}:{port}") + print(f"WebSocket endpoint: ws://{host}:{port}/ws") + try: + uvicorn.run(self.app, host=host, port=port) + except Exception as e: + print(f"Server crashed, {e}") + finally: + print("Server stopped.") + exit(1) + +def serve(cfg: ServerConfig) -> None: + overwatch.info("Server :: Initializing Policy") + assert cfg.policy is not None, "which policy to serve?" + assert cfg.rtc, "this server is for rtc" + server = Server( + cfg.policy, + Path(cfg.run_dir), + cfg.ckpt_step, + cfg.device, + cfg.rtc, + cfg.action_exec_horizon) + + print("Server :: Spinning Up") + server.run(cfg.host, cfg.port) + +if __name__ == "__main__": + from dotenv import load_dotenv + load_dotenv() # take environment variables from .env file + config = tyro.cli(ServerConfig, config=(tyro.conf.ConsolidateSubcommandArgs,)) + serve(config) diff --git a/src/psi/deploy/psi_serve_rtc-trainingtimertc.py b/src/psi/deploy/psi_serve_rtc-trainingtimertc.py new file mode 100644 index 0000000000000000000000000000000000000000..41d65651f018955cfa2e514a5605b242cbda7f55 --- /dev/null +++ b/src/psi/deploy/psi_serve_rtc-trainingtimertc.py @@ -0,0 +1,538 @@ +import asyncio +from fastapi import FastAPI, WebSocket, WebSocketDisconnect + +from pydantic import BaseModel +from collections import deque +import threading +import time +import copy + +import os +import sys +import json +import tyro +# import draccus +import uvicorn +import numpy as np +from PIL import Image +from pathlib import Path +from typing import Dict, Any, List +import os.path as osp + +from psi.models.psi0 import Psi0Model + +# os.environ["TRANSFORMERS_OFFLINE"] = "1" + +# Ensure imports work regardless of current working directory +repo_root = os.path.abspath(os.path.join(os.path.dirname(__file__), "..")) +if repo_root not in sys.path: + sys.path.insert(0, repo_root) + +from psi.config.config import LaunchConfig, ServerConfig +from psi.deploy.helpers import * + +# from pipelines import ActionPipeline +# from misc import move_to_device +from psi.utils import parse_args_to_tyro_config, pad_to_len, seed_everything + +from psi.utils.overwatch import initialize_overwatch +overwatch = initialize_overwatch(__name__) + + + +PREDICT_HORIZON = 30 # == H +MIN_EXEC_HORIZON = 15 # == s_min # TODO: should match D_INIT, ideally s_min >= d_real +DELAY_BUFFER_SIZE = 6 # == delay_buffer_size +D_INIT = 6 # == d_init # TODO: placeholder, needs calculation +CTRL_PERIOD_SEC = 1. / 30 # 30Hz + + + +class RealTimeChunkController: + def __init__(self, + policy: Psi0Model, + prediction_horizon: int = PREDICT_HORIZON, + min_exec_horizon: int = MIN_EXEC_HORIZON, + delay_buf_size: int = DELAY_BUFFER_SIZE, + d_init: int = D_INIT, + o_first: np.ndarray | None = None): # type: ignore + + self.policy : Psi0Model = policy + self.device = self.policy.device + self.H = prediction_horizon + + self.s_min = min_exec_horizon + + self.t: int = 0 + assert o_first != None, "please provide o_first" + + A_first = self._predict_action(o_first) # (H, D) + + # warmup the model + for i in range (2): + _ = self._predict_action_rtc(copy.deepcopy(o_first), np.concatenate([copy.deepcopy(A_first[self.s_min:, :]), np.zeros((self.s_min, A_first.shape[1]), dtype=A_first.dtype)], axis=0), d_init) + print("Model warmed up") + + self.A_cur = A_first # (H, D) + self.o_cur: Dict[str, Any] | None = None + + self.Q = deque([d_init], maxlen=delay_buf_size) + + self.M = threading.Lock() + self.C = threading.Condition(self.M) + + self._infer_th = threading.Thread(target=self._inference_loop, daemon=True) + self._infer_th.start() + + def replace_prev_actions_to_obs(self, o, previous_rpy, previous_height): + o['obs'] = np.concatenate([o['obs'][:, :, :28], previous_rpy[np.newaxis, np.newaxis, :], previous_height[np.newaxis, np.newaxis, :], o['obs'][:, :, 32:]], axis=-1) # (1, 1, 28) -> (1, 1, 32) + return o + + + def step(self, obs_next: Dict[str, Any]): # consume a_(t-1) and provide o_t + with self.C: + self.t += 1 + self.o_cur = obs_next + self.C.notify() + if self.t-1 >= len(self.A_cur): + single_action = self.A_cur[-1] + print("failed") + else: + single_action = self.A_cur[self.t - 1] + return single_action[np.newaxis, :] # (1, D) + + def _inference_loop(self): + while True: + with self.C: + try: + while self.t < self.s_min: + self.C.wait() # wait until notified and get the lock + s = self.t + + # FIXME: + # 1. maybe bug at "s-2" + # 2. inputs should be : normalize_states(denormalize_action(A_cur[s-2])) + # but in our current data, the stats for rpy and height are nearly the same, + # so "normalize_states(denormalize_action())" equals to doing nothing. + + assert (s-2) >= 0 + self.o_cur = self.replace_prev_actions_to_obs(self.o_cur, copy.deepcopy(self.A_cur[s-2, 28:31]), copy.deepcopy(self.A_cur[s-2, 31:32])) + # + + o = copy.deepcopy(self.o_cur) + d = max(self.Q) + # A_prev = copy.deepcopy(torch.cat([self.A_cur[s:, :], torch.zeros((s, self.A_cur.shape[1]), device=self.A_cur.device, dtype=self.A_cur.dtype)], dim=0)) # (H, D) + A_prev = np.concatenate([copy.deepcopy(self.A_cur[s:, :]), np.zeros((s, self.A_cur.shape[1]), dtype=self.A_cur.dtype)], axis=0) # (H, D) + + inference_start = time.perf_counter() + self.C.release() + A_new = self._predict_action_rtc(o, A_prev, d) + self.C.acquire() + + self.A_cur = A_new + self.t = self.t - s + self.Q.append(self.t) + # self.C.notify_all() + print(f"[inference] latency={time.perf_counter()-inference_start:.4f}s s={s} d={d} self.t={self.t}") + except Exception as e: + print(f"\n[ERROR] Inference loop crashed!") + print(f"Error: {e}") + import traceback + traceback.print_exc() + print("\n[FATAL] Stopping program...") + os._exit(1) # 强制退出整个程序 + + def _predict_action_rtc(self, o, A_prev, d): + A_new = self.policy.predict_action_with_training_rtc_flow( + observations=o['imgs'], + states=torch.from_numpy(o['obs']).to(self.device), + traj2ds=None, + instructions=o['text_instructions'], + num_inference_steps = 8, + prev_actions=torch.from_numpy(A_prev[np.newaxis, :, :]).to(self.device), # (H, D) -> (1, H, D) + inference_delay=d, + max_delay=8 + )[0].float().detach().cpu().numpy() # (1, H, D) -> (H, D) + return A_new + + def _predict_action(self, o): + normalized_actions = self.policy.predict_action( + observations=o['imgs'], + states=torch.from_numpy(o['obs']).to(self.device), + traj2ds=None, + instructions=o['text_instructions'], + num_inference_steps = 8, + )[0].float().detach().cpu().numpy() # (1, H, D) -> (H, D) + + return normalized_actions + + +class Server: + + def __init__( + self, + policy:str, + run_dir: Path, + ckpt_step: int | str = "latest", + device: str = "cuda:0", + enable_rtc: bool = False, + action_exec_horizon: int | None = None + ): + if not torch.cuda.is_available(): + raise RuntimeError("CUDA is not available. Please check your CUDA installation.") + + self.device = torch.device(device) + overwatch.info(f"Using device: {self.device}") + overwatch.info(f"Serving {policy}") + + overwatch.info(f"Using device: {self.device}") + overwatch.info(f"Serving {policy}") + + assert osp.exists(run_dir), f"run_dir {run_dir} does not exist!" + assert osp.exists(run_dir / "checkpoints" / f"ckpt_{ckpt_step}"), f"ckpt {ckpt_step} does not exist!" + assert osp.exists(run_dir / "run_config.json"), f"run config does not exist!" + + # first build dynamic config + config_: LaunchConfig = parse_args_to_tyro_config(run_dir / "argv.txt") # type: ignore + # then load it from previsously saved json + conf = (run_dir / "run_config.json").open("r").read() + launch_config = config_.model_validate_json(conf) + seed_everything(launch_config.seed or 42) + + + overwatch.info("loading action model...") + from psi.models.psi0 import Psi0Model + self.model = Psi0Model.from_pretrained(run_dir, ckpt_step, launch_config, device=device) + self.model.to(device) + self.model.eval() + + from psi.config.transform import SimpleRepackTransform, Psi0ModelTransform, ActionStateTransform + self.maxmin:ActionStateTransform = launch_config.data.transform.field # type:ignore + self.model_transform:Psi0ModelTransform = launch_config.data.transform.model # type:ignore + + + self.Da = launch_config.model.action_dim # type:ignore + self.Tp = launch_config.model.action_chunk_size # type:ignore + self.Ta = action_exec_horizon or launch_config.model.action_exec_horizon # type:ignore + assert self.Ta <= self.Tp, "action_exec_horizon is too big" + self.launch_cfg = launch_config + self.count = 0 + + + # control - shared state with locks + self.latest_obs = None + self.latest_action = None + self.action_version = 0 # Used by client to check if there's a new action + + self.obs_lock = threading.Lock() + self.action_lock = threading.Lock() + + self.controller = None + self._control_loop_started = False + + # WebSocket: asyncio event to notify when new action is ready + self.app = FastAPI() + self._setup_routes() + + self._action_ready_event: asyncio.Event = None # Will be created in async context + self._active_websocket: WebSocket = None + self._loop = None # asyncio event loop reference for thread-safe notification + self.start_time = time.time() + self.start_time_obs = time.time() + + def _init_controller(self, o_first): + controller = RealTimeChunkController(policy=self.model, o_first=o_first) + return controller + + def _postprocess_action(self, action): + # return self.launch_cfg.data.data_transforms.denormalize_action(action) + return self.maxmin.denormalize(action) # denormalization is done in the pipeline + + + + def preprocess_image(self, image_dict: Dict[str, Any]) -> Dict[str, Any]: + imgs = {} + # # FIXME + # image_key_to_cam_idx = {'front_stereo_left': 0, 'front_stereo_right': 1, 'left_future_traj_2d': 4, 'right_future_traj_2d': 5, 'side': 3, 'side_future_traj_2d': 7, 'wrist': 2, 'wrist_future_traj_2d': 6} + # for img_key in self.launch_config.data.transform.repack.image_keys: + # cam_idx = image_key_to_cam_idx[img_key] #self.launch_config.data.transform.repack.image_key_to_cam_idx[img_key] + # imgs[f"cam{cam_idx}"] = self._process_img(image_dict[f"{img_key}".replace("image_", "")])#[None, ...] + + for k in image_dict.keys(): + imgs[k] = self._process_img(image_dict[k]) + + + return imgs + + def _process_img(self, img): + from torchvision.transforms import v2 + + transforms = [self.model_transform.resize(), self.model_transform.center_crop()] + t = v2.Compose(transforms) + + return [t(img)] + + def _parse_obs_payload(self, payload: Dict[str, Any]) -> Dict[str, Any]: + """Parse observation payload and return processed obs dict""" + request = RequestMessage.deserialize(payload) + image_dict, instruction, history_dict, state_dict, gt_action, dataset_name = \ + request.image, request.instruction, request.history, request.state, request.gt_action, request.dataset_name + condition_dict = request.condition + overwatch.info(f"Instruction: {instruction}") + + # parts = instruction.split(".") + # if len(parts) > 1 and parts[-1].isdigit(): + # instruction = parts[0].lower() + "." + # img_id = int(parts[-1]) + # assert False + # else: + instruction = instruction.lower() + # img_id = -1 + + + # TODO support image history + # img dict: {"video": np.array(...).shape(480, 640, 3)} + imgs = {} + for cam_idx, img_key in enumerate(self.launch_cfg.data.transform.repack.image_keys): + imgs[f"cam{cam_idx}"] = Image.fromarray(np.clip(image_dict[img_key], 0, 255).astype(np.uint8)) + + hand_joints = state_dict["hand_joints"].copy() # shape (14,) + arm_joints = state_dict["arm_joints"].copy() # shape (14,) + tmp_torso_rpy = np.array([0.0, 0.0, 0.0], dtype=np.float32) + tmp_torso_height = np.array([0.75], dtype=np.float32) + obs = np.concatenate([hand_joints, arm_joints, tmp_torso_rpy, tmp_torso_height], axis=-1) # (32,) + + # normalize states + assert self.maxmin.normalize_state, "check" + if self.maxmin.pad_state_dim != len(obs): + obs = pad_to_len(obs, self.maxmin.pad_state_dim, dim=0)[0] + obs = self.maxmin.normalize_state_func(obs) # shape (32,) + obs = obs[np.newaxis, np.newaxis, :] # (32,) -> (1, 1, 32) + + + image_input = self.preprocess_image(imgs) + batch_images = [image_input['cam0']] # batch size == 1 + + + conditions = {} + + text_instructions = [instruction] # len == 1 + + return {'imgs': batch_images, 'text_instructions': text_instructions, 'obs': obs, 'conditions': conditions} + + async def websocket_handler(self, websocket: WebSocket): + """ + WebSocket handler for bidirectional communication: + - Receive obs from client at high frequency + - Send action to client immediately when new action is ready + """ + await websocket.accept() + self._active_websocket = websocket + + # Create asyncio event for action notification + self._action_ready_event = asyncio.Event() + + print("[WebSocket] Client connected") + async def receive_obs(): + """Continuously receive obs from client""" + try: + while True: + # Receive obs from client + data = await websocket.receive_text() + payload = json.loads(data) + interval = time.time() - self.start_time_obs + self.start_time_obs = time.time() + print(f"[WebSocket] receive_obs interval: {interval} seconds") + # Parse and update latest_obs + this_o = self._parse_obs_payload(payload) + with self.obs_lock: + self.latest_obs = this_o + + # If control loop hasn't started, start it + if not self._control_loop_started and self.latest_obs is not None: + self._start_control_loop() + + # # 清空缓冲区,只保留最新的 + # latest_data = None + # while True: + # try: + # # 非阻塞地读取所有可用消息 + # data = await asyncio.wait_for( + # websocket.receive_text(), + # timeout=0.001 # 1ms超时 + # ) + # latest_data = data # 保留最新的 + # except asyncio.TimeoutError: + # break # 没有更多消息了 + + # if latest_data: + # payload = json.loads(latest_data) + # interval = time.time() - self.start_time_obs + # self.start_time_obs = time.time() + # print(f"[WebSocket] receive_obs interval: {interval} seconds") + + # this_o = self._parse_obs_payload(payload) + # with self.obs_lock: + # self.latest_obs = this_o + + # if not self._control_loop_started and self.latest_obs is not None: + # self._start_control_loop() + + except WebSocketDisconnect: + print("[WebSocket] Client disconnected (receive)") + except Exception as e: + import traceback + traceback.print_exc() + print(f"[WebSocket] Receive error: {e}") + + async def send_action(): + """Send action to client when new action is ready""" + try: + while True: + # Wait for new action to be ready + await self._action_ready_event.wait() + self._action_ready_event.clear() + + interval = time.time() - self.start_time + self.start_time = time.time() + print(f"[WebSocket] send_action interval: {interval} seconds") + + # Get the action + with self.action_lock: + action = self.latest_action + version = self.action_version + self.latest_action = None # Reset after sending + + if action is not None: + # Send action to client + response = ResponseMessage(action, err=0.0) + resp_dict = response.serialize() + resp_dict["version"] = version + await websocket.send_text(json.dumps(resp_dict)) + print(f"[WebSocket] Sent action, version={version}") + else: + assert False, "action is None" + + except WebSocketDisconnect: + print("[WebSocket] Client disconnected (send)") + except Exception as e: + print(f"[WebSocket] Send error: {e}") + + try: + # Run both tasks concurrently + await asyncio.gather(receive_obs(), send_action()) + except Exception as e: + print(f"[WebSocket] Connection closed: {e}") + finally: + self._active_websocket = None + print("[WebSocket] Handler finished") + + def _start_control_loop(self): + """Start control loop thread""" + if self._control_loop_started: + return + self._control_loop_started = True + + # Initialize controller with first obs + with self.obs_lock: + o_first = copy.deepcopy(self.latest_obs) + + self.controller = self._init_controller(o_first) # wait for model warm up + + # Start control loop thread + self._control_thread = threading.Thread(target=self._control_loop, daemon=True) + self._control_thread.start() + print("[control loop] started") + + def _control_loop(self): + """ + Control loop: Execute controller.step strictly every CTRL_PERIOD_SEC + And expect at next time, the obs_next sent from client is the one after executing the action + """ + next_tick = time.perf_counter() + prev_tick = time.perf_counter() + + while True: + # loop_start = time.time() + + # 1. Get latest obs + with self.obs_lock: + obs_next = copy.deepcopy(self.latest_obs) + + # 2. Execute step + action = self.controller.step(obs_next) # (1, D) + pred_action = self._postprocess_action(action) # (1, D) + + # 3. Update latest_action + with self.action_lock: + self.latest_action = pred_action + self.action_version += 1 + + # 4. Notify WebSocket that new action is ready + if self._action_ready_event is not None: + # Thread-safe way to set asyncio event from another thread + try: + self._loop.call_soon_threadsafe(self._action_ready_event.set) + except Exception as e: + print(f"[control loop] Failed to notify WebSocket: {e}") + + # elapsed = (time.time() - loop_start) * 1000 + # print(f"[control loop] step took {elapsed:.1f}ms, version={self.action_version}") + + # 5. Wait until next ctrl period + next_tick += CTRL_PERIOD_SEC + sleep_time = next_tick - time.perf_counter() + now = time.perf_counter() + interval = now - prev_tick + prev_tick = now + print(f"[control loop] interval: {interval} seconds") + if sleep_time > 0: + time.sleep(sleep_time) + # delay_ms(sleep_time*1000) + else: + print(f"[control loop] WARNING: missed tick by {-sleep_time*1000:.1f}ms") + next_tick = time.perf_counter() + + + def _setup_routes(self): + """设置所有路由""" + @self.app.websocket("/ws") + async def websocket_endpoint(websocket: WebSocket): + self._loop = asyncio.get_event_loop() + await self.websocket_handler(websocket) + + @self.app.get("/health") + async def health_check(): + return {"status": "ok"} + + def run(self, host: str = "0.0.0.0", port: int = 8000) -> None: + print(f"Server listens on {host}:{port}") + print(f"WebSocket endpoint: ws://{host}:{port}/ws") + try: + uvicorn.run(self.app, host=host, port=port) + except Exception as e: + print(f"Server crashed, {e}") + finally: + print("Server stopped.") + exit(1) + +def serve(cfg: ServerConfig) -> None: + overwatch.info("Server :: Initializing Policy") + assert cfg.policy is not None, "which policy to serve?" + assert cfg.rtc, "this server is for rtc" + server = Server( + cfg.policy, + Path(cfg.run_dir), + cfg.ckpt_step, + cfg.device, + cfg.rtc, + cfg.action_exec_horizon) + + print("Server :: Spinning Up") + server.run(cfg.host, cfg.port) + +if __name__ == "__main__": + from dotenv import load_dotenv + load_dotenv() # take environment variables from .env file + config = tyro.cli(ServerConfig, config=(tyro.conf.ConsolidateSubcommandArgs,)) + serve(config) \ No newline at end of file