commit 693921b6226a80f9f8ee25dde37d370d0d9d9bb9 Author: ZhaiShuaiShuai <18264798931@163.com> Date: Mon Jul 28 12:35:44 2025 +0800 first commit diff --git a/Open_Duck_Mini-2/.gitignore b/Open_Duck_Mini-2/.gitignore new file mode 100644 index 0000000..786bdaf --- /dev/null +++ b/Open_Duck_Mini-2/.gitignore @@ -0,0 +1,11 @@ +__pycache__/ +gym/logs/* +*.pkl +mini_bdx/mini_bdx.egg-info/* +experiments/RL/data/* +experiments/RL/new/logs/ +experiments/RL/new/sac/ +experiments/RL/new/ppo/ +*.zip +*.txt +*.onnx diff --git a/Open_Duck_Mini-2/FUNDING.yml b/Open_Duck_Mini-2/FUNDING.yml new file mode 100644 index 0000000..22a38ad --- /dev/null +++ b/Open_Duck_Mini-2/FUNDING.yml @@ -0,0 +1,3 @@ +# These are supported funding model platforms + +github: apirrone diff --git a/Open_Duck_Mini-2/LICENSE b/Open_Duck_Mini-2/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/Open_Duck_Mini-2/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/.gitignore b/Open_Duck_Mini-2/Open_Duck_Mini-2/.gitignore new file mode 100644 index 0000000..786bdaf --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/.gitignore @@ -0,0 +1,11 @@ +__pycache__/ +gym/logs/* +*.pkl +mini_bdx/mini_bdx.egg-info/* +experiments/RL/data/* +experiments/RL/new/logs/ +experiments/RL/new/sac/ +experiments/RL/new/ppo/ +*.zip +*.txt +*.onnx diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/FUNDING.yml b/Open_Duck_Mini-2/Open_Duck_Mini-2/FUNDING.yml new file mode 100644 index 0000000..22a38ad --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/FUNDING.yml @@ -0,0 +1,3 @@ +# These are supported funding model platforms + +github: apirrone diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/LICENSE b/Open_Duck_Mini-2/Open_Duck_Mini-2/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/README.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/README.md new file mode 100644 index 0000000..0becdbe --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/README.md @@ -0,0 +1,97 @@ +# Open Duck Mini v2 + + + + + + + +
1 2 3
+ +We are making a miniature version of the BDX Droid by Disney. It is about 42 centimeters tall with its legs extended. +The full BOM cost should be under $400 ! + +This repo is kind of a hub where we centralize all resources related to this project. This is a working repo, so there are a lot of undocumented scripts :) We'll try to clean things up at some point. + + +# State of sim2real + +https://github.com/user-attachments/assets/58721d0f-2f95-4088-8900-a5d02f41bba7 + +https://github.com/user-attachments/assets/4129974a-9d97-4651-9474-c078043bb182 + +https://github.com/user-attachments/assets/a0afcd38-15d8-40c6-8171-a619107406b8 + + +# Updates + +> Update 02/04/2024: You can try two policies we trained : [this one](BEST_WALK_ONNX.onnx) and [this one](BEST_WALK_ONNX_2.onnx) +> Run with the following arguments : +> python v2_rl_walk_mujoco.py --onnx_model_path ~/BEST_WALK_ONNX_2.onnx + +> Update 15/03/2025: join our discord server to get help or show us your duck :) https://discord.gg/UtJZsgfQGe + +> Update 07/02/2025: Big progress on sim2real, see videos above :) + +> Update 24/02/2025: Working hard on sim2real ! + +> Update 07/02/2025 : We are writing documentation on the go, but the design and BOM should not change drastically. Still missing the "expression" features, but they can be added after building the robot! + +> Update 22/01/2025 : The mechanical design is pretty much finalized (fixing some mistakes here and there). The current version does not include all the "expression" features we want to include in the final robot (LEDs for the eyes, a camera, a speaker and a microphone). We are now working on making it walk with reinforcement learning ! + +# Community + +![duck_collage](https://github.com/user-attachments/assets/e240c06e-769f-4c87-b65f-189a442cf1e9) + +Join our discord community ! https://discord.gg/UtJZsgfQGe + +# CAD + +https://cad.onshape.com/documents/64074dfcfa379b37d8a47762/w/3650ab4221e215a4f65eb7fe/e/0505c262d882183a25049d05 + +See [this document](docs/prepare_robot.md) for getting from a onshape design to a simulated robot in MuJoCo (Warning, outdated. Has not been updated in a while) + +# RL stuff + +We are switching to Mujoco Playground, see this [repo](https://github.com/apirrone/Open_Duck_Playground) + +https://github.com/user-attachments/assets/037a1790-7ac1-4140-b154-2c901d20d5f5 + + +## Reference motion generation for imitation learning + +https://github.com/user-attachments/assets/4cb52e17-99a5-47a8-b841-4141596b7afb + +See [this repo](https://github.com/apirrone/Open_Duck_reference_motion_generator) + +## Actuator identification + +We used Rhoban's [BAM](https://github.com/Rhoban/bam) + +# BOM + +https://docs.google.com/spreadsheets/d/1gq4iWWHEJVgAA_eemkTEsshXqrYlFxXAPwO515KpCJc/edit?usp=sharing + +Chinese: https://zihao-ai.feishu.cn/wiki/AfAtw69vRigXaRk5UkbcrAiLnJw?from=from_copylink + +# Build Guide + +Chinese: https://zihao-ai.feishu.cn/wiki/space/7488517034406625281 + +## Print Guide + +See [print_guide](docs/print_guide.md). + +## Assembly Guide + +See [assembly guide (incomplete)](docs/assembly_guide.md). + +# Embedded runtime + +This repo contains the code to run the policies on the onboard computer (Raspberry pi zero 2w) https://github.com/apirrone/Open_Duck_Mini_Runtime + +# Training your own policies + +If you want to train your own policies, and contribute to making the ducks walk nicely, see [this document](docs/sim2real.md) + +> Thanks a lot to HuggingFace and Pollen Robotics for sponsoring this project ! diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/assembly_guide.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/assembly_guide.md new file mode 100644 index 0000000..e264fe5 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/assembly_guide.md @@ -0,0 +1,251 @@ +# Assembly guide + +> Before assembling the duck, you should first [configure your motors](./configure_motors.md) + +## Requirements : + +You will need : +- A soldering iron, and basic electronics tools and skills +- X m3 screws (TODO : add the exact number) +- Some wire +- Loctite Threadlocker blue 243 + +> General note : Everytime you screw something in the motors metal against metal, you want to use a little loctite threadlocker. This will prevent the screws from coming loose due to the vibrations during the operation of the robot. It adds a little time to to the build, but you'll be glad you took the time ;) +> +> Don't use loctite with the plastic screws + +> At any time, you can refer to the CAD here : https://cad.onshape.com/documents/64074dfcfa379b37d8a47762/w/3650ab4221e215a4f65eb7fe/e/0505c262d882183a25049d05 + +## Steps : + +### Assemble the trunk + +Place the bearings in `trunk_bottom` like so, and insert M3 inserts in these holes. It's also a good time to insert the 4 M3 inserts in the bottom of this part to mount body parts later on. + +1 + +Then assamble `trunk_bottom` and `trunk_top`, and screw them together with 2 `M3x10` screws through these holes + +1 + +Mount the middle motor like so and screw it with the plastic screws that came with the motors : + +1 + +Insert `roll_motor_bottom` like this + +1 + + +### Assemble the feet + +Both feet are the same. + +First, assemble `foot_bottom_tpu` with `foot_bottom_pla`. Insert M3 inserts in these holes : + +1 + +And screw the two parts together with two `M3x6` screws. + +Then, insert M3 inserts in these holes in `foot_top` here : + +1 + +And assemble everything like so. Make sure the driver side of the motor is on the `foot_top` part side : + + + + + + +
1 1
+ +You can add the foot switches like this too : + +> You press fit them so that the switch is activated when the foot touches the ground + + + + + + +
1 1
+ + +### Assemble the shins + +Insert M3 Inserts in these holes of `leg_spacer` (on both sides. Insert 4 M3 inserts in total) : + +1 + +Then, first plug the motor cable in the foot's motor, and make it go through the `right_sheet` like so + +1 + +Then assemble like below: + +1 + +### Assemble the thighs + +The thigh is pretty much the same thing, except the `hip_pitch` motor is mounted this way (important for the zero position) + +1 + +### Assemble the hips + +Mount `left_roll_to_pitch` or `right_roll_to_pitch`, here the parts are symmetrical so you have to use the right one. + +1 + +Mount `roll_motor_top` to the `hip_yaw` servo (screw from the bottom). Don't mount the servo to the trunk yet. + + +1 + +Then mount `hip_roll` like this + +1 + +And insert the sub assembly like this + +1 + +Screw everything you can (with the plastic screws provided with the servos) + +You can now mount the leg like this : + +1 + +And do the same for the other leg :) + +Your duck should now look like this + +1 + +### Assemble the neck + +You know the drill + +1 + +### Assemble the head mechanism + +First, mount `head_pitch_to_yaw` like this + +1 + +Then, independently mount `head_yaw_to_roll` and `head_roll_mount` to `head_roll dof` + +1 + +(You can insert `head_bot_plate` and `body_middle_top` now too to avoid having to disassamble the head later) + +Then + +1 + +Then + +1 + +Your duck should now look like this + +1 + +### Mount the servo driver board + +TODO take a photo + +### Mount the IMU + +Like this + +> It's actually better to mount the IMU with the correct natural orientation, which would be flipped along the X axis compared to the pictures below +> In the picture below, the IMU is mounted upside down. +> It probably doesn't really matter a lot if you mount it upside down or not. You can configure how you mounted it later + + + + + + +
1 2
+ +## Electronics + +Here is the global electonics schematic for reference + + + + + + +
1 2
+ +Here is how to wire the feet + +![feet](https://github.com/user-attachments/assets/d3376494-4690-4484-8352-132e6284731a) + + + +### Battery pack + +> To be safe, make sure your cells are charged to the same voltage before placing them in the holder. + + + + + + +
1 2
+ + +### Head + +First, insert the M3 inserts in all these holes + +![image](https://github.com/user-attachments/assets/ef4cd513-6b8d-41fa-9cc3-149fc8333d3e) + +> TODO add instructions for expression features (camera, antennas, eye leds, projector and speaker) + +Then insert the bearing, mount the ear motors and the raspberry pi zero 2w. + +For reference, the inside of the head looks like this now + +![image](https://github.com/user-attachments/assets/91284081-a563-4c02-bb3d-194b3afbc25c) + + +Then assemble the neck with the head like this + +![image](https://github.com/user-attachments/assets/96fd5347-bff7-47e9-a7bd-fde17ab1bcd2) + + +## Body + +First screw on `body_middle_bottom` + +![Capture d’écran du 2025-02-09 12-10-00](https://github.com/user-attachments/assets/081d8840-8e88-4938-9d9a-4d97614e6261) + +Then insert the M3 inserts in all the holes of `body_middle_bottom` and `body_middle_top` on which we'll mount the battery pack and `body_front`. + +Then mount `body_middle_top`, `body_front` and the battery pack + + + + + + +
1 2
+ +Et voila :) + + + + + + +
1 2
+ + +> Now that your duck is fully assembled, you setup the raspberry pi and the runtime software [here](https://github.com/apirrone/Open_Duck_Mini_Runtime) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/configure_motors.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/configure_motors.md new file mode 100644 index 0000000..a2bf15a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/configure_motors.md @@ -0,0 +1,40 @@ +# Configure the motors + +> This sould be done independently on each motor *before* builiding the duck. +> +> During the process, the motor will move to its zero position. You can then install the horn while trying to align it the best you can like in the photo below. (Don't worry if it's not perfect, we will compensate for that later) + +![Capture d’écran du 2025-04-25 09-55-34](https://github.com/user-attachments/assets/e3c4aefa-5e0a-4d4e-89f4-82df9bf30e29) + + +Clone and install (`pip install -e .`) the runtime repo on the `v2` branch : `https://github.com/apirrone/Open_Duck_Mini_Runtime` + +You can either install it on your own computer or on the raspberry pi for the configuration, as you want. You'll just want a way to power the servos, for example, the battery pack. + + +Then for each motor, run the following command : + +```bash +python configure_motor.py --id +``` + +The motors ids are : + +```python +{ + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} +``` diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/feetech_identification.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/feetech_identification.md new file mode 100644 index 0000000..c9dbd45 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/feetech_identification.md @@ -0,0 +1,29 @@ +$\theta_d^f$ : $\theta$ requested in firmware units + +$\epsilon = \theta_d^f - \theta^f$ + +### Error in firmware units + +> $\lambda$ is the duty cycle, aka the **PWM** + + + +$\epsilon^f = \epsilon \frac{4096}{2\pi}$ + +> 12 bits + +### Firmware duty cycle + + +$\lambda^f = K_p \epsilon^f$ + +$\lambda = K_p K_g\epsilon$ + + +$R = 2.5\Omega$ + +------ + +$\lambda = \epsilon K_p K_g$ + +$K_g = \frac{\lambda}{\epsilon K_p }$ \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/head_schematic.xcf b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/head_schematic.xcf new file mode 100644 index 0000000..0141713 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/head_schematic.xcf differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png new file mode 100644 index 0000000..3b39e10 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio new file mode 100644 index 0000000..c60ad8c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio @@ -0,0 +1,396 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png new file mode 100644 index 0000000..d90ebe3 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/prepare_robot.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/prepare_robot.md new file mode 100644 index 0000000..4a4f0c7 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/prepare_robot.md @@ -0,0 +1,123 @@ +# Preparing a robot for simulation in MuJoCo + +## If you designed your robot in OnShape + +### Make sure to design you robot according to onshape-to-robot constraints +https://onshape-to-robot.readthedocs.io/en/latest/design.html + + +The urdf will contain frames named `closing_<...>_1` and `closing_<...>_2` that you can use to close the loops in the mjcf file. + +### Get get robot urdf from onshape + +Run + +```bash +$ onshape-to-robot robots/bd1/ +``` + +#### (Optional) If you have closing loops, follow the instructions for handling them in the documentation of onshape-to-robotn then: + +In `robot.urdf`, add : +```xml + + + + + ... + +``` + +# Convert URDF to MJCF (MuJoCo) + +## Get MuJoCo binaries + +Download mujoco binaries somewhere https://github.com/google-deepmind/mujoco/releases + +unpack and run + +```bash +$ ./compile robot.urdf robot.xml +``` + +## In robot.xml, add actuators : +Example : +```xml + + + + + + + + + + + + + + + +``` + +## Add a freejoint + +encapsulate the body in a freejoint +```xml + + + + ... + ... + + +``` + +## (Optional) Constrain closing loop + +Add the following to the mjcf file +```xml + + + +``` + +the x, y, z values can be found in the .urdf + +```xml + + + ... + +``` + +## Setup collision groups, damping and friction +/!\ remove actuatorfrcrange in joints +Put that inside the bracket +```xml + + + + + + + ... + ... + +``` + +still need to add : +- change frames to sites + + +## Visualize + +```bash +$ python3 -m mujoco.viewer --mjcf=/scene.xml +``` + +or + +```bash +$ /bin/simulate /scene.xml +``` diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/print_guide.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/print_guide.md new file mode 100644 index 0000000..aa64eba --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/print_guide.md @@ -0,0 +1,43 @@ +# Print guide + +You can find the `.stl` files under the `print/` directory at the root of this repo. + +All the parts are printed in standard PLA with 15% infill, except for `foot_bottom_tpu.stl`, which is to be printed in TPU at 40% infill. + +## Parts to print +- foot_top.stl x2 +- foot_side.stl x2 +- foot_bottom_pla.stl x2 +- foot_bottom_tpu.stl x2 (TPU) +- knee_to_ankle_left_sheet.stl x4 +- knee_to_ankle_right_sheet.stl x4 +- leg_spacer.stl x4 +- left_roll_to_pitch.stl x1 +- right_roll_to_pitch.stl x1 +- roll_motor_bottom.stl x2 +- roll_motor_top.stl x2 +- trunk_bottom.stl x1 +- trunk_top.stl x1 +- neck_left_sheet.stl x1 +- neck_right_sheet.stl x1 +- head_pitch_to_yaw.stl x1 +- head_yaw_to_roll.stl x1 +- head_roll_mount.stl x1 +- head.stl x1 +- head_bot_sheet.stl x1 +- left_antenna_holder.stl x1 +- right_antenna_holder.stl x1 +- left_cache.stl x1 +- right_cache.stl x1 +- body_front.stl x1 +- body_middle_bottom.stl x1 +- body_middle_top.stl x1 +- body_back.stl x1 +- battery_pack_lid.stl x1 +- bulb.stl x1 +- flash_light_module.stl x1 +- flash_reflector_interface.stl x1 +- left_eye.stl x1 +- right_eye.stl x1 +- speaker_interface.stl x1 +- speaker_stand.stl x1 diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/sim2real.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/sim2real.md new file mode 100644 index 0000000..1cce56b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/sim2real.md @@ -0,0 +1,45 @@ +> Not finalized yet + +# Training policies that transfer to the real robot (sim2real) + +We want to train policies that transfer well to the real robot. This is the sim2real problem. It's a hard problem, especially for us since we are using cheap servomotors that are hard to model and not overly powerful. + +Below, I'll roughly explain the steps we went through to get there, but you won't have to do everything again yourself since we provide the results of each process. + +## Make an accurate model of the robot (URDF/MJCF) + +### Robot structure + +In the [Onhape document](https://cad.onshape.com/documents/64074dfcfa379b37d8a47762/w/3650ab4221e215a4f65eb7fe/e/0505c262d882183a25049d05), we specify the material of each part. But to be more accurate, because we print with infill we override the mass of the parts with the (pretty accurate) estimation of the slicer. + +We use [onshape-to-robot](https://github.com/Rhoban/onshape-to-robot) to export URDF/MJCF descriptions. For MJX, we have to make a lightweight model, see our [config.json](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/xmls/config.json). + +This gives us a MJCF (Mujoco format) [description of the robot](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2.xml) which describes the masses and moments of inertia of the full robot. + +### Motors + +Another very important part of having an accurate model is modeling the motors's behavior. We use [BAM](https://github.com/Rhoban/bam/) for that. You don't have to go through the identification process yourself, we provide the results [here](https://github.com/Rhoban/bam/tree/main/params/feetech_sts3215_7_4V) + +It's critical that the simulator simulates the motors accurately, because we will train a policy (a neural network) to output motor positions based on sensory inputs (motors positions/speeds, imu and feet sensors). If the motors behave differently in the simulation than in the real world, the policy won't work, or at worst, produce chaotic movements. + +`BAM` allows us to export the main identified parameters to mujoco units (using `bam.to_mujoco`). These values are the ones we set in out mjcf model for the actuators and joints properties. + +- damping +- kp +- frictionloss +- armature +- forcerange + +## Training policies + +We use our own [mujoco playground](https://github.com/google-deepmind/mujoco_playground) based framework, [Open Duck Playground](https://github.com/apirrone/Open_Duck_Playground) + +In the [joystick](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/joystick.py) env, you can try to enable/disable different rewards, write your own, play with the weights, noise, randomization etc. + +We obtained good results by implementing the imitation reward described by Disney in their [BDX paper](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/joystick.py). + +To use this reward, we need reference motion. We made [this repo](https://github.com/apirrone/Open_Duck_reference_motion_generator) to generate such motion using a parametric walk engine. Following the instructions there, you can generate a `polynomial_coefficients.pkl` file which contains the reference motions. There is already such a file in the playground repo under the `data/` directory. + +Once your policy is trained, you can try to run it on the real robot using [this script](https://github.com/apirrone/Open_Duck_Mini_Runtime/blob/v2/scripts/v2_rl_walk_mujoco.py) in the runtime repo. Make sure you completed all the steps in the [checklist](https://github.com/apirrone/Open_Duck_Mini_Runtime/blob/v2/checklist.md) before running this. + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.drawio b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.drawio new file mode 100644 index 0000000..f533332 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.drawio @@ -0,0 +1,503 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.png b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.png new file mode 100644 index 0000000..2c60f39 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.png differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/README.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/README.md new file mode 100644 index 0000000..3ce568b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/README.md @@ -0,0 +1,4 @@ +# Current state of things + +Tried to clean up `record_episodes_hdf5.py` by using `bdx_mujoco_server.py` but got weird behavior with the walk engine. + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py new file mode 100644 index 0000000..c80de03 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py @@ -0,0 +1,166 @@ +import argparse +import os +import time +from glob import glob +from typing import Dict, List + +import h5py +import mujoco +import mujoco.viewer +import numpy as np +import placo +from scipy.spatial.transform import Rotation as R + +from mini_bdx.bdx_mujoco_server import BDXMujocoServer +from mini_bdx.utils.mujoco_utils import check_contact + +# from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser("Record episodes") +parser.add_argument( + "-n", + "--session_name", + type=str, + required=True, +) +parser.add_argument( + "-r", + "--sampling_rate", + type=int, + required=False, + default=30, + help="Sampling rate in Hz", +) +parser.add_argument( + "-l", + "--episode_length", + type=int, + required=False, + default=10, + help="Episode length in seconds", +) +args = parser.parse_args() + +session_name = args.session_name + "_raw" +session_path = os.path.join("data", session_name) +os.makedirs(session_path, exist_ok=True) + +bdx_mujoco_server = BDXMujocoServer( + model_path="/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx" +) + +recording = False +data_dict: Dict[str, List[float]] = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], +} + + +def key_callback(keycode): + if keycode == 257: # enter + start_stop_recording() + + +def start_stop_recording(): + global recording, data_dict + recording = not recording + if recording: + print("Recording started") + pass + else: + print("Recording stopped") + episode_id = len(glob(f"{session_path}/*.hdf5")) + episode_path = os.path.join(session_path, f"episode_{episode_id}.hdf5") + print(f"Saving episode in {episode_path} ...") + max_timesteps = len(data_dict["/action"]) + with h5py.File( + episode_path, + "w", + rdcc_nbytes=1024**2 * 2, + ) as root: + obs = root.create_group("observations") + obs.create_dataset("qpos", (max_timesteps, 20)) + obs.create_dataset("qvel", (max_timesteps, 19)) + root.create_dataset("action", (max_timesteps, 13)) + + for name, array in data_dict.items(): + root[name][...] = array + print("Done") + data_dict = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], + } + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +walking = True + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine(robot) + +bdx_mujoco_server.start() + +try: + prev = bdx_mujoco_server.data.time + last = bdx_mujoco_server.data.time + episode_start = bdx_mujoco_server.data.time + # start_stop_recording() + while True: + dt = bdx_mujoco_server.data.time - prev + + # if bdx_mujoco_server.data.time - episode_start > args.episode_length: + # start_stop_recording() + # episode_start = bdx_mujoco_server.data.time + # start_stop_recording() + + # Update the walk engine + right_contact, left_contact = bdx_mujoco_server.get_feet_contact() + gyro, accelerometer = bdx_mujoco_server.get_imu() + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + # Get the angles from the walk engine + angles = walk_engine.get_angles() + bdx_mujoco_server.send_action(list(angles.values())) + + # if recording and bdx_mujoco_server.data.time - last > (1 / args.sampling_rate): + # last = bdx_mujoco_server.data.time + # action = list(angles.values()) + # qpos, qvel = bdx_mujoco_server.get_state() + + # data_dict["/action"].append(action) + # data_dict["/observations/qpos"].append(qpos) + # data_dict["/observations/qvel"].append(qvel) + + prev = bdx_mujoco_server.data.time + time.sleep(0.0001) + +except KeyboardInterrupt: + print("stop") + exit() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py new file mode 100644 index 0000000..c7f56ff --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py @@ -0,0 +1,232 @@ +import argparse +import os +import time +from glob import glob +from typing import Dict, List + +import h5py +import mujoco +import mujoco.viewer +import numpy as np +import placo +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.xbox_controller import XboxController + +# from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser("Record episodes") +parser.add_argument( + "-n", + "--session_name", + type=str, + required=True, +) +parser.add_argument( + "-r", + "--sampling_rate", + type=int, + required=False, + default=30, + help="Sampling rate in Hz", +) +parser.add_argument( + "-l", + "--episode_length", + type=int, + required=False, + default=10, + help="Episode length in seconds", +) +args = parser.parse_args() + +session_name = args.session_name + "_raw" +session_path = os.path.join("data", session_name) +os.makedirs(session_path, exist_ok=True) + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + +recording = False +data_dict: Dict[str, List[float]] = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], + "/observations/target": [], # [target_step_size_x, target_step_size_y, target_yaw, target_head_pitch, target_head_yaw, target_head_z_offset] + "/observations/feet_contact": [], # [right_contact, left_contact] +} + + +def key_callback(keycode): + if keycode == 257: # enter + start_stop_recording() + + +def start_stop_recording(): + global recording, data_dict + recording = not recording + if recording: + print("Recording started") + pass + else: + print("Recording stopped") + episode_id = len(glob(f"{session_path}/*.hdf5")) + episode_path = os.path.join(session_path, f"episode_{episode_id}.hdf5") + print(f"Saving episode in {episode_path} ...") + max_timesteps = len(data_dict["/action"]) + with h5py.File( + episode_path, + "w", + rdcc_nbytes=1024**2 * 2, + ) as root: + obs = root.create_group("observations") + obs.create_dataset("qpos", (max_timesteps, 20)) + obs.create_dataset("qvel", (max_timesteps, 19)) + obs.create_dataset("target", (max_timesteps, 6)) + obs.create_dataset("feet_contact", (max_timesteps, 2)) + root.create_dataset("action", (max_timesteps, 13)) + + for name, array in data_dict.items(): + root[name][...] = array + print("Done") + data_dict = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], + "/observations/target": [], + "/observations/feet_contact": [], + } + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +walking = True +xbox = XboxController() + + +def xbox_input(): + global target_velocity, target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.5: + target_head_pitch = inputs["r_y"] * np.deg2rad(45) + target_head_yaw = -inputs["r_x"] * np.deg2rad(120) + target_head_z_offset = inputs["r_trigger"] * 0.08 + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + target_velocity = np.array([-inputs["l_y"], inputs["l_x"], -inputs["r_x"]]) + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine(robot) + + +def get_imu(data): + + rot_mat = np.array(data.body("base").xmat).reshape(3, 3) + gyro = R.from_matrix(rot_mat).as_euler("xyz") + + accelerometer = np.array(data.body("base").cvel)[3:] + + return gyro, accelerometer + + +def get_feet_contact(data, model): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +try: + prev = data.time + last = data.time + episode_start = data.time + # start_stop_recording() + while True: + dt = data.time - prev + xbox_input() + + # if data.time - episode_start > args.episode_length: + # start_stop_recording() + # episode_start = data.time + # start_stop_recording() + + # Update the walk engine + right_contact, left_contact = get_feet_contact(data, model) + gyro, accelerometer = get_imu(data) + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + # Get the angles from the walk engine + angles = walk_engine.get_angles() + + # Apply the angles to the robot + data.ctrl[:] = list(angles.values()) + + if recording and data.time - last > (1 / args.sampling_rate): + last = data.time + action = list(angles.values()) + qpos = data.qpos.flat.copy() + qvel = data.qvel.flat.copy() + + # TODO merge all observations into one array "state" ? + # Don't understand very well how it is handled in lerobot + data_dict["/action"].append(action) + data_dict["/observations/qpos"].append(qpos) + data_dict["/observations/qvel"].append(qvel) + data_dict["/observations/target"].append( + [ + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + ] + ) + data_dict["/observations/feet_contact"].append( + [right_contact, left_contact] + ) + + prev = data.time + mujoco.mj_step(model, data) + viewer.sync() + # time.sleep(model.opt.timestep) + time.sleep(0.001) + +except KeyboardInterrupt: + print("stop") + exit() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/README.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/README.md new file mode 100644 index 0000000..6e99a1d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/README.md @@ -0,0 +1,3 @@ +# Experiments + +These are undocumented experiments, some of them very old. I wouldn't recommend trying to run them :) \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env.py new file mode 100644 index 0000000..de51cc4 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env.py @@ -0,0 +1,401 @@ +import numpy as np +import placo +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +from mini_bdx.walk_engine import WalkEngine + +init_pos = { + "right_hip_yaw": 0, + "right_hip_roll": 0, + "right_hip_pitch": np.deg2rad(45), + "right_knee_pitch": np.deg2rad(-90), + "right_ankle_pitch": np.deg2rad(45), + "left_hip_yaw": 0, + "left_hip_roll": 0, + "left_hip_pitch": np.deg2rad(45), + "left_knee_pitch": np.deg2rad(-90), + "left_ankle_pitch": np.deg2rad(45), + "head_pitch1": np.deg2rad(-45), + "head_pitch2": np.deg2rad(-45), + "head_yaw": 0, +} + + +dofs = { + "right_hip_yaw": 0, + "right_hip_roll": 1, + "right_hip_pitch": 2, + "right_knee_pitch": 3, + "right_ankle_pitch": 4, + "left_hip_yaw": 5, + "left_hip_roll": 6, + "left_hip_pitch": 7, + "left_knee_pitch": 8, + "left_ankle_pitch": 9, + "head_pitch1": 10, + "head_pitch2": 11, + "head_yaw": 12, +} + + +class BDXEnv(MujocoEnv, utils.EzPickle): + """ + ## Action space + + | Num | Action | Control Min | Control Max | Name (in corresponding XML file) | Joint | Unit | + | ---- | ----------------------------------------------------------------- | ----------- | ----------- | -------------------------------- | -------- |------------- | + | 0 | Set position of right_hip_yaw | -0.58TODO | 0.58TODO | right_hip_yaw | cylinder | pos (rad) | + | 1 | Set position of right_hip_roll | -0.58TODO | 0.58TODO | right_hip_roll | cylinder | pos (rad) | + | 2 | Set position of right_hip_pitch | -0.58TODO | 0.58TODO | right_hip_pitch | cylinder | pos (rad) | + | 3 | Set position of right_knee_pitch | -0.58TODO | 0.58TODO | right_knee_pitch | cylinder | pos (rad) | + | 4 | Set position of right_ankle_pitch | -0.58TODO | 0.58TODO | right_ankle_pitch | cylinder | pos (rad) | + | 5 | Set position of left_hip_yaw | -0.58TODO | 0.58TODO | left_hip_yaw | cylinder | pos (rad) | + | 6 | Set position of left_hip_roll | -0.58TODO | 0.58TODO | left_hip_roll | cylinder | pos (rad) | + | 7 | Set position of left_hip_pitch | -0.58TODO | 0.58TODO | left_hip_pitch | cylinder | pos (rad) | + | 8 | Set position of left_knee_pitch | -0.58TODO | 0.58TODO | left_knee_pitch | cylinder | pos (rad) | + | 9 | Set position of left_ankle_pitch | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch1 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch2 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_yaw | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + + + ## Observation space + + | Num | Observation | Min | Max | Name (in corresponding XML file) | Joint | Unit | + | --- | -------------------------------------------------------- | ---- | --- | -------------------------------- | -------- | ------------------------ | + | 0 | Rotation right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | angle (rad) | + | 1 | Rotation right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | angle (rad) | + | 2 | Rotation right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | angle (rad) | + | 3 | Rotation right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | angle (rad) | + | 4 | Rotation right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | angle (rad) | + | 5 | Rotation left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | angle (rad) | + | 6 | Rotation left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | angle (rad) | + | 7 | Rotation left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | angle (rad) | + | 8 | Rotation left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | angle (rad) | + | 9 | Rotation left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | angle (rad) | + | 10 | Rotation head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | angle (rad) | + | 11 | Rotation head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | angle (rad) | + | 12 | Rotation head_yaw | -Inf | Inf | head_yaw | cylinder | angle (rad) | + | 13 | Velocity of right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | speed (rad/s) | + | 14 | Velocity of right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | speed (rad/s) | + | 15 | Velocity of right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | speed (rad/s) | + | 16 | Velocity of right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | speed (rad/s) | + | 17 | Velocity of right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | speed (rad/s) | + | 18 | Velocity of left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | speed (rad/s) | + | 19 | Velocity of left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | speed (rad/s) | + | 20 | Velocity of left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | speed (rad/s) | + | 21 | Velocity of left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | speed (rad/s) | + | 22 | Velocity of left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | speed (rad/s) | + | 23 | Velocity of head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | speed (rad/s) | + | 24 | Velocity of head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | speed (rad/s) | + | 25 | Velocity of head_yaw | -Inf | Inf | head_yaw | cylinder | speed (rad/s) | + + + | 26 | roll angular velocity | -Inf | Inf | | | |e + | 27 | pitch angular velocity | -Inf | Inf | | | | + | 28 | yaw angular velocity | -Inf | Inf | | | | + | 29 | current x linear velocity | -Inf | Inf | | | | + | 30 | current y linear velocity | -Inf | Inf | | | | + | 31 | current z linear velocity | -Inf | Inf | | | | + | 32 | current x target linear velocity | -Inf | Inf | | | | + | 33 | current y target linear velocity | -Inf | Inf | | | | + | 34 | current yaw target angular velocity | -Inf | Inf | | | | + + | 35 | t-1 right_hip_yaw rotation error | -Inf | Inf | | | | + | 36 | t-1 right_hip_roll rotation error | -Inf | Inf | | | | + | 37 | t-1 right_hip_pitch rotation error | -Inf | Inf | | | | + | 38 | t-1 right_knee_pitch rotation error | -Inf | Inf | | | | + | 39 | t-1 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 40 | t-1 left_hip_yaw rotation error | -Inf | Inf | | | | + | 41 | t-1 left_hip_roll rotation error | -Inf | Inf | | | | + | 42 | t-1 left_hip_pitch rotation error | -Inf | Inf | | | | + | 43 | t-1 left_knee_pitch rotation error | -Inf | Inf | | | | + | 44 | t-1 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 45 | t-1 head_pitch1 rotation error | -Inf | Inf | | | | + | 46 | t-1 head_pitch2 rotation error | -Inf | Inf | | | | + | 47 | t-1 head_yaw rotation error | -Inf | Inf | | | | + | 48 | t-2 right_hip_yaw rotation error | -Inf | Inf | | | | + | 49 | t-2 right_hip_roll rotation error | -Inf | Inf | | | | + | 50 | t-2 right_hip_pitch rotation error | -Inf | Inf | | | | + | 51 | t-2 right_knee_pitch rotation error | -Inf | Inf | | | | + | 52 | t-2 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 53 | t-2 left_hip_yaw rotation error | -Inf | Inf | | | | + | 54 | t-2 left_hip_roll rotation error | -Inf | Inf | | | | + | 55 | t-2 left_hip_pitch rotation error | -Inf | Inf | | | | + | 56 | t-2 left_knee_pitch rotation error | -Inf | Inf | | | | + | 57 | t-2 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 58 | t-2 head_pitch1 rotation error | -Inf | Inf | | | | + | 59 | t-2 head_pitch2 rotation error | -Inf | Inf | | | | + | 60 | t-2 head_yaw rotation error | -Inf | Inf | | | | + | 61 | t-3 right_hip_yaw rotation error | -Inf | Inf | | | | + | 62 | t-3 right_hip_roll rotation error | -Inf | Inf | | | | + | 63 | t-3 right_hip_pitch rotation error | -Inf | Inf | | | | + | 64 | t-3 right_knee_pitch rotation error | -Inf | Inf | | | | + | 65 | t-3 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 66 | t-3 left_hip_yaw rotation error | -Inf | Inf | | | | + | 67 | t-3 left_hip_roll rotation error | -Inf | Inf | | | | + | 68 | t-3 left_hip_pitch rotation error | -Inf | Inf | | | | + | 69 | t-3 left_knee_pitch rotation error | -Inf | Inf | | | | + | 70 | t-3 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 71 | t-3 head_pitch1 rotation error | -Inf | Inf | | | | + | 72 | t-3 head_pitch2 rotation error | -Inf | Inf | | | | + | 73 | t-3 head_yaw rotation error | -Inf | Inf | | | | + + | 74 | left foot in contact with the floor | -Inf | Inf | | | | + | 75 | right foot in contact with the floor | -Inf | Inf | | | | + | 76 | t | -Inf | Inf | | | | + + | x74 | sinus | -Inf | Inf | | | | + + """ + + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 100, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + observation_space = Box(low=-np.inf, high=np.inf, shape=(77,), dtype=np.float64) + self.target_velocity = np.asarray([1, 0, 0]) # x, y, yaw + self.joint_history_length = 3 + self.joint_error_history = self.joint_history_length * [13 * [0]] + self.joint_ctrl_history = self.joint_history_length * [13 * [0]] + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + + self.prev_t = 0 + + robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions + ) + self.walk_engine = WalkEngine(robot) + + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + 5, + observation_space=observation_space, + **kwargs, + ) + # self.frame_skip = 30 + + def check_contact(self, body1_name, body2_name): + body1_id = self.data.body(body1_name).id + body2_id = self.data.body(body2_name).id + + for i in range(self.data.ncon): + contact = self.data.contact[i] + + if ( + self.model.geom_bodyid[contact.geom1] == body1_id + and self.model.geom_bodyid[contact.geom2] == body2_id + ) or ( + self.model.geom_bodyid[contact.geom1] == body2_id + and self.model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + def smoothness_reward(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(13): + smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + t0[i] - 2 * t_minus1[i] + t_minus2[i] + ) + + return -smooth + + def feet_contact_reward(self): + + return self.right_foot_in_contact + self.left_foot_in_contact + + def velocity_tracking_reward(self): + base_velocity = list(self.data.body("base").cvel[3:][:2]) + [ + self.data.body("base").cvel[:3][2] + ] + base_velocity = np.asarray(base_velocity) + return np.exp(-np.square(base_velocity - self.target_velocity).sum()) + + def joint_angle_deviation_reward(self): + current_ctrl = self.data.ctrl + init_ctrl = np.array(list(init_pos.values())) + return -np.square(current_ctrl - init_ctrl).sum() + + def walking_height_reward(self): + return ( + -np.square((self.get_body_com("base")[2] - 0.14)) * 100 + ) # "normal" walking height is about 0.14m + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def is_terminated(self) -> bool: + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + upright = np.array([0, 0, 1]) + return ( + self.data.body("base").xpos[2] < 0.08 or np.dot(upright, Z_vec) <= 0 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def follow_walk_engine_reward(self, dt): + self.walk_engine.update( + True, + [0, 0, 0], + [0, 0, 0], + self.left_foot_in_contact, + self.right_foot_in_contact, + 0.03, + 0, + 0, + 0, + 0, + 0, + dt, + ignore_feet_contact=True, + ) + angles = self.walk_engine.get_angles() + return -np.square(self.data.ctrl - list(angles.values())).sum() + + def step(self, a): + # https://www.nature.com/articles/s41598-023-38259-7.pdf + + # add random force + # if np.random.rand() < 0.05: + # self.data.xfrc_applied[self.data.body("base").id][:3] = [ + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # ] # absolute + + t = self.data.time + dt = t - self.prev_t + + self.do_simulation(a, 1) + # self.do_simulation( + # a, self.frame_skip + # ) # TODO maybe set frame_skip to 1 when bootstrapping with walk engine + + self.right_foot_in_contact = self.check_contact("foot_module", "floor") + self.left_foot_in_contact = self.check_contact("foot_module_2", "floor") + + reward = ( + 0.005 # time reward + # + 0.1 * self.walking_height_reward() + # + 0.1 * self.upright_reward() + # + 0.1 * self.velocity_tracking_reward() + # + 0.1 * self.smoothness_reward() + # + 0.1 * self.feet_contact_reward() + # + 0.1 * self.joint_angle_deviation_reward() + # + 0.01 * self.follow_walk_engine_reward(dt) + ) + + # print("time reward", 0.005) + # print("walking height reward", 0.1 * self.walking_height_reward()) + # print("upright reward", 0.1 * self.upright_reward()) + # print("velocity tracking reward", 0.1 * self.velocity_tracking_reward()) + # # print("smoothness reward", 0.1 * self.smoothness_reward()) + # print("feet contact reward", 0.1 * self.feet_contact_reward()) + # # print("joint angle deviation reward", 0.1 * self.joint_angle_deviation_reward()) + # # print("follow walk engine reward", 0.01 * self.follow_walk_engine_reward(dt)) + # print("reward", reward) + # print("===") + + # if self.is_terminated(): + # reward = -10 + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + self.prev_t = t + + return ( + ob, + reward, + self.is_terminated(), # terminated + False, # truncated + dict( + time_reward=0.5, + walking_height_reward=0.5 * self.walking_height_reward(), + upright_reward=0.5 * self.upright_reward(), + velocity_tracking_reward=1.0 * self.velocity_tracking_reward(), + smoothness_reward=0.1 * self.smoothness_reward(), + feet_contact_reward=0.2 * self.feet_contact_reward(), + # joint_angle_deviation_reward=0.1 * self.joint_angle_deviation_reward(), + ), + ) + + def reset_model(self): + self.prev_t = self.data.time + + # self.model.opt.gravity[:] = [0, 0, 0] # no gravity + + qpos = self.data.qpos + + # LATEST + # added randomization to the initial position + for i in range(7, len(qpos)): + qpos[i] = np.random.uniform(-0.3, 0.3) + + self.init_qpos = qpos.copy().flatten() + + # Randomize later + # self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.target_velocity = np.asarray([2, 0, 0]) # x, y, yaw + + self.set_state(qpos, self.init_qvel) + return self._get_obs() + + def goto_init(self): + self.data.qpos[:] = self.init_qpos[:] + for i, value in enumerate(init_pos.values()): + self.data.qpos[i + 7] = value + + def _get_obs(self): + + joints_rotations = self.data.qpos[7 : 7 + 13] + joints_velocities = self.data.qvel[6 : 6 + 13] + + joints_error = self.data.ctrl - self.data.qpos[7 : 7 + 13] + self.joint_error_history.append(joints_error) + self.joint_error_history = self.joint_error_history[ + -self.joint_history_length : + ] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.joint_ctrl_history.append(self.data.ctrl.copy()) + self.joint_ctrl_history = self.joint_ctrl_history[-self.joint_history_length :] + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocity, + np.array(self.joint_error_history).flatten(), + [self.left_foot_in_contact, self.right_foot_in_contact], + [self.data.time], + ] + ) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env_humanoid.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env_humanoid.py new file mode 100644 index 0000000..0b68c2c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env_humanoid.py @@ -0,0 +1,218 @@ +# Mimicking the humanoidv4 reward + +import numpy as np +import placo +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +from mini_bdx.walk_engine import WalkEngine + + +def mass_center(model, data): + mass = np.expand_dims(model.body_mass, axis=1) + xpos = data.xipos + return (np.sum(mass * xpos, axis=0) / np.sum(mass))[0:2].copy() + + +class BDXEnv(MujocoEnv, utils.EzPickle): + """ + ## Action space + + | Num | Action | Control Min | Control Max | Name (in corresponding XML file) | Joint | Unit | + | ---- | ----------------------------------------------------------------- | ----------- | ----------- | -------------------------------- | -------- |------------- | + | 0 | Set position of right_hip_yaw | -0.58TODO | 0.58TODO | right_hip_yaw | cylinder | pos (rad) | + | 1 | Set position of right_hip_roll | -0.58TODO | 0.58TODO | right_hip_roll | cylinder | pos (rad) | + | 2 | Set position of right_hip_pitch | -0.58TODO | 0.58TODO | right_hip_pitch | cylinder | pos (rad) | + | 3 | Set position of right_knee_pitch | -0.58TODO | 0.58TODO | right_knee_pitch | cylinder | pos (rad) | + | 4 | Set position of right_ankle_pitch | -0.58TODO | 0.58TODO | right_ankle_pitch | cylinder | pos (rad) | + | 5 | Set position of left_hip_yaw | -0.58TODO | 0.58TODO | left_hip_yaw | cylinder | pos (rad) | + | 6 | Set position of left_hip_roll | -0.58TODO | 0.58TODO | left_hip_roll | cylinder | pos (rad) | + | 7 | Set position of left_hip_pitch | -0.58TODO | 0.58TODO | left_hip_pitch | cylinder | pos (rad) | + | 8 | Set position of left_knee_pitch | -0.58TODO | 0.58TODO | left_knee_pitch | cylinder | pos (rad) | + | 9 | Set position of left_ankle_pitch | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch1 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch2 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_yaw | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + + + ## Observation space + OBSOLETE + + | Num | Observation | Min | Max | Name (in corresponding XML file) | Joint | Unit | + | --- | -------------------------------------------------------- | ---- | --- | -------------------------------- | -------- | ------------------------ | + | 0 | Rotation right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | angle (rad) | + | 1 | Rotation right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | angle (rad) | + | 2 | Rotation right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | angle (rad) | + | 3 | Rotation right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | angle (rad) | + | 4 | Rotation right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | angle (rad) | + | 5 | Rotation left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | angle (rad) | + | 6 | Rotation left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | angle (rad) | + | 7 | Rotation left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | angle (rad) | + | 8 | Rotation left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | angle (rad) | + | 9 | Rotation left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | angle (rad) | + | 10 | Rotation head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | angle (rad) | + | 11 | Rotation head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | angle (rad) | + | 12 | Rotation head_yaw | -Inf | Inf | head_yaw | cylinder | angle (rad) | + | 13 | Velocity of right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | speed (rad/s) | + | 14 | Velocity of right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | speed (rad/s) | + | 15 | Velocity of right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | speed (rad/s) | + | 16 | Velocity of right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | speed (rad/s) | + | 17 | Velocity of right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | speed (rad/s) | + | 18 | Velocity of left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | speed (rad/s) | + | 19 | Velocity of left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | speed (rad/s) | + | 20 | Velocity of left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | speed (rad/s) | + | 21 | Velocity of left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | speed (rad/s) | + | 22 | Velocity of left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | speed (rad/s) | + | 23 | Velocity of head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | speed (rad/s) | + | 24 | Velocity of head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | speed (rad/s) | + | 25 | Velocity of head_yaw | -Inf | Inf | head_yaw | cylinder | speed (rad/s) | + | 26 | roll angular velocity | -Inf | Inf | | | | + | 27 | pitch angular velocity | -Inf | Inf | | | | + | 28 | yaw angular velocity | -Inf | Inf | | | | + | 29 | current x linear velocity | -Inf | Inf | | | | + | 30 | current y linear velocity | -Inf | Inf | | | | + | 31 | current z linear velocity | -Inf | Inf | | | | + + | 32 | left foot in contact with the floor | -Inf | Inf | | | | + | 33 | right foot in contact with the floor | -Inf | Inf | | | | + """ + + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 100, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + observation_space = Box(low=-np.inf, high=np.inf, shape=(41,), dtype=np.float64) + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + + self._healthy_z_range = (0.1, 0.2) + + self._forward_reward_weight = 1.25 + self._ctrl_cost_weight = 0.3 + self.healthy_reward = 5.0 + + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + 5, # frame_skip TODO set to 1 to test + observation_space=observation_space, + **kwargs, + ) + + def check_contact(self, body1_name, body2_name): + body1_id = self.data.body(body1_name).id + body2_id = self.data.body(body2_name).id + + for i in range(self.data.ncon): + contact = self.data.contact[i] + + if ( + self.model.geom_bodyid[contact.geom1] == body1_id + and self.model.geom_bodyid[contact.geom2] == body2_id + ) or ( + self.model.geom_bodyid[contact.geom1] == body2_id + and self.model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + def control_cost(self): + control_cost = self._ctrl_cost_weight * np.sum(np.square(self.data.ctrl)) + return control_cost + + @property + def is_healthy(self): + min_z, max_z = self._healthy_z_range + is_healthy = min_z < self.data.qpos[2] < max_z + + return is_healthy + + @property + def terminated(self): + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + upright = np.array([0, 0, 1]) + return not self.is_healthy or np.dot(upright, Z_vec) <= 0 + + def step(self, a): + xy_position_before = mass_center(self.model, self.data) + self.do_simulation(a, self.frame_skip) + xy_position_after = mass_center(self.model, self.data) + xy_velocity = (xy_position_after - xy_position_before) / self.dt + x_velocity, y_velocity = xy_velocity + + self.right_foot_in_contact = self.check_contact("foot_module", "floor") + self.left_foot_in_contact = self.check_contact("foot_module_2", "floor") + + ctrl_cost = self.control_cost() + moving_cost = 1.0 * (abs(x_velocity) + np.abs(x_velocity)) + + forward_reward = self._forward_reward_weight * x_velocity + healthy_reward = self.healthy_reward + + # rewards = forward_reward + healthy_reward + rewards = healthy_reward + reward = rewards - ctrl_cost - moving_cost + + # print("healthy", healthy_reward) + # print("ctrl", ctrl_cost) + # print("moving", moving_cost) + # print(("total reward", reward)) + # print("-") + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + return (ob, reward, self.terminated, False, {}) + + def reset_model(self): + noise_low = -1e-2 + noise_high = 1e-2 + + qpos = self.init_qpos + self.np_random.uniform( + low=noise_low, high=noise_high, size=self.model.nq + ) + qvel = self.init_qvel + self.np_random.uniform( + low=noise_low, high=noise_high, size=self.model.nv + ) + self.set_state(qpos, qvel) + + observation = self._get_obs() + return observation + + def _get_obs(self): + + position = ( + self.data.qpos.flat.copy() + ) # position/rotation of trunk + position of all joints + velocity = ( + self.data.qvel.flat.copy() + ) # positional/angular velocity of trunk + of all joints + + # com_inertia = self.data.cinert.flat.copy() + # com_velocity = self.data.cvel.flat.copy() + + # actuator_forces = self.data.qfrc_actuator.flat.copy() + # external_contact_forces = self.data.cfrc_ext.flat.copy() + + obs = np.concatenate( + [ + position, + velocity, + [self.left_foot_in_contact, self.right_foot_in_contact], + ] + ) + # print("OBS SIZE", len(obs)) + return obs diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/.gitignore b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/.gitignore new file mode 100644 index 0000000..314f02b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/.gitignore @@ -0,0 +1 @@ +*.txt \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/env.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/env.py new file mode 100644 index 0000000..16bd3fb --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/env.py @@ -0,0 +1,389 @@ +import numpy as np +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +FRAME_SKIP = 10 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + """ + ## Action space + + | Num | Action | Control Min | Control Max | Name (in corresponding XML file) | Joint | Unit | + | ---- | ----------------------------------------------------------------- | ----------- | ----------- | -------------------------------- | -------- |------------- | + | 0 | Set position of right_hip_yaw | -0.58TODO | 0.58TODO | right_hip_yaw | cylinder | pos (rad) | + | 1 | Set position of right_hip_roll | -0.58TODO | 0.58TODO | right_hip_roll | cylinder | pos (rad) | + | 2 | Set position of right_hip_pitch | -0.58TODO | 0.58TODO | right_hip_pitch | cylinder | pos (rad) | + | 3 | Set position of right_knee_pitch | -0.58TODO | 0.58TODO | right_knee_pitch | cylinder | pos (rad) | + | 4 | Set position of right_ankle_pitch | -0.58TODO | 0.58TODO | right_ankle_pitch | cylinder | pos (rad) | + | 5 | Set position of left_hip_yaw | -0.58TODO | 0.58TODO | left_hip_yaw | cylinder | pos (rad) | + | 6 | Set position of left_hip_roll | -0.58TODO | 0.58TODO | left_hip_roll | cylinder | pos (rad) | + | 7 | Set position of left_hip_pitch | -0.58TODO | 0.58TODO | left_hip_pitch | cylinder | pos (rad) | + | 8 | Set position of left_knee_pitch | -0.58TODO | 0.58TODO | left_knee_pitch | cylinder | pos (rad) | + | 9 | Set position of left_ankle_pitch | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch1 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch2 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_yaw | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + + + ## Observation space + + | Num | Observation | Min | Max | Name (in corresponding XML file) | Joint | Unit | + | --- | -------------------------------------------------------- | ---- | --- | -------------------------------- | -------- | ------------------------ | + | 0 | Rotation right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | angle (rad) | + | 1 | Rotation right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | angle (rad) | + | 2 | Rotation right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | angle (rad) | + | 3 | Rotation right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | angle (rad) | + | 4 | Rotation right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | angle (rad) | + | 5 | Rotation left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | angle (rad) | + | 6 | Rotation left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | angle (rad) | + | 7 | Rotation left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | angle (rad) | + | 8 | Rotation left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | angle (rad) | + | 9 | Rotation left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | angle (rad) | + | 10 | Rotation neck_pitch | -Inf | Inf | head_pitch1 | cylinder | angle (rad) | + | 11 | Rotation head_pitch | -Inf | Inf | head_pitch2 | cylinder | angle (rad) | + | 12 | Rotation head_yaw | -Inf | Inf | head_yaw | cylinder | angle (rad) | + | 13 | Velocity of right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | speed (rad/s) | + | 14 | Velocity of right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | speed (rad/s) | + | 15 | Velocity of right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | speed (rad/s) | + | 16 | Velocity of right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | speed (rad/s) | + | 17 | Velocity of right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | speed (rad/s) | + | 18 | Velocity of left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | speed (rad/s) | + | 19 | Velocity of left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | speed (rad/s) | + | 20 | Velocity of left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | speed (rad/s) | + | 21 | Velocity of left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | speed (rad/s) | + | 22 | Velocity of left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | speed (rad/s) | + | 23 | Velocity of neck_pitch | -Inf | Inf | head_pitch1 | cylinder | speed (rad/s) | + | 24 | Velocity of head_pitch | -Inf | Inf | head_pitch2 | cylinder | speed (rad/s) | + | 25 | Velocity of head_yaw | -Inf | Inf | head_yaw | cylinder | speed (rad/s) | + + + | 26 | roll angular velocity | -Inf | Inf | | | |e + | 27 | pitch angular velocity | -Inf | Inf | | | | + | 28 | yaw angular velocity | -Inf | Inf | | | | + | 29 | current x linear velocity | -Inf | Inf | | | | + | 30 | current y linear velocity | -Inf | Inf | | | | + | 31 | current z linear velocity | -Inf | Inf | | | | + | 32 | current x target linear velocity | -Inf | Inf | | | | + | 33 | current y target linear velocity | -Inf | Inf | | | | + | 34 | current yaw target angular velocity | -Inf | Inf | | | | + + Changed to control history + | 35 | t-1 right_hip_yaw rotation error | -Inf | Inf | | | | + | 36 | t-1 right_hip_roll rotation error | -Inf | Inf | | | | + | 37 | t-1 right_hip_pitch rotation error | -Inf | Inf | | | | + | 38 | t-1 right_knee_pitch rotation error | -Inf | Inf | | | | + | 39 | t-1 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 40 | t-1 left_hip_yaw rotation error | -Inf | Inf | | | | + | 41 | t-1 left_hip_roll rotation error | -Inf | Inf | | | | + | 42 | t-1 left_hip_pitch rotation error | -Inf | Inf | | | | + | 43 | t-1 left_knee_pitch rotation error | -Inf | Inf | | | | + | 44 | t-1 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 45 | t-1 neck_pitch rotation error | -Inf | Inf | | | | + | 46 | t-1 head_pitch rotation error | -Inf | Inf | | | | + | 47 | t-1 head_yaw rotation error | -Inf | Inf | | | | + + | 48 | t-2 right_hip_yaw rotation error | -Inf | Inf | | | | + | 49 | t-2 right_hip_roll rotation error | -Inf | Inf | | | | + | 50 | t-2 right_hip_pitch rotation error | -Inf | Inf | | | | + | 51 | t-2 right_knee_pitch rotation error | -Inf | Inf | | | | + | 52 | t-2 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 53 | t-2 left_hip_yaw rotation error | -Inf | Inf | | | | + | 54 | t-2 left_hip_roll rotation error | -Inf | Inf | | | | + | 55 | t-2 left_hip_pitch rotation error | -Inf | Inf | | | | + | 56 | t-2 left_knee_pitch rotation error | -Inf | Inf | | | | + | 57 | t-2 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 58 | t-2 neck_pitch rotation error | -Inf | Inf | | | | + | 59 | t-2 head_pitch rotation error | -Inf | Inf | | | | + | 60 | t-2 head_yaw rotation error | -Inf | Inf | | | | + | 61 | t-3 right_hip_yaw rotation error | -Inf | Inf | | | | + | 62 | t-3 right_hip_roll rotation error | -Inf | Inf | | | | + | 63 | t-3 right_hip_pitch rotation error | -Inf | Inf | | | | + | 64 | t-3 right_knee_pitch rotation error | -Inf | Inf | | | | + | 65 | t-3 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 66 | t-3 left_hip_yaw rotation error | -Inf | Inf | | | | + | 67 | t-3 left_hip_roll rotation error | -Inf | Inf | | | | + | 68 | t-3 left_hip_pitch rotation error | -Inf | Inf | | | | + | 69 | t-3 left_knee_pitch rotation error | -Inf | Inf | | | | + | 70 | t-3 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 71 | t-3 neck_pitch rotation error | -Inf | Inf | | | | + | 72 | t-3 head_pitch rotation error | -Inf | Inf | | | | + | 73 | t-3 head_yaw rotation error | -Inf | Inf | | | | + + | 74 | left foot in contact with the floor | -Inf | Inf | | | | + | 75 | right foot in contact with the floor | -Inf | Inf | | | | + + | x76 | t | -Inf | Inf | | | | + | x74 | sinus | -Inf | Inf | | | | + + """ + + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 50, + } + + # Ideas + # Low pass filter on the joint angles + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + observation_space = Box(low=-np.inf, high=np.inf, shape=(86,), dtype=np.float64) + self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.joint_history_length = 3 + self.joint_error_history = self.joint_history_length * [15 * [0]] + self.joint_ctrl_history = self.joint_history_length * [15 * [0]] + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + self.startup_cooldown = 1.0 + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def check_contact(self, body1_name, body2_name): + body1_id = self.data.body(body1_name).id + body2_id = self.data.body(body2_name).id + + for i in range(self.data.ncon): + contact = self.data.contact[i] + + if ( + self.model.geom_bodyid[contact.geom1] == body1_id + and self.model.geom_bodyid[contact.geom2] == body2_id + ) or ( + self.model.geom_bodyid[contact.geom1] == body2_id + and self.model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + def feet_contact_reward(self): + return self.right_foot_in_contact + self.left_foot_in_contact + + def velocity_tracking_reward(self): + base_velocity = list(self.data.body("base").cvel[3:][:2]) + [ + self.data.body("base").cvel[:3][2] + ] + base_velocity = np.asarray(base_velocity) + return np.exp(-np.square(base_velocity - self.target_velocity).sum()) + + def smoothness_reward(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(15): + smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + t0[i] - 2 * t_minus1[i] + t_minus2[i] + ) + + return -smooth + + def smoothness_reward2(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(15): + smooth += np.square(t0[i] - t_minus1[i]) + np.square( + t_minus1[i] - t_minus2[i] + ) + # smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + # t0[i] - 2 * t_minus1[i] + t_minus2[i] + # ) + + return -smooth + + def joint_velocity_reward(self): + return -np.square(self.data.qvel[:]).sum() + + def walking_height_reward(self): + return ( + -np.square((self.get_body_com("base")[2] - 0.15)) * 100 + ) # "normal" walking height is about 0.15m + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def init_position_reward(self): + return -np.square(self.data.qpos[7 : 7 + 15] - self.init_pos).sum() + + def is_terminated(self) -> bool: + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + upright = np.array([0, 0, 1]) + return ( + self.data.body("base").xpos[2] < 0.08 or np.dot(upright, Z_vec) <= 0 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def action_LFP(self, action): + # Low pass filter for the actions + action_tminus1 = self.joint_ctrl_history[0] + action_tminus2 = self.joint_ctrl_history[1] + action_tminus3 = self.joint_ctrl_history[2] + # return action * 0.5 + action_tminus1 * 0.5 + return ( + action * 0.5 + + action_tminus1 * 0.3 + + action_tminus2 * 0.15 + + action_tminus3 * 0.05 + ) + + d_action = action - action_tminus1 + + action = [ + a if abs(d) < 0.1 else b + for a, b, d in zip(action, action_tminus1, d_action) + ] + return action + + def step(self, a): + # https://www.nature.com/articles/s41598-023-38259-7.pdf + + # add random force + # if np.random.rand() < 0.05: + # self.data.xfrc_applied[self.data.body("base").id][:3] = [ + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # ] # absolute + + t = self.data.time + dt = t - self.prev_t + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + + if self.startup_cooldown > 0: + self.do_simulation(self.init_pos, FRAME_SKIP) + reward = 0 + else: + self.do_simulation(a, FRAME_SKIP) + # self.do_simulation(self.action_LFP(a), 4) + self.right_foot_in_contact = self.check_contact("foot_module", "floor") + self.left_foot_in_contact = self.check_contact("foot_module_2", "floor") + + reward = ( + 0.05 # time reward + # + 0.1 * self.walking_height_reward() + # + 0.1 * self.upright_reward() + # + 0.1 * self.velocity_tracking_reward() + + 0.01 * self.smoothness_reward2() + + 0.1 * self.init_position_reward() + # + 0.1 * self.feet_contact_reward() + # + 0.1 * self.joint_angle_deviation_reward() + # + 0.01 * self.follow_walk_engine_reward(dt) + # + 0.001 * self.joint_velocity_reward() + ) + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + self.prev_t = t + + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + + # self.model.opt.gravity[:] = [0, 0, 0] # no gravity + + self.goto_init() + + self.joint_error_history = self.joint_history_length * [15 * [0]] + self.joint_ctrl_history = self.joint_history_length * [15 * [0]] + # qpos = self.data.qpos + + # LATEST + # added randomization to the initial position + # for i in range(7, len(qpos)): + # qpos[i] = np.random.uniform(-0.3, 0.3) + + # self.init_qpos = qpos.copy().flatten() + + # Randomize later + # self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qpos[:] = np.zeros(len(self.data.qpos[:])) + self.data.qpos[2] = 0.15 + # self.data.qpos[3 : 3 + 4] = [1, 0, 0, 0] + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + self.data.qpos[7 : 7 + 15] = self.init_pos + self.data.ctrl[:] = self.init_pos + + def _get_obs(self): + joints_rotations = self.data.qpos[7 : 7 + 15] + joints_velocities = self.data.qvel[6 : 6 + 15] + + # joints_error = self.data.ctrl - self.data.qpos[7 : 7 + 13] + # self.joint_error_history.append(joints_error) + # self.joint_error_history = self.joint_error_history[ + # -self.joint_history_length : + # ] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.joint_ctrl_history.append(self.data.ctrl.copy()) + self.joint_ctrl_history = self.joint_ctrl_history[-self.joint_history_length :] + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocity, + np.array(self.joint_ctrl_history).flatten(), + [self.left_foot_in_contact, self.right_foot_in_contact], + ] + ) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval.py new file mode 100644 index 0000000..8085b15 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval.py @@ -0,0 +1,130 @@ +import argparse +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import gymnasium as gym +import mujoco +import numpy as np +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + +register( + id="BDX_env", + entry_point="footsteps_env:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + + +def draw_clock(clock): + # clock [a, b] + clock_radius = 100 + im = np.zeros((clock_radius * 2, clock_radius * 2, 3), np.uint8) + im = cv2.circle(im, (clock_radius, clock_radius), clock_radius, (255, 255, 255), -1) + im = cv2.line( + im, + (clock_radius, clock_radius), + ( + int(clock_radius + clock_radius * clock[0]), + int(clock_radius + clock_radius * clock[1]), + ), + (0, 0, 255), + 2, + ) + cv2.imshow("clock", im) + cv2.waitKey(1) + + +def draw_frame(pose, i, env): + pose = fv_utils.rotateInSelf(pose, [0, 90, 0]) + # env.mujoco_renderer._get_viewer(render_mode="human") + env.mujoco_renderer._get_viewer(render_mode="human").add_marker( + pos=pose[:3, 3], + mat=pose[:3, :3], + size=[0.005, 0.005, 0.1], + type=mujoco.mjtGeom.mjGEOM_ARROW, + rgba=[1, 0, 0, 1], + label=str(i), + ) + + +def test(env, sb3_algo, path_to_model): + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + + print("Using latest model: ", latest_model_path) + + path_to_model = latest_model_path + + match sb3_algo: + case "SAC": + model = SAC.load(path_to_model, env=env) + case "TD3": + model = TD3.load(path_to_model, env=env) + case "A2C": + model = A2C.load(path_to_model, env=env) + case "TQC": + model = TQC.load(path_to_model, env=env) + case "PPO": + model = PPO.load(path_to_model, env=env) + case _: + print("Algorithm not found") + return + + obs = env.reset()[0] + done = False + extra_steps = 500 + while True: + action, _ = model.predict(obs) + obs, _, done, _, _ = env.step(action) + footsteps = env.next_footsteps + base_target_2D = np.mean( + [footsteps[2][:3, 3][:2], footsteps[3][:3, 3][:2]], axis=0 + ) + base_target_frame = np.eye(4) + base_target_frame[:3, 3][:2] = base_target_2D + draw_frame(base_target_frame, "base target", env) + base_pos_2D = env.data.body("base").xpos[:2] + base_pos_frame = np.eye(4) + base_pos_frame[:3, 3][:2] = base_pos_2D + draw_frame(base_pos_frame, "base pos", env) + + # draw_clock(env.get_clock_signal()) + + for i, footstep in enumerate(footsteps[2:]): + draw_frame(footstep, i, env) + + if done: + extra_steps -= 1 + + if extra_steps < 0: + break + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + gymenv = gym.make("BDX_env", render_mode="human") + test(gymenv, args.algo, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py new file mode 100644 index 0000000..c168b41 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py @@ -0,0 +1,142 @@ +import argparse +from scipy.spatial.transform import Rotation as R +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import gymnasium as gym +import mujoco +import numpy as np +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + +register( + id="BDX_env", + entry_point="simple_env:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + + +def draw_clock(clock): + # clock [a, b] + clock_radius = 100 + im = np.zeros((clock_radius * 2, clock_radius * 2, 3), np.uint8) + im = cv2.circle(im, (clock_radius, clock_radius), clock_radius, (255, 255, 255), -1) + im = cv2.line( + im, + (clock_radius, clock_radius), + ( + int(clock_radius + clock_radius * clock[0]), + int(clock_radius + clock_radius * clock[1]), + ), + (0, 0, 255), + 2, + ) + cv2.imshow("clock", im) + cv2.waitKey(1) + + +def draw_frame(pose, i, env): + pose = fv_utils.rotateInSelf(pose, [0, 90, 0]) + # env.mujoco_renderer._get_viewer(render_mode="human") + env.mujoco_renderer._get_viewer(render_mode="human").add_marker( + pos=pose[:3, 3], + mat=pose[:3, :3], + size=[0.005, 0.005, 0.1], + type=mujoco.mjtGeom.mjGEOM_ARROW, + rgba=[1, 0, 0, 1], + label=str(i), + ) + + +def draw_velocities(robot_orig_xy, velocities_xytheta, env): + horizon = 10 # seconds + + robot_orig_xyz = np.array([robot_orig_xy[0], robot_orig_xy[1], 0]) + for i in range(horizon): + j = i * 0.1 + frame = np.eye(4) + frame[:3, 3] = robot_orig_xyz + np.array( + [velocities_xytheta[0] * j, velocities_xytheta[1] * j, 0] + ) + # rotate frame to point in the direction of the velocity + + frame = fv_utils.rotateAbout( + frame, + [0, 0, velocities_xytheta[2] * j], + center=robot_orig_xyz, + degrees=False, + ) + + draw_frame(frame, i, env) + + +def test(env, sb3_algo, path_to_model): + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + + print("Using latest model: ", latest_model_path) + + path_to_model = latest_model_path + + match sb3_algo: + case "SAC": + model = SAC.load(path_to_model, env=env) + case "TD3": + model = TD3.load(path_to_model, env=env) + case "A2C": + model = A2C.load(path_to_model, env=env) + case "TQC": + model = TQC.load(path_to_model, env=env) + case "PPO": + model = PPO.load(path_to_model, env=env) + + # model = PPO("MlpPolicy", env) + # model.policy.load(path_to_model) + case _: + print("Algorithm not found") + return + + obs = env.reset()[0] + done = False + extra_steps = 500 + while True: + action, _ = model.predict(obs) + obs, _, done, _, _ = env.step(action) + + draw_velocities(env.data.body("base").xpos[:2], env.target_velocities, env) + + if done: + extra_steps -= 1 + + if extra_steps < 0: + break + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + gymenv = gym.make("BDX_env", render_mode="human") + test(gymenv, args.algo, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py new file mode 100644 index 0000000..d8d9e91 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py @@ -0,0 +1,384 @@ +import numpy as np +import placo +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact, get_contact_force + +FRAME_SKIP = 4 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + # Inspired by https://arxiv.org/pdf/2207.12644 + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 125, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + self.nb_dofs = 15 + + observation_space = Box( + np.array( + [ + *(-np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(-10 * np.ones(self.nb_dofs)), # joints_velocities + *(-10 * np.ones(3)), # angular_velocity + *(-10 * np.ones(3)), # linear_velocity + *(-10 * np.ones(8)), # next two footsteps 2*[x, y, z, theta] + *(-np.pi * np.ones(2)), # clock signal + ] + ), + np.array( + [ + *(np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(10 * np.ones(self.nb_dofs)), # joints_velocities + *(10 * np.ones(3)), # angular_velocity + *(10 * np.ones(3)), # linear_velocity + *(10 * np.ones(8)), # next two footsteps 2*[x, y, z, theta] + *(np.pi * np.ones(2)), # clock signal + ] + ), + ) + + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + + self.pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/robot.urdf", + ignore_feet_contact=True, + ) + + self.startup_cooldown = -self.pwe.initial_delay + self.next_footsteps = self.pwe.get_footsteps_in_world().copy() + for i in range(len(self.next_footsteps)): + self.next_footsteps[i][:3, 3][2] = 0 + + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def is_terminated(self) -> bool: + left_antenna_contact = check_contact( + self.data, self.model, "left_antenna_assembly", "floor" + ) + right_antenna_contact = check_contact( + self.data, self.model, "right_antenna_assembly", "floor" + ) + body_contact = check_contact(self.data, self.model, "body_module", "floor") + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + Z_vec /= np.linalg.norm(Z_vec) + upright = np.array([0, 0, 1]) + + base_pos_2D = self.data.body("base").xpos[:2] + upcoming_footstep = self.next_footsteps[2] + second_footstep = self.next_footsteps[3] + base_target_2D = np.mean( + [upcoming_footstep[:3, 3][:2], second_footstep[:3, 3][:2]], axis=0 + ) + base_dist_to_target = np.linalg.norm(base_pos_2D - base_target_2D) + if base_dist_to_target > 0.3: + print("TERMINATED BECAUSE TOO FAR FROM TARGET") + return ( + self.data.body("base").xpos[2] < 0.08 + or np.dot(upright, Z_vec) <= 0.4 + or left_antenna_contact + or right_antenna_contact + or body_contact + or base_dist_to_target > 0.3 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def gait_reward(self): + # During single support: + # - reward force on the supporting foot and speed on the flying foot + # - penalize force on the flying foot and speed on the supporting foot + # During double support: + # - reward force on both feet + # - penalize speed on both feet + + support_phase = self.pwe.get_current_support_phase() # left, right, both + right_contact_force = np.sum( + get_contact_force(self.data, self.model, "right_foot", "floor") + ) + left_contact_force = np.sum( + get_contact_force(self.data, self.model, "left_foot", "floor") + ) + right_speed = self.data.body("right_foot").cvel[3:] # [rot:vel] size 6 + left_speed = self.data.body("left_foot").cvel[3:] # [rot:vel] size 6 + + if support_phase == "both": + return ( + right_contact_force + + left_contact_force + - np.linalg.norm(right_speed) + - np.linalg.norm(left_speed) + ) + elif support_phase is placo.HumanoidRobot_Side.left: + return ( + left_contact_force + - np.linalg.norm(left_speed) + + right_contact_force + + np.linalg.norm(right_speed) + ) + elif support_phase is placo.HumanoidRobot_Side.right: + return ( + right_contact_force + - np.linalg.norm(right_speed) + + left_contact_force + + np.linalg.norm(left_speed) + ) + + def support_flying_reward(self): + # Idea : reward when there is a support foot and a flying foot + # penalize when both feet are in the air or both feet are on the ground + right_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "right_foot", "floor")) + ) + left_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "left_foot", "floor")) + ) + right_speed = np.linalg.norm( + self.data.body("right_foot").cvel[3:] + ) # [rot:vel] size 6 + left_speed = np.linalg.norm( + self.data.body("left_foot").cvel[3:] + ) # [rot:vel] size 6 + + return left_contact_force - right_contact_force + right_speed - left_speed + + def step_reward(self): + # Incentivize the robot to step and orient the body toward targets + # dfoot : distance of the closest foot to the upcoming footstep + # hit reward : reward any foot that hits the upcoming footstep. Only when either or both feet are within a radius of the target + # progress reward : encourage the moving base to move toward he target + + # Using absolute positions here, while in the paper they use relative + + target_radius = ( + 0.05 # Only when either or both feet are within a radius of the target ?? + ) + + base_pos_2D = self.data.body("base").xpos[:2] + upcoming_footstep = self.next_footsteps[2] + second_footstep = self.next_footsteps[3] + + base_target_2D = np.mean( + [upcoming_footstep[:3, 3][:2], second_footstep[:3, 3][:2]], axis=0 + ) + + pos = self.data.body("base").xpos + mat = self.data.body("base").xmat + T_world_body = np.eye(4) + T_world_body[:3, :3] = mat.reshape(3, 3) + T_world_body[:3, 3] = pos + + T_world_rightFoot = np.eye(4) + T_world_rightFoot[:3, 3] = self.data.body("right_foot").xpos + T_world_rightFoot[:3, :3] = self.data.body("right_foot").xmat.reshape(3, 3) + + T_world_leftFoot = np.eye(4) + T_world_leftFoot[:3, 3] = self.data.body("left_foot").xpos + T_world_leftFoot[:3, :3] = self.data.body("left_foot").xmat.reshape(3, 3) + + right_foot_dist = np.linalg.norm( + upcoming_footstep[:3, 3] - T_world_rightFoot[:3, 3] + ) + left_foot_dist = np.linalg.norm( + upcoming_footstep[:3, 3] - T_world_leftFoot[:3, 3] + ) + + dfoot = min(right_foot_dist, left_foot_dist) + droot = np.linalg.norm(base_pos_2D - base_target_2D) + if dfoot > target_radius: + dfoot = 2 # penalize if the foot is too far from the target + + khit = 0.8 + return khit * np.exp(-dfoot / 0.25) + (1 - khit) * np.exp(-droot / 2) + + def orient_reward(self): + euler = R.from_matrix(self.pwe.robot.get_T_world_fbase()[:3, :3]).as_euler( + "xyz" + ) + desired_yaw = euler[2] + current_yaw = R.from_matrix( + np.array(self.data.body("base").xmat).reshape(3, 3) + ).as_euler("xyz")[2] + return -((abs(desired_yaw) - abs(current_yaw)) ** 2) + + def height_reward(self): + current_height = self.data.body("base").xpos[2] + return np.exp(-40 * (0.15 - current_height) ** 2) + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def action_reward(self, a): + current_action = a.copy() + + # This can explode, don't understand why + return min( + 2, np.exp(-5 * np.sum((self.prev_action - current_action)) / self.nb_dofs) + ) + + def torque_reward(self): + current_torque = self.data.qfrc_actuator + return np.exp( + -0.25 * np.sum((self.prev_torque - current_torque)) / self.nb_dofs + ) + + def step(self, a): + t = self.data.time + dt = t - self.prev_t + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + + if self.startup_cooldown > 0: + self.do_simulation(self.init_pos, FRAME_SKIP) + reward = 0 + else: + # We want to learn deltas from the initial position + a += self.init_pos + + # Maybe use that too :) + current_ctrl = self.data.ctrl.copy() + delta_max = 0.05 + a = np.clip(a, current_ctrl - delta_max, current_ctrl + delta_max) + + self.do_simulation(a, FRAME_SKIP) + + self.pwe.tick(dt) + + # TODO penalize auto collisions + # check all collisions and penalize all that is not foot on the ground. + # Need to ignore the auto collisions that occur because of the robot's assembly. + # Tried contact exclude but does not seem to work + # https://github.com/google-deepmind/mujoco/issues/104 + + reward = ( + # 0.30 * self.gait_reward() + 0.30 * self.support_flying_reward() + + 0.6 * self.step_reward() + + 0.05 * self.orient_reward() + + 0.15 * self.height_reward() + + 0.05 * self.upright_reward() + + 0.05 * self.action_reward(a) + + 0.05 * self.torque_reward() + ) + + ob = self._get_obs() + + if self.render_mode == "human": + if self.startup_cooldown <= 0: + print("support flying reward: ", 0.30 * self.support_flying_reward()) + # print("Gait reward: ", 0.30 * self.gait_reward()) + print("Step reward: ", 0.6 * self.step_reward()) + print("Orient reward: ", 0.05 * self.orient_reward()) + print("Height reward: ", 0.15 * self.height_reward()) + print("Upright reward: ", 0.05 * self.upright_reward()) + print("Action reward: ", 0.05 * self.action_reward(a)) + print("Torque reward: ", 0.05 * self.torque_reward()) + print("===") + self.render() + + self.prev_t = t + self.prev_action = a.copy() + self.prev_torque = self.data.qfrc_actuator.copy() + + # self.viz.display(self.pwe.robot.state.q) + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + self.pwe.reset() + + d_x = np.random.uniform(0.01, 0.03) + d_y = np.random.uniform(-0.01, 0.01) + d_theta = np.random.uniform(-0.1, 0.1) + # self.pwe.set_traj(d_x, d_y, d_theta) + self.pwe.set_traj(0.03, 0, 0.001) + + self.goto_init() + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + noise = np.random.uniform(-0.01, 0.01, self.nb_dofs) + self.data.qpos[7 : 7 + self.nb_dofs] = self.init_pos + noise + self.data.qpos[2] = 0.15 + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.ctrl[:] = self.init_pos + + def get_clock_signal(self): + a = np.sin(2 * np.pi * (self.pwe.t % self.pwe.period) / self.pwe.period) + b = np.cos(2 * np.pi * (self.pwe.t % self.pwe.period) / self.pwe.period) + return [a, b] + + def _get_obs(self): + joints_rotations = self.data.qpos[7 : 7 + self.nb_dofs] + joints_velocities = self.data.qvel[6 : 6 + self.nb_dofs] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.next_footsteps = self.pwe.get_footsteps_in_world().copy() + for i in range(len(self.next_footsteps)): + self.next_footsteps[i][:3, 3][2] = 0 + next_two_footsteps = [] # 2*[x, y, z, theta] + for footstep in self.next_footsteps[2:4]: + yaw = R.from_matrix(footstep[:3, :3]).as_euler("xyz")[2] + next_two_footsteps.append(list(footstep[:3, 3]) + [yaw]) + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + np.array(next_two_footsteps).flatten(), + self.get_clock_signal(), + ] + ) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh new file mode 100644 index 0000000..4f12c82 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh @@ -0,0 +1,3 @@ +last=$(ssh -p4242 apirrone@s-nguyen.net "cd /home/apirrone/MISC/mini_BDX/experiments/RL/new/$1/ ; ls -lt | sed -n '2 p' | grep -io '[$1]*_[0123456789]*.zip'") + +scp -p4242 apirrone@s-nguyen.net:/home/apirrone/MISC/mini_BDX/experiments/RL/new/$1/$last ./$1/ \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py new file mode 100644 index 0000000..5a2dfa2 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py @@ -0,0 +1,258 @@ +import numpy as np +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact + +# from placo_utils.visualization import robot_viz + + +FRAME_SKIP = 4 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 125, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + self.nb_dofs = 15 + + self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.joint_history_length = 3 + self.joint_error_history = self.joint_history_length * [self.nb_dofs * [0]] + self.joint_ctrl_history = self.joint_history_length * [self.nb_dofs * [0]] + + # observation_space = Box( + # low=-np.inf, high=np.inf, shape=(101,), dtype=np.float64 + # ) + + observation_space = Box( + np.array( + [ + *(-np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(-10 * np.ones(self.nb_dofs)), # joints_velocities + *(-10 * np.ones(3)), # angular_velocity + *(-10 * np.ones(3)), # linear_velocity + *(-10 * np.ones(3)), # target_velocity + *( + -np.pi * np.ones(self.nb_dofs * self.joint_history_length) + ), # joint_ctrl_history + *(np.zeros(2)), # feet_contact + *(-np.pi * np.ones(self.nb_dofs)), # placo_angles + ] + ), + np.array( + [ + *(np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(10 * np.ones(self.nb_dofs)), # joints_velocities + *(10 * np.ones(3)), # angular_velocity + *(10 * np.ones(3)), # linear_velocity + *(10 * np.ones(3)), # target_velocity + *( + np.pi * np.ones(self.nb_dofs * self.joint_history_length) + ), # joint_ctrl_history + *(np.ones(2)), # feet_contact + *(np.pi * np.ones(self.nb_dofs)), # placo_angles + ] + ), + ) + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + self.startup_cooldown = 1.0 + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + + self.pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/robot.urdf", + ignore_feet_contact=True, + ) + + # self.viz = robot_viz(self.pwe.robot) + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def is_terminated(self) -> bool: + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + Z_vec /= np.linalg.norm(Z_vec) + upright = np.array([0, 0, 1]) + return ( + self.data.body("base").xpos[2] < 0.08 or np.dot(upright, Z_vec) <= 0.2 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def get_feet_contact(self): + right_contact = check_contact(self.data, self.model, "foot_module", "floor") + left_contact = check_contact(self.data, self.model, "foot_module_2", "floor") + return right_contact, left_contact + + def follow_placo_reward(self): + current_pos = self.data.qpos[7 : 7 + self.nb_dofs] + placo_angles = list(self.pwe.get_angles().values()) + # print(np.around(placo_angles, 2)) + error = np.linalg.norm(placo_angles - current_pos) + return -np.square(error) + + def walking_height_reward(self): + return ( + -np.square((self.get_body_com("base")[2] - 0.14)) * 100 + ) # "normal" walking height is about 0.14m + + def velocity_tracking_reward(self): + base_velocity = list(self.data.body("base").cvel[3:][:2]) + [ + self.data.body("base").cvel[:3][2] + ] + base_velocity = np.asarray(base_velocity) + return np.exp(-np.square(base_velocity - self.target_velocity).sum()) + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def smoothness_reward2(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(15): + smooth += np.square(t0[i] - t_minus1[i]) + np.square( + t_minus1[i] - t_minus2[i] + ) + # smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + # t0[i] - 2 * t_minus1[i] + t_minus2[i] + # ) + + return -smooth + + def step(self, a): + + t = self.data.time + dt = t - self.prev_t + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + + if self.startup_cooldown > 0: + self.do_simulation(self.init_pos, FRAME_SKIP) + reward = 0 + else: + + current_ctrl = self.data.ctrl.copy() + # Limiting the control + delta_max = 0.1 + # print("a before clipping", a) + a = np.clip(a, current_ctrl - delta_max, current_ctrl + delta_max) + # print("a after clipping", a) + + self.do_simulation(a, FRAME_SKIP) + + # self.right_foot_in_contact, self.left_foot_in_contact = ( + # self.get_feet_contact() + # ) + + self.pwe.tick( + dt + ) # , self.left_foot_in_contact, self.right_foot_in_contact) + + reward = ( + 0.05 # time reward + + 0.1 * self.walking_height_reward() + + 0.1 * self.upright_reward() + + 0.1 * self.velocity_tracking_reward() + + 0.01 * self.smoothness_reward2() + ) + # print(self.follow_placo_reward(a)) + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + self.prev_t = t + + # self.viz.display(self.pwe.robot.state.q) + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + self.pwe.reset() + + self.goto_init() + + self.joint_error_history = self.joint_history_length * [self.nb_dofs * [0]] + self.joint_ctrl_history = self.joint_history_length * [self.nb_dofs * [0]] + + self.target_velocity = np.asarray([0.2, 0, 0]) # x, y, yaw + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + self.data.qpos[7 : 7 + self.nb_dofs] = self.init_pos + self.data.qpos[2] = 0.15 + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.ctrl[:] = self.init_pos + + def _get_obs(self): + + joints_rotations = self.data.qpos[7 : 7 + self.nb_dofs] + joints_velocities = self.data.qvel[6 : 6 + self.nb_dofs] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.joint_ctrl_history.append(self.data.ctrl.copy()) + self.joint_ctrl_history = self.joint_ctrl_history[-self.joint_history_length :] + placo_angles = list(self.pwe.get_angles().values()) + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocity, + np.array(self.joint_ctrl_history).flatten(), + [self.left_foot_in_contact, self.right_foot_in_contact], + placo_angles, + ] + ) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py new file mode 100644 index 0000000..126ec58 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py @@ -0,0 +1,41 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms import bc +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="simple_env:BDXEnv") + +env = gym.make("BDX_env", render_mode=None) + +rng = np.random.default_rng(0) + +bc_trainer = bc.BC( + observation_space=env.observation_space, + action_space=env.action_space, + demonstrations=dataset, + rng=rng, + device="cpu", + policy=PPO( + "MlpPolicy", env, policy_kwargs=dict(net_arch=[400, 300]) + ).policy, # not working with SAC for some reason +) +bc_trainer.train(n_epochs=100) + +bc_trainer.policy.save("bc_policy_ppo.zip") + +# reward, _ = evaluate_policy(bc_trainer.policy, env, 1) +# print(reward) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py new file mode 100644 index 0000000..9a8ca50 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py @@ -0,0 +1,90 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms.adversarial.gail import GAIL +from imitation.data.wrappers import RolloutInfoWrapper +from imitation.rewards.reward_nets import BasicRewardNet +from imitation.util.networks import RunningNorm +from imitation.util.util import make_vec_env +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy +from stable_baselines3.ppo import MlpPolicy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="simple_env:BDXEnv") + +SEED = 42 +rng = np.random.default_rng(SEED) +# env = gym.make("BDX_env", render_mode=None) +env = make_vec_env( + "BDX_env", + rng=rng, + n_envs=8, + post_wrappers=[lambda env, _: RolloutInfoWrapper(env)], # to compute rollouts +) + + +learner = PPO( + env=env, + policy=MlpPolicy, + batch_size=64, + ent_coef=0.0, + learning_rate=0.0004, + gamma=0.95, + n_epochs=5, + seed=SEED, + tensorboard_log="logs", +) +reward_net = BasicRewardNet( + observation_space=env.observation_space, + action_space=env.action_space, + normalize_input_layer=RunningNorm, +) +gail_trainer = GAIL( + demonstrations=dataset, + demo_batch_size=1024, + gen_replay_buffer_capacity=512, + n_disc_updates_per_round=8, + venv=env, + gen_algo=learner, + reward_net=reward_net, + allow_variable_horizon=True, +) + +# print("evaluate the learner before training") +# env.seed(SEED) +# learner_rewards_before_training, _ = evaluate_policy( +# learner, +# env, +# 10, +# return_episode_rewards=True, +# ) + +print("train the learner and evaluate again") +env.seed(SEED) +gail_trainer.train(2000000) # Train for 800_000 steps to match expert. + +# env.seed(SEED) +# learner_rewards_after_training, _ = evaluate_policy( +# learner, +# env, +# 10, +# return_episode_rewards=True, +# ) + +# print("mean episode reward before training:", np.mean(learner_rewards_before_training)) +# print("mean episode reward after training:", np.mean(learner_rewards_after_training)) + +print("Save the policy") +gail_trainer.policy.save("gail_policy_ppo.zip") diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py new file mode 100644 index 0000000..e898ddc --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py @@ -0,0 +1,79 @@ +import argparse +import pickle +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import gymnasium as gym +import mujoco +import numpy as np +from gymnasium.envs.registration import register +from mini_bdx.placo_walk_engine import PlacoWalkEngine + +register( + id="BDX_env", + entry_point="simple_env:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + +pwe = PlacoWalkEngine( + "../../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True +) + +EPISODE_LENGTH = 20 +NB_EPISODES_TO_RECORD = 100 + + +def run(env): + episodes = [] + current_episode = {"observations": [], "actions": []} + while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + obs = env.reset()[0] + done = False + prev = env.data.time + start = env.data.time + while not done: + t = env.data.time + dt = t - prev + pwe.tick(dt) + angles = pwe.get_angles() + action = list(angles.values()) + action -= env.init_pos + action = np.array(action) + + obs, _, done, _, _ = env.step(action) + current_episode["observations"].append(list(obs)) + current_episode["actions"].append(list(action)) + + if env.data.time - start > EPISODE_LENGTH: + print("Episode done") + current_episode["observations"].append(list(env.reset()[0])) + + episode = Trajectory( + np.array(current_episode["observations"]), + np.array(current_episode["actions"]), + None, + True, + ) + episodes.append(episode) + + with open("dataset.pkl", "wb") as f: + pickle.dump(episodes, f) + + current_episode = {"observations": [], "actions": []} + done = True + + prev = t + + +if __name__ == "__main__": + gymenv = gym.make("BDX_env", render_mode="human") + run(gymenv) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py new file mode 100644 index 0000000..b4eb728 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py @@ -0,0 +1,152 @@ +import argparse +import json +import os +import time +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco_viewer +import numpy as np +from imitation.data.types import Trajectory +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.rl_utils import mujoco_to_isaac +from scipy.spatial.transform import Rotation as R + +import mujoco + +pwe = PlacoWalkEngine("../../../mini_bdx/robots/bdx/robot.urdf") + +EPISODE_LENGTH = 100 +NB_EPISODES_TO_RECORD = 1 +FPS = 60 + +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + + +episodes = [] + +current_episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], +} + +model = mujoco.MjModel.from_xml_path("../../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +mujoco_init_pos = np.array( + [ + # right_leg + -0.014, + 0.08, + 0.53, + -1.62, + 0.91, + # left leg + 0.013, + 0.077, + 0.59, + -1.63, + 0.86, + # head + -0.17, + -0.17, + 0.0, + 0.0, + 0.0, + ] +) + +data.qpos[3 : 3 + 4] = [1, 0, 0.08, 1] +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos + +while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + done = False + prev = time.time() + start = time.time() + last_record = time.time() + pwe.set_traj(0.0, 0.0, 0.001) + while not done: + t = time.time() + dt = t - prev + + # qpos = env.data.qpos[:3].copy() + # qpos[2] = 0.15 + # env.data.qpos[:3] = qpos + right_contact, left_contact = get_feet_contact() + + pwe.tick(dt, left_contact, right_contact) + # if pwe.t < 0: # for stand + angles = pwe.get_angles() + action = list(angles.values()) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 7) # 4 seems good + + if pwe.t <= 0: + start = time.time() + print("waiting ...") + prev = t + viewer.render() + continue + + if t - last_record >= 1 / FPS: + root_position = list(np.around(data.body("base").xpos, 3)) + root_orientation = list(np.around(data.body("base").xquat, 3)) # w, x, y, z + + # convert to x, y, z, w + root_orientation = [ + root_orientation[1], + root_orientation[2], + root_orientation[3], + root_orientation[0], + ] + + joints_positions = list(np.around(data.qpos[7 : 7 + 15], 3)) + + joints_positions = mujoco_to_isaac(joints_positions) + + current_episode["Frames"].append( + root_position + root_orientation + joints_positions + ) + last_record = time.time() + + if time.time() - start > EPISODE_LENGTH * 2: + print("Episode done") + print(len(current_episode["Frames"])) + episodes.append(current_episode) + + # save json as bdx_walk.txt + with open("bdx_walk.txt", "w") as f: + json.dump(current_episode, f) + + current_episode = { + "LoopMode": "Wrap", + "FrameDuration": 0.01667, + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], + } + done = True + viewer.render() + prev = t diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py new file mode 100644 index 0000000..3a80d5d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py @@ -0,0 +1,132 @@ +import argparse +import mujoco_viewer +import time +from mini_bdx.utils.rl_utils import mujoco_to_isaac +import json +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco +import numpy as np +from mini_bdx.placo_walk_engine import PlacoWalkEngine + +pwe = PlacoWalkEngine( + "../../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True +) + +EPISODE_LENGTH = 60 +NB_EPISODES_TO_RECORD = 1 +FPS = 60 + +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + + +episodes = [] + +current_episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], +} + +model = mujoco.MjModel.from_xml_path("../../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + +while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + done = False + prev = time.time() + start = time.time() + last_record = time.time() + pwe.set_traj(0.02, 0.0, 0.001) + while not done: + t = time.time() + dt = t - prev + + # qpos = env.data.qpos[:3].copy() + # qpos[2] = 0.15 + # env.data.qpos[:3] = qpos + # if pwe.t <= 0: # for stand + pwe.tick(dt) + angles = pwe.get_angles() + action = list(angles.values()) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 10) # 4 seems good + + if pwe.t <= 0: + start = time.time() + print("waiting ...") + prev = t + continue + + if t - last_record >= 1 / FPS: + root_position = list(np.around(data.body("base").xpos, 3)) + root_orientation = list(np.around(data.body("base").xquat, 3)) # w, x, y, z + + # convert to x, y, z, w + root_orientation = [ + root_orientation[1], + root_orientation[2], + root_orientation[3], + root_orientation[0], + ] + + joints_positions = list(np.around(data.qpos[7 : 7 + 15], 3)) + + # joints_positions = [ + # joints_positions[0], + # joints_positions[1], + # joints_positions[2], + # joints_positions[3], + # joints_positions[4], + # joints_positions[10], + # joints_positions[11], + # joints_positions[12], + # joints_positions[13], + # joints_positions[14], + # joints_positions[5], + # joints_positions[6], + # joints_positions[7], + # joints_positions[8], + # joints_positions[9], + # ] + joints_positions = mujoco_to_isaac(joints_positions) + + current_episode["Frames"].append( + root_position + root_orientation + joints_positions + ) + last_record = time.time() + + if time.time() - start > EPISODE_LENGTH * 2: + print("Episode done") + print(len(current_episode["Frames"])) + episodes.append(current_episode) + + # save json as bdx_walk.txt + with open("bdx_walk.txt", "w") as f: + json.dump(current_episode, f) + + current_episode = { + "LoopMode": "Wrap", + "FrameDuration": 0.01667, + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], + } + done = True + viewer.render() + prev = t diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/simple_env.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/simple_env.py new file mode 100644 index 0000000..8d600ec --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/simple_env.py @@ -0,0 +1,320 @@ +import numpy as np +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact, get_contact_force + +FRAME_SKIP = 4 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 125, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + self.nb_dofs = 15 + + observation_space = Box( + np.array( + [ + *(-np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(-10 * np.ones(self.nb_dofs)), # joints_velocities + *(-10 * np.ones(3)), # angular_velocity + *(-10 * np.ones(3)), # linear_velocity + *(-10 * np.ones(3)), # target velocity [x, y, theta] + *(0 * np.ones(2)), # feet contact [left, right] + *(-np.pi * np.ones(2)), # clock signal + ] + ), + np.array( + [ + *(np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(10 * np.ones(self.nb_dofs)), # joints_velocities + *(10 * np.ones(3)), # angular_velocity + *(10 * np.ones(3)), # linear_velocity + *(10 * np.ones(3)), # target velocity [x, y, theta] + *(1 * np.ones(2)), # feet contact [left, right] + *(np.pi * np.ones(2)), # clock signal + ] + ), + ) + + self.right_foot_contact = True + self.left_foot_contact = True + + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + + self.startup_cooldown = 1.0 + self.walk_period = 1.0 + self.target_velocities = np.asarray([0, 0, 0]) # x, y, yaw + self.cumulated_reward = 0.0 + self.last_time_both_feet_on_the_ground = 0 + self.init_pos_noise = np.zeros(self.nb_dofs) + + MujocoEnv.__init__( + self, + "../../../mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def is_terminated(self) -> bool: + left_antenna_contact = check_contact( + self.data, self.model, "left_antenna_assembly", "floor" + ) + right_antenna_contact = check_contact( + self.data, self.model, "right_antenna_assembly", "floor" + ) + body_contact = check_contact(self.data, self.model, "body_module", "floor") + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + Z_vec /= np.linalg.norm(Z_vec) + upright = np.array([0, 0, 1]) + + return ( + self.data.body("base").xpos[2] < 0.08 + or np.dot(upright, Z_vec) <= 0.4 + or left_antenna_contact + or right_antenna_contact + or body_contact + ) + + def support_flying_reward(self): + # Idea : reward when there is a support foot and a flying foot + # penalize when both feet are in the air or both feet are on the ground + right_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "right_foot", "floor")) + ) + left_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "left_foot", "floor")) + ) + right_speed = np.linalg.norm( + self.data.body("right_foot").cvel[3:] + ) # [rot:vel] size 6 + left_speed = np.linalg.norm( + self.data.body("left_foot").cvel[3:] + ) # [rot:vel] size 6 + + return abs(left_contact_force - right_contact_force) + abs( + right_speed - left_speed + ) + + def orient_reward(self): + desired_yaw = self.target_velocities[2] + current_yaw = R.from_matrix( + np.array(self.data.body("base").xmat).reshape(3, 3) + ).as_euler("xyz")[2] + + return -((abs(desired_yaw) - abs(current_yaw)) ** 2) + + def follow_xy_target_reward(self): + x_velocity = self.data.body("base").cvel[3:][0] + y_velocity = self.data.body("base").cvel[3:][1] + x_error = abs(self.target_velocities[0] - x_velocity) + y_error = abs(self.target_velocities[1] - y_velocity) + return -(x_error + y_error) + + def follow_yaw_target_reward(self): + yaw_velocity = self.data.body("base").cvel[:3][2] + yaw_error = abs(self.target_velocities[2] - yaw_velocity) + return -yaw_error + + def height_reward(self): + current_height = self.data.body("base").xpos[2] + return np.exp(-40 * (0.15 - current_height) ** 2) + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def action_reward(self, a): + current_action = a.copy() + + # This can explode, don't understand why + return min( + 2, np.exp(-5 * np.sum((self.prev_action - current_action)) / self.nb_dofs) + ) + + def torque_reward(self): + current_torque = self.data.qfrc_actuator + return np.exp( + -0.25 * np.sum((self.prev_torque - current_torque)) / self.nb_dofs + ) + + def feet_spacing_reward(self): + target_spacing = 0.12 + left_pos = self.data.body("left_foot").xpos + right_pos = self.data.body("right_foot").xpos + spacing = np.linalg.norm(left_pos - right_pos) + return np.exp(-10 * (spacing - target_spacing) ** 2) + + def both_feet_on_the_ground_penalty(self): + elapsed = self.data.time - self.last_time_both_feet_on_the_ground + if elapsed > self.walk_period: + return -1 + else: + return 0 + + def step(self, a): + t = self.data.time + dt = t - self.prev_t + + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + self.do_simulation(self.init_pos + self.init_pos_noise, FRAME_SKIP) + reward = 0 + self.last_time_both_feet_on_the_ground = t + else: + self.right_foot_contact = check_contact( + self.data, self.model, "right_foot", "floor" + ) + self.left_foot_contact = check_contact( + self.data, self.model, "left_foot", "floor" + ) + + if self.right_foot_contact and self.left_foot_contact: + self.last_time_both_feet_on_the_ground = t + + # We want to learn deltas from the initial position + a += self.init_pos + + # Maybe use that too :) + current_ctrl = self.data.ctrl.copy() + delta_max = 0.05 + a = np.clip(a, current_ctrl - delta_max, current_ctrl + delta_max) + a[10:] = self.init_pos[10:] # Only control the legs + + self.do_simulation(a, FRAME_SKIP) + + # IDEA : normalize reward by the episode length ? + reward = ( + # 0.1 * self.support_flying_reward() + 0.15 * self.follow_xy_target_reward() + + 0.15 * self.follow_yaw_target_reward() + # + 0.15 * self.height_reward() + # + 0.05 * self.upright_reward() + # + 0.05 * self.action_reward(a) + + 0.05 * self.torque_reward() + # + 0.05 * self.feet_spacing_reward() + # + 0.05 * self.both_feet_on_the_ground_penalty() + ) + + self.cumulated_reward += reward + + ob = self._get_obs() + + if self.render_mode == "human": + if self.startup_cooldown <= 0: + # print("support flying reward: ", 0.1 * self.support_flying_reward()) + # print( + # "Follow xy target reward: ", 0.15 * self.follow_xy_target_reward() + # ) + # print( + # "Follow yaw target reward: ", 0.15 * self.follow_yaw_target_reward() + # ) + # print("Height reward: ", 0.15 * self.height_reward()) + # print("Upright reward: ", 0.05 * self.upright_reward()) + # print("Action reward: ", 0.05 * self.action_reward(a)) + # print("Torque reward: ", 0.05 * self.torque_reward()) + # print("Feet spacing reward: ", 0.05 * self.feet_spacing_reward()) + # print(s + # print("TARGET : ", self.target_velocities) + # print("===") + pass + self.render() + + self.prev_t = t + self.prev_action = a.copy() + self.prev_torque = self.data.qfrc_actuator.copy() + + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + # self.model.opt.gravity[:] = [0, 0, 0] # no gravity + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + print("CUMULATED REWARD: ", self.cumulated_reward) + self.cumulated_reward = 0.0 + self.last_time_both_feet_on_the_ground = self.data.time + + v_x = np.random.uniform(0.0, 0.05) + v_y = np.random.uniform(-0.03, 0.03) + v_theta = np.random.uniform(-0.1, 0.1) + # self.target_velocities = np.asarray([v_x, v_y, v_theta]) # x, y, yaw + self.target_velocities = np.asarray([0.05, 0, 0]) # x, y, yaw + + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + + self.goto_init() + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + # self.init_pos_noise = np.random.uniform(-0.01, 0.01, self.nb_dofs) + self.init_pos_noise = np.zeros(self.nb_dofs) + self.data.qpos[7 : 7 + self.nb_dofs] = self.init_pos + self.init_pos_noise + self.data.qpos[2] = 0.15 + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.ctrl[:] = self.init_pos + + def get_clock_signal(self): + a = np.sin(2 * np.pi * (self.data.time % self.walk_period) / self.walk_period) + b = np.cos(2 * np.pi * (self.data.time % self.walk_period) / self.walk_period) + return [a, b] + + def _get_obs(self): + joints_rotations = self.data.qpos[7 : 7 + self.nb_dofs] + joints_velocities = self.data.qvel[6 : 6 + self.nb_dofs] + + # TODO this is imu, add noise to it later + angular_velocity = self.data.body("base").cvel[:3] + linear_velocity = self.data.body("base").cvel[3:] + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocities, + [self.left_foot_contact, self.right_foot_contact], + self.get_clock_signal(), + ] + ) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/train.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/train.py new file mode 100644 index 0000000..fb7dee7 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/train.py @@ -0,0 +1,151 @@ +import argparse +import os +from datetime import datetime + +import gymnasium as gym +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + + +def train(env, sb3_algo, model_dir, log_dir, pretrained=None, device="cuda"): + # SAC parameters found here https://github.com/hill-a/stable-baselines/issues/840#issuecomment-623171534 + if pretrained is None: + match sb3_algo: + case "SAC": + model = SAC( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "TD3": + model = TD3( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "A2C": + model = A2C( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "TQC": + model = TQC( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "PPO": + model = PPO( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case _: + print("Algorithm not found") + return + else: + match sb3_algo: + case "SAC": + model = SAC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TD3": + model = TD3.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "A2C": + model = A2C.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TQC": + model = TQC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "PPO": + model = PPO( + "MlpPolicy", env, verbose=1, device="cuda", tensorboard_log=log_dir + ) + model.policy.load(pretrained) + # model = PPO.load( + # pretrained, + # env=env, + # verbose=1, + # device="cuda", + # tensorboard_log=log_dir, + # ) + case _: + print("Algorithm not found") + return + + TIMESTEPS = 10000 + iters = 0 + while True: + iters += 1 + + model.learn( + total_timesteps=TIMESTEPS, + reset_num_timesteps=False, + progress_bar=True, + ) + model.save(f"{model_dir}/{sb3_algo}_{TIMESTEPS*iters}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Train BDX") + parser.add_argument( + "-a", + "--algo", + type=str, + choices=["SAC", "TD3", "A2C", "TQC", "PPO"], + default="SAC", + ) + parser.add_argument("-p", "--pretrained", type=str, required=False) + parser.add_argument("-d", "--device", type=str, required=False, default="cuda") + + parser.add_argument( + "-n", + "--name", + type=str, + required=False, + default=datetime.now().strftime("%Y-%m-%d_%H-%M-%S"), + help="Name of the experiment", + ) + + args = parser.parse_args() + + register( + id="BDX_env", + entry_point="simple_env:BDXEnv", + max_episode_steps=None, # formerly 500 + autoreset=True, + ) + # register( + # id="BDX_env", + # entry_point="env:BDXEnv", + # max_episode_steps=None, # formerly 500 + # autoreset=True, + # ) + + env = gym.make("BDX_env", render_mode=None) + # Create directories to hold models and logs + model_dir = args.name + log_dir = "logs/" + args.name + os.makedirs(model_dir, exist_ok=True) + os.makedirs(log_dir, exist_ok=True) + + train( + env, + args.algo, + pretrained=args.pretrained, + model_dir=model_dir, + log_dir=log_dir, + device=args.device, + ) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/old_test.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/old_test.py new file mode 100644 index 0000000..a28bcdc --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/old_test.py @@ -0,0 +1,83 @@ +import argparse +import os +from glob import glob + +import gymnasium as gym +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + +register( + id="BDX_env", + entry_point="env_humanoid:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + + +def test(env, sb3_algo, path_to_model): + + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + + print("Using latest model: ", latest_model_path) + + path_to_model = latest_model_path + + match sb3_algo: + case "SAC": + model = SAC.load(path_to_model, env=env) + case "TD3": + model = TD3.load(path_to_model, env=env) + case "A2C": + model = A2C.load(path_to_model, env=env) + case "TQC": + model = TQC.load(path_to_model, env=env) + case "PPO": + model = PPO("MlpPolicy", env) + model.policy.load(path_to_model) + # model = PPO.load(path_to_model, env=env) + + case _: + print("Algorithm not found") + return + + obs = env.reset()[0] + done = False + extra_steps = 500 + while True: + action, _ = model.predict(obs) + obs, _, done, _, _ = env.step(action) + + if done: + extra_steps -= 1 + + if extra_steps < 0: + break + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + gymenv = gym.make("BDX_env", render_mode="human") + test(gymenv, args.algo, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/play_policy.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/play_policy.py new file mode 100644 index 0000000..f62c360 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/play_policy.py @@ -0,0 +1,118 @@ +import argparse +import time +from glob import glob + +import gymnasium as gym +import mujoco +import mujoco.viewer +import numpy as np +from gymnasium.envs.registration import register +from stable_baselines3 import PPO, SAC + +from mini_bdx.utils.mujoco_utils import check_contact + + +def get_observation(data, left_contact, right_contact): + + position = ( + data.qpos.flat.copy() + ) # position/rotation of trunk + position of all joints + velocity = ( + data.qvel.flat.copy() + ) # positional/angular velocity of trunk + of all joints + + obs = np.concatenate( + [ + position, + velocity, + [left_contact, right_contact], + ] + ) + # print("OBS SIZE", len(obs)) + return obs + + +def key_callback(keycode): + pass + + +def get_model_from_dir(path_to_model): + + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + else: + latest_model_path = path_to_model + + return latest_model_path + + +def get_feet_contact(data, model): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +def play(env, path_to_model): + model_path = get_model_from_dir(path_to_model) + + model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") + data = mujoco.MjData(model) + + left_contact = False + right_contact = False + + viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + + # nn_model = SAC.load(model_path, env) + + nn_model = PPO("MlpPolicy", env) + nn_model.policy.load(model_path) + + try: + while True: + + right_contact, left_contact = get_feet_contact(data, model) + obs = get_observation( + data, + left_contact, + right_contact, + ) + action, _ = nn_model.predict(obs) + data.ctrl[:] = action + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep / 2.5) + + except KeyboardInterrupt: + viewer.close() + + viewer.close() + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + register(id="BDX_env", entry_point="env_humanoid:BDXEnv") + env = gym.make("BDX_env", render_mode=None) + play(env, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py new file mode 100644 index 0000000..9dc98b0 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py @@ -0,0 +1,41 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms import bc +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="env_humanoid:BDXEnv") + +env = gym.make("BDX_env", render_mode=None) + +rng = np.random.default_rng(0) + +bc_trainer = bc.BC( + observation_space=env.observation_space, + action_space=env.action_space, + demonstrations=dataset, + rng=rng, + device="cpu", + policy=PPO( + "MlpPolicy", env, policy_kwargs=dict(net_arch=[400, 300]) + ).policy, # not working with SAC for some reason +) +bc_trainer.train(n_epochs=10) + +bc_trainer.policy.save("bc_policy_ppo.zip") + +# reward, _ = evaluate_policy(bc_trainer.policy, env, 1) +# print(reward) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py new file mode 100644 index 0000000..3aaae72 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py @@ -0,0 +1,64 @@ +import argparse +import pickle +import pprint + +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms import density as db +from imitation.data import serialize +from imitation.util import util +from stable_baselines3 import PPO +from stable_baselines3.common.policies import ActorCriticPolicy + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + +rng = np.random.default_rng(0) + +register(id="BDX_env", entry_point="env:BDXEnv") +env = util.make_vec_env("BDX_env", rng=rng, n_envs=2) + +dataset = pickle.load(open(args.dataset, "rb")) + +imitation_trainer = PPO( + ActorCriticPolicy, env, learning_rate=3e-4, gamma=0.95, ent_coef=1e-4, n_steps=2048 +) +density_trainer = db.DensityAlgorithm( + venv=env, + rng=rng, + demonstrations=dataset, + rl_algo=imitation_trainer, + density_type=db.DensityType.STATE_ACTION_DENSITY, + is_stationary=True, + kernel="gaussian", + kernel_bandwidth=0.4, + standardise_inputs=True, + allow_variable_horizon=True, +) +density_trainer.train() + + +def print_stats(density_trainer, n_trajectories): + stats = density_trainer.test_policy(n_trajectories=n_trajectories) + print("True reward function stats:") + pprint.pprint(stats) + stats_im = density_trainer.test_policy( + true_reward=False, n_trajectories=n_trajectories + ) + print("Imitation reward function stats:") + pprint.pprint(stats_im) + + +print("Stats before training:") +print_stats(density_trainer, 1) + +density_trainer.train_policy( + 1000000, + progress_bar=True, +) # Train for 1_000_000 steps to approach expert performance. + +print("Stats after training:") +print_stats(density_trainer, 1) + +density_trainer.policy.save("density_policy_ppo.zip") diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py new file mode 100644 index 0000000..7007671 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py @@ -0,0 +1,89 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms.adversarial.gail import GAIL +from imitation.data.wrappers import RolloutInfoWrapper +from imitation.rewards.reward_nets import BasicRewardNet +from imitation.util.networks import RunningNorm +from imitation.util.util import make_vec_env +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy +from stable_baselines3.ppo import MlpPolicy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="env:BDXEnv") + +SEED = 42 +rng = np.random.default_rng(SEED) +# env = gym.make("BDX_env", render_mode=None) +env = make_vec_env( + "BDX_env", + rng=rng, + n_envs=8, + post_wrappers=[lambda env, _: RolloutInfoWrapper(env)], # to compute rollouts +) + + +learner = PPO( + env=env, + policy=MlpPolicy, + batch_size=64, + ent_coef=0.0, + learning_rate=0.0004, + gamma=0.95, + n_epochs=5, + seed=SEED, + tensorboard_log="logs", +) +reward_net = BasicRewardNet( + observation_space=env.observation_space, + action_space=env.action_space, + normalize_input_layer=RunningNorm, +) +gail_trainer = GAIL( + demonstrations=dataset, + demo_batch_size=1024, + gen_replay_buffer_capacity=512, + n_disc_updates_per_round=8, + venv=env, + gen_algo=learner, + reward_net=reward_net, + allow_variable_horizon=True, +) + +print("evaluate the learner before training") +env.seed(SEED) +learner_rewards_before_training, _ = evaluate_policy( + learner, + env, + 100, + return_episode_rewards=True, +) + +print("train the learner and evaluate again") +gail_trainer.train(500000) # Train for 800_000 steps to match expert. + +env.seed(SEED) +learner_rewards_after_training, _ = evaluate_policy( + learner, + env, + 100, + return_episode_rewards=True, +) + +print("mean episode reward before training:", np.mean(learner_rewards_before_training)) +print("mean episode reward after training:", np.mean(learner_rewards_after_training)) + +print("Save the policy") +gail_trainer.policy.save("gail_policy_ppo.zip") diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/record_episodes.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/record_episodes.py new file mode 100644 index 0000000..0fa2250 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/record_episodes.py @@ -0,0 +1,194 @@ +import pickle +import time + +import mujoco +import mujoco.viewer +import numpy as np +import placo +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact + +# from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +# xbox = XboxController() + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + +EPISODE_LENGTH = 2000 + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +walking = True +recording = False +episodes = [] +current_episode = {"observations": [], "actions": []} + + +left_contact = False +right_contact = False + +start_button_timeout = time.time() + + +def xbox_input(): + global target_velocity, target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.5: + target_head_pitch = inputs["r_y"] * np.deg2rad(45) + target_head_yaw = -inputs["r_x"] * np.deg2rad(120) + target_head_z_offset = inputs["r_trigger"] * 0.08 + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + target_velocity = np.array([-inputs["l_y"], inputs["l_x"], -inputs["r_x"]]) + + +def key_callback(keycode): + global recording, walking, target_step_size_x, target_step_size_y, target_yaw, walk_engine, data, t + if keycode == 257: # enter + start_stop_recording() + if keycode == 261: # delete + walking = False + target_step_size_x = 0 + target_step_size_y = 0 + target_yaw = 0 + walk_engine.reset() + data.qpos[:7] = 0 + data.qpos[2] = 0.19 + data.ctrl[:] = 0 + + +def get_observation(): + global left_contact, right_contact, data + + position = ( + data.qpos.flat.copy() + ) # position/rotation of trunk + position of all joints + velocity = ( + data.qvel.flat.copy() + ) # positional/angular velocity of trunk + of all joints + + obs = np.concatenate( + [ + position, + velocity, + [left_contact, right_contact], + ] + ) + # print("OBS SIZE", len(obs)) + return obs + + +def start_stop_recording(): + global recording, current_episode + recording = not recording + if not recording: + print("Stop recording") + # store one last observation here + current_episode["observations"].append(list(get_observation())) + + episode = Trajectory( + np.array(current_episode["observations"]), + np.array(current_episode["actions"]), + None, + True, + ) + episodes.append(episode) + with open("dataset.pkl", "wb") as f: + pickle.dump(episodes, f) + else: + print("Start recording") + current_episode = {"observations": [], "actions": []} + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine(robot) + + +def get_imu(data): + + rot_mat = np.array(data.body("base").xmat).reshape(3, 3) + gyro = R.from_matrix(rot_mat).as_euler("xyz") + + accelerometer = np.array(data.body("base").cvel)[3:] + + return gyro, accelerometer + + +def get_feet_contact(data, model): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +start_stop_recording() # start recording +prev = data.time +try: + while True: + dt = data.time - prev + + # xbox_input() + + # Get sensor data + right_contact, left_contact = get_feet_contact(data, model) + gyro, accelerometer = get_imu(data) + + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + angles = walk_engine.get_angles() + + # store obs here + if recording: + current_episode["observations"].append(list(get_observation())) + current_episode["actions"].append(list(angles.values())) + + if len(current_episode["observations"]) > EPISODE_LENGTH: + start_stop_recording() # stop recording + start_stop_recording() # start recording + + # apply the angles to the robot + data.ctrl[:] = list(angles.values()) + + prev = data.time + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep / 2.5) + +except KeyboardInterrupt: + viewer.close() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/replay_episodes.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/replay_episodes.py new file mode 100644 index 0000000..2b09382 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/replay_episodes.py @@ -0,0 +1,54 @@ +import argparse +import pickle +import time + +import mujoco +import mujoco.viewer + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + +episodes = pickle.load(open(args.dataset, "rb")) + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + + +def key_callback(keycode): + pass + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +current_episode_id = 0 +current_episode = episodes[current_episode_id] + +prev = data.time +try: + while True: + for i in range(len(current_episode.acts)): + angles = current_episode.acts[i] + # obs = current_episode.obs[i] + # print(len(obs)) + dt = data.time - prev + + data.ctrl[:] = angles + + prev = data.time + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep / 2.5) + print("new episode") + current_episode_id += 1 + if current_episode_id >= len(episodes): + print("saw all episodes, restarting") + current_episode_id = 0 + + current_episode = episodes[current_episode_id] + + +except KeyboardInterrupt: + viewer.close() + +viewer.close() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/train.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/train.py new file mode 100644 index 0000000..ffa1f8d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/train.py @@ -0,0 +1,150 @@ +import argparse +import os +from datetime import datetime + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 +from stable_baselines3.common.noise import NormalActionNoise + + +def train(env, sb3_algo, model_dir, log_dir, pretrained=None, device="cuda"): + n_actions = env.action_space.shape[-1] + # SAC parameters found here https://github.com/hill-a/stable-baselines/issues/840#issuecomment-623171534 + if pretrained is None: + match sb3_algo: + case "SAC": + model = SAC( + "MlpPolicy", + env, + verbose=1, + device=device, + tensorboard_log=log_dir, + # action_noise=NormalActionNoise( + # mean=np.zeros(n_actions), sigma=0.1 * np.ones(n_actions) + # ), + # learning_starts=10000, + # batch_size=100, + # learning_rate=1e-3, + # train_freq=1000, + # gradient_steps=1000, + # policy_kwargs=dict(net_arch=[400, 300]), + ) + case "TD3": + model = TD3( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "A2C": + model = A2C( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "TQC": + model = TQC( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "PPO": + model = PPO( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case _: + print("Algorithm not found") + return + else: + match sb3_algo: + case "SAC": + model = SAC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TD3": + model = TD3.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "A2C": + model = A2C.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TQC": + model = TQC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case _: + print("Algorithm not found") + return + + TIMESTEPS = 10000 + iters = 0 + while True: + iters += 1 + + model.learn( + total_timesteps=TIMESTEPS, + reset_num_timesteps=False, + progress_bar=True, + ) + model.save(f"{model_dir}/{sb3_algo}_{TIMESTEPS*iters}") + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="Train BDX") + parser.add_argument( + "-a", + "--algo", + type=str, + choices=["SAC", "TD3", "A2C", "TQC", "PPO"], + default="SAC", + ) + parser.add_argument("-p", "--pretrained", type=str, required=False) + parser.add_argument("-d", "--device", type=str, required=False, default="cuda") + + parser.add_argument( + "-n", + "--name", + type=str, + required=False, + default=datetime.now().strftime("%Y-%m-%d_%H-%M-%S"), + help="Name of the experiment", + ) + + args = parser.parse_args() + + register( + id="BDX_env", + entry_point="env_humanoid:BDXEnv", + max_episode_steps=None, # formerly 500 + autoreset=True, + ) + + env = gym.make("BDX_env", render_mode=None) + # Create directories to hold models and logs + model_dir = args.name + log_dir = "logs/" + args.name + os.makedirs(model_dir, exist_ok=True) + os.makedirs(log_dir, exist_ok=True) + + train( + env, + args.algo, + pretrained=args.pretrained, + model_dir=model_dir, + log_dir=log_dir, + device=args.device, + ) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/view_hdf5.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/view_hdf5.py new file mode 100644 index 0000000..27e7086 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/view_hdf5.py @@ -0,0 +1,11 @@ +import h5py + +data = h5py.File( + "/home/antoine/MISC/mini_BDX/experiments/RL/data/test_raw/episode_0.hdf5", "r" +) +print(len(data["/action"])) +exit() +for i in range(len(data["/action"])): + print(data["/action"][i]) + print(data["/observations/qpos"][i]) + print(data["/observations/qvel"][i]) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py new file mode 100644 index 0000000..b65c0fe --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py @@ -0,0 +1,116 @@ +import placo +from placo_utils.visualization import robot_viz + +import numpy as np +import time + +from mini_bdx_runtime.io_330 import Dxl330IO + +dxl_io = Dxl330IO("/dev/ttyUSB0", baudrate=3000000, use_sync_read=True) + +# Based on performance graph here https://emanual.robotis.com/docs/en/dxl/x/xc330-m288/ +A = 2.69 +B = 0.19 +current_limit = 2.3 +torque_limit = 1.0 + + +def current_to_torque(current: float) -> float: + """ + Input current in A + Output torque in Nm + """ + torque = (current - B) / A + return min(torque_limit, max(-torque_limit, torque)) + + +def torque_to_current(torque: float) -> float: + """ + Input torque in Nm + Output current in A + """ + current = A * torque + B + return min(current_limit, max(-current_limit, current)) + + +def torque_to_current2(torque: float) -> float: + """ + Input torque in Nm + Output current in A + """ + kt = 2.73 + return torque / kt + + +joints = { + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} + +dxl_io.set_operating_mode({id: 0x0 for id in joints.values()}) # set in current mode + + +def get_right_leg_position(): + present_positions_list = dxl_io.get_present_position(joints.values()) + present_positions = { + joint: -np.deg2rad(position) + for joint, position in zip(joints, present_positions_list) + } + return present_positions + + +robot = placo.HumanoidRobot("../../mini_bdx/robots/bdx/robot.urdf") + +input("press any key to record position") +right_leg_position = get_right_leg_position() +for joint, position in right_leg_position.items(): + robot.set_joint(joint, position) + + +# viz = robot_viz(robot) +# while True: +# viz.display(robot.state.q) +# time.sleep(1 / 20) +# exit() +def get_target_current(): + target_torques = robot.static_gravity_compensation_torques_dict("trunk") + right_leg_target_torques = {} + for joint, torque in target_torques.items(): + if joint in list(joints.keys()): + right_leg_target_torques[joint] = torque + + print("target torque", right_leg_target_torques) + + right_leg_target_current = {} + for joint, torque in right_leg_target_torques.items(): + right_leg_target_current[joint] = -torque_to_current2(torque) * 1000 + + print("target current", right_leg_target_current) + right_leg_target_current_id = { + joints[joint]: round(current) + for joint, current in right_leg_target_current.items() + } + print("target current id", right_leg_target_current_id) + return right_leg_target_current_id + + +time.sleep(1) +input("press enter to set torques") +# exit() +dxl_io.enable_torque(joints.values()) +dxl_io.set_goal_current(get_target_current()) +try: + while True: + right_leg_position = get_right_leg_position() + for joint, position in right_leg_position.items(): + robot.set_joint(joint, position) + dxl_io.set_goal_current(get_target_current()) + print("running") + time.sleep(1.0) +except KeyboardInterrupt: + print("STOP") + dxl_io.disable_torque(joints.values()) + pass diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/check_speed.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/check_speed.py new file mode 100644 index 0000000..ce20a01 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/check_speed.py @@ -0,0 +1,59 @@ +from mini_bdx_runtime.hwi import HWI +from mini_bdx_runtime.rl_utils import make_action_dict, mujoco_joints_order +import time +import numpy as np +import mujoco +import mujoco_viewer +import pickle + +hwi = HWI("/dev/ttyUSB0") + +hwi.turn_on() +hwi.set_pid_all([1100, 0, 0]) +# hwi.set_pid([500, 0, 0], "neck_pitch") +# hwi.set_pid([500, 0, 0], "head_pitch") +# hwi.set_pid([500, 0, 0], "head_yaw") + +dt = 0.0001 + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +init_pos = list(hwi.init_pos.values()) +# init_pos += [0, 0] +data.qpos[:13] = init_pos +data.ctrl[:13] = init_pos + +dof = 7 +a = 0.3 +f = 3 + +recording = {} +recording["mujoco_vel"] = [] +recording["robot_vel"] = [] +try: + while True: + target = a * np.sin(2 * np.pi * f * time.time()) + full_target = init_pos.copy() + full_target[dof] += target + + # + data.ctrl[:13] = full_target + action_dict = make_action_dict(full_target, mujoco_joints_order) + hwi.set_position_all(action_dict) + + mujoco.mj_step(model, data, 50) + + mujoco_vel = data.qvel[dof] + robot_vel = hwi.get_present_velocities()[dof] + + recording["mujoco_vel"].append(mujoco_vel) + recording["robot_vel"].append(robot_vel) + + viewer.render() +except KeyboardInterrupt: + pickle.dump(recording, open("speeds.pkl", "wb")) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/get_data.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/get_data.py new file mode 100644 index 0000000..0fdca7a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/get_data.py @@ -0,0 +1,146 @@ +# run sin motion joints, get command vs data +# in mujoco and on real robot + +import argparse +import os +import pickle +import time + +import mujoco +import mujoco_viewer +import numpy as np +from mini_bdx_runtime.hwi import HWI +from mini_bdx_runtime.rl_utils import ( + ActionFilter, + make_action_dict, + mujoco_joints_order, +) +from utils import dof_to_id, id_to_dof, mujoco_init_pos + +parser = argparse.ArgumentParser() +parser.add_argument("--dof", type=str, default="left_ankle") +parser.add_argument("--move_freq", type=float, default=10) +parser.add_argument("--move_amp", type=float, default=0.5) +parser.add_argument("--ctrl_freq", type=float, default=30) +parser.add_argument("--sampling_freq", type=float, default=100) +parser.add_argument("--duration", type=float, default=5) +parser.add_argument("--save_dir", type=str, default="./data") +parser.add_argument("--saved_actions", type=str, required=False) +args = parser.parse_args() + +if args.saved_actions is not None: + saved_actions = pickle.loads(open(args.saved_actions, "rb").read()) + +os.makedirs(args.save_dir, exist_ok=True) + +dt = 0.001 + +assert args.dof in id_to_dof.values() + + +## === Init mujoco === +# Commented freejoint +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) +data.qpos = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos +mujoco_command_value = [] + +## === Init robot === +hwi = HWI(usb_port="/dev/ttyUSB0") +time.sleep(1) +hwi.turn_on() + +pid = [500, 0, 500] +hwi.set_pid_all(pid) +time.sleep(3) +robot_command_value = [] + +action_filter = ActionFilter(window_size=10) + + +prev = time.time() +last_control = time.time() +last_sample = time.time() +start = time.time() +if args.saved_actions is None: + last_target = 0 +else: + last_target = np.zeros(15) +i = 0 +while True: + t = time.time() + dt = t - prev + if t - last_control > 1 / args.ctrl_freq: + last_control = t + if args.saved_actions is None: + last_target = ( + mujoco_init_pos[dof_to_id[args.dof]] + + np.sin(2 * np.pi * args.move_freq * t) * args.move_amp + ) + data.ctrl[dof_to_id[args.dof]] = last_target + + action_filter.push(last_target) + filtered_action = action_filter.get_filtered_action() + hwi.set_position(args.dof, filtered_action) + else: + last_target = saved_actions[i] + data.ctrl[:] = last_target + action_dict = make_action_dict(last_target, mujoco_joints_order) + hwi.set_position_all(action_dict) + mujoco.mj_step(model, data, 5) + + if t - last_sample > 1 / args.sampling_freq: + last_sample = t + + mujoco_command_value.append( + [ + data.ctrl[:].copy(), + data.qpos[:].copy(), + ] + ) + if args.saved_actions is None: + last_robot_command = np.zeros(15) + print(last_target) + last_robot_command[dof_to_id[args.dof]] = last_target + else: + last_robot_command = last_target + robot_command_value.append( + [ + last_robot_command, + list(hwi.get_present_positions()) + [0, 0], + ] + ) + + mujoco_dof_vel = np.around(data.qvel[dof_to_id[args.dof]], 3) + robot_dof_vel = np.around(hwi.get_present_velocities()[dof_to_id[args.dof]], 3) + print(mujoco_dof_vel, robot_dof_vel) + i += 1 + + if t - start > args.duration: + break + if args.saved_actions is not None: + if i > len(saved_actions) - 1: + break + + viewer.render() + prev = t + +path = os.path.join(args.save_dir, f"{args.dof}.pkl") +data_dict = { + "config": { + "dof": args.dof, + "move_freq": args.move_freq, + "move_amp": args.move_amp, + "ctrl_freq": args.ctrl_freq, + "sampling_freq": args.sampling_freq, + "duration": args.duration, + }, + "mujoco": mujoco_command_value, + "robot": robot_command_value, +} +pickle.dump(data_dict, open(path, "wb")) +print("saved to", path) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot.py new file mode 100644 index 0000000..0772021 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot.py @@ -0,0 +1,71 @@ +import argparse +import pickle + +import matplotlib.pyplot as plt +import numpy as np + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--data", type=str, required=True) +args = parser.parse_args() +data = pickle.load(open(args.data, "rb")) +if "robot" in data and "mujoco" in data: + res = None + while res is None or res not in ["1", "2"]: + res = input("plot robot (1) or mujoco (2) ? ") + if res == "1": + command_value = data["robot"] + title = "Robot" + else: + command_value = data["mujoco"] + title = "Mujoco" +elif "mujoco" in data: + command_value = data["mujoco"] + title = "Mujoco" +elif "robot" in data: + command_value = data["robot"] + title = "Robot" +else: + print("NO DATA") + exit() + + +dofs = { + 0: "right_hip_yaw", + 1: "right_hip_roll", + 2: "right_hip_pitch", + 3: "right_knee", + 4: "right_ankle", + 5: "left_hip_yaw", + 6: "left_hip_roll", + 7: "left_hip_pitch", + 8: "left_knee", + 9: "left_ankle", + 10: "neck_pitch", + 11: "head_pitch", + 12: "head_yaw", + 13: "left_antenna", + 14: "right_antenna", +} +# command_value = np.array(command_value) +fig, axs = plt.subplots(4, 4) +dof_id = 0 +for i in range(4): + for j in range(4): + if i == 3 and j == 3: + break + print(4 * i + j) + command = [] + value = [] + for k in range(len(command_value)): + command.append(command_value[k][0][4 * i + j]) + value.append(command_value[k][1][4 * i + j]) + axs[i, j].plot(command, label="command") + axs[i, j].plot(value, label="value") + axs[i, j].legend() + axs[i, j].set_title(f"{dofs[dof_id]}") + dof_id += 1 + + +name = args.data.split("/")[-1].split(".")[0] +fig.suptitle(title) +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py new file mode 100644 index 0000000..96692a9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py @@ -0,0 +1,204 @@ +import argparse +import pickle + +import matplotlib.pyplot as plt +import numpy as np +from scipy.spatial.transform import Rotation as R + +isaac_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + +parser = argparse.ArgumentParser() +parser.add_argument("--robot_obs", type=str, required=True) +parser.add_argument("--hardware", action="store_true") +args = parser.parse_args() + +robot_obs = pickle.load(open(args.robot_obs, "rb")) + +robot_channels = [] + + +# convert quat to euler for easier reading by a simple human +if not args.hardware: + for i in range(len(robot_obs)): + robot_quat = robot_obs[i][:4] + robot_euler = R.from_quat(robot_quat).as_euler("xyz") + + robot_obs[i] = robot_obs[i][1:] + + robot_obs[i][:3] = robot_euler + + +if not args.hardware: + channels = [ + "base_roll", + "base_pitch", + "base_yaw", + "base_ang_vel[0]", + "base_ang_vel[1]", + "base_ang_vel[2]", + "dof_pos[0]", + "dof_pos[1]", + "dof_pos[2]", + "dof_pos[3]", + "dof_pos[4]", + "dof_pos[5]", + "dof_pos[6]", + "dof_pos[7]", + "dof_pos[8]", + "dof_pos[9]", + "dof_pos[10]", + "dof_pos[11]", + "dof_pos[12]", + "dof_pos[13]", + "dof_pos[14]", + "dof_vel[0]", + "dof_vel[1]", + "dof_vel[2]", + "dof_vel[3]", + "dof_vel[4]", + "dof_vel[5]", + "dof_vel[6]", + "dof_vel[7]", + "dof_vel[8]", + "dof_vel[9]", + "dof_vel[10]", + "dof_vel[11]", + "dof_vel[12]", + "dof_vel[13]", + "dof_vel[14]", + "prev_action[0]", + "prev_action[1]", + "prev_action[2]", + "prev_action[3]", + "prev_action[4]", + "prev_action[5]", + "prev_action[6]", + "prev_action[7]", + "prev_action[8]", + "prev_action[9]", + "prev_action[10]", + "prev_action[11]", + "prev_action[12]", + "prev_action[13]", + "prev_action[14]", + "commands[0]", + "commands[1]", + "commands[2]", + ] +else: + channels = [ + "base_lin_vel[0]", + "base_lin_vel[1]", + "base_lin_vel[2]", + "base_ang_vel[0]", + "base_ang_vel[1]", + "base_ang_vel[2]", + "projected_gravity[0]", + "projected_gravity[1]", + "projected_gravity[2]", + "commands[0]", + "commands[1]", + "commands[2]", + "dof_pos[0]", + "dof_pos[1]", + "dof_pos[2]", + "dof_pos[3]", + "dof_pos[4]", + "dof_pos[5]", + "dof_pos[6]", + "dof_pos[7]", + "dof_pos[8]", + "dof_pos[9]", + "dof_pos[10]", + "dof_pos[11]", + "dof_pos[12]", + "dof_pos[13]", + "dof_pos[14]", + "dof_vel[0]", + "dof_vel[1]", + "dof_vel[2]", + "dof_vel[3]", + "dof_vel[4]", + "dof_vel[5]", + "dof_vel[6]", + "dof_vel[7]", + "dof_vel[8]", + "dof_vel[9]", + "dof_vel[10]", + "dof_vel[11]", + "dof_vel[12]", + "dof_vel[13]", + "dof_vel[14]", + "actions[0]", + "actions[1]", + "actions[2]", + "actions[3]", + "actions[4]", + "actions[5]", + "actions[6]", + "actions[7]", + "actions[8]", + "actions[9]", + "actions[10]", + "actions[11]", + "actions[12]", + "actions[13]", + "actions[14]", + ] + +# base_lin_vel * self.obs_scales.lin_vel, +# base_ang_vel * self.obs_scales.ang_vel, +# self.projected_gravity, +# self.commands[:, :3] * self.commands_scale, +# (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos, +# self.dof_vel * self.obs_scales.dof_vel, +# self.actions, + +nb_channels = len(robot_obs[0]) +dof_poses = [] +prev_actions = [] + +# select dof_pos and prev_action +for i in range(nb_channels): + if "dof_pos" in channels[i]: + dof_poses.append([obs[i] for obs in robot_obs]) + elif "action" in channels[i]: + prev_actions.append([obs[i] for obs in robot_obs]) + +# print(len(dof_poses)) +# print(len(prev_actions)) +# exit() + +# plot prev action vs dof pos + +nb_dofs = len(dof_poses) +nb_rows = int(np.sqrt(nb_dofs)) +nb_cols = int(np.ceil(nb_dofs / nb_rows)) + +fig, axs = plt.subplots(nb_rows, nb_cols, sharex=True, sharey=True) +for i in range(nb_rows): + for j in range(nb_cols): + if i * nb_cols + j >= nb_dofs: + break + axs[i, j].plot(prev_actions[i * nb_cols + j], label="command") + axs[i, j].plot(dof_poses[i * nb_cols + j], label="value") + axs[i, j].legend() + axs[i, j].set_title(f"{isaac_joints_order[i * nb_cols + j]}") + +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_obs.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_obs.py new file mode 100644 index 0000000..f9f12a1 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_obs.py @@ -0,0 +1,121 @@ +import argparse +import pickle + +import matplotlib.pyplot as plt +import numpy as np +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("--mujoco_obs", type=str, required=True) +parser.add_argument("--robot_obs", type=str, required=True) +args = parser.parse_args() + +mujoco_obs = pickle.load(open(args.mujoco_obs, "rb")) +robot_obs = pickle.load(open(args.robot_obs, "rb")) + + +mujoco_channels = [] +robot_channels = [] + +# # convert quat to euler for easier reading by a simple human +# for i in range(min(len(mujoco_obs), len(robot_obs))): +# mujoco_quat = mujoco_obs[i][:4] +# mujoco_euler = R.from_quat(mujoco_quat).as_euler("xyz") + +# robot_quat = robot_obs[i][:4] +# robot_euler = R.from_quat(robot_quat).as_euler("xyz") + +# mujoco_obs[i] = mujoco_obs[i][1:] +# robot_obs[i] = robot_obs[i][1:] + +# mujoco_obs[i][:3] = mujoco_euler +# robot_obs[i][:3] = robot_euler + +nb_channels = len(mujoco_obs[0]) + +for i in range(nb_channels): + mujoco_channels.append([obs[i] for obs in mujoco_obs]) + robot_channels.append([obs[i] for obs in robot_obs]) + +channels = [ + "base_roll", + "base_pitch", + "base_yaw", + "base_quat[0]", + "base_quat[1]", + "base_quat[2]", + "base_quat[3]", + "base_ang_vel[0]", + "base_ang_vel[1]", + "base_ang_vel[2]", + "dof_pos[0]", + "dof_pos[1]", + "dof_pos[2]", + "dof_pos[3]", + "dof_pos[4]", + "dof_pos[5]", + "dof_pos[6]", + "dof_pos[7]", + "dof_pos[8]", + "dof_pos[9]", + "dof_pos[10]", + "dof_pos[11]", + "dof_pos[12]", + "dof_pos[13]", + "dof_pos[14]", + "dof_vel[0]", + "dof_vel[1]", + "dof_vel[2]", + "dof_vel[3]", + "dof_vel[4]", + "dof_vel[5]", + "dof_vel[6]", + "dof_vel[7]", + "dof_vel[8]", + "dof_vel[9]", + "dof_vel[10]", + "dof_vel[11]", + "dof_vel[12]", + "dof_vel[13]", + "dof_vel[14]", + "prev_action[0]", + "prev_action[1]", + "prev_action[2]", + "prev_action[3]", + "prev_action[4]", + "prev_action[5]", + "prev_action[6]", + "prev_action[7]", + "prev_action[8]", + "prev_action[9]", + "prev_action[10]", + "prev_action[11]", + "prev_action[12]", + "prev_action[13]", + "prev_action[14]", + "commands[0]", + "commands[1]", + "commands[2]", +] + +# one sub plot per channel, robot vs mujoco +# arrange as an array of sqrt(nb_channels) x sqrt(nb_channels) + +nb_rows = int(np.sqrt(nb_channels)) +nb_cols = int(np.ceil(nb_channels / nb_rows)) + +fig, axs = plt.subplots(nb_rows, nb_cols, sharex=True, sharey=True) +for i in range(nb_rows): + for j in range(nb_cols): + if i * nb_cols + j >= nb_channels: + break + axs[i, j].plot(mujoco_channels[i * nb_cols + j], label="mujoco") + axs[i, j].plot(robot_channels[i * nb_cols + j], label="robot") + axs[i, j].legend() + axs[i, j].set_title(f"{channels[i * nb_cols + j]}") + + +fig.suptitle("Mujoco vs Robot") +# tight layout +# plt.tight_layout() +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_speeds.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_speeds.py new file mode 100644 index 0000000..a5c48cc --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_speeds.py @@ -0,0 +1,22 @@ +import pickle + +# data is +# recording = {} +# recording["mujoco_vel"] = [] +# recording["robot_vel"] = [] +data = pickle.load(open("speeds.pkl", "rb")) + + +import matplotlib.pyplot as plt + +mujoco_vel = data.get("mujoco_vel", []) +robot_vel = data.get("robot_vel", []) + +plt.figure() +plt.plot(mujoco_vel, label="Mujoco Velocity") +plt.plot(robot_vel, label="Robot Velocity") +plt.xlabel("Time Step") +plt.ylabel("Velocity") +plt.title("Mujoco Velocity vs Robot Velocity") +plt.legend() +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/utils.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/utils.py new file mode 100644 index 0000000..2919a7b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/utils.py @@ -0,0 +1,42 @@ +import numpy as np + +mujoco_init_pos = np.array( + [ + # right_leg + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + # left leg + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + # head + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] +) +id_to_dof = { + 0: "right_hip_yaw", + 1: "right_hip_roll", + 2: "right_hip_pitch", + 3: "right_knee", + 4: "right_ankle", + 5: "left_hip_yaw", + 6: "left_hip_roll", + 7: "left_hip_pitch", + 8: "left_knee", + 9: "left_ankle", + 10: "neck_pitch", + 11: "head_pitch", + 12: "head_yaw", + 13: "left_antenna", + 14: "right_antenna", +} +dof_to_id = {v: k for k, v in id_to_dof.items()} diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/.gitignore b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/.gitignore new file mode 100644 index 0000000..fac783a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/.gitignore @@ -0,0 +1 @@ +*.onnx \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py new file mode 100644 index 0000000..9d3d7b2 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py @@ -0,0 +1,137 @@ +import argparse +import json +import os +import time +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco +import mujoco_viewer +import pygame +import numpy as np +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.rl_utils import action_to_pd_targets, mujoco_to_isaac +from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.utils.rl_utils import ( + isaac_to_mujoco, +) + +parser = argparse.ArgumentParser() +parser.add_argument("-x", "--xbox_controller", action="store_true") +parser.add_argument("-k", "--keyboard", action="store_true") +args = parser.parse_args() + +if args.xbox_controller: + xbox = XboxController() + + +if args.keyboard: + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf") + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.0001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +# lower com 0.16 +isaac_init_pos = np.array( + [ + -0.03455234018541292, + 0.055730747490168285, + 0.5397158397618105, + -1.3152788306721914, + 0.6888361815639528, + -0.1745314896173976, + -0.17453429522668937, + 0, + 0, + 0, + -0.03646051060835733, + -0.03358034284950263, + 0.5216150220237578, + -1.326235199315616, + 0.7179857110436013, + ] +) + +mujoco_init_pos = np.array(isaac_to_mujoco(isaac_init_pos)) + + +def xbox_input(): + inputs = xbox.read() + step_size_x = -inputs["l_y"] * 0.02 + step_size_y = -inputs["l_x"] * 0.02 + yaw = -inputs["r_x"] * 0.2 + 0.001 + + return step_size_x, step_size_y, yaw + + +def keyboard_input(): + keys = pygame.key.get_pressed() + step_size_x = 0 + step_size_y = 0 + yaw = 0 + if keys[pygame.K_z]: + step_size_x = 0.03 + if keys[pygame.K_s]: + step_size_x = -0.03 + if keys[pygame.K_q]: + yaw = 0.2 + if keys[pygame.K_d]: + yaw = -0.2 + pygame.event.pump() # process event queue + + return step_size_x, step_size_y, yaw + + +data.qpos[3 : 3 + 4] = [1, 0, 0.05, 0] +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos +prev = time.time() +while True: + t = time.time() + + if args.xbox_controller: + step_size_x, step_size_y, yaw = xbox_input() + elif args.keyboard: + step_size_x, step_size_y, yaw = keyboard_input() + else: + step_size_x, step_size_y, yaw = 0.02, 0, 0.001 + + pwe.set_traj(step_size_x, step_size_y, yaw) + + right_contact, left_contact = get_feet_contact() + + pwe.tick(t - prev, left_contact, right_contact) + # if pwe.t < 0: # for stand + angles = pwe.get_angles() + action = isaac_to_mujoco(list(angles.values())) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 50) # 4 seems good + + # if pwe.t <= 0: + # print("waiting ...") + # viewer.render() + # continue + prev = t + viewer.render() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py new file mode 100644 index 0000000..04c6cbf --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py @@ -0,0 +1,159 @@ +import argparse +import json +import os +import time +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco +import mujoco_viewer +import numpy as np +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.rl_utils import mujoco_to_isaac + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf") + +EPISODE_LENGTH = 10 +NB_EPISODES_TO_RECORD = 1 +FPS = 60 + +# For IsaacGymEnvs +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + +parser = argparse.ArgumentParser() +parser.add_argument( + "--hardware", action="store_true", help="use AMP_for_hardware format" +) +args = parser.parse_args() + +episodes = [] + +current_episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], +} + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +mujoco_init_pos = np.array( + [ + # right_leg + -0.014, + 0.08, + 0.53, + -1.62, + 0.91, + # left leg + 0.013, + 0.077, + 0.59, + -1.63, + 0.86, + # head + -0.17, + -0.17, + 0.0, + 0.0, + 0.0, + ] +) + +data.qpos[3 : 3 + 4] = [1, 0, 0.05, 0] +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos +x_qvels = [] +while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + done = False + prev = time.time() + start = time.time() + last_record = time.time() + pwe.set_traj(0.02, 0.0, 0.001) + while not done: + t = time.time() + dt = t - prev + x_qvels.append(data.qvel[0].copy()) + print(np.around(np.mean(x_qvels[-30:]), 3)) + # qpos = env.data.qpos[:3].copy() + # qpos[2] = 0.15 + # env.data.qpos[:3] = qpos + right_contact, left_contact = get_feet_contact() + + pwe.tick(dt, left_contact, right_contact) + # if pwe.t < 0: # for stand + angles = pwe.get_angles() + action = list(angles.values()) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 7) # 4 seems good + + if pwe.t <= 0: + start = time.time() + print("waiting ...") + prev = t + viewer.render() + continue + + if t - last_record >= 1 / FPS: + root_position = list(np.around(data.body("base").xpos, 3)) + root_orientation = list(np.around(data.body("base").xquat, 3)) # w, x, y, z + + # convert to x, y, z, w + root_orientation = [ + root_orientation[1], + root_orientation[2], + root_orientation[3], + root_orientation[0], + ] + + joints_positions = list(np.around(data.qpos[7 : 7 + 15], 3)) + + joints_positions = mujoco_to_isaac(joints_positions) + + current_episode["Frames"].append( + root_position + root_orientation + joints_positions + ) + last_record = time.time() + + if time.time() - start > EPISODE_LENGTH * 2: + print("Episode done") + print(len(current_episode["Frames"])) + episodes.append(current_episode) + + # save json as bdx_walk.txt + with open("bdx_walk.txt", "w") as f: + json.dump(current_episode, f) + + current_episode = { + "LoopMode": "Wrap", + "FrameDuration": 0.01667, + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], + } + done = True + viewer.render() + prev = t diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py new file mode 100644 index 0000000..35e7e27 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py @@ -0,0 +1,189 @@ +import argparse +import time + +import mujoco +import mujoco.viewer +import numpy as np +import placo +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser() +parser.add_argument("-x", "--xbox_controller", action="store_true") +args = parser.parse_args() + +if args.xbox_controller: + xbox = XboxController() + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +time_since_last_left_contact = 0 +time_since_last_right_contact = 0 +walking = False + +start_button_timeout = time.time() + + +def xbox_input(): + global target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.5: + target_head_pitch = inputs["r_y"] * np.deg2rad(45) + target_head_yaw = -inputs["r_x"] * np.deg2rad(120) + target_head_z_offset = inputs["r_trigger"] * 0.08 + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + +# Tune the walk +def key_callback(keycode): + global target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, max_target_step_size_x, max_target_step_size_y, max_target_yaw + if keycode == 265: # up + max_target_step_size_x += 0.005 + if keycode == 264: # down + max_target_step_size_x -= 0.005 + if keycode == 263: # left + max_target_step_size_y -= 0.005 + if keycode == 262: # right + max_target_step_size_y += 0.005 + if keycode == 81: # a + max_target_yaw += np.deg2rad(1) + if keycode == 69: # e + max_target_yaw -= np.deg2rad(1) + if keycode == 257: # enter + walking = not walking + if keycode == 86: # v + walk_engine.trunk_pitch_roll_compensation = ( + not walk_engine.trunk_pitch_roll_compensation + ) + if keycode == 79: # o + walk_engine.swing_gain -= 0.005 + if keycode == 80: # p + walk_engine.swing_gain += 0.005 + if keycode == 76: # l + walk_engine.tune_trunk_x_offset += 0.001 + if keycode == 59: # m + walk_engine.tune_trunk_x_offset -= 0.001 + if keycode == 266: # page up + walk_engine.frequency += 0.1 + if keycode == 267: # page down + walk_engine.frequency -= 0.1 + if keycode == 268: # begin line + walk_engine.default_trunk_z_offset -= 0.001 + if keycode == 269: # end line + walk_engine.default_trunk_z_offset += 0.001 + if keycode == 32: # space + target_step_size_x = 0 + target_step_size_y = 0 + target_yaw = 0 + if keycode == 261: # delete + walking = False + target_step_size_x = 0 + target_step_size_y = 0 + target_yaw = 0 + walk_engine.reset() + data.qpos[:7] = 0 + data.qpos[2] = 0.19 + data.ctrl[:] = 0 + t = 0 + print(keycode) + print("----------------") + print("walking" if walking else "not walking") + print( + "trunk pitch roll compensation (v)", walk_engine.trunk_pitch_roll_compensation + ) + print("MAX_TARGET_STEP_SIZE_X (up, down)", max_target_step_size_x) + print("MAX_TARGET_STEP_SIZE_Y (left, right)", max_target_step_size_y) + print("MAX_TARGET_YAW (a, e)", np.rad2deg(max_target_yaw)) + print("swing gain (o, p)", walk_engine.swing_gain) + print("trunk x offset (l, m)", walk_engine.trunk_x_offset) + print("default trunk z offset (begin, end)", walk_engine.default_trunk_z_offset) + print("frequency (pageup, pagedown)", walk_engine.frequency) + print("----------------") + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine( + robot, default_trunk_x_offset=-0.03, frequency=3 +) # , max_rise_gain=0.1) + + +def get_imu(data): + + rot_mat = np.array(data.body("base").xmat).reshape(3, 3) + gyro = R.from_matrix(rot_mat).as_euler("xyz") + + accelerometer = np.array(data.body("base").cvel)[3:] + + return gyro, accelerometer + + +def get_feet_contact(data): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +prev = data.time +try: + while True: + dt = data.time - prev + + if args.xbox_controller: + xbox_input() + + # Get sensor data + right_contact, left_contact = get_feet_contact(data) + gyro, accelerometer = get_imu(data) + + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + angles = walk_engine.get_angles() + # apply the angles to the robot + data.ctrl[:] = list(angles.values()) + [0, 0] + + prev = data.time + mujoco.mj_step(model, data, 4) + viewer.sync() + # time.sleep(model.opt.timestep / 2.5) + +except KeyboardInterrupt: + viewer.close() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py new file mode 100644 index 0000000..ca9dc59 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py @@ -0,0 +1,297 @@ +import argparse +import pickle +import time + +import mujoco +import mujoco_viewer +import numpy as np +import pygame +from scipy.spatial.transform import Rotation as R + +from mini_bdx.onnx_infer import OnnxInfer +from mini_bdx.utils.rl_utils import ( + action_to_pd_targets, + isaac_to_mujoco, + mujoco_to_isaac, +) +from mini_bdx_runtime.rl_utils import LowPassActionFilter + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("-k", action="store_true", default=False) +parser.add_argument("--rma", action="store_true", default=False) +parser.add_argument("--awd", action="store_true", default=False) +parser.add_argument("--adaptation_module_path", type=str, required=False) +parser.add_argument("--replay_obs", type=str, required=False, default=None) +args = parser.parse_args() + +if args.k: + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + + +if args.replay_obs is not None: + path = args.replay_obs + path = path[: -len("obs.pkl")] + actions_path = path + "actions.pkl" + replay_obs = pickle.load(open(args.replay_obs, "rb")) + replay_actions = pickle.load(open(actions_path, "rb")) +replay_index = 0 + +# Params +dt = 0.0001 +linearVelocityScale = 2.0 if not args.awd else 0.5 +angularVelocityScale = 0.25 +dof_pos_scale = 1.0 +dof_vel_scale = 0.05 +action_clip = (-1, 1) +obs_clip = (-5, 5) +action_scale = 0.25 if not args.awd else 1.0 + + +isaac_init_pos = np.array( + [ + -0.0285397830292128, + 0.01626303761810685, + 1.0105624704499077, + -1.4865015965817336, + 0.6504953719748071, + -0.17453292519943295, + -0.17453292519943295, + 0, + 0, + 0, + 0.001171696610228082, + 0.006726989242258406, + 1.0129772861831692, + -1.4829304760981399, + 0.6444901047812701, + ] +) + + +mujoco_init_pos = np.array(isaac_to_mujoco(isaac_init_pos)) + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) +# model.opt.gravity[:] = [0, 0, 0] # no gravity + +NUM_OBS = 51 + +policy = OnnxInfer(args.onnx_model_path, awd=args.awd) +if args.rma: + adaptation_module = OnnxInfer(args.adaptation_module_path, "obs_history") + obs_history_size = 15 + obs_history = np.zeros((obs_history_size, NUM_OBS)).tolist() + + +class ObsDelaySimulator: + def __init__(self, delay_ms): + self.delay_ms = delay_ms + self.obs = [] + self.t0 = None + + def push(self, obs, t): + self.obs.append(obs) + if self.t0 is None: + self.t0 = t + + def get(self, t): + if t - self.t0 < self.delay_ms / 1000: + return np.zeros(NUM_OBS) + print(len(self.obs)) + return self.obs.pop(0) + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +def get_obs(data, prev_isaac_action, commands): + global replay_index + if args.replay_obs is not None: + obs = replay_obs[replay_index] + return obs + + base_lin_vel = ( + data.sensor("linear-velocity").data.astype(np.double) * linearVelocityScale + ) + + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + # # Remove yaw component + # rot_euler = R.from_quat(base_quat).as_euler("xyz", degrees=False) + # rot_euler[1] += np.deg2rad(-15) + # base_quat = R.from_euler("xyz", rot_euler, degrees=False).as_quat() + + base_ang_vel = ( + data.sensor("angular-velocity").data.astype(np.double) * angularVelocityScale + ) + + mujoco_dof_pos = data.qpos[7 : 7 + 15].copy() + isaac_dof_pos = mujoco_to_isaac(mujoco_dof_pos) + + isaac_dof_pos_scaled = (isaac_dof_pos - isaac_init_pos) * dof_pos_scale + + mujoco_dof_vel = data.qvel[6 : 6 + 15].copy() + isaac_dof_vel = mujoco_to_isaac(mujoco_dof_vel) + isaac_dof_vel_scaled = list(np.array(isaac_dof_vel) * dof_vel_scale) + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + if not args.awd: + obs = np.concatenate( + [ + projected_gravity, + commands, + isaac_dof_pos_scaled, + isaac_dof_vel_scaled, + prev_isaac_action, + ] + ) + else: + obs = np.concatenate( + [ + projected_gravity, + isaac_dof_pos, + isaac_dof_vel, + prev_isaac_action, + commands, + ] + ) + return obs + + +prev_isaac_action = np.zeros(15) +commands = [0.14 * 2, 0.0, 0.0] +prev = data.time +last_control = data.time +control_freq = 30 # hz +hist_freq = 30 # hz +adaptation_module_freq = 5 # hz +last_adaptation = data.time +last_hist = data.time +i = 0 +data.qpos[3 : 3 + 4] = [1, 0, 0.0, 0] +cutoff_frequency = 40 + + +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos + +action_filter = LowPassActionFilter( + control_freq=control_freq, cutoff_frequency=cutoff_frequency +) + +mujoco_saved_obs = [] +# obs_delay_simulator = ObsDelaySimulator(0) +start = time.time() +saved_latent = [] +try: + start = time.time() + while True: + # t = time.time() + t = data.time + if time.time() - start < 1: + last_control = t + if t - last_control >= 1 / control_freq: + isaac_obs = get_obs(data, prev_isaac_action, commands) + # obs_delay_simulator.push(isaac_obs, t) + # isaac_obs = obs_delay_simulator.get(t) + if args.rma: + if t - last_hist >= 1 / hist_freq: + obs_history.append(isaac_obs) + obs_history = obs_history[-obs_history_size:] + last_hist = t + + mujoco_saved_obs.append(isaac_obs) + + if args.rma: + if t - last_adaptation >= 1 / adaptation_module_freq: + latent = adaptation_module.infer(np.array(obs_history).flatten()) + last_adaptation = t + saved_latent.append(latent) + policy_input = np.concatenate([isaac_obs, latent]) + isaac_action = policy.infer(policy_input) + else: + isaac_action = policy.infer(isaac_obs) + + if args.replay_obs: + replayed_action = replay_actions[replay_index] + print("infered action", np.around(isaac_action, 3)) + print("replayed action", np.around(replayed_action, 3)) + print("diff", np.around(isaac_action - replayed_action, 3)) + print("==") + # action_filter.push(isaac_action) + # isaac_action = action_filter.get_filtered_action() + + prev_isaac_action = isaac_action.copy() # * action_scale + + isaac_action = isaac_action * action_scale + isaac_init_pos + + mujoco_action = isaac_to_mujoco(isaac_action) + + data.ctrl[:] = mujoco_action.copy() + + last_control = t + i += 1 + + if args.k: + keys = pygame.key.get_pressed() + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keys[pygame.K_z]: + lin_vel_x = 0.14 + if keys[pygame.K_s]: + lin_vel_x = -0.14 + if keys[pygame.K_q]: + lin_vel_y = 0.1 + if keys[pygame.K_d]: + lin_vel_y = -0.1 + if keys[pygame.K_a]: + ang_vel = 0.3 + if keys[pygame.K_e]: + ang_vel = -0.3 + + commands[0] = lin_vel_x + commands[1] = lin_vel_y + commands[2] = ang_vel + + commands = list( + np.array(commands) + * np.array( + [ + linearVelocityScale, + linearVelocityScale, + angularVelocityScale, + ] + ) + ) + pygame.event.pump() # process event queue + replay_index += 1 + print(commands) + mujoco.mj_step(model, data, 50) + + viewer.render() + prev = t +except KeyboardInterrupt: + pickle.dump(mujoco_saved_obs, open("mujoco_saved_obs.pkl", "wb")) + pickle.dump(saved_latent, open("mujoco_saved_latent.pkl", "wb")) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py new file mode 100644 index 0000000..59f0603 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py @@ -0,0 +1,278 @@ +import argparse +import pickle +import time + +import mujoco +import mujoco_viewer +import numpy as np +from scipy.spatial.transform import Rotation as R + +from mini_bdx.onnx_infer import OnnxInfer +from mini_bdx.utils.rl_utils import ( + action_to_pd_targets, + isaac_to_mujoco, + mujoco_to_isaac, +) + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("--saved_obs", type=str, required=False) +parser.add_argument("--saved_actions", type=str, required=False) +args = parser.parse_args() + +if args.saved_obs is not None: + saved_obs = pickle.loads(open("saved_obs.pkl", "rb").read()) +if args.saved_actions is not None: + saved_actions = pickle.loads(open("saved_actions.pkl", "rb").read()) + + +# Params +dt = 0.0001 +linearVelocityScale = 2.0 +angularVelocityScale = 0.25 +dof_pos_scale = 1.0 +dof_vel_scale = 0.05 +action_clip = (-1, 1) +obs_clip = (-5, 5) +action_scale = 1.0 + + +# mujoco_init_pos = np.array( +# [ +# # right_leg +# -0.014, +# 0.08, +# 0.53, +# -1.32, +# # -1.52, +# 0.91, +# # left leg +# 0.013, +# 0.077, +# 0.59, +# -1.33, +# # -1.53, +# 0.86, +# # head +# -0.17, +# -0.17, +# 0.0, +# 0.0, +# 0.0, +# ] +# ) + +# Higher +mujoco_init_pos = np.array( + [ + -0.03676731090962078, + -0.030315211140564333, + 0.4065815100399598, + -1.0864064934571644, + 0.5932324840794684, + -0.03485756878823724, + 0.052286054888550475, + 0.36623601032755765, + -0.964204465274923, + 0.5112970996901808, + -0.17453292519943295, + -0.17453292519943295, + 0, + 0.0, + 0.0, + ] +) + +pd_action_offset = [ + 0.0000, + -0.5672, + 0.5236, + 0.0000, + 0.0000, + -0.5672, + 0.0000, + 0.0000, + 0.4800, + -0.4800, + 0.0000, + -0.5672, + 0.5236, + 0.0000, + 0.0000, +] + +pd_action_scale = [ + 0.9774, + 1.4050, + 1.4661, + 2.9322, + 2.1991, + 1.0385, + 0.9774, + 2.9322, + 2.2602, + 2.2602, + 0.9774, + 1.4050, + 1.4661, + 2.9322, + 2.1991, +] + + +isaac_init_pos = np.array(mujoco_to_isaac(mujoco_init_pos)) + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) +# model.opt.gravity[:] = [0, 0, 0] # no gravity + +policy = OnnxInfer(args.onnx_model_path) + + +class ImuDelaySimulator: + def __init__(self, delay_ms): + self.delay_ms = delay_ms + self.rot = [] + self.ang_rot = [] + self.t0 = None + + def push(self, rot, ang_rot, t): + self.rot.append(rot) + self.ang_rot.append(ang_rot) + if self.t0 is None: + self.t0 = t + + def get(self): + if time.time() - self.t0 < self.delay_ms / 1000: + return [0, 0, 0, 0], [0, 0, 0] + rot = self.rot.pop(0) + ang_rot = self.ang_rot.pop(0) + + return rot, ang_rot + + +def get_obs(data, isaac_action, commands, imu_delay_simulator: ImuDelaySimulator): + base_lin_vel = ( + data.sensor("linear-velocity").data.astype(np.double) * linearVelocityScale + ) + + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + # Remove yaw component + rot_euler = R.from_quat(base_quat).as_euler("xyz", degrees=False) + rot_euler[2] = 0 + base_quat = R.from_euler("xyz", rot_euler, degrees=False).as_quat() + + base_ang_vel = ( + data.sensor("angular-velocity").data.astype(np.double) * angularVelocityScale + ) + + mujoco_dof_pos = data.qpos[7 : 7 + 15].copy() + isaac_dof_pos = mujoco_to_isaac(mujoco_dof_pos) + + isaac_dof_pos_scaled = (isaac_dof_pos - isaac_init_pos) * dof_pos_scale + + mujoco_dof_vel = data.qvel[6 : 6 + 15].copy() + isaac_dof_vel = mujoco_to_isaac(mujoco_dof_vel) + isaac_dof_vel_scaled = list(np.array(isaac_dof_vel) * dof_vel_scale) + + imu_delay_simulator.push(base_quat, base_ang_vel, time.time()) + base_quat, base_ang_vel = imu_delay_simulator.get() + + obs = np.concatenate( + [ + base_quat, + # base_lin_vel, + base_ang_vel, + isaac_dof_pos_scaled, + isaac_dof_vel_scaled, + isaac_action, + commands, + ] + ) + + return obs + + +prev_isaac_action = np.zeros(15) +commands = [0.15, 0.0, 0.0] +# commands = [0.0, 0.0, 0.0] +# prev = time.time() +# last_control = time.time() +prev = data.time +last_control = data.time +control_freq = 60 # hz +i = 0 +# data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + +# init_rot = [0, -0.1, 0] +# init_rot = [0, 0, 0] +# init_quat = R.from_euler("xyz", init_rot, degrees=False).as_quat() +# data.qpos[3 : 3 + 4] = init_quat +# data.qpos[3 : 3 + 4] = [init_quat[3], init_quat[1], init_quat[2], init_quat[0]] +# data.qpos[3 : 3 + 4] = [1, 0, 0.13, 0] + + +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos + +mujoco_saved_obs = [] +mujoco_saved_actions = [] +command_value = [] +imu_delay_simulator = ImuDelaySimulator(1) +try: + start = time.time() + while True: + # t = time.time() + t = data.time + if t - last_control >= 1 / control_freq: + isaac_obs = get_obs(data, prev_isaac_action, commands, imu_delay_simulator) + mujoco_saved_obs.append(isaac_obs) + + if args.saved_obs is not None: + isaac_obs = saved_obs[i] # works with saved obs + + isaac_obs = np.clip(isaac_obs, obs_clip[0], obs_clip[1]) + + isaac_action = policy.infer(isaac_obs) + if args.saved_actions is not None: + isaac_action = saved_actions[i][0] + isaac_action = np.clip(isaac_action, action_clip[0], action_clip[1]) + prev_isaac_action = isaac_action.copy() + + # isaac_action = action_to_pd_targets( + # isaac_action, pd_action_offset, pd_action_scale + # ) + + isaac_action = isaac_init_pos + isaac_action + + mujoco_action = isaac_to_mujoco(isaac_action) + + last_control = t + i += 1 + + data.ctrl[:] = mujoco_action.copy() + # data.ctrl[:] = mujoco_init_pos + # euler_rot = [np.sin(2 * np.pi * 0.5 * t), 0, 0] + # quat = R.from_euler("xyz", euler_rot, degrees=False).as_quat() + # data.qpos[3 : 3 + 4] = quat + mujoco_saved_actions.append(mujoco_action) + + command_value.append([data.ctrl.copy(), data.qpos[7:].copy()]) + mujoco.mj_step(model, data, 50) + + viewer.render() + prev = t +except KeyboardInterrupt: + data = { + "config": {}, + "mujoco": command_value, + } + pickle.dump(data, open("mujoco_command_value.pkl", "wb")) + pickle.dump(mujoco_saved_obs, open("mujoco_saved_obs.pkl", "wb")) + pickle.dump(mujoco_saved_actions, open("mujoco_saved_actions.pkl", "wb")) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py new file mode 100644 index 0000000..fe445b7 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py @@ -0,0 +1,15 @@ +import pickle +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument( + "--latent", type=str, required=False, default="mujoco_saved_latent.pkl" +) +args = parser.parse_args() + +latent = pickle.load(open(args.latent, "rb")) + +import matplotlib.pyplot as plt + +plt.plot(latent) +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/.gitignore b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/.gitignore new file mode 100644 index 0000000..8587f59 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/.gitignore @@ -0,0 +1,2 @@ +test/ +placo_walk_examples/ \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py new file mode 100644 index 0000000..da2b1fb --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py @@ -0,0 +1,45 @@ +import os +import argparse +from placo_record_amp import record +from mini_bdx.placo_walk_engine import PlacoWalkEngine +import numpy as np + +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--output_dir", type=str, default="recordings") +parser.add_argument( + "-n", "--num_samples", type=int, default=10, help="Number of samples" +) +args = parser.parse_args() + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True) + +dx_range = [-0.04, 0.04] +dy_range = [-0.05, 0.05] +dtheta_range = [-0.15, 0.15] +length = 8 +num_samples = args.num_samples + +args_dict = {} +args_dict["name"] = "test" +args_dict["dx"] = 0 +args_dict["dy"] = 0 +args_dict["dtheta"] = 0 +args_dict["length"] = length +args_dict["meshcat_viz"] = False +args_dict["skip_warmup"] = False +args_dict["stand"] = False +args_dict["hardware"] = True +args_dict["output_dir"] = args.output_dir + +for i in range(num_samples): + args_dict["dx"] = round(np.random.uniform(dx_range[0], dx_range[1]), 2) + args_dict["dy"] = round(np.random.uniform(dy_range[0], dy_range[1]), 2) + args_dict["dtheta"] = round(np.random.uniform(dtheta_range[0], dtheta_range[1]), 2) + args_dict["name"] = str(i) + print("RECORDING ", args_dict["name"]) + print("dx", args_dict["dx"], "dy", args_dict["dy"], "dtheta", args_dict["dtheta"]) + print("==") + pwe.reset() + record(pwe, args_dict) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/bdx_walk.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/bdx_walk.py new file mode 100644 index 0000000..5391892 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/bdx_walk.py @@ -0,0 +1,322 @@ +import argparse +import time +import warnings + +import numpy as np +import placo +from placo_utils.visualization import ( + footsteps_viz, + frame_viz, + line_viz, + robot_frame_viz, + robot_viz, +) + +from mini_bdx.bdx_mujoco_server import BDXMujocoServer +from mini_bdx.hwi import HWI + +warnings.filterwarnings("ignore") + +parser = argparse.ArgumentParser(description="Process some integers.") +parser.add_argument( + "-p", "--pybullet", action="store_true", help="PyBullet visualization" +) +parser.add_argument( + "-m", "--meshcat", action="store_true", help="MeshCat visualization" +) +parser.add_argument("-r", "--robot", action="store_true", help="run on real robot") +parser.add_argument("--mujoco", action="store_true", help="Mujoco visualization") +args = parser.parse_args() + +DT = 0.01 +REFINE = 10 +model_filename = "../../mini_bdx/robots/bdx/robot.urdf" + +# Loading the robot +robot = placo.HumanoidRobot(model_filename) +robot.set_joint_limits("left_knee", -2, -0.01) +robot.set_joint_limits("right_knee", -2, -0.01) + +# Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency +parameters = placo.HumanoidParameters() + +parameters.double_support_ratio = 0.2 # Ratio of double support (0.0 to 1.0) +parameters.startend_double_support_ratio = ( + 1.5 # Ratio duration of supports for starting and stopping walk +) +parameters.planned_timesteps = 48 # Number of timesteps planned ahead +parameters.replan_timesteps = 10 # Replanning each n timesteps +# parameters.zmp_reference_weight = 1e-6 + +# Posture parameters +parameters.walk_com_height = 0.15 # Constant height for the CoM [m] +parameters.walk_foot_height = 0.006 # Height of foot rising while walking [m] +parameters.walk_trunk_pitch = np.deg2rad(10) # Trunk pitch angle [rad] +parameters.walk_foot_rise_ratio = ( + 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0) +) + +# Timing parameters +# parameters.single_support_duration = 1 / ( +# 0.5 * np.sqrt(9.81 / parameters.walk_com_height) +# ) # Constant height for the CoM [m] # Duration of single support phase [s] +parameters.single_support_duration = 0.2 # Duration of single support phase [s] +parameters.single_support_timesteps = ( + 10 # Number of planning timesteps per single support phase +) + +# Feet parameters +parameters.foot_length = 0.06 # Foot length [m] +parameters.foot_width = 0.006 # Foot width [m] +parameters.feet_spacing = 0.12 # Lateral feet spacing [m] +parameters.zmp_margin = 0.0 # ZMP margin [m] +parameters.foot_zmp_target_x = 0.0 # Reference target ZMP position in the foot [m] +parameters.foot_zmp_target_y = 0.0 # Reference target ZMP position in the foot [m] + +# Limit parameters +parameters.walk_max_dtheta = 1 # Maximum dtheta per step [rad] +parameters.walk_max_dy = 0.1 # Maximum dy per step [m] +parameters.walk_max_dx_forward = 0.08 # Maximum dx per step forward [m] +parameters.walk_max_dx_backward = 0.03 # Maximum dx per step backward [m] + +# Creating the kinematics solver +solver = placo.KinematicsSolver(robot) +solver.enable_velocity_limits(True) +# solver.enable_joint_limits(False) +robot.set_velocity_limits(12.0) +solver.dt = DT / REFINE + +# Creating the walk QP tasks +tasks = placo.WalkTasks() +# tasks.trunk_mode = True +# tasks.com_x = 0.04 +tasks.initialize_tasks(solver, robot) +tasks.left_foot_task.orientation().mask.set_axises("yz", "local") +tasks.right_foot_task.orientation().mask.set_axises("yz", "local") +# tasks.trunk_orientation_task.configure("trunk_orientation", "soft", 1e-4) +# tasks.left_foot_task.orientation().configure("left_foot_orientation", "soft", 1e-6) +# tasks.right_foot_task.orientation().configure("right_foot_orientation", "soft", 1e-6) + +# Creating a joint task to assign DoF values for upper body +elbow = -50 * np.pi / 180 +shoulder_roll = 0 * np.pi / 180 +shoulder_pitch = 20 * np.pi / 180 +joints_task = solver.add_joints_task() +joints_task.set_joints( + { + # "left_shoulder_roll": shoulder_roll, + # "left_shoulder_pitch": shoulder_pitch, + # "left_elbow": elbow, + # "right_shoulder_roll": -shoulder_roll, + # "right_shoulder_pitch": shoulder_pitch, + # "right_elbow": elbow, + "head_pitch": 0.0, + "head_yaw": 0.0, + "neck_pitch": 0.0, + "left_antenna": 0.0, + "right_antenna": 0.0, + # "left_ankle_roll": 0.0, + # "right_ankle_roll": 0.0 + } +) +joints_task.configure("joints", "soft", 1.0) + +# cam = solver.add_centroidal_momentum_task(np.array([0., 0., 0.])) +# cam.mask.set_axises("x", "custom") +# cam.mask.R_custom_world = robot.get_T_world_frame("trunk")[:3, :3].T +# cam.configure("cam", "soft", 1e-3) + +# Placing the robot in the initial position +print("Placing the robot in the initial position...") +tasks.reach_initial_pose( + np.eye(4), + parameters.feet_spacing, + parameters.walk_com_height, + parameters.walk_trunk_pitch, +) +print("Initial position reached") + + +# Creating the FootstepsPlanner +repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive(parameters) +d_x = 0.1 +d_y = 0.0 +d_theta = 0.0 +nb_steps = 5 +repetitive_footsteps_planner.configure(d_x, d_y, d_theta, nb_steps) + +# Planning footsteps +T_world_left = placo.flatten_on_floor(robot.get_T_world_left()) +T_world_right = placo.flatten_on_floor(robot.get_T_world_right()) +footsteps = repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, T_world_left, T_world_right +) + +supports = placo.FootstepsPlanner.make_supports( + footsteps, True, parameters.has_double_support(), True +) + +# Creating the pattern generator and making an initial plan +walk = placo.WalkPatternGenerator(robot, parameters) +trajectory = walk.plan(supports, robot.com_world(), 0.0) + +if args.pybullet: + # Loading the PyBullet simulation + import pybullet as p + from onshape_to_robot.simulation import Simulation + + sim = Simulation(model_filename, realTime=True, dt=DT, ignore_self_collisions=True) +elif args.meshcat: + # Starting Meshcat viewer + viz = robot_viz(robot) + footsteps_viz(trajectory.get_supports()) +elif args.robot: + hwi = HWI() + hwi.turn_on() + time.sleep(2) +elif args.mujoco: + time_since_last_right_contact = 0.0 + time_since_last_left_contact = 0.0 + bdx_mujoco_server = BDXMujocoServer( + model_path="../../mini_bdx/robots/bdx", gravity_on=True + ) + bdx_mujoco_server.start() +else: + print("No visualization selected, use either -p or -m") + exit() + +# Timestamps +start_t = time.time() +initial_delay = -1.0 +t = initial_delay +last_display = time.time() +last_replan = 0 +petage_de_gueule = False +got_input = False +while True: + if got_input: + repetitive_footsteps_planner.configure(d_x, d_y, d_theta, nb_steps) + got_input = False + + # Invoking the IK QP solver + for k in range(REFINE): + # Updating the QP tasks from planned trajectory + if not petage_de_gueule: + tasks.update_tasks_from_trajectory(trajectory, t - DT + k * DT / REFINE) + + robot.update_kinematics() + qd_sol = solver.solve(True) + # solver.dump_status() + + # Ensuring the robot is kinematically placed on the floor on the proper foot to avoid integration drifts + # if not trajectory.support_is_both(t): + # robot.update_support_side(str(trajectory.support_side(t))) + # robot.ensure_on_floor() + + # If enough time elapsed and we can replan, do the replanning + if ( + t - last_replan > parameters.replan_timesteps * parameters.dt() + and walk.can_replan_supports(trajectory, t) + ): + last_replan = t + + # Replanning footsteps from current trajectory + supports = walk.replan_supports(repetitive_footsteps_planner, trajectory, t) + + # Replanning CoM trajectory, yielding a new trajectory we can switch to + trajectory = walk.replan(supports, trajectory, t) + + if args.meshcat: + # Drawing footsteps + footsteps_viz(supports) + + # Drawing planned CoM trajectory on the ground + coms = [ + [*trajectory.get_p_world_CoM(t)[:2], 0.0] + for t in np.linspace(trajectory.t_start, trajectory.t_end, 100) + ] + line_viz("CoM_trajectory", np.array(coms), 0xFFAA00) + + # During the warmup phase, the robot is enforced to stay in the initial position + if args.pybullet: + if t < -2: + T_left_origin = sim.transformation("origin", "left_foot_frame") + T_world_left = sim.poseToMatrix(([0.0, 0.0, 0.05], [0.0, 0.0, 0.0, 1.0])) + T_world_origin = T_world_left @ T_left_origin + + sim.setRobotPose(*sim.matrixToPose(T_world_origin)) + + joints = {joint: robot.get_joint(joint) for joint in sim.getJoints()} + applied = sim.setJoints(joints) + sim.tick() + + # Updating meshcat display periodically + elif args.meshcat: + if time.time() - last_display > 0.01: + last_display = time.time() + viz.display(robot.state.q) + + # frame_viz("left_foot_target", trajectory.get_T_world_left(t)) + # frame_viz("right_foot_target", trajectory.get_T_world_right(t)) + # robot_frame_viz(robot, "left_foot") + # robot_frame_viz(robot, "right_foot") + + T_world_trunk = np.eye(4) + T_world_trunk[:3, :3] = trajectory.get_R_world_trunk(t) + T_world_trunk[:3, 3] = trajectory.get_p_world_CoM(t) + frame_viz("trunk_target", T_world_trunk) + # footsteps_viz(trajectory.get_supports()) + + if args.robot or args.mujoco: + angles = { + "right_hip_yaw": robot.get_joint("right_hip_yaw"), + "right_hip_roll": robot.get_joint("right_hip_roll"), + "right_hip_pitch": robot.get_joint("right_hip_pitch"), + "right_knee": robot.get_joint("right_knee"), + "right_ankle": robot.get_joint("right_ankle"), + "left_hip_yaw": robot.get_joint("left_hip_yaw"), + "left_hip_roll": robot.get_joint("left_hip_roll"), + "left_hip_pitch": robot.get_joint("left_hip_pitch"), + "left_knee": robot.get_joint("left_knee"), + "left_ankle": robot.get_joint("left_ankle"), + "neck_pitch": robot.get_joint("neck_pitch"), + "head_pitch": robot.get_joint("head_pitch"), + "head_yaw": robot.get_joint("head_yaw"), + } + if args.robot: + hwi.set_position_all(angles) + elif args.mujoco: + right_contact, left_contact = bdx_mujoco_server.get_feet_contact() + if left_contact: + time_since_last_left_contact = 0.0 + if right_contact: + time_since_last_right_contact = 0.0 + # print("time since last left contact :", time_since_last_left_contact) + # print("time since last right contact :", time_since_last_right_contact) + bdx_mujoco_server.send_action(list(angles.values())) + + if ( + time_since_last_left_contact > parameters.single_support_duration + or time_since_last_right_contact > parameters.single_support_duration + ): + petage_de_gueule = True + # print("pétage de gueule") + else: + petage_de_gueule = False + + time_since_last_left_contact += DT + time_since_last_right_contact += DT + + if bdx_mujoco_server.key_pressed is not None: + got_input = True + d_x = 0.05 + else: + got_input = True + d_x = 0 + d_y = 0 + d_theta = 0 + + t += DT + # print(t) + while time.time() < start_t + t: + time.sleep(1e-5) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py new file mode 100644 index 0000000..eea7eeb --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py @@ -0,0 +1,56 @@ +import argparse +import json +import os + +import FramesViewer.utils as fv_utils +import numpy as np +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-f", "--file", type=str, required=True) +parser.add_argument("-o", "--output_dir", type=str, required=True) +args = parser.parse_args() + +os.makedirs(args.output_dir, exist_ok=True) + +episode = json.load(open(args.file)) + +frame_duration = episode["FrameDuration"] + +frames = episode["Frames"] + +step = 5 +yaw_orientations = np.arange(360, step=step) - (180 - step) +# print(yaw_orientations) +# yaw_orientations = [180] + +for yaw_orientation in yaw_orientations: + new_frames = [] + for i, frame in enumerate(frames): + root_position = frame[:3] # x, y, z + root_orientation_quat = frame[3:7] # quat + root_orientation_mat = R.from_quat(root_orientation_quat).as_matrix() + # rotate around z axis + root_orientation_mat = ( + R.from_euler("z", yaw_orientation, degrees=True).as_matrix() + @ root_orientation_mat + ) + root_orientation_quat = R.from_matrix(root_orientation_mat).as_quat() + + # rotate root position too around z at origin + root_position = ( + R.from_euler("z", yaw_orientation, degrees=True).as_matrix() @ root_position + ) + + new_frames.append(list(root_position) + list(root_orientation_quat) + frame[7:]) + + new_episode = episode.copy() + new_episode["Frames"] = new_frames + + output_file = os.path.join( + args.output_dir, + os.path.basename(args.file).replace(".txt", f"_{yaw_orientation}.txt"), + ) + + with open(output_file, "w") as f: + json.dump(new_episode, f) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py new file mode 100644 index 0000000..62bcabe --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py @@ -0,0 +1,223 @@ +import argparse +import json +import os +import time +from os.path import join +from threading import current_thread + +import numpy as np +from numpy.ma.extras import average +import placo +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.rl_utils import mujoco_to_isaac, test + +FPS = 60 +MESHCAT_FPS = 20 + +# For IsaacGymEnvs +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + + +# For amp for hardware +# [root position, root orientation, joint poses (e.g. rotations), target toe positions, linear velocity, angular velocity, joint velocities, target toe velocities] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15, l_toe_x, l_toe_y, l_toe_z, r_toe_x, r_toe_y, r_toe_z, lin_vel_x, lin_vel_y, lin_vel_z, ang_vel_x, ang_vel_y, ang_vel_z, j1_vel, j2_vel, j3_vel, j4_vel, j5_vel, j6_vel, j7_vel, j8_vel, j9_vel, j10_vel, j11_vel, j12_vel, j13_vel, j14_vel, j15_vel, l_toe_vel_x, l_toe_vel_y, l_toe_vel_z, r_toe_vel_x, r_toe_vel_y, r_toe_vel_z] +def record(pwe, args_dict): + episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Debug_info": [], + "Frames": [], + "MotionWeight": 1, + } + + first_joints_positions = list(pwe.get_angles().values()) + first_T_world_fbase = pwe.robot.get_T_world_fbase() + first_T_world_leftFoot = pwe.robot.get_T_world_left() + first_T_world_rightFoot = pwe.robot.get_T_world_right() + + # pwe.parameters.single_support_duration = 0.25 # slow + # pwe.parameters.single_support_duration = 0.20 # normal + pwe.parameters.single_support_duration = 0.2 # Fast ? + + pwe.set_traj(args_dict["dx"], args_dict["dy"], args_dict["dtheta"] + 0.001) + if args_dict["meshcat_viz"]: + viz = robot_viz(pwe.robot) + DT = 0.001 + start = time.time() + + last_record = 0 + last_meshcat_display = 0 + prev_root_position = [0, 0, 0] + prev_root_orientation_euler = [0, 0, 0] + prev_left_toe_pos = [0, 0, 0] + prev_right_toe_pos = [0, 0, 0] + prev_joints_positions = [0] * 15 + i = 0 + prev_initialized = False + avg_x_lin_vel = [] + avg_yaw_vel = [] + while True: + # print("t", pwe.t) + pwe.tick(DT) + if pwe.t <= 0 + args_dict["skip_warmup"] * 1: + start = pwe.t + last_record = pwe.t + 1 / FPS + last_meshcat_display = pwe.t + 1 / MESHCAT_FPS + continue + + # print(np.around(pwe.robot.get_T_world_fbase()[:3, 3], 3)) + + if pwe.t - last_record >= 1 / FPS: + if args_dict["stand"]: + T_world_fbase = first_T_world_fbase + else: + T_world_fbase = pwe.robot.get_T_world_fbase() + root_position = list(T_world_fbase[:3, 3]) + root_orientation_quat = list(R.from_matrix(T_world_fbase[:3, :3]).as_quat()) + + if args_dict["stand"]: + joints_positions = first_joints_positions + else: + joints_positions = list(pwe.get_angles().values()) + + if args_dict["stand"]: + T_world_leftFoot = first_T_world_leftFoot + T_world_rightFoot = first_T_world_rightFoot + else: + T_world_leftFoot = pwe.robot.get_T_world_left() + T_world_rightFoot = pwe.robot.get_T_world_right() + + T_body_leftFoot = np.linalg.inv(T_world_fbase) @ T_world_leftFoot + T_body_rightFoot = np.linalg.inv(T_world_fbase) @ T_world_rightFoot + + left_toe_pos = list(T_body_leftFoot[:3, 3]) + right_toe_pos = list(T_body_rightFoot[:3, 3]) + + world_linear_vel = list( + (np.array(root_position) - np.array(prev_root_position)) / (1 / FPS) + ) + avg_x_lin_vel.append(world_linear_vel[0]) + body_rot_mat = T_world_fbase[:3, :3] + body_linear_vel = list(body_rot_mat.T @ world_linear_vel) + + world_angular_vel = list( + ( + R.from_quat(root_orientation_quat).as_euler("xyz") + - prev_root_orientation_euler + ) + / (1 / FPS) + ) + avg_yaw_vel.append(world_angular_vel[2]) + body_angular_vel = list(body_rot_mat.T @ world_angular_vel) + # print("world angular vel", world_angular_vel) + # print("body angular vel", body_angular_vel) + + joints_vel = list( + (np.array(joints_positions) - np.array(prev_joints_positions)) + / (1 / FPS) + ) + left_toe_vel = list( + (np.array(left_toe_pos) - np.array(prev_left_toe_pos)) / (1 / FPS) + ) + right_toe_vel = list( + (np.array(right_toe_pos) - np.array(prev_right_toe_pos)) / (1 / FPS) + ) + + if prev_initialized: + if args_dict["hardware"]: + episode["Frames"].append( + root_position + + root_orientation_quat + + joints_positions + + left_toe_pos + + right_toe_pos + + world_linear_vel + + world_angular_vel + + joints_vel + + left_toe_vel + + right_toe_vel + ) + else: + episode["Frames"].append( + root_position + root_orientation_quat + joints_positions + ) + episode["Debug_info"].append( + { + "left_foot_pose": list(T_world_leftFoot.flatten()), + "right_foot_pose": list(T_world_rightFoot.flatten()), + } + ) + + prev_root_position = root_position.copy() + prev_root_orientation_euler = ( + R.from_quat(root_orientation_quat).as_euler("xyz").copy() + ) + prev_left_toe_pos = left_toe_pos.copy() + prev_right_toe_pos = right_toe_pos.copy() + prev_joints_positions = joints_positions.copy() + prev_initialized = True + + # print("avg x vel", np.mean(avg_x_lin_vel[-50:])) + # print("avg yaw vel", np.mean(avg_yaw_vel[-50:])) + # print("=") + + last_record = pwe.t + # print("saved frame") + + if args_dict["meshcat_viz"] and pwe.t - last_meshcat_display >= 1 / MESHCAT_FPS: + last_meshcat_display = pwe.t + viz.display(pwe.robot.state.q) + footsteps_viz(pwe.trajectory.get_supports()) + + if pwe.t - start > args_dict["length"]: + break + + i += 1 + + print("recorded", len(episode["Frames"]), "frames") + file_name = args_dict["name"] + str(".txt") + file_path = os.path.join(args_dict["output_dir"], file_name) + os.makedirs(args_dict["output_dir"], exist_ok=True) + print("DONE, saving", file_name) + with open(file_path, "w") as f: + json.dump(episode, f) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--name", type=str, required=True) + parser.add_argument("-o", "--output_dir", type=str, default="recordings") + parser.add_argument("--dx", type=float, required=True) + parser.add_argument("--dy", type=float, required=True) + parser.add_argument("--dtheta", type=float, required=True) + parser.add_argument("-l", "--length", type=int, default=10) + parser.add_argument("-m", "--meshcat_viz", action="store_true", default=False) + parser.add_argument( + "-s", + "--skip_warmup", + action="store_true", + default=False, + help="don't record warmup motion", + ) + parser.add_argument( + "--stand", + action="store_true", + default=False, + help="hack to record a standing pose", + ) + parser.add_argument( + "--hardware", + action="store_true", + help="use AMP_for_hardware format. If false, use IsaacGymEnvs format", + ) + args = parser.parse_args() + pwe = PlacoWalkEngine( + "../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True + ) + record(pwe, vars(args)) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py new file mode 100644 index 0000000..56cfb7a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py @@ -0,0 +1,178 @@ +import time +import json +import os +import warnings +from placo_utils.visualization import robot_viz +import numpy as np +import placo +import FramesViewer.utils as fv_utils +from scipy.spatial.transform import Rotation as R + +FPS = 60 + +episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Debug_info": [], + "Frames": [], + "MotionWeight": 1, +} + +DT = 0.01 +REFINE = 10 +MESHCAT_FPS = 20 +robot = placo.HumanoidRobot("../../mini_bdx/robots/bdx/robot.urdf") +solver = placo.KinematicsSolver(robot) +# viz = robot_viz(robot) + +robot.set_joint_limits("left_knee", -2, -0.01) +robot.set_joint_limits("right_knee", -2, -0.01) + +T_world_trunk = np.eye(4) +T_world_trunk[:3, 3] = [0, 0, 0.175] +T_world_trunk = fv_utils.rotateInSelf(T_world_trunk, [0, 2, 0]) +trunk_task = solver.add_frame_task("trunk", T_world_trunk) +trunk_task.configure("trunk", "hard") + +T_trunk_leftFoot = np.eye(4) +T_trunk_leftFoot[:3, 3] = [-0.03, 0.06, -0.17] +T_world_leftFoot = T_world_trunk @ T_trunk_leftFoot +T_world_leftFoot = placo.flatten_on_floor(T_world_leftFoot) +left_foot_task = solver.add_frame_task("left_foot_frame", T_world_leftFoot) +left_foot_task.configure("left_foot_frame", "soft", 1.0) + +T_trunk_rightFoot = np.eye(4) +T_trunk_rightFoot[:3, 3] = [-0.03, -0.06, -0.17] +T_world_rightFoot = T_world_trunk @ T_trunk_rightFoot +T_world_rightFoot = placo.flatten_on_floor(T_world_rightFoot) +right_foot_task = solver.add_frame_task("right_foot_frame", T_world_rightFoot) +right_foot_task.configure("right_foot_frame", "soft", 1.0) + + +left_foot_task.orientation().mask.set_axises("yz", "local") +right_foot_task.orientation().mask.set_axises("yz", "local") + + +def get_angles(): + angles = { + "left_hip_yaw": robot.get_joint("left_hip_yaw"), + "left_hip_roll": robot.get_joint("left_hip_roll"), + "left_hip_pitch": robot.get_joint("left_hip_pitch"), + "left_knee": robot.get_joint("left_knee"), + "left_ankle": robot.get_joint("left_ankle"), + "neck_pitch": robot.get_joint("neck_pitch"), + "head_pitch": robot.get_joint("head_pitch"), + "head_yaw": robot.get_joint("head_yaw"), + "left_antenna": robot.get_joint("left_antenna"), + "right_antenna": robot.get_joint("right_antenna"), + "right_hip_yaw": robot.get_joint("right_hip_yaw"), + "right_hip_roll": robot.get_joint("right_hip_roll"), + "right_hip_pitch": robot.get_joint("right_hip_pitch"), + "right_knee": robot.get_joint("right_knee"), + "right_ankle": robot.get_joint("right_ankle"), + } + + return angles + + +last_meshcat_display = 0 +last_record = 0 +prev_root_position = [0, 0, 0] +prev_root_orientation_euler = [0, 0, 0] +prev_left_toe_pos = [0, 0, 0] +prev_right_toe_pos = [0, 0, 0] +prev_joints_positions = [0] * 15 +prev_initialized = False + +t = 0 +start = 0 +while True: + # if t - last_meshcat_display > 1 / MESHCAT_FPS: + # last_meshcat_display = t + # viz.display(robot.state.q) + + trunk_task.T_world_frame = fv_utils.translateInSelf( + T_world_trunk, [0, 0.01 * np.sin(2 * t), 0.01 * np.sin(3 * t)] + ) + + if t - last_record >= 1 / FPS: + T_world_fbase = robot.get_T_world_fbase() + root_position = list(T_world_fbase[:3, 3]) + root_orientation_quat = list(R.from_matrix(T_world_fbase[:3, :3]).as_quat()) + joints_positions = list(get_angles().values()) + + T_world_leftFoot = robot.get_T_world_left() + T_world_rightFoot = robot.get_T_world_right() + + T_body_leftFoot = np.linalg.inv(T_world_fbase) @ T_world_leftFoot + T_body_rightFoot = np.linalg.inv(T_world_fbase) @ T_world_rightFoot + + left_toe_pos = list(T_body_leftFoot[:3, 3]) + right_toe_pos = list(T_body_rightFoot[:3, 3]) + + world_linear_vel = list( + (np.array(root_position) - np.array(prev_root_position)) / (1 / FPS) + ) + + world_angular_vel = list( + ( + R.from_quat(root_orientation_quat).as_euler("xyz") + - prev_root_orientation_euler + ) + / (1 / FPS) + ) + + joints_vel = list( + (np.array(joints_positions) - np.array(prev_joints_positions)) / (1 / FPS) + ) + left_toe_vel = list( + (np.array(left_toe_pos) - np.array(prev_left_toe_pos)) / (1 / FPS) + ) + right_toe_vel = list( + (np.array(right_toe_pos) - np.array(prev_right_toe_pos)) / (1 / FPS) + ) + + if prev_initialized: + episode["Frames"].append( + root_position + + root_orientation_quat + + joints_positions + + left_toe_pos + + right_toe_pos + + world_linear_vel + + world_angular_vel + + joints_vel + + left_toe_vel + + right_toe_vel + ) + + episode["Debug_info"].append( + { + "left_foot_pose": list(T_world_leftFoot.flatten()), + "right_foot_pose": list(T_world_rightFoot.flatten()), + } + ) + + prev_root_position = root_position.copy() + prev_root_orientation_euler = ( + R.from_quat(root_orientation_quat).as_euler("xyz").copy() + ) + prev_left_toe_pos = left_toe_pos.copy() + prev_right_toe_pos = right_toe_pos.copy() + prev_joints_positions = joints_positions.copy() + prev_initialized = True + last_record = t + if t - start >= 10: + break + + robot.update_kinematics() + _ = solver.solve(True) + t += DT + +print("recorded", len(episode["Frames"]), "frames") +file_path = "wiggle" + str(".txt") +print("DONE, saving", file_path) +with open(file_path, "w") as f: + json.dump(episode, f) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py new file mode 100644 index 0000000..6a8aee3 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py @@ -0,0 +1,114 @@ +import argparse + +import mujoco +import mujoco.viewer +import numpy as np + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.xbox_controller import XboxController + +parser = argparse.ArgumentParser() +parser.add_argument("-x", action="store_true", default=False) +args = parser.parse_args() + +if args.x: + xbox = XboxController() + +d_x = 0 +d_y = 0 +d_theta = 0 + + +# TODO placo mistakes the antennas for leg joints ? +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf") + + +def xbox_input(): + global d_x, d_y, d_theta + inputs = xbox.read() + + d_x = -inputs["l_y"] * pwe.parameters.walk_max_dx_forward / 2 + d_y = -inputs["l_x"] * pwe.parameters.walk_max_dy / 3 + d_theta = -inputs["r_x"] * pwe.parameters.walk_max_dtheta / 3 + + print(d_x, d_y, d_theta) + + +def key_callback(keycode): + global d_x, d_y, d_theta + if keycode == 265: # up + d_x = 0.05 + # max_target_step_size_x += 0.005 + # if keycode == 264: # down + # max_target_step_size_x -= 0.005 + # if keycode == 263: # left + # max_target_step_size_y -= 0.005 + # if keycode == 262: # right + # max_target_step_size_y += 0.005 + # if keycode == 81: # a + # max_target_yaw += np.deg2rad(1) + # if keycode == 69: # e + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +# joints_velocities_min = 1000 +# joints_velocities_max = -1000 +# angular_velocity_min = 1000 +# angular_velocity_max = -1000 +# linear_velocity_min = 1000 +# linear_velocity_max = -1000 +speed = 4 # 1 is slowest, 3 looks real time on my machine +prev = data.time +while True: + t = data.time + dt = t - prev + + if args.x: + xbox_input() + + pwe.d_x = d_x + pwe.d_y = d_y + pwe.d_theta = d_theta + right_contact, left_contact = get_feet_contact() + pwe.tick(dt, left_contact, right_contact) + + angles = pwe.get_angles() + data.ctrl[:] = list(angles.values()) + + # joints_velocities = data.qvel[6 : 6 + 15] + # angular_velocity = data.body("base").cvel[:3] + # linear_velocity = data.body("base").cvel[3:] + # # print(np.around(joints_velocities, 3)) + # if np.min(joints_velocities) < joints_velocities_min: + # joints_velocities_min = np.min(joints_velocities) + # if np.max(joints_velocities) > joints_velocities_max: + # joints_velocities_max = np.max(joints_velocities) + + # if np.min(angular_velocity) < angular_velocity_min: + # angular_velocity_min = np.min(angular_velocity) + # if np.max(angular_velocity) > angular_velocity_max: + # angular_velocity_max = np.max(angular_velocity) + + # if np.min(linear_velocity) < linear_velocity_min: + # linear_velocity_min = np.min(linear_velocity) + # if np.max(linear_velocity) > linear_velocity_max: + # linear_velocity_max = np.max(linear_velocity) + + # print("joints velocities amplitude", joints_velocities_min, joints_velocities_max) + # print("angular velocity amplitude", angular_velocity_min, angular_velocity_max) + # print("linear velocity amplitude", linear_velocity_min, linear_velocity_max) + + mujoco.mj_step(model, data, speed) # 4 seems good + viewer.sync() + prev = t diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py new file mode 100644 index 0000000..5c12b53 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py @@ -0,0 +1,51 @@ +import argparse +import time + +import numpy as np +import placo +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.xbox_controller import XboxController + +parser = argparse.ArgumentParser() +parser.add_argument("-x", action="store_true", default=False) +args = parser.parse_args() + +if args.x: + xbox = XboxController() + +d_x = 0.05 +d_y = 0 +d_theta = 0.2 + + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True) + +pwe.set_traj(d_x, d_y, d_theta) +viz = robot_viz(pwe.robot) + + +def xbox_input(): + global d_x, d_y, d_theta + inputs = xbox.read() + + d_x = -inputs["l_y"] * pwe.parameters.walk_max_dx_forward / 2 + d_y = -inputs["l_x"] * pwe.parameters.walk_max_dy / 3 + d_theta = -inputs["r_x"] * pwe.parameters.walk_max_dtheta / 3 + + print(d_x, d_y, d_theta) + + +prev = time.time() +start = time.time() +while True: + t = time.time() + dt = t - prev + if args.x: + xbox_input() + + viz.display(pwe.robot.state.q) + + pwe.tick(dt) + prev = t diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/replay_amp.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/replay_amp.py new file mode 100644 index 0000000..150e48c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/replay_amp.py @@ -0,0 +1,85 @@ +import argparse +import json +import time + +import FramesViewer.utils as fv_utils +import numpy as np +from FramesViewer.viewer import Viewer +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-f", "--file", type=str, required=True) +parser.add_argument( + "--hardware", + action="store_true", + help="use AMP_for_hardware format. If false, use IsaacGymEnvs format", +) +args = parser.parse_args() + +fv = Viewer() +fv.start() + +episode = json.load(open(args.file)) + +frame_duration = episode["FrameDuration"] + +frames = episode["Frames"] +if "Debug_info" in episode: + debug = episode["Debug_info"] +else: + debug = None +pose = np.eye(4) +if args.hardware: + vels = {} + vels["linear_vel"] = [] + vels["angular_vel"] = [] + vels["joint_vels"] = [] +for i, frame in enumerate(frames): + root_position = frame[:3] + root_orientation_quat = frame[3:7] + root_orientation_mat = R.from_quat(root_orientation_quat).as_matrix() + + pose[:3, 3] = root_position + pose[:3, :3] = root_orientation_mat + + fv.pushFrame(pose, "aze") + + if debug is not None: + left_foot_pose = np.array(debug[i]["left_foot_pose"]).reshape(4, 4) + right_foot_pose = np.array(debug[i]["right_foot_pose"]).reshape(4, 4) + fv.pushFrame(left_foot_pose, "left") + fv.pushFrame(right_foot_pose, "right") + + if args.hardware: + vels["linear_vel"].append(frame[28:31]) + vels["angular_vel"].append(frame[31:34]) + vels["joint_vels"].append(frame[34:49]) + + left_toe_pos = frame[22:25] + right_toe_pos = frame[25:28] + fv.pushFrame(fv_utils.make_pose(left_toe_pos, [0, 0, 0]), "left_toe") + fv.pushFrame(fv_utils.make_pose(right_toe_pos, [0, 0, 0]), "right_toe") + + time.sleep(frame_duration) + + +if args.hardware: + # plot vels + from matplotlib import pyplot as plt + + # TODO + x_lin_vel = [vels["linear_vel"][i][0] for i in range(len(frames))] + y_lin_vel = [vels["linear_vel"][i][1] for i in range(len(frames))] + z_lin_vel = [vels["linear_vel"][i][2] for i in range(len(frames))] + + joints_vel = [vels["joint_vels"][i] for i in range(len(frames))] + angular_vel_x = [vels["angular_vel"][i][0] for i in range(len(frames))] + angular_vel_y = [vels["angular_vel"][i][1] for i in range(len(frames))] + angular_vel_z = [vels["angular_vel"][i][2] for i in range(len(frames))] + + plt.plot(angular_vel_x, label="angular_vel_x") + plt.plot(angular_vel_y, label="angular_vel_y") + plt.plot(angular_vel_z, label="angular_vel_z") + + plt.legend() + plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_bdx.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_bdx.py new file mode 100644 index 0000000..3479ba3 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_bdx.py @@ -0,0 +1,88 @@ +import argparse +import time +import warnings + +import numpy as np +import placo +from placo_utils.visualization import robot_viz +from placo_utils.tf import tf +import time +import pickle +import FramesViewer.utils as fv_utils + +warnings.filterwarnings("ignore") +model_filename = "../../mini_bdx/robots/bdx/robot.urdf" +robot = placo.RobotWrapper(model_filename) +robot.set_joint_limits("left_knee", -2, -0.01) +robot.set_joint_limits("right_knee", -2, -0.01) +solver = placo.KinematicsSolver(robot) +# solver.mask_fbase(True) + +left_foot_task = solver.add_frame_task("left_foot_frame", np.eye(4)) +left_foot_task.configure("left_foot_frame", "soft", 1.0) +right_foot_task = solver.add_frame_task("right_foot_frame", np.eye(4)) +right_foot_task.configure("right_foot_frame", "soft", 1.0) + +T_world_trunk = np.eye(4) +T_world_trunk = fv_utils.rotateInSelf(T_world_trunk, [0, -2, 0]) +trunk_task = solver.add_frame_task("trunk", T_world_trunk) +trunk_task.configure("trunk", "hard") + +T_world_head = np.eye(4) +T_world_head[:3, 3][2] = 0.1 +head_task = solver.add_frame_task( + "head", + T_world_head, +) +head_task.configure("head", "soft") + +robot.update_kinematics() + +move = [] +viz = robot_viz(robot) +while True: # some main loop + # Update tasks data here + + # left_foot_task.T_world_frame = tf.translation_matrix( + # [0, -0.12 / 2, np.sin(time.time())] + # ) + z_offset = 0.015 * np.sin(2 * time.time()) + left_foot_task.T_world_frame = tf.translation_matrix( + [-0.005, 0.12 / 2, -0.17 + z_offset] + ) + right_foot_task.T_world_frame = tf.translation_matrix( + [-0.005, -0.12 / 2, -0.17 + z_offset] + ) + + head_task.T_world_frame = tf.translation_matrix( + [0.05 * np.sin(2 * np.pi * 2 * time.time()), 0, 0.1] + ) + # Solve the IK + solver.solve(True) + + # Update frames and jacobians + robot.update_kinematics() + + angles = { + "right_hip_yaw": robot.get_joint("right_hip_yaw"), + "right_hip_roll": robot.get_joint("right_hip_roll"), + "right_hip_pitch": robot.get_joint("right_hip_pitch"), + "right_knee": robot.get_joint("right_knee"), + "right_ankle": robot.get_joint("right_ankle"), + "left_hip_yaw": robot.get_joint("left_hip_yaw"), + "left_hip_roll": robot.get_joint("left_hip_roll"), + "left_hip_pitch": robot.get_joint("left_hip_pitch"), + "left_knee": robot.get_joint("left_knee"), + "left_ankle": robot.get_joint("left_ankle"), + "neck_pitch": robot.get_joint("neck_pitch"), + "head_pitch": robot.get_joint("head_pitch"), + "head_yaw": robot.get_joint("head_yaw"), + "left_antenna": robot.get_joint("left_antenna"), + "right_antenna": robot.get_joint("right_antenna"), + } + move.append(angles) + time.sleep(1 / 60) + pickle.dump(move, open("move.pkl", "wb")) + # Optionally: dump the solver status + solver.dump_status() + viz.display(robot.state.q) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py new file mode 100644 index 0000000..a9eb1f4 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py @@ -0,0 +1,18 @@ +import time + +import numpy as np +from mini_bdx_runtime.hwi import HWI + +hwi = HWI(usb_port="/dev/ttyUSB0") + +hwi.set_pid_all([1000, 0, 500]) +hwi.turn_on() +time.sleep(1) + +while True: + + ankle_pos = 0.3 * np.sin(2 * np.pi * 0.5 * time.time()) + hwi.set_position("right_ankle", ankle_pos) + hwi.set_position("left_ankle", ankle_pos) + + time.sleep(1 / 60) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py new file mode 100644 index 0000000..5bb5f64 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py @@ -0,0 +1,78 @@ +import argparse +import pickle +import time + +import FramesViewer.utils as fv_utils +import numpy as np +from FramesViewer.viewer import Viewer +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--data", type=str, required=True) +args = parser.parse_args() +# from utils import ImuFilter + +FPS = 30 +gyro_data = pickle.load(open(args.data, "rb")) +fv = Viewer() +fv.start() + +pose_euler = fv_utils.make_pose([0.1, 0.1, 0.1], [0, 0, 0]) + +quat = gyro_data[0][0] +quat = [quat[3], quat[0], quat[1], quat[2]] +rot = R.from_quat(quat).as_matrix() +rot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) @ rot +pose_euler[:3, :3] = rot +pose_euler = fv_utils.rotateInSelf(pose_euler, [0, 0, 90]) +initial_pose = pose_euler.copy() +initial_pose = fv_utils.translateAbsolute(initial_pose, [0.1, 0.1, 0]) + +pose_ang_vel = initial_pose.copy() +i = 1 +while True: + quat = gyro_data[i][0] + ang_vel = gyro_data[i][1] + ang_vel = [-ang_vel[1], ang_vel[0], ang_vel[2]] + + quat = [quat[3], quat[0], quat[1], quat[2]] + rot = R.from_quat(quat).as_matrix() + + rot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) @ rot + + pose_euler[:3, :3] = rot + + pose_euler = fv_utils.rotateInSelf(pose_euler, [0, 0, 90]) + + rot_euler = R.from_matrix(pose_ang_vel[:3, :3]).as_euler("xyz", degrees=False) + new_rot_euler = np.array(rot_euler) + (np.array(ang_vel) * (1 / FPS)) + rot = R.from_euler("xyz", new_rot_euler, degrees=False).as_matrix() + pose_ang_vel[:3, :3] = rot + + fv.pushFrame(pose_euler, "euler", color=(255, 0, 0)) + fv.pushFrame(pose_ang_vel, "ang_vel") + time.sleep(1 / FPS) + i += 1 + if i >= len(gyro_data) - 1: + print("end, looping ") + time.sleep(2) + pose_ang_vel = initial_pose.copy() + i = 0 + + +# imu_filter = ImuFilter(window_size=100) + +linear_velocity = np.array([0.0, 0.0, 0.0]) +position = np.array([0.0, 0.0, 0.0]) +for linear_acceleration in linear_accelerations: + linear_acceleration = np.array(linear_acceleration) + imu_filter.push_data(linear_acceleration) + linear_acceleration = imu_filter.get_filtered_data() + # print(linear_acceleration) + linear_velocity += linear_acceleration * (1 / FPS) + position += linear_velocity * (1 / FPS) + # pose[:3, 3] = linear_acceleration * 0.1 + pose[:3, 3] = position + print(position) + fv.pushFrame(pose, "imu") + time.sleep(1 / FPS) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/move_test.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/move_test.py new file mode 100644 index 0000000..a097192 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/move_test.py @@ -0,0 +1,45 @@ +from mini_bdx.hwi import HWI +import pickle +import time +import numpy as np + +move = pickle.load(open("move.pkl", "rb")) +hwi = HWI(usb_port="/dev/ttyUSB0") + +move_without_antennas = [] +# remove antennas keys +for m in move: + m = {k: v for k, v in m.items() if "antenna" not in k} + move_without_antennas.append(m) + + +command_value = [] + +hwi.turn_on() +time.sleep(1) +ctrl_freq = 60 # hz +start = time.time() +i = 0 +while True: + # pos = hwi.init_pos.copy() + # pos["left_hip_pitch"] += np.sin(5 * time.time()) / 3 + # pos["left_knee"] -= np.sin(5 * time.time()) / 3 + # pos["left_ankle"] -= np.sin(5 * time.time()) / 3 + + # pos["right_hip_pitch"] += np.sin(5 * time.time()) / 3 + # pos["right_knee"] -= np.sin(5 * time.time()) / 3 + # pos["right_ankle"] -= np.sin(5 * time.time()) / 3 + # print(pos["right_ankle"]) + + pos = move_without_antennas[i] + hwi.set_position_all(pos) + + command_value.append((list(pos.values()), hwi.get_present_positions())) + + time.sleep(1 / ctrl_freq) + + i += 1 + if i >= len(move) - 1: + i = 0 + +pickle.dump(command_value, open("command_value.pkl", "wb")) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/new_run.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/new_run.py new file mode 100644 index 0000000..32f826c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/new_run.py @@ -0,0 +1,38 @@ +from mini_bdx_runtime.hwi import HWI +import time +import numpy as np +from mini_bdx.placo_walk_engine import PlacoWalkEngine + +PLACO_DT = 0.001 + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True) +pwe.set_traj(0.00, 0, 0 + 0.001) + +hwi = HWI("/dev/ttyUSB0") + +pid = (1000, 0, 100) + +hwi.turn_on() +hwi.set_pid_all(pid) + +# exit() + +control_freq = 60 +prev = time.time() +while True: + t = time.time() + dt = t - prev + pwe.tick(dt) + print(pwe.t) + + joints_positions = pwe.get_angles() + del joints_positions["left_antenna"] + del joints_positions["right_antenna"] + print(joints_positions) + if pwe.t >= 0: + exit() + hwi.set_position_all(joints_positions) + + took = time.time() - t + time.sleep(max(0, (1 / control_freq) - took)) + prev = t diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot.py new file mode 100644 index 0000000..d466892 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot.py @@ -0,0 +1,45 @@ +import pickle +import matplotlib.pyplot as plt + +import numpy as np + +command_value = pickle.load(open("command_value.pkl", "rb")) + +dofs = { + 0: "right_hip_yaw", + 1: "right_hip_roll", + 2: "right_hip_pitch", + 3: "right_knee", + 4: "right_ankle", + 5: "left_hip_yaw", + 6: "left_hip_roll", + 7: "left_hip_pitch", + 8: "left_knee", + 9: "left_ankle", + 10: "neck_pitch", + 11: "head_pitch", + 12: "head_yaw", + # 13: "left_antenna", + # 14: "right_antenna", +} +# command_value = np.array(command_value) +fig, axs = plt.subplots(4, 4) +dof_id = 0 +for i in range(4): + for j in range(4): + if 4 * i + j >= 13: + continue + print(4 * i + j) + command = [] + value = [] + for k in range(len(command_value)): + command.append(command_value[k][0][4 * i + j]) + value.append(command_value[k][1][4 * i + j]) + axs[i, j].plot(command, label="command") + axs[i, j].plot(value, label="value") + axs[i, j].legend() + axs[i, j].set_title(f"{dofs[dof_id]}") + dof_id += 1 + + +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py new file mode 100644 index 0000000..a667469 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py @@ -0,0 +1,33 @@ +import pickle +import numpy as np +import time +import matplotlib.pyplot as plt +from utils import ImuFilter + +FPS = 60 +linear_accelerations = pickle.load(open("gyro_data.pkl", "rb")) + +# plot +filtered_linear_accelerations = [] +imu_filter = ImuFilter(window_size=100) +for linear_acceleration in linear_accelerations: + imu_filter.push_data(linear_acceleration) + filtered_linear_accelerations.append(imu_filter.get_filtered_data()) + +linear_accelerations = filtered_linear_accelerations + + +axes = ["x", "y", "z"] +fig, axs = plt.subplots(3, 1) +for i in range(3): + linear_acceleration = [] + for k in range(len(linear_accelerations)): + linear_acceleration.append(linear_accelerations[k][i]) + axs[i].plot(linear_acceleration, label="linear_acceleration") + axs[i].legend() + axs[i].set_title(f"linear_acceleration {axes[i]}") + # same range for all plots + + axs[i].set_ylim([-10, 10]) + +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py new file mode 100644 index 0000000..3add6cd --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py @@ -0,0 +1,15 @@ +import time + +from mini_bdx.hwi import HWI + +hwi = HWI(usb_port="/dev/ttyUSB0") + +while True: + present_current_right_ankle = hwi.get_present_current("right_ankle") + present_current_left_ankle = hwi.get_present_current("left_ankle") + + goal_current_right_ankle = hwi.get_goal_current("right_ankle") + goal_current_left_ankle = hwi.get_goal_current("left_ankle") + + print("present", present_current_right_ankle, "goal", goal_current_right_ankle) + time.sleep(0.1) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py new file mode 100644 index 0000000..1329134 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py @@ -0,0 +1,50 @@ +import argparse +import pickle +import time + +import FramesViewer.utils as fv_utils +import numpy as np +from FramesViewer.viewer import Viewer +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--data", type=str, required=True) +args = parser.parse_args() + +FPS = 30 +gyro_data = pickle.load(open(args.data, "rb")) +fv = Viewer() +fv.start() + +pose_quat = fv_utils.make_pose([0.1, 0.1, 0.1], [0, 0, 0]) + +quat = gyro_data[0][0] +# quat = [quat[3], quat[0], quat[1], quat[2]] +rot = R.from_quat(quat).as_matrix() +pose_quat[:3, :3] = rot +initial_pose = pose_quat.copy() +initial_pose = fv_utils.translateAbsolute(initial_pose, [0.1, 0.1, 0]) + +pose_ang_vel = initial_pose.copy() +i = 1 +while True: + print(i) + quat = gyro_data[i][0] + ang_vel = gyro_data[i][1] + rot = R.from_quat(quat).as_matrix() + pose_quat[:3, :3] = rot + + rot_ang_vel = R.from_matrix(pose_ang_vel[:3, :3]).as_euler("xyz", degrees=False) + new_rot_ang_vel = np.array(rot_ang_vel) + (np.array(ang_vel) * (1 / FPS)) + rot_ang_vel = R.from_euler("xyz", new_rot_ang_vel, degrees=False).as_matrix() + pose_ang_vel[:3, :3] = rot + + fv.pushFrame(pose_quat, "quat", color=(255, 0, 0)) + fv.pushFrame(pose_ang_vel, "ang_vel") + time.sleep(1 / FPS) + i += 1 + if i >= len(gyro_data) - 1: + print("end, looping ") + time.sleep(2) + pose_ang_vel = initial_pose.copy() + i = 0 diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py new file mode 100644 index 0000000..5b3f99e --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py @@ -0,0 +1,44 @@ +""" +Replays imu data from a .pkl which is a list of quaternions +Shows the orientation in a 3D viewer +""" + +from FramesViewer.viewer import Viewer +import pickle +from scipy.spatial.transform import Rotation as R +import numpy as np +import time + +data = pickle.load(open("imu_data.pkl", "rb")) + +fv = Viewer() +fv.start() + + +def reorient(quat): + """ + Reorients because the IMU is mounted upside down + """ + + euler = R.from_quat(quat).as_euler("xyz") + # euler = [np.pi-euler[1], euler[2], -euler[0]] + reoriented_quat = R.from_euler("xyz", euler).as_quat() + return reoriented_quat + + +imu = np.eye(4) +imu[:3, 3] = [0.1, 0.1, 0.1] # just easier to see + +for raw_quat in data: + try: + quat = reorient(raw_quat) + except: + continue + + rot_mat = R.from_quat(quat).as_matrix() + imu[:3, :3] = rot_mat + fv.pushFrame(imu, "imu") + time.sleep(1/30) + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py new file mode 100644 index 0000000..a9e0de6 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py @@ -0,0 +1,103 @@ +import pickle +import time + +import numpy as np + +from mini_bdx.hwi import HWI +from mini_bdx.onnx_infer import OnnxInfer +from mini_bdx.utils.rl_utils import ( + action_to_pd_targets, + isaac_joints_order, + isaac_to_mujoco, +) + +pd_action_offset = [ + 0.0, + -0.57, + 0.52, + 0.0, + 0.0, + -0.57, + 0.0, + 0.0, + 0.48, + -0.48, + 0.0, + -0.57, + 0.52, + 0.0, + 0.0, +] +pd_action_scale = [ + 0.98, + 1.4, + 1.47, + 2.93, + 2.2, + 1.04, + 0.98, + 2.93, + 2.26, + 2.26, + 0.98, + 1.4, + 1.47, + 2.93, + 2.2, +] + + +def make_action_dict(action): + action_dict = {} + for i, a in enumerate(action): + if "antenna" not in isaac_joints_order[i]: + action_dict[isaac_joints_order[i]] = a + + return action_dict + + +# TODO +def get_obs(hwi, imu): + # Don't forget to re invert the angles from the hwi + pass + + +policy = OnnxInfer("/home/antoine/MISC/IsaacGymEnvs/isaacgymenvs/ONNX_NO_PUSH.onnx") + +fake_obs = pickle.loads(open("saved_obs.pkl", "rb").read()) +fake_actions = pickle.loads(open("saved_actions.pkl", "rb").read()) + +hwi = HWI(usb_port="/dev/ttyUSB0") + +command_value = [] + +hwi.turn_on() +time.sleep(1) +skip = 10 +i = 0 +ctrl_freq = 30 # hz +while True: + if skip > 0: + skip -= 1 + start = time.time() + obs = fake_obs[i] # for now + # obs = get_obs(hwi, imu) + + obs = np.clip(obs, -5, 5) + action = policy.infer(obs) + action = fake_actions[i][0] + action = np.clip(action, -1, 1) + action = action_to_pd_targets(action, pd_action_offset, pd_action_scale) + action_dict = make_action_dict(action) + # print(action_dict) + hwi.set_position_all(action_dict) + took = time.time() - start + # time.sleep((max(0, (1 / ctrl_freq) - took))) + time.sleep(1 / ctrl_freq) + i += 1 + if i >= len(fake_obs) - 1: + break + + command_value.append((list(action_dict.values()), hwi.get_present_positions())) + # print(len(command_value[0][0]), len(command_value[0][1])) + pickle.dump(command_value, open("command_value.pkl", "wb")) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/run.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/run.py new file mode 100644 index 0000000..ead6d90 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/run.py @@ -0,0 +1,166 @@ +import argparse +import time + +import cv2 +import numpy as np +import placo +from mini_bdx_runtime.hwi import HWI + +# from mini_bdx.hwi import HWI +from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser() +parser.add_argument("-x", action="store_true", default=False) +args = parser.parse_args() + +if args.x: + xbox = XboxController() + + +hwi = HWI(usb_port="/dev/ttyUSB0") + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +time_since_last_left_contact = 0 +time_since_last_right_contact = 0 +walking = False +start_button_timeout = time.time() + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine( + robot, + frequency=1.5, + swing_gain=0.0, + default_trunk_x_offset=-0.013, + default_trunk_z_offset=-0.023, + target_trunk_pitch=-11.0, + max_rise_gain=0.01, +) + + +def xbox_input(): + global target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + # print(inputs) + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.2: + target_head_pitch = inputs["r_y"] / 2 * np.deg2rad(70) + print("=== target head pitch", target_head_pitch) + target_head_yaw = -inputs["r_x"] / 2 * np.deg2rad(150) + target_head_z_offset = inputs["r_trigger"] * 4 * 0.2 + print(target_head_z_offset) + # print("======", target_head_z_offset) + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + +im = np.zeros((80, 80, 3), dtype=np.uint8) + + +# TODO +def get_imu(): + return [0, 0, 0], [0, 0, 0] + + +# hwi.turn_off() +# exit() +hwi.turn_on() +time.sleep(1) +# hwi.goto_init() +# exit() +gyro = [0, 0.0, 0] +accelerometer = [0, 0, 0] + +skip = 10 +prev = time.time() +while True: + dt = time.time() - prev + t = time.time() + if args.x: + xbox_input() + + # Get sensor data + # gyro, accelerometer = get_imu() + right_contact = abs(hwi.get_present_current("right_ankle")) > 1 + left_contact = abs(hwi.get_present_current("left_ankle")) > 1 + # print("left_contact", left_contact, "right_contact", right_contact) + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ignore_feet_contact=True, + ) + angles = walk_engine.get_angles() + + if skip > 0: + skip -= 1 + continue + hwi.set_position_all(angles) + + # print("-") + cv2.imshow("image", im) + key = cv2.waitKey(1) + if key == ord("p"): + # gyro[1] += 0.001 + walk_engine.target_trunk_pitch += 0.1 + if key == ord("o"): + walk_engine.target_trunk_pitch -= 0.1 + # gyro[1] -= 0.001 + if key == ord("m"): + walk_engine.max_rise_gain += 0.001 + if key == ord("l"): + walk_engine.max_rise_gain -= 0.001 + if key == ord("b"): + walk_engine.default_trunk_x_offset += 0.001 + if key == ord("n"): + walk_engine.default_trunk_x_offset -= 0.001 + if key == ord("i"): + walk_engine.default_trunk_z_offset += 0.001 + if key == ord("u"): + walk_engine.default_trunk_z_offset -= 0.001 + if key == ord("f"): + walk_engine.frequency -= 0.1 + if key == ord("g"): + walk_engine.frequency += 0.1 + if key == ord("c"): + walk_engine.swing_gain -= 0.001 + if key == ord("v"): + walk_engine.swing_gain += 0.001 + if key == ord("w"): + walking = not walking + + # print("gyro : ", gyro) + # print("target_trunk pitch", walk_engine.trunk_pitch) + # print("trunk x offset", walk_engine.default_trunk_x_offset) + # print("trunk z offset", walk_engine.default_trunk_z_offset) + # print("max rise gain", walk_engine.max_rise_gain) + # print("frequency", walk_engine.frequency) + # print("swing gain", walk_engine.swing_gain) + # print("===") + + prev = t diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/utils.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/utils.py new file mode 100644 index 0000000..f3ecf4c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/utils.py @@ -0,0 +1,15 @@ +import numpy as np + + +class ImuFilter: + def __init__(self, window_size=10): + self.window_size = window_size + self.data = [] + self.filtered_data = [] + + def push_data(self, data): + self.data.append(data) + + def get_filtered_data(self): + data = self.data[-self.window_size :] + return np.mean(data, axis=0) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/bench_com_time.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/bench_com_time.py new file mode 100644 index 0000000..7da04e8 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/bench_com_time.py @@ -0,0 +1,25 @@ +from pypot.feetech import FeetechSTS3215IO +import time +import numpy as np + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) + +# id = 24 +ids = [10, 11, 12, 13, 14, 20, 21, 22, 23, 24, 30, 31, 32, 33] + +io.enable_torque(ids) +io.set_mode({id: 0 for id in ids}) +times = [] +for i in range(1000): + s = time.time() + io.get_present_position(ids) + io.set_goal_position({id: 0 for id in ids}) + times.append(time.time() - s) + + time.sleep(1 / 100) + +print("avg :", np.mean(times)) \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/configure_motors.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/configure_motors.py new file mode 100644 index 0000000..141b67a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/configure_motors.py @@ -0,0 +1,123 @@ +from pypot.feetech import FeetechSTS3215IO +import argparse +import time + +parser = argparse.ArgumentParser() +parser.add_argument("--usb_port", type=str, default="/dev/ttyACM0") +args = parser.parse_args() + + +joints = { + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, +} + +# TODO scan baudrates +io = FeetechSTS3215IO( + args.usb_port, + baudrate=1000000, + use_sync_read=True, +) + + +id = 200 +SKIP_SCAN = False +if not SKIP_SCAN: + try: + io.get_present_position([id]) + except Exception: + print( + "Didn't find motor with id 1, motor has probably been configured before. scanning for other motors" + ) + print("Scanning... ") + found_ids = io.scan() + print("Found ids: ", found_ids) + if len(found_ids) > 1: + print("More than one motor found, please connect only one motor") + exit() + elif len(found_ids) == 0: + print("No motor found") + exit() + + id = found_ids[0] + +exit() +print("Select the dof you want to configure : ") +for i, key in enumerate(joints.keys()): + print(f"{i}: {key}") + +dof_index = int(input("> ")) +if dof_index not in range(len(joints)): + print("Invalid choice") + exit() + +dof_name = list(joints.keys())[dof_index] +dof_id = joints[dof_name] + +print("") +print("===") +print("Configuring motor ", dof_name, " with id ", dof_id) +print("===") + +io.set_lock({id: 1}) + +io.set_offset({id: 0}) +print("- setting new id ") +io.change_id({id: dof_id}) +id = dof_id +print("- setting new baudrate") +io.change_baudrate({id: 0}) # 1 000 000 + +exit() +# WARNING offset management is not understood yet. + +print("") +print("The motor will now move to the zero position.") +print("Press enter to continue") +input() + +io.enable_torque([id]) +io.set_goal_position({id: 0}) +time.sleep(1) +zero_pos = io.get_present_position([id])[0] + +print("") +print( + "Now, place the contraption (???) on the motor, and adjust the position to fit (???) (TODO: clarify)" +) +print("Press enter to continue") + +io.disable_torque([id]) +input() +new_pos = io.get_present_position([id])[0] +offset = zero_pos - new_pos +print("Offset: ", offset) +io.set_offset({id: offset}) +time.sleep(1) + + +io.set_lock({id: 0}) + +print("") +print( + "To confirm the offset, please move the motor to a random position, then press enter to go back to zero." +) +input() +io.enable_torque([id]) +io.set_goal_position({id: 0}) +time.sleep(1) +print("position: ", io.get_present_position([id])[0]) + +io.disable_torque([id]) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/identification.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/identification.py new file mode 100644 index 0000000..f19f605 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/identification.py @@ -0,0 +1,80 @@ +from pypot.feetech import FeetechSTS3215IO +import time +import pickle + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) + +id = 24 +max_acceleration = 254 +io.set_D_coefficient({id: 0}) +io.set_acceleration({id: max_acceleration}) + +io.set_goal_position({id: 0}) + +# present position +# goal position +# present load +# present current +# present speed +# 0 deg pendant 2s, 90° pendant 2s etc + + +present_positions = [] +goal_positions = [] +present_loads = [] +present_currents = [] +present_speeds = [] +times = [] + +input("press enter to start") + +io.set_goal_position({id: 90}) +s = time.time() +while time.time() - s < 2: + present_positions.append(io.get_present_position([id])[0]) + goal_positions.append(io.get_goal_position([id])[0]) + present_loads.append(io.get_present_load([id])[0]) + present_currents.append(io.get_present_current([id])[0]) + present_speeds.append(io.get_present_speed([id])[0]) + times.append(time.time()) + + +io.set_goal_position({id: 0}) +s = time.time() +while time.time() - s < 2: + present_positions.append(io.get_present_position([id])[0]) + goal_positions.append(io.get_goal_position([id])[0]) + present_loads.append(io.get_present_load([id])[0]) + present_currents.append(io.get_present_current([id])[0]) + present_speeds.append(io.get_present_speed([id])[0]) + times.append(time.time()) + +io.set_goal_position({id: 90}) +s = time.time() +while time.time() - s < 2: + present_positions.append(io.get_present_position([id])[0]) + goal_positions.append(io.get_goal_position([id])[0]) + present_loads.append(io.get_present_load([id])[0]) + present_currents.append(io.get_present_current([id])[0]) + present_speeds.append(io.get_present_speed([id])[0]) + times.append(time.time()) + + +data = { + "present_positions": present_positions, + "goal_positions": goal_positions, + "present_loads": present_loads, + "present_currents": present_currents, + "present_speeds": present_speeds, + "times": times, +} + +pickle.dump(data, open(f"data_max_acc_{max_acceleration}.pkl", "wb")) + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py new file mode 100644 index 0000000..9bb8de3 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py @@ -0,0 +1,86 @@ +from mini_bdx.placo_walk_engine.placo_walk_engine import PlacoWalkEngine +import time +import json +import mujoco +import mujoco.viewer +import pickle +from mini_bdx.utils.mujoco_utils import check_contact +import numpy as np + +# DT = 0.01 +DT = 0.002 +decimation = 10 +pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2", + model_filename="robot.urdf", + init_params=json.load(open("placo_defaults.json")), + ignore_feet_contact=True, +) +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/openduckminiv2_playground/env/locomotion/open_duck_mini_v2/xmls/scene_mjx_flat_terrain.xml" +) +model.opt.timestep = DT +data = mujoco.MjData(model) + +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + # 0.0, + # 0, + # 0, + # 0, + # 0, + # 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +# angles = pickle.load(open("init_angles.pkl", "rb")) + +data.ctrl[:] = init_pos +data.qpos[3 + 4 :] = init_pos +data.qpos[3 : 3 + 4] = [1, 0, 0.06, 0] + + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return right_contact, left_contact + + +pwe.set_traj(0.05, 0, 0.0) +counter = 0 +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + while True: + # right_contact, left_contact = get_feet_contact() + if counter % decimation == 0: + pwe.tick(DT * decimation) + angles = list( + pwe.get_angles( + ignore=[ + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + ] + ).values() + ) + data.ctrl[:] = angles + + counter += 1 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(DT) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py new file mode 100644 index 0000000..b47a717 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py @@ -0,0 +1,243 @@ +import mujoco +import numpy as np + +import mujoco.viewer +import time +import pygame +import argparse +from mini_bdx.utils.mujoco_utils import check_contact + +from mini_bdx_runtime.onnx_infer import OnnxInfer +import pickle +from bam.model import load_model +from bam.mujoco import MujocoController +from mini_bdx_runtime.rl_utils import mujoco_joints_order + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("-k", action="store_true", default=False) +parser.add_argument("--bam", action="store_true", default=False) +# parser.add_argument("--rma", action="store_true", default=False) +# parser.add_argument("--awd", action="store_true", default=False) +# parser.add_argument("--adaptation_module_path", type=str, required=False) +parser.add_argument("--replay_obs", type=str, required=False, default=None) +args = parser.parse_args() + +if args.k: + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + +if args.replay_obs is not None: + with open(args.replay_obs, "rb") as f: + replay_obs = pickle.load(f) + replay_obs = np.array(replay_obs) + +# Params +linearVelocityScale = 1.0 +angularVelocityScale = 1.0 +dof_pos_scale = 1.0 +dof_vel_scale = 1.0 +action_scale = 0.25 + + +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +# model = mujoco.MjModel.from_xml_path( +# "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene_position.xml" +# ) +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mujoco_menagerie/open_duck_mini_v2/scene.xml" +) +model.opt.timestep = 0.005 +# model.opt.timestep = 1 / 240 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) + +if args.bam: + sts3215_model = load_model("params_m6.json") + mujoco_controllers = {} + for joint_name in mujoco_joints_order: + mujoco_controllers[joint_name] = MujocoController( + sts3215_model, joint_name, model, data + ) + + +NUM_OBS = 56 + +policy = OnnxInfer(args.onnx_model_path, awd=True) + +COMMANDS_RANGE_X = [-0.2, 0.3] +COMMANDS_RANGE_Y = [-0.2, 0.2] +COMMANDS_RANGE_THETA = [-0.3, 0.3] + +prev_action = np.zeros(16) +commands = [0.3, 0.0, 0.0] +decimation = 4 +data.qpos[3 : 3 + 4] = [1, 0, 0.0, 0] + +data.qpos[7 : 7 + 16] = init_pos +data.ctrl[:16] = init_pos + +replay_index = 0 +saved_obs = [] + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos[7 : 7 + 16].copy() + + dof_vel = data.qvel[6 : 6 + 16].copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + feet_contacts = get_feet_contact() + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + + +def handle_keyboard(): + global commands + keys = pygame.key.get_pressed() + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keys[pygame.K_z]: + lin_vel_x = COMMANDS_RANGE_X[1] + if keys[pygame.K_s]: + lin_vel_x = COMMANDS_RANGE_X[0] + if keys[pygame.K_q]: + lin_vel_y = COMMANDS_RANGE_Y[1] + if keys[pygame.K_d]: + lin_vel_y = COMMANDS_RANGE_Y[0] + if keys[pygame.K_a]: + ang_vel = COMMANDS_RANGE_THETA[1] + if keys[pygame.K_e]: + ang_vel = COMMANDS_RANGE_THETA[0] + + commands[0] = lin_vel_x + commands[1] = lin_vel_y + commands[2] = ang_vel + + commands = list( + np.array(commands) + * np.array( + [ + linearVelocityScale, + linearVelocityScale, + angularVelocityScale, + ] + ) + ) + print(commands) + pygame.event.pump() # process event queue + + +try: + with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False + ) as viewer: + counter = 0 + while True: + + step_start = time.time() # Was + # step_start = data.time + + mujoco.mj_step(model, data) + + counter += 1 + if counter % decimation == 0: + + if args.replay_obs is not None: + obs = replay_obs[replay_index] + else: + obs = get_obs(data, prev_action, commands) + saved_obs.append(obs) + + obs = list(obs) + list(np.zeros(18)) + action = policy.infer(obs) + + prev_action = action.copy() + + action = action * action_scale + init_pos + + # if args.bam: + # for i, joint_name in enumerate(mujoco_joints_order): + # mujoco_controllers[joint_name].update(action[i]) + # else: + # data.ctrl = action.copy() + + if args.k: + handle_keyboard() + # print(commands) + + replay_index += 1 + if args.replay_obs is not None and replay_index >= len(replay_obs): + replay_index = 0 + + viewer.sync() + + # Rudimentary time keeping, will drift relative to wall clock. + # time_until_next_step = model.opt.timestep - (data.time - step_start) + # if time_until_next_step > 0: + # time.sleep(time_until_next_step) + + # Was + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + +except KeyboardInterrupt: + pickle.dump(saved_obs, open("mujoco_saved_obs.pkl", "wb")) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py new file mode 100644 index 0000000..0a45410 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py @@ -0,0 +1,262 @@ +import mujoco.viewer + +import time +import mujoco +import argparse +import pickle +import numpy as np + +from mini_bdx.utils.mujoco_utils import check_contact + +from mini_bdx_runtime.onnx_infer import OnnxInfer + + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("-k", action="store_true", default=False) +parser.add_argument("--replay_obs", type=str, required=False, default=None) +parser.add_argument("--rma", action="store_true", default=False) +parser.add_argument("--adaptation_module_path", type=str, required=False) +parser.add_argument("--zero_head", action="store_true", default=False) +args = parser.parse_args() + +if args.k: + import pygame + + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + +if args.replay_obs is not None: + with open(args.replay_obs, "rb") as f: + replay_obs = pickle.load(f) + replay_obs = np.array(replay_obs) + + +def pd_control(target_q, q, kp, target_dq, dq, kd, clip_val, init_pos, action_scale): + """Calculates torques from position commands""" + tau = (target_q * action_scale + init_pos - q) * kp# - (dq * kd) + tau = np.clip(tau, -clip_val, clip_val) + tau -= dq * kd + # tau += (target_dq - dq) * kd + return tau + # return (target_q - q) * kp + (target_dq - dq) * kd + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos[7 : 7 + 16].copy() + + dof_vel = data.qvel[6 : 6 + 16].copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + feet_contacts = get_feet_contact() + # feet_contacts = [0, 0] + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + +def set_zero_head(pos): + pos[5] = np.deg2rad(10) + pos[6] = np.deg2rad(-10) + # pos[5] = 0 + # pos[6] = 0 + pos[7] = 0 + pos[8] = 0 + return pos + +def handle_keyboard(): + global commands + keys = pygame.key.get_pressed() + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keys[pygame.K_z]: + lin_vel_x = 0.3 + if keys[pygame.K_s]: + lin_vel_x = -0.2 + if keys[pygame.K_q]: + lin_vel_y = 0.2 + if keys[pygame.K_d]: + lin_vel_y = -0.2 + if keys[pygame.K_a]: + ang_vel = 0.2 + if keys[pygame.K_e]: + ang_vel = -0.2 + + commands[0] = lin_vel_x + commands[1] = lin_vel_y + commands[2] = ang_vel + + # commands = list( + # np.array(commands) + # * np.array( + # [ + # linearVelocityScale, + # linearVelocityScale, + # angularVelocityScale, + # ] + # ) + # ) + pygame.event.pump() # process event queue + + +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene.xml" +) +# model = mujoco.MjModel.from_xml_path( +# "/home/antoine/MISC/mujoco_menagerie/open_duck_mini_v2/scene.xml" +# ) +model.opt.timestep = 0.005 +# model.opt.timestep = 1 / 120 +# model.opt.timestep = 1 / 60 # /2 substeps ? +# model.opt.integrator = mujoco.mjtIntegrator.mjINT_IMPLICITFAST +data = mujoco.MjData(model) +# mujoco.mj_step(model, data) +control_decimation = 4 + +data.qpos[3 : 3 + 4] = [1, 0, 0.0, 0] +data.qpos[7 : 7 + 16] = init_pos +data.ctrl[:16] = init_pos + +NUM_OBS = 56 + +policy = OnnxInfer(args.onnx_model_path, awd=True) +if args.rma: + adaptation_module = OnnxInfer(args.adaptation_module_path, "rma_history", awd=True) + obs_history_size = 20 + obs_history = np.zeros((obs_history_size, NUM_OBS)).tolist() + rma_decimation = 5 # 10 hz if control_decimation = 4 + rma_counter = 0 + +commands = [0.2, 0.0, 0.0] + +# define context variables +prev_action = np.zeros(16) +target_dof_pos = init_pos.copy() +action_scale = 0.5 + +kps = np.array([6.55] * 16) +kds = np.array([0.65] * 16) + +counter = 0 +replay_counter = 0 +latent = None +start = time.time() +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + adaptation_module_latents = [] + while True: + step_start = time.time() + + tau = pd_control( + target_dof_pos, + data.qpos[7:].copy(), + kps, + np.zeros_like(kds), + data.qvel[6:].copy(), + kds, + 3.57, + init_pos, + action_scale, + ) + data.ctrl[:] = tau + + mujoco.mj_step(model, data) + counter += 1 + + if counter % control_decimation == 0 and time.time() - start > 0: + if args.replay_obs is not None: + obs = replay_obs[replay_counter] + replay_counter += 1 + else: + obs = get_obs(data, prev_action, commands) + if args.rma: + rma_counter += 1 + + # Shift to right and set new obs at index 0 + obs_history = np.roll(obs_history, 1, axis=0) + obs_history[0] = obs + + # obs_history.append(obs) + # obs_history = obs_history[-obs_history_size:] + + if rma_counter % rma_decimation == 0 or latent is None: + latent = adaptation_module.infer(np.array(obs_history).flatten()) + adaptation_module_latents.append(latent) + pickle.dump(adaptation_module_latents, open("adaptation_module_latents.pkl", "wb")) + obs = np.concatenate([obs, latent]) + + obs = np.clip(obs, -100, 100) + action = policy.infer(obs) + action = np.clip(action, -5, 5) + if args.zero_head: + action = set_zero_head(action) + prev_action = action.copy() + + + target_dof_pos = np.array(action) # * action_scale + init_pos + + viewer.sync() + + if args.k: + handle_keyboard() + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/params_m6.json b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/params_m6.json new file mode 100644 index 0000000..684ca71 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/params_m6.json @@ -0,0 +1 @@ +{"kt": 1.4303213704900366, "R": 2.0430791581753445, "armature": 0.009877609369241102, "friction_base": 0.010907201185572422, "friction_stribeck": 0.024827228916629952, "load_friction_motor": 0.190200316386506, "load_friction_external": 0.14527733955866906, "load_friction_motor_stribeck": 0.04566008837303702, "load_friction_external_stribeck": 0.7340661702898804, "load_friction_motor_quad": 0.006871499097705957, "load_friction_external_quad": 0.006948188153376718, "dtheta_stribeck": 0.09985941135196887, "alpha": 2.653878573867841, "friction_viscous": 0.023286828043639525, "model": "m6", "actuator": "sts3215"} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_defaults.json b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_defaults.json new file mode 100644 index 0000000..cba3d00 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_defaults.json @@ -0,0 +1,52 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 8, + "hardware": true, + "double_support_ratio": 0.2, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.2, + "walk_foot_height": 0.02, + "walk_trunk_pitch": 0, + "walk_foot_rise_ratio": 0.3, + "single_support_duration": 0.17, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.16, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": 0.0, + "walk_max_dtheta": 0.3, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna" + ], + "joint_angles": { + "neck_pitch": 0, + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py new file mode 100644 index 0000000..0dd057f --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py @@ -0,0 +1,167 @@ +import time +import placo +import numpy as np +from ischedule import schedule, run_loop +from placo_utils.visualization import robot_viz, robot_frame_viz, frame_viz +from placo_utils.tf import tf + +from mini_bdx_runtime.hwi_feetech_pypot import HWI + +MESHCAT_VIZ = False + +joints = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", + # "neck_pitch", + # "head_pitch", + # "head_yaw", +] + +if not MESHCAT_VIZ: + hwi = HWI() + hwi.turn_on() + +time.sleep(1) +exit() + + +DT = 0.01 +# robot = placo.HumanoidRobot("/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/robot.urdf") +robot = placo.RobotWrapper( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/robot.urdf", + placo.Flags.ignore_collisions, +) + +# Placing the left foot in world origin +robot.set_joint("left_knee", 0.1) +robot.set_joint("right_knee", 0.1) +robot.update_kinematics() +robot.set_T_world_frame("left_foot", np.eye(4)) +robot.update_kinematics() + +solver = placo.KinematicsSolver(robot) + +# Retrieving initial position of the feet, com and trunk orientation +T_world_left = robot.get_T_world_frame("left_foot") +T_world_right = robot.get_T_world_frame("right_foot") + +if MESHCAT_VIZ: + viz = robot_viz(robot) + +T_world_trunk = robot.get_T_world_frame("trunk") +T_world_trunk[2, 3] = 0.25 +trunk_task = solver.add_frame_task("trunk", T_world_trunk) +trunk_task.configure("trunk_task", "soft", 1e3, 1e3) + +# Keep left and right foot on the floor +left_foot_task = solver.add_frame_task("left_foot", T_world_left) +left_foot_task.configure("left_foot", "soft", 1.0, 1.0) + +right_foot_task = solver.add_frame_task("right_foot", T_world_right) +right_foot_task.configure("right_foot", "soft", 1e3, 1e3) + +# Regularization task +posture_regularization_task = solver.add_joints_task() +posture_regularization_task.set_joints({dof: 0.0 for dof in robot.joint_names()}) +posture_regularization_task.configure("reg", "soft", 1e-5) + + +# Initializing robot position before enabling constraints +for _ in range(32): + solver.solve(True) + robot.update_kinematics() + +# Enabling joint and velocity limits +solver.enable_joint_limits(True) +solver.enable_velocity_limits(True) + +t = 0 +dt = 0.01 +last = 0 +solver.dt = dt +start_t = time.time() +robot.update_kinematics() + + +def get_angles(): + angles = {joint: robot.get_joint(joint) for joint in joints} + return angles + + +# original_T_world_frame = T_world_trunk.copy() +while True: + + # T_world_frame = original_T_world_frame.copy() + + trunk_task.T_world_frame = tf.translation_matrix([0, 0, 0.25]) @ tf.rotation_matrix( + 0.25 * np.sin(2 * np.pi * 0.5 * t), [0, 0, 1] + ) + + # y_targ = 0.05 * np.sin(2 * np.pi * 0.5 * t) + # x_targ = 0.05 * np.cos(2 * np.pi * 0.5 * t) + + # T_world_frame[:3, 3] += [0, y_targ, 0] + + # trunk_task.T_world_frame = T_world_frame.copy() + + solver.solve(True) + robot.update_kinematics() + + if not MESHCAT_VIZ: + all_angles = list(get_angles().values()) + + angles = {} + for i, motor_name in enumerate(hwi.joints.keys()): + angles[motor_name] = all_angles[i] + + hwi.set_position_all(angles) + + if MESHCAT_VIZ: + viz.display(robot.state.q) + robot_frame_viz(robot, "left_foot") + frame_viz("left_foot_target", left_foot_task.T_world_frame, opacity=0.25) + + time.sleep(DT) + t += DT + print(t) + +# @schedule(interval=dt) +# def loop(): +# global t + +# # # Updating left foot target +# # left_foot_task.T_world_frame = tf.translation_matrix( +# # [np.sin(t * 2.5) * 0.05, np.sin(t * 3) * 0.1, 0.04] +# # ) @ tf.rotation_matrix(np.sin(t) * 0.25, [1, 0, 0]) + +# trunk_task.T_world_frame = tf.translation_matrix( +# [0, 0, 0.25] +# ) @ tf.rotation_matrix(np.sin(t) * 0.25, [1, 0, 0]) + +# solver.solve(True) +# robot.update_kinematics() + + +# if MESHCAT_VIZ: +# viz.display(robot.state.q) +# robot_frame_viz(robot, "left_foot") +# frame_viz("left_foot_target", left_foot_task.T_world_frame, opacity=0.25) + +# t += dt + + +# run_loop() +# # task = + +# # while True: + +# # robot.update_kinematics() +# # time.sleep(DT) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py new file mode 100644 index 0000000..b3ec714 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py @@ -0,0 +1,114 @@ +from mini_bdx_runtime.hwi_feetech_pypot import HWI +from mini_bdx.placo_walk_engine.placo_walk_engine import PlacoWalkEngine +from placo_utils.visualization import robot_viz +import json +import time +import argparse +from queue import Queue + +parser = argparse.ArgumentParser() +parser.add_argument("--xbox", action="store_true") +parser.add_argument("--viz", action="store_true") +args = parser.parse_args() + + +MAX_X = 0.02 +MAX_Y = 0.02 +MAX_THETA = 0.1 + + +if args.xbox: + from threading import Thread + import pygame + + pygame.init() + _p1 = pygame.joystick.Joystick(0) + _p1.init() + print(f"Loaded joystick with {_p1.get_numaxes()} axes.") + + cmd_queue = Queue(maxsize=1) + + def commands_worker(): + global cmd_queue + + while True: + for event in pygame.event.get(): + l_x = round(_p1.get_axis(0), 3) + l_y = round(_p1.get_axis(1), 3) + r_x = round(_p1.get_axis(3), 3) + r_y = round(_p1.get_axis(4), 3) + l_x = 0.0 if abs(l_x) < 0.1 else l_x + l_y = 0.0 if abs(l_y) < 0.1 else l_y + r_x = 0.0 if abs(r_x) < 0.1 else r_x + r_y = 0.0 if abs(r_y) < 0.1 else r_y + + pygame.event.pump() # process event queue + cmd = { + "l_x": l_x, + "l_y": l_y, + "r_x": r_x, + "r_y": r_y, + } + + cmd_queue.put(cmd) + time.sleep(1 / 30) + + Thread(target=commands_worker, daemon=True).start() + last_commands = { + "l_x": 0.0, + "l_y": 0.0, + "r_x": 0.0, + "r_y": 0.0, + } + + def get_last_command(): + global cmd_queue, last_commands + try: + last_commands = cmd_queue.get(False) # non blocking + except Exception: + pass + + return last_commands + + +if not args.viz: + hwi = HWI() + hwi.turn_on() + + +DT = 0.01 +pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2", + model_filename="robot.urdf", + init_params=json.load(open("placo_defaults.json")), + ignore_feet_contact=True, +) +if args.viz: + viz = robot_viz(pwe.robot) + +pwe.set_traj(0.0, 0, 0.0) +while True: + pwe.tick(DT) + + if args.xbox: + commands = get_last_command() + placo_commands = [ + -commands["l_y"] * MAX_X, + -commands["l_x"] * MAX_Y, + -commands["r_x"] * MAX_THETA, + ] + print(placo_commands) + print("====") + if commands is not None: + pwe.set_traj(*placo_commands) + + if not args.viz: + all_angles = list(pwe.get_angles().values()) + angles = {} + for i, motor_name in enumerate(hwi.joints.keys()): + angles[motor_name] = all_angles[i] + hwi.set_position_all(angles) + else: + viz.display(pwe.robot.state.q) + + time.sleep(DT) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot.py new file mode 100644 index 0000000..ea07bc0 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot.py @@ -0,0 +1,40 @@ +import pickle + +data = pickle.load(open("data_pwm_control.pkl", "rb")) + + +# data looks like: +# data = { +# "present_positions": present_positions, +# "goal_positions": goal_positions, +# "present_loads": present_loads, +# "present_currents": present_currents, +# "present_speeds": present_speeds, +# "times": times, +# } + +# present_positions, goal_positions etc are lists. All of the same size. + +# plot all on the same plot against time with shared x +# Label everything + + +import matplotlib.pyplot as plt + +fig, axs = plt.subplots(4, 1, sharex=True) + +axs[0].plot(data["times"], data["present_positions"], label="Present positions") +axs[0].plot(data["times"], data["goal_positions"], label="Goal positions") +axs[0].set_ylabel("Positions") +axs[0].legend() + +axs[1].plot(data["times"], data["present_loads"], label="Present loads") +axs[1].set_ylabel("Loads") + +axs[2].plot(data["times"], data["present_currents"], label="Present currents") +axs[2].set_ylabel("Currents") + +axs[3].plot(data["times"], data["present_speeds"], label="Present speeds") +axs[3].set_ylabel("Speeds") + +plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py new file mode 100644 index 0000000..14035a9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py @@ -0,0 +1,11 @@ +import pickle + +# [[x_t0, y_t0, z_t0...], [x_t1, y_t1, z_t1...], ...] +# adaptation_module_latent = pickle.load(open("adaptation_module_latents.pkl", "rb")) +adaptation_module_latent = pickle.load(open("robot_latents.pkl", "rb")) + +from matplotlib import pyplot as plt + +plt.plot(adaptation_module_latent) +plt.show() + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ new file mode 100644 index 0000000..e67326d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ @@ -0,0 +1,11 @@ +import pickle + +# [[x_t0, y_t0, z_t0...], [x_t1, y_t1, z_t1...], ...] +adaptation_module_latent = pickle.load(open("adaptation_module_latents.pkl", "rb")) +# adaptation_module_latent = pickle.load(open("robot_latents.pkl", "rb")) + +from matplotlib import pyplot as plt + +plt.plot(adaptation_module_latent) +plt.show() + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py new file mode 100644 index 0000000..b9ee743 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py @@ -0,0 +1,142 @@ +from pypot.feetech import FeetechSTS3215IO +import time +import numpy as np +from threading import Thread +import pickle + + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) +# io.disable_torque([1]) +# input() +# io.enable_torque([1]) +# input() +# io.disable_torque([1]) +# exit() + +class FeetechPWMControl: + def __init__(self): + self.io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, + ) + self.id = 1 + + # TODO zero first + self.io.enable_torque([self.id]) + self.io.set_mode({self.id: 0}) + self.io.set_goal_position({self.id: 0}) + time.sleep(1) + # exit() + + self.io.set_mode({self.id: 2}) + self.kp = self.io.get_P_coefficient([self.id])[0] + + self.control_freq = 100 # Hz + self.goal_position = 0 + self.present_position = 0 + Thread(target=self.update, daemon=True).start() + + def disable_torque(self): + self.io.set_mode({self.id: 0}) + self.io.disable_torque([self.id]) + + def enable_torque(self): + self.io.enable_torque([self.id]) + self.io.set_mode({self.id: 2}) + + + def update(self): + while True: + self.present_position = self.io.get_present_position([self.id])[0] + error = self.goal_position - self.present_position + + pwm = self.kp * error + # pwm *= 10 + pwm = np.int16(pwm) + + pwm_magnitude = abs(pwm) + if pwm_magnitude >= 2**10: + pwm_magnitude = (2**10) - 1 + + direction_bit = 1 if pwm >= 0 else 0 + + goal_time = (direction_bit << 10) | pwm_magnitude + + self.io.set_goal_time({self.id: goal_time}) + + time.sleep(1 / self.control_freq) + + +motor = FeetechPWMControl() + +s = time.time() +while True: + + target = 20 * np.sin(2 * np.pi * 0.5 * time.time()) + motor.goal_position = target + + if time.time() - s > 3 and time.time() - s < 6: + motor.disable_torque() + print("disbale") + + if time.time() - s > 6: + motor.enable_torque() + print("enable") + time.sleep(1 / 60) + + +present_positions = [] +goal_positions = [] +present_loads = [] +present_currents = [] +present_speeds = [] +times = [] + +motor.goal_position = 90 + +log_start = time.time() +s = time.time() +while time.time() - s < 2: + present_positions.append(motor.io.get_present_position([motor.id])[0]) + goal_positions.append(motor.goal_position) + present_loads.append(motor.io.get_present_load([motor.id])[0]) + present_currents.append(motor.io.get_present_current([motor.id])[0]) + present_speeds.append(motor.io.get_present_speed([motor.id])[0]) + times.append(time.time() - log_start) + +motor.goal_position = -90 +s = time.time() +while time.time() - s < 2: + present_positions.append(motor.io.get_present_position([motor.id])[0]) + goal_positions.append(motor.goal_position) + present_loads.append(motor.io.get_present_load([motor.id])[0]) + present_currents.append(motor.io.get_present_current([motor.id])[0]) + present_speeds.append(motor.io.get_present_speed([motor.id])[0]) + times.append(time.time() - log_start) +motor.goal_position = 90 + +s = time.time() +while time.time() - s < 2: + present_positions.append(motor.io.get_present_position([motor.id])[0]) + goal_positions.append(motor.goal_position) + present_loads.append(motor.io.get_present_load([motor.id])[0]) + present_currents.append(motor.io.get_present_current([motor.id])[0]) + present_speeds.append(motor.io.get_present_speed([motor.id])[0]) + times.append(time.time() - log_start) + + +data = { + "present_positions": present_positions, + "goal_positions": goal_positions, + "present_loads": present_loads, + "present_currents": present_currents, + "present_speeds": present_speeds, + "times": times, +} + +pickle.dump(data, open("data_pwm_control.pkl", "wb")) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test.py new file mode 100644 index 0000000..6198845 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test.py @@ -0,0 +1,39 @@ +import mujoco +import numpy as np +import mujoco_viewer +from mini_bdx_runtime.rl_utils import mujoco_joints_order + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene.xml" +) +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + +target = [0] * 16 +# data.ctrl[:] = np.zeros((16)) +id = 4 +max_vel = 0 +min_vel = 10000 +while True: + target[id] = 0.2*np.sin(2*np.pi*0.5*data.time) + # target[id] = 0.2 + tau = 6.16*(np.array(target) - data.qpos) - 0.1*data.qvel + data.ctrl[:] = tau + for i, joint_name in enumerate(mujoco_joints_order): + if i == id: + pos = np.around(data.qpos[i], 2) + vel = np.around(data.qvel[i], 2) + # print(f"{joint_name}: pos : {pos}, vel : {vel}") + if vel > max_vel: + max_vel = vel + + if vel < min_vel: + min_vel = vel + + print(f"max vel : {max_vel}, min vel : {min_vel}") + + print("==") + mujoco.mj_step(model, data, 15) + viewer.render() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer.py new file mode 100644 index 0000000..b3303b8 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer.py @@ -0,0 +1,117 @@ +import mujoco +import pickle +from mini_bdx.utils.mujoco_utils import check_contact +import mujoco.viewer +import time +import numpy as np + + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene_position.xml" +) +model.opt.timestep = 0.005 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +decimation = 4 +ctrl_dt = model.opt.timestep * decimation +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos.copy() + + dof_vel = data.qvel.copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + feet_contacts = get_feet_contact() + # feet_contacts = [0, 0] + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + +target = [0] * 16# + init_pos +data.qpos = target +data.ctrl = target + +A = 0.3 +F = 2.0 +joint_id = 2 +counter = 0 +saved_obs = [] +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + t = 0 + while True: + t += model.opt.timestep + # step_start = data.time + mujoco.mj_step(model, data) + counter += 1 + if counter % decimation == 0: + target[joint_id] = A * np.sin(2 * np.pi * F * t)# - init_pos[joint_id] + data.ctrl[joint_id] = target[joint_id] + obs = get_obs(data, data.ctrl, [0, 0, 0]) + saved_obs.append(obs) + + if len(saved_obs) > (1/ctrl_dt) * 10 : # 10 seconds + break + + + viewer.sync() + + # time_until_next_step = model.opt.timestep - (data.time - step_start) + # if time_until_next_step > 0: + # time.sleep(time_until_next_step) + + +pickle.dump(saved_obs, open("mujoco_saved_obs.pkl", "wb")) \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer_motor_control.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer_motor_control.py new file mode 100644 index 0000000..a83052e --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer_motor_control.py @@ -0,0 +1,140 @@ +import mujoco +import pickle +from mini_bdx.utils.mujoco_utils import check_contact +import mujoco.viewer +import time +import numpy as np + + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene.xml" +) +model.opt.timestep = 0.005 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +decimation = 4 +ctrl_dt = model.opt.timestep * decimation +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +def pd_control(target_q, q, kp, target_dq, dq, kd, clip_val, init_pos, action_scale): + """Calculates torques from position commands""" + tau = (target_q * action_scale - q) * kp - (dq * kd) + tau = np.clip(tau, -clip_val, clip_val) + return tau + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos.copy() + + dof_vel = data.qvel.copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + feet_contacts = get_feet_contact() + # feet_contacts = [0, 0] + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + +target = [0] * 16# + init_pos +data.qpos = target +data.ctrl = target + +kps = np.array([9.5] * 16) +kds = np.array([1.0] * 16) + +A = 0.3 +F = 2.0 +joint_id = 2 +counter = 0 +saved_obs = [] +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + t = 0 + while True: + t += model.opt.timestep + # step_start = data.time + + tau = pd_control( + target, + data.qpos.copy(), + kps, + np.zeros_like(kds), + data.qvel.copy(), + kds, + 5.2, + init_pos, + 1, + ) + data.ctrl = tau + + mujoco.mj_step(model, data) + counter += 1 + if counter % decimation == 0: + target[joint_id] = A * np.sin(2 * np.pi * F * t)# - init_pos[joint_id] + # data.ctrl[joint_id] = target[joint_id] + obs = get_obs(data, target, [0, 0, 0]) + saved_obs.append(obs) + + if len(saved_obs) > (1/ctrl_dt) * 10 : # 10 seconds + break + + + viewer.sync() + + # time_until_next_step = model.opt.timestep - (data.time - step_start) + # if time_until_next_step > 0: + # time.sleep(time_until_next_step) + + +pickle.dump(saved_obs, open("mujoco_saved_obs.pkl", "wb")) \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/walk_test.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/walk_test.py new file mode 100644 index 0000000..4ab6f54 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/walk_test.py @@ -0,0 +1,16 @@ +from mini_bdx.placo_walk_engine.placo_walk_engine import PlacoWalkEngine +import time +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz +import json + + + +DT = 0.01 +pwe = PlacoWalkEngine("/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2", model_filename="robot.urdf", init_params=json.load(open("placo_defaults.json"))) +viz = robot_viz(pwe.robot) + +pwe.set_traj(0.0, 0, 0.0) +while True: + pwe.tick(DT) + viz.display(pwe.robot.state.q) + time.sleep(DT) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/__init__.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/__init__.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/__init__.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/__init__.py new file mode 100644 index 0000000..eb1ba4b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/__init__.py @@ -0,0 +1 @@ +from .walk_engine import WalkEngine diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/walk_engine.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/walk_engine.py new file mode 100644 index 0000000..47482c9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/walk_engine.py @@ -0,0 +1,477 @@ +# Based on https://github.com/Rhoban/walk_engine + +import FramesViewer.utils as fv_utils +import numpy as np +import placo + +from mini_bdx.utils import PolySpline + + +class FootPose: + def __init__(self): + self.x = 0 + self.y = 0 + self.z = 0 + self.yaw = 0 + + def __eq__(self, other): + return ( + self.x == other.x + and self.y == other.y + and self.z == other.z + and self.yaw == other.yaw + ) + + @property + def foot_to_trunk(self): + # Returns a frame from foot to trunk frame (this is actually a 2d matrix) + # TODO + pass + + +class Foot: + def __init__(self): + self.x_spline = PolySpline() + self.y_spline = PolySpline() + self.z_spline = PolySpline() + self.yaw_spline = PolySpline() + self.trunk_y_offset = 0 + + def get_position(self, t): + # Get the foot position, t is from 0 to 1, playing the footstep + foot_pose = FootPose() + foot_pose.x = self.x_spline.get(t) + foot_pose.y = self.y_spline.get(t) + foot_pose.z = self.z_spline.get(t) + foot_pose.yaw = self.yaw_spline.get(t) + return foot_pose + + def clear_splines(self): + self.x_spline.clear() + self.y_spline.clear() + self.z_spline.clear() + self.yaw_spline.clear() + + def copy(self): + foot = Foot() + foot.x_spline = self.x_spline.copy() + foot.y_spline = self.y_spline.copy() + foot.z_spline = self.z_spline.copy() + foot.yaw_spline = self.yaw_spline.copy() + foot.trunk_y_offset = self.trunk_y_offset + return foot + + +class WalkEngine: + def __init__( + self, + robot: placo.RobotWrapper, + default_trunk_x_offset: float = 0.007, # 0.007 + default_trunk_z_offset: float = -0.003, + foot_y_offset: float = 0.0, + max_rise_gain: float = 0.015, + rise_duration: float = 0.2, + frequency: float = 2.0, + swing_gain: float = -0.001, + swing_phase: float = 0.0, + foot_y_offset_per_step_size_y: float = 0.02, + target_trunk_pitch: float = 0, + target_trunk_roll: float = 0, + step_size_x: float = 0, + step_size_y: float = 0, + step_size_yaw: float = 0, + ): + kinematics_solver = placo.KinematicsSolver(robot) + self.left = Foot() + self.right = Foot() + self.swing_spline = PolySpline() + + self.trunk_pitch = 0 + self.trunk_roll = 0 + + self.trunk_pitch_timeout = 1.0 # s + + self.target_trunk_pitch = target_trunk_pitch + self.target_trunk_roll = target_trunk_roll + + self.trunk_pitch_roll_compensation = False + + self.robot = robot + self.kinematics_solver = kinematics_solver + + self.T_world_trunk = np.eye(4) + self.T_world_trunk = fv_utils.rotateInSelf( + self.T_world_trunk, [0, self.trunk_pitch, 0], degrees=True + ) + + self.T_world_head = robot.get_T_world_frame("head") + self.T_world_head = fv_utils.translateInSelf( + self.T_world_head, [-0.05, 0, -0.05] + ) + self.head_task = self.kinematics_solver.add_frame_task( + "head", self.T_world_head + ) + self.head_task.configure("head", "soft") + + self.trunk_task = self.kinematics_solver.add_frame_task( + "trunk", self.T_world_trunk + ) + self.trunk_task.configure("trunk", "hard") + + self.right_foot_task = self.kinematics_solver.add_frame_task( + "right_foot", robot.get_T_world_frame("right_foot") + ) + self.right_foot_task.configure("right_foot", "soft", 5.0, 0.1) + + self.left_foot_task = self.kinematics_solver.add_frame_task( + "left_foot", robot.get_T_world_frame("left_foot") + ) + self.left_foot_task.configure("left_foot", "soft", 5.0, 0.1) + + self.is_left_support = False + + self.trunk_height = -robot.get_T_world_frame("left_foot")[:3, 3][2] / 1.2 + self.foot_distance = np.abs(robot.get_T_world_frame("left_foot")[:3, 3][1]) + + self.default_trunk_x_offset = default_trunk_x_offset + self.forward_trunk_x_offset = self.default_trunk_x_offset - 0.002 + self.backward_trunk_x_offset = self.default_trunk_x_offset + self.tune_trunk_x_offset = 0 + + self.default_trunk_z_offset = default_trunk_z_offset + self.foot_y_offset = foot_y_offset + self.max_rise_gain = max_rise_gain + self.rise_gain = 0 + self.rise_duration = rise_duration + self.frequency = frequency + self.swing_gain = swing_gain + self.swing_phase = swing_phase + self.foot_y_offset_per_step_size_y = foot_y_offset_per_step_size_y + self.step_size_x = step_size_x + self.step_size_y = step_size_y + self.step_size_yaw = step_size_yaw + + self.time_since_last_step = 0 + self.time_since_last_left_contact = 0 + self.time_since_last_right_contact = 0 + + self.step_duration = 0 + self._swing_gain = 0 + + self.reset() + + def get_left_foot_pose(self, t): + left_position = self.left.get_position(t) + + T_world_left_foot = np.eye(4) + T_world_left_foot[:3, 3] = [ + left_position.x, + left_position.y, + left_position.z, + ] + T_world_left_foot = fv_utils.rotateInSelf( + T_world_left_foot, [0, 0, left_position.yaw], degrees=False + ) + + return T_world_left_foot + + def get_right_foot_pose(self, time_since_last_step): + right_position = self.right.get_position(time_since_last_step) + T_world_right_foot = np.eye(4) + T_world_right_foot[:3, 3] = [ + right_position.x, + right_position.y, + right_position.z, + ] + T_world_right_foot = fv_utils.rotateInSelf( + T_world_right_foot, [0, 0, right_position.yaw], degrees=False + ) + return T_world_right_foot + + # gyro is angular position of the trunk [roll, pitch, yaw] + # accelerometer is the acceleration of the trunk [x, y, z] + def update( + self, + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_x, + target_step_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ignore_feet_contact=False, + ): + if self.trunk_pitch_timeout > 0: + self.trunk_pitch_timeout -= dt + if left_contact: + self.time_since_last_left_contact = 0 + if right_contact: + self.time_since_last_right_contact = 0 + + if ignore_feet_contact or ( + not self.time_since_last_left_contact > self.rise_duration + and not self.time_since_last_right_contact > self.rise_duration + ): + self.time_since_last_step += dt + + self.time_since_last_left_contact += dt + self.time_since_last_right_contact += dt + + if self.time_since_last_step > self.step_duration: + self.time_since_last_step = 0 + self.new_step() + + target_rise_gain = self.max_rise_gain if walking else 0 + + # slowly increase self.step_size_x and self.step_size_y to target_step_x and target_step_y + # target can be negative or positive or 0 + delta_x = target_step_x - self.step_size_x + delta_y = target_step_y - self.step_size_y + delta_yaw = target_yaw - self.step_size_yaw + delta_rise_gain = target_rise_gain - self.rise_gain + + self.step_size_x = self.step_size_x + (delta_x / 100) + self.step_size_y = self.step_size_y + (delta_y / 100) + self.step_size_yaw = self.step_size_yaw + (delta_yaw / 100) + self.rise_gain = self.rise_gain + (delta_rise_gain / 100) + + swing = 0 + if walking: + self.left_foot_task.T_world_frame = self.get_left_foot_pose( + self.time_since_last_step + ) + self.right_foot_task.T_world_frame = self.get_right_foot_pose( + self.time_since_last_step + ) + + swing_P = 0 if self.is_left_support else np.pi + swing_P += np.pi * 2 * self.swing_phase + swing = self._swing_gain * np.sin( + np.pi * self.time_since_last_step / self.step_duration + swing_P + ) + else: + self.step_size_x = 0 + self.step_size_y = 0 + self.step_size_yaw = 0 + self.left_foot_task.T_world_frame = self.get_left_foot_pose(0) + self.right_foot_task.T_world_frame = self.get_right_foot_pose(0) + + # Trunk pitch and roll + if self.trunk_pitch_roll_compensation: + self.trunk_pitch = max(-30, min(30, -np.rad2deg(gyro[1]) * 5)) + self.trunk_roll = max(-10, min(10, np.rad2deg(gyro[0]))) + # else: + # self.trunk_pitch = 0 + # self.trunk_roll = 0 + + if self.trunk_pitch_timeout <= 0: + delta_trunk_pitch = self.target_trunk_pitch - self.trunk_pitch + self.trunk_pitch = self.trunk_pitch + (delta_trunk_pitch / 100) + + self.T_world_trunk = np.eye(4) + self.T_world_trunk = fv_utils.rotateInSelf( + self.T_world_trunk, [self.trunk_roll, self.trunk_pitch, 0], degrees=True + ) + self.T_world_trunk[:3, 3] = [0, swing, 0] + + fr = self.T_world_trunk + fr[:3, 3] = [0, swing, 0] + self.trunk_task.T_world_frame = fr + + # Head + tmp = self.T_world_head.copy() + tmp = fv_utils.translateInSelf(tmp, [0, 0, -target_head_z_offset]) + tmp = fv_utils.rotateInSelf( + tmp, [0, target_head_pitch, target_head_yaw], degrees=False + ) + self.head_task.T_world_frame = tmp + + self.robot.update_kinematics() + self.kinematics_solver.solve(True) + + def get_angles(self): + angles = { + "right_hip_yaw": self.robot.get_joint("right_hip_yaw"), + "right_hip_roll": self.robot.get_joint("right_hip_roll"), + "right_hip_pitch": self.robot.get_joint("right_hip_pitch"), + "right_knee": self.robot.get_joint("right_knee"), + "right_ankle": self.robot.get_joint("right_ankle"), + "left_hip_yaw": self.robot.get_joint("left_hip_yaw"), + "left_hip_roll": self.robot.get_joint("left_hip_roll"), + "left_hip_pitch": self.robot.get_joint("left_hip_pitch"), + "left_knee": self.robot.get_joint("left_knee"), + "left_ankle": self.robot.get_joint("left_ankle"), + "neck_pitch": self.robot.get_joint("neck_pitch"), + "head_pitch": self.robot.get_joint("head_pitch"), + "head_yaw": self.robot.get_joint("head_yaw"), + } + return angles + + def reset(self): + self.left.trunk_y_offset = self.foot_distance + self.foot_y_offset + self.right.trunk_y_offset = -(self.foot_distance + self.foot_y_offset) + + self.is_left_support = False + + self.step_duration = 1.0 / (2 * self.frequency) + self.left.clear_splines() + self.right.clear_splines() + self.left.x_spline.add_point(self.step_duration, self.trunk_x_offset, 0) + self.left.y_spline.add_point(self.step_duration, self.left.trunk_y_offset, 0) + self.left.yaw_spline.add_point(self.step_duration, 0, 0) + self.right.x_spline.add_point(self.step_duration, self.trunk_x_offset, 0) + self.right.y_spline.add_point(self.step_duration, self.right.trunk_y_offset, 0) + self.right.yaw_spline.add_point(self.step_duration, 0, 0) + + self.trunk_pitch = 0 + self.trunk_roll = 0 + + self.new_step() + + def new_step(self): + self._swing_gain = self.swing_gain + previous_step_duration = self.step_duration + self.step_duration = 1.0 / (2 * self.frequency) + + old_left = self.left.copy() + old_right = self.right.copy() + self.left.clear_splines() + self.right.clear_splines() + + self.left.trunk_y_offset = ( + self.foot_distance + + self.foot_y_offset + + self.foot_y_offset_per_step_size_y * np.abs(self.step_size_y) + ) + self.right.trunk_y_offset = -( + self.foot_distance + + self.foot_y_offset + + self.foot_y_offset_per_step_size_y * np.abs(self.step_size_y) + ) + + self.left.x_spline.add_point( + 0, + old_left.x_spline.get(previous_step_duration), + old_left.x_spline.get_vel(previous_step_duration), + ) + self.left.y_spline.add_point( + 0, + old_left.y_spline.get(previous_step_duration), + old_left.y_spline.get_vel(previous_step_duration), + ) + self.left.yaw_spline.add_point( + 0, + old_left.yaw_spline.get(previous_step_duration), + old_left.yaw_spline.get_vel(previous_step_duration), + ) + + self.right.x_spline.add_point( + 0, + old_right.x_spline.get(previous_step_duration), + old_right.x_spline.get_vel(previous_step_duration), + ) + self.right.y_spline.add_point( + 0, + old_right.y_spline.get(previous_step_duration), + old_right.y_spline.get_vel(previous_step_duration), + ) + self.right.yaw_spline.add_point( + 0, + old_right.yaw_spline.get(previous_step_duration), + old_right.yaw_spline.get_vel(previous_step_duration), + ) + + self.is_left_support = not self.is_left_support + + step_low = -self.trunk_height + self.default_trunk_z_offset + step_high = -self.trunk_height + self.default_trunk_z_offset + self.rise_gain + self.support_foot.z_spline.add_point(0, step_low, 0) + self.support_foot.z_spline.add_point(self.step_duration, step_low, 0) + + self.flying_foot.z_spline.add_point(0, step_low, 0) + if self.rise_duration > 0: + self.flying_foot.z_spline.add_point( + self.step_duration * (0.5 - self.rise_duration / 2.0), + step_high, + 0, + ) + self.flying_foot.z_spline.add_point( + self.step_duration * (0.5 + self.rise_duration / 2.0), + step_high, + 0, + ) + else: + self.flying_foot.z_spline.add_point(self.step_duration * 0.5, step_high, 0) + self.flying_foot.z_spline.add_point(self.step_duration, step_low, 0) + + self.plan_step_end() + + def plan_step_end(self): + self.support_foot.x_spline.add_point( + self.step_duration, self.trunk_x_offset - self.step_size_x / 2.0, 0 + ) + self.support_foot.y_spline.add_point( + self.step_duration, + self.support_foot.trunk_y_offset - self.step_size_y / 2.0, + 0, + ) + + self.flying_foot.x_spline.add_point( + self.step_duration, self.trunk_x_offset + self.step_size_x / 2.0, 0 + ) + self.flying_foot.y_spline.add_point( + self.step_duration, + self.flying_foot.trunk_y_offset + self.step_size_y / 2.0, + 0, + ) + + self.support_foot.yaw_spline.add_point( + self.step_duration, -self.step_size_yaw / 2, 0 + ) + self.flying_foot.yaw_spline.add_point( + self.step_duration, self.step_size_yaw / 2, 0 + ) + + def replan(self): + splines = [ + self.support_foot.x_spline, + self.flying_foot.x_spline, + self.support_foot.y_spline, + self.flying_foot.y_spline, + self.support_foot.yaw_spline, + self.flying_foot.yaw_spline, + ] + + for spline in splines: + old_spline = spline.copy() + spline.clear() + spline.add_point(0, old_spline.get(0), old_spline.get_vel(0)) + spline.add_point( + self.time_since_last_step, + old_spline.get(self.time_since_last_step), + old_spline.get_vel(self.time_since_last_step), + ) + + self.plan_step_end() + + @property + def support_foot(self): + return self.left if self.is_left_support else self.right + + @property + def flying_foot(self): + return self.right if self.is_left_support else self.left + + @property + def trunk_x_offset(self): + if self.step_size_x > 0: + return self.forward_trunk_x_offset + self.tune_trunk_x_offset + elif self.step_size_x == 0: + return self.default_trunk_x_offset + self.tune_trunk_x_offset + else: + return self.backward_trunk_x_offset + self.tune_trunk_x_offset diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/__init__.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/__init__.py new file mode 100644 index 0000000..fca1e25 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/__init__.py @@ -0,0 +1 @@ +from .placo_walk_engine import PlacoWalkEngine diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py new file mode 100644 index 0000000..f2aa415 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py @@ -0,0 +1,301 @@ +import time +import warnings +import json + +import numpy as np +import placo +import os + +warnings.filterwarnings("ignore") + +DT = 0.01 +REFINE = 10 + + +class PlacoWalkEngine: + def __init__( + self, + asset_path: str = "", + model_filename: str = "go_bdx.urdf", + init_params: dict = {}, + ignore_feet_contact: bool = False, + ) -> None: + model_filename = os.path.join(asset_path, model_filename) + self.asset_path = asset_path + self.model_filename = model_filename + self.ignore_feet_contact = ignore_feet_contact + + # Loading the robot + self.robot = placo.HumanoidRobot(model_filename) + + self.parameters = placo.HumanoidParameters() + if init_params is not None: + self.load_parameters(init_params) + else: + defaults_filename = os.path.join(asset_path, "placo_defaults.json") + self.load_defaults(defaults_filename) + + # Creating the kinematics solver + self.solver = placo.KinematicsSolver(self.robot) + self.solver.enable_velocity_limits(True) + self.robot.set_velocity_limits(12.0) + self.solver.enable_joint_limits(False) + self.solver.dt = DT / REFINE + + self.robot.set_joint_limits("left_knee", 2, 0.01) + self.robot.set_joint_limits("right_knee", 2, 0.01) + + # Creating the walk QP tasks + self.tasks = placo.WalkTasks() + if hasattr(self.parameters, "trunk_mode"): + self.tasks.trunk_mode = self.parameters.trunk_mode + self.tasks.com_x = 0.0 + self.tasks.initialize_tasks(self.solver, self.robot) + self.tasks.left_foot_task.orientation().mask.set_axises("yz", "local") + self.tasks.right_foot_task.orientation().mask.set_axises("yz", "local") + # tasks.trunk_orientation_task.configure("trunk_orientation", "soft", 1e-4) + # tasks.left_foot_task.orientation().configure("left_foot_orientation", "soft", 1e-6) + # tasks.right_foot_task.orientation().configure("right_foot_orientation", "soft", 1e-6) + + # # Creating a joint task to assign DoF values for upper body + self.joints = self.parameters.joints + joint_degrees = self.parameters.joint_angles + joint_radians = { + joint: np.deg2rad(degrees) for joint, degrees in joint_degrees.items() + } + self.joints_task = self.solver.add_joints_task() + self.joints_task.set_joints(joint_radians) + self.joints_task.configure("joints", "soft", 1.0) + + # Placing the robot in the initial position + print("Placing the robot in the initial position...") + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + print("Initial position reached") + + print(self.get_angles()) + # exit() + + # Creating the FootstepsPlanner + self.repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive( + self.parameters + ) + self.d_x = 0.0 + self.d_y = 0.0 + self.d_theta = 0.0 + self.nb_steps = 5 + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + + # Creating the pattern generator and making an initial plan + self.walk = placo.WalkPatternGenerator(self.robot, self.parameters) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + self.start = None + self.initial_delay = -1.0 + # self.initial_delay = 0 + self.t = self.initial_delay + self.last_replan = 0 + + # TODO remove startend_double_support_duration() when starting and ending ? + self.period = ( + 2 * self.parameters.single_support_duration + + 2 * self.parameters.double_support_duration() + ) + + def load_defaults(self, filename): + with open(filename, "r") as f: + data = json.load(f) + params = self.parameters + load_parameters(data) + + def load_parameters(self, data): + params = self.parameters + params.double_support_ratio = data.get( + "double_support_ratio", params.double_support_ratio + ) + params.startend_double_support_ratio = data.get( + "startend_double_support_ratio", params.startend_double_support_ratio + ) + params.planned_timesteps = data.get( + "planned_timesteps", params.planned_timesteps + ) + params.replan_timesteps = data.get("replan_timesteps", params.replan_timesteps) + params.walk_com_height = data.get("walk_com_height", params.walk_com_height) + params.walk_foot_height = data.get("walk_foot_height", params.walk_foot_height) + params.walk_trunk_pitch = np.deg2rad( + data.get("walk_trunk_pitch", np.rad2deg(params.walk_trunk_pitch)) + ) + params.walk_foot_rise_ratio = data.get( + "walk_foot_rise_ratio", params.walk_foot_rise_ratio + ) + params.single_support_duration = data.get( + "single_support_duration", params.single_support_duration + ) + params.single_support_timesteps = data.get( + "single_support_timesteps", params.single_support_timesteps + ) + params.foot_length = data.get("foot_length", params.foot_length) + params.feet_spacing = data.get("feet_spacing", params.feet_spacing) + params.zmp_margin = data.get("zmp_margin", params.zmp_margin) + params.foot_zmp_target_x = data.get( + "foot_zmp_target_x", params.foot_zmp_target_x + ) + params.foot_zmp_target_y = data.get( + "foot_zmp_target_y", params.foot_zmp_target_y + ) + params.walk_max_dtheta = data.get("walk_max_dtheta", params.walk_max_dtheta) + params.walk_max_dy = data.get("walk_max_dy", params.walk_max_dy) + params.walk_max_dx_forward = data.get( + "walk_max_dx_forward", params.walk_max_dx_forward + ) + params.walk_max_dx_backward = data.get( + "walk_max_dx_backward", params.walk_max_dx_backward + ) + params.joints = data.get("joints", []) + params.joint_angles = data.get("joint_angles", []) + if "trunk_mode" in data: + params.trunk_mode = data.get("trunk_mode") + + def get_angles(self, ignore=[]): + angles = {joint: self.robot.get_joint(joint) for joint in self.joints} + for joint in ignore: + angles.pop(joint, None) + return angles + + def reset(self): + self.t = self.initial_delay + self.start = None + self.last_replan = 0 + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + def set_traj(self, d_x, d_y, d_theta): + self.d_x = d_x + self.d_y = d_y + self.d_theta = d_theta + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + def get_footsteps_in_world(self): + footsteps = self.trajectory.get_supports() + footsteps_in_world = [] + for footstep in footsteps: + if not footstep.is_both(): + footsteps_in_world.append(footstep.frame()) + + for i in range(len(footsteps_in_world)): + footsteps_in_world[i][:3, 3][1] += self.parameters.feet_spacing / 2 + + return footsteps_in_world + + def get_footsteps_in_robot_frame(self): + T_world_fbase = self.robot.get_T_world_fbase() + + footsteps = self.trajectory.get_supports() + footsteps_in_robot_frame = [] + for footstep in footsteps: + if not footstep.is_both(): + T_world_footstepFrame = footstep.frame().copy() + T_fbase_footstepFrame = ( + np.linalg.inv(T_world_fbase) @ T_world_footstepFrame + ) + T_fbase_footstepFrame = placo.flatten_on_floor(T_fbase_footstepFrame) + T_fbase_footstepFrame[:3, 3][2] = -T_world_fbase[:3, 3][2] + + footsteps_in_robot_frame.append(T_fbase_footstepFrame) + + return footsteps_in_robot_frame + + def get_current_support_phase(self): + if self.trajectory.support_is_both(self.t): + return "both" + + return self.trajectory.support_side(self.t) + + def tick(self, dt, left_contact=True, right_contact=True): + if self.start is None: + self.start = time.time() + + if not self.ignore_feet_contact: + if left_contact: + self.time_since_last_left_contact = 0.0 + if right_contact: + self.time_since_last_right_contact = 0.0 + + falling = not self.ignore_feet_contact and ( + self.time_since_last_left_contact > self.parameters.single_support_duration + or self.time_since_last_right_contact + > self.parameters.single_support_duration + ) + + for k in range(REFINE): + # Updating the QP tasks from planned trajectory + if not falling: + self.tasks.update_tasks_from_trajectory( + self.trajectory, self.t - dt + k * dt / REFINE + ) + + self.robot.update_kinematics() + _ = self.solver.solve(True) + + # If enough time elapsed and we can replan, do the replanning + if ( + self.t - self.last_replan + > self.parameters.replan_timesteps * self.parameters.dt() + and self.walk.can_replan_supports(self.trajectory, self.t) + ): + self.last_replan = self.t + + # Replanning footsteps from current trajectory + self.supports = self.walk.replan_supports( + self.repetitive_footsteps_planner, self.trajectory, self.t + ) + + # Replanning CoM trajectory, yielding a new trajectory we can switch to + self.trajectory = self.walk.replan(self.supports, self.trajectory, self.t) + + self.time_since_last_left_contact += dt + self.time_since_last_right_contact += dt + self.t += dt + + # while time.time() < self.start_t + self.t: + # time.sleep(1e-3) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py.bak b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py.bak new file mode 100644 index 0000000..4eccafc --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py.bak @@ -0,0 +1,303 @@ +import time +import warnings + +import numpy as np +import placo + +warnings.filterwarnings("ignore") + +DT = 0.01 +REFINE = 10 + + +class PlacoWalkEngine: + def __init__( + self, + model_filename: str = "../../robots/bdx/robot.urdf", + ignore_feet_contact: bool = False, + ) -> None: + self.model_filename = model_filename + self.ignore_feet_contact = ignore_feet_contact + + # Loading the robot + self.robot = placo.HumanoidRobot(model_filename) + + # Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency + self.parameters = placo.HumanoidParameters() + + self.parameters.double_support_ratio = ( + 0.2 # Ratio of double support (0.0 to 1.0) + ) + self.parameters.startend_double_support_ratio = ( + 1.5 # Ratio duration of supports for starting and stopping walk + ) + self.parameters.planned_timesteps = 48 # Number of timesteps planned ahead + self.parameters.replan_timesteps = 10 # Replanning each n timesteps + # parameters.zmp_reference_weight = 1e-6 + + # Posture parameters + # self.parameters.walk_com_height = 0.16 # Constant height for the CoM [m] + self.parameters.walk_com_height = 0.165 # Constant height for the CoM [m] + self.parameters.walk_foot_height = ( + 0.025 # Height of foot rising while walking [m] # 3 + ) + # self.parameters.walk_trunk_pitch = 0 # Trunk pitch angle [rad] + self.parameters.walk_trunk_pitch = np.deg2rad(-5) # Trunk pitch angle [rad] + self.parameters.walk_foot_rise_ratio = ( + 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0) + ) + self.parameters.single_support_duration = ( + 0.3 # Duration of single support phase [s] + ) + self.parameters.single_support_timesteps = ( + 10 # Number of planning timesteps per single support phase + ) + + # Feet parameters + self.parameters.foot_length = 0.06 # Foot length [m] + # self.parameters.foot_width = 0.006 # Foot width [m] + self.parameters.feet_spacing = 0.14 # Lateral feet spacing [m] # 12 + self.parameters.zmp_margin = 0.00 # ZMP margin [m] + self.parameters.foot_zmp_target_x = ( + 0.0 # Reference target ZMP position in the foot [m] + ) + self.parameters.foot_zmp_target_y = ( + 0.0 # Reference target ZMP position in the foot [m] + ) + + # Limit parameters + self.parameters.walk_max_dtheta = 1 # Maximum dtheta per step [rad] + self.parameters.walk_max_dy = 0.1 # Maximum dy per step [m] + self.parameters.walk_max_dx_forward = 0.08 # Maximum dx per step forward [m] + self.parameters.walk_max_dx_backward = 0.03 # Maximum dx per step backward [m] + + # Creating the kinematics solver + self.solver = placo.KinematicsSolver(self.robot) + self.solver.enable_velocity_limits(True) + self.robot.set_velocity_limits(12.0) + self.solver.enable_joint_limits(False) + self.solver.dt = DT / REFINE + + self.robot.set_joint_limits("left_knee", -2, -0.01) + self.robot.set_joint_limits("right_knee", -2, -0.01) + + # Creating the walk QP tasks + self.tasks = placo.WalkTasks() + # tasks.trunk_mode = True + # self.tasks.com_x = -0.015 + self.tasks.com_x = 0.0 + self.tasks.initialize_tasks(self.solver, self.robot) + self.tasks.left_foot_task.orientation().mask.set_axises("yz", "local") + self.tasks.right_foot_task.orientation().mask.set_axises("yz", "local") + # tasks.trunk_orientation_task.configure("trunk_orientation", "soft", 1e-4) + # tasks.left_foot_task.orientation().configure("left_foot_orientation", "soft", 1e-6) + # tasks.right_foot_task.orientation().configure("right_foot_orientation", "soft", 1e-6) + + # # Creating a joint task to assign DoF values for upper body + self.joints_task = self.solver.add_joints_task() + self.joints_task.set_joints( + { + "head_pitch": np.deg2rad(-10), + "head_yaw": 0.0, + "neck_pitch": np.deg2rad(-10), + "left_antenna": np.deg2rad(0), + "right_antenna": np.deg2rad(0), + # "right_knee": np.deg2rad(-10), + # "left_knee": np.deg2rad(-10), + } + ) + self.joints_task.configure("joints", "soft", 1.0) + + # Placing the robot in the initial position + print("Placing the robot in the initial position...") + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + print("Initial position reached") + + print(self.get_angles()) + # exit() + + # Creating the FootstepsPlanner + self.repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive( + self.parameters + ) + self.d_x = 0.0 + self.d_y = 0.0 + self.d_theta = 0.0 + self.nb_steps = 5 + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + + # Creating the pattern generator and making an initial plan + self.walk = placo.WalkPatternGenerator(self.robot, self.parameters) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + self.start = None + self.initial_delay = -1.0 + # self.initial_delay = 0 + self.t = self.initial_delay + self.last_replan = 0 + + # TODO remove startend_double_support_duration() when starting and ending ? + self.period = ( + 2 * self.parameters.single_support_duration + + 2 * self.parameters.double_support_duration() + ) + + def get_angles(self): + angles = { + "left_hip_yaw": self.robot.get_joint("left_hip_yaw"), + "left_hip_roll": self.robot.get_joint("left_hip_roll"), + "left_hip_pitch": self.robot.get_joint("left_hip_pitch"), + "left_knee": self.robot.get_joint("left_knee"), + "left_ankle": self.robot.get_joint("left_ankle"), + "neck_pitch": self.robot.get_joint("neck_pitch"), + "head_pitch": self.robot.get_joint("head_pitch"), + "head_yaw": self.robot.get_joint("head_yaw"), + "left_antenna": self.robot.get_joint("left_antenna"), + "right_antenna": self.robot.get_joint("right_antenna"), + "right_hip_yaw": self.robot.get_joint("right_hip_yaw"), + "right_hip_roll": self.robot.get_joint("right_hip_roll"), + "right_hip_pitch": self.robot.get_joint("right_hip_pitch"), + "right_knee": self.robot.get_joint("right_knee"), + "right_ankle": self.robot.get_joint("right_ankle"), + } + + return angles + + def reset(self): + self.t = self.initial_delay + self.start = None + self.last_replan = 0 + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + def set_traj(self, d_x, d_y, d_theta): + self.d_x = d_x + self.d_y = d_y + self.d_theta = d_theta + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + def get_footsteps_in_world(self): + footsteps = self.trajectory.get_supports() + footsteps_in_world = [] + for footstep in footsteps: + if not footstep.is_both(): + footsteps_in_world.append(footstep.frame()) + + for i in range(len(footsteps_in_world)): + footsteps_in_world[i][:3, 3][1] += self.parameters.feet_spacing / 2 + + return footsteps_in_world + + def get_footsteps_in_robot_frame(self): + T_world_fbase = self.robot.get_T_world_fbase() + + footsteps = self.trajectory.get_supports() + footsteps_in_robot_frame = [] + for footstep in footsteps: + if not footstep.is_both(): + T_world_footstepFrame = footstep.frame().copy() + T_fbase_footstepFrame = ( + np.linalg.inv(T_world_fbase) @ T_world_footstepFrame + ) + T_fbase_footstepFrame = placo.flatten_on_floor(T_fbase_footstepFrame) + T_fbase_footstepFrame[:3, 3][2] = -T_world_fbase[:3, 3][2] + + footsteps_in_robot_frame.append(T_fbase_footstepFrame) + + return footsteps_in_robot_frame + + def get_current_support_phase(self): + if self.trajectory.support_is_both(self.t): + return "both" + + return self.trajectory.support_side(self.t) + + def tick(self, dt, left_contact=True, right_contact=True): + if self.start is None: + self.start = time.time() + + if not self.ignore_feet_contact: + if left_contact: + self.time_since_last_left_contact = 0.0 + if right_contact: + self.time_since_last_right_contact = 0.0 + + falling = not self.ignore_feet_contact and ( + self.time_since_last_left_contact > self.parameters.single_support_duration + or self.time_since_last_right_contact + > self.parameters.single_support_duration + ) + + for k in range(REFINE): + # Updating the QP tasks from planned trajectory + if not falling: + self.tasks.update_tasks_from_trajectory( + self.trajectory, self.t - dt + k * dt / REFINE + ) + + self.robot.update_kinematics() + _ = self.solver.solve(True) + + # If enough time elapsed and we can replan, do the replanning + if ( + self.t - self.last_replan + > self.parameters.replan_timesteps * self.parameters.dt() + and self.walk.can_replan_supports(self.trajectory, self.t) + ): + self.last_replan = self.t + + # Replanning footsteps from current trajectory + self.supports = self.walk.replan_supports( + self.repetitive_footsteps_planner, self.trajectory, self.t + ) + + # Replanning CoM trajectory, yielding a new trajectory we can switch to + self.trajectory = self.walk.replan(self.supports, self.trajectory, self.t) + + self.time_since_last_left_contact += dt + self.time_since_last_right_contact += dt + self.t += dt + + # while time.time() < self.start_t + self.t: + # time.sleep(1e-3) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py new file mode 100644 index 0000000..faf8926 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py @@ -0,0 +1 @@ +from .poly_spline import PolySpline diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/mujoco_utils.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/mujoco_utils.py new file mode 100644 index 0000000..8edbfa2 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/mujoco_utils.py @@ -0,0 +1,70 @@ +import mujoco +import numpy as np + + +def check_contact(data, model, body1_name, body2_name): + body1_id = data.body(body1_name).id + body2_id = data.body(body2_name).id + + for i in range(data.ncon): + try: + contact = data.contact[i] + except Exception as e: + return False + + if ( + model.geom_bodyid[contact.geom1] == body1_id + and model.geom_bodyid[contact.geom2] == body2_id + ) or ( + model.geom_bodyid[contact.geom1] == body2_id + and model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + +def get_contact_force(data, model, body1_name, body2_name): + body1_id = data.body(body1_name).id + body2_id = data.body(body2_name).id + + contacts = [] + for i in range(data.ncon): + try: + contact = data.contact[i] + except Exception as e: + return 0 + + if ( + model.geom_bodyid[contact.geom1] == body1_id + and model.geom_bodyid[contact.geom2] == body2_id + ) or ( + model.geom_bodyid[contact.geom1] == body2_id + and model.geom_bodyid[contact.geom2] == body1_id + ): + contacts.append((i, contact)) + + if len(contacts) == 0: + return 0 + + force = 0 + for i, con in contacts: + c_array = np.zeros(6, dtype=np.float64) + mujoco.mj_contactForce(model, data, i, c_array) + force += np.linalg.norm(c_array) + + return force + + +def get_actuator_name(model, index: int) -> str: + return mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, index) + + +def get_actuator_index(model, name: str) -> int: + return mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) + + +def list_actuators(model): + for i in range(20): + name = get_actuator_name(model, i) + print(i, name) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/poly_spline.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/poly_spline.py new file mode 100644 index 0000000..ad00ed3 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/poly_spline.py @@ -0,0 +1,149 @@ +import numpy as np + + +class Point: + def __init__(self, position: float, value: float, delta: float): + self.position = position + self.value = value + self.delta = delta + + +class Points: + def __init__(self): + self.points = [] + + +class Polynom: + def __init__(self, a: float, b: float, c: float, d: float): + self.a = a + self.b = b + self.c = c + self.d = d + + +class Spline: + def __init__(self, poly: Polynom, min: float, max: float): + self.poly = poly + self.min = min + self.max = max + + +class Splines: + def __init__(self): + self.splines = [] + + +class PolySpline: + def __init__(self): + self._points = [] + self._splines = [] + + def add_point(self, position: float, value: float, delta: float): + if len(self._points) > 0 and position <= self._points[-1].position: + raise Exception( + "Trying to add a point in a cublic spline before a previous one" + ) + self._points.append(Point(position, value, delta)) + + self.compute_splines() + + def copy(self): + poly_spline = PolySpline() + for point in self._points: + poly_spline.add_point(point.position, point.value, point.delta) + return poly_spline + + def get(self, x: float): + return self.interpolation(x, "value") + + def get_vel(self, x: float): + return self.interpolation(x, "speed") + + def get_mod(self, x: float): + if x < 0.0: + x = 1.0 + (x - ((int(x) / 1))) + elif x > 1.0: + x = x - ((int(x) / 1)) + return self.get(x) + + def clear(self): + self._points = [] + self._splines = [] + + def compute_splines(self): + self._splines = [] + if len(self._points) < 2: + return + + for i in range(1, len(self._points)): + if ( + np.abs(self._points[i - 1].position - self._points[i].position) + < 0.00001 + ): + continue + poly = self.polynom_fit( + self._points[i - 1].value, + self._points[i - 1].delta, + self._points[i].value, + self._points[i].delta, + ) + spline = Spline( + poly, self._points[i - 1].position, self._points[i].position + ) + self._splines.append(spline) + + def polynom_fit(self, val1: float, delta1: float, val2: float, delta2: float): + a = 2.0 * val1 + delta1 + delta2 - 2.0 * val2 + b = 3.0 * val2 - 2.0 * delta1 - 3.0 * val1 - delta2 + c = delta1 + d = val1 + return Polynom(a, b, c, d) + + def interpolation( + self, x: float, value_type="value" + ): # value_type can be "value" or "speed" + if value_type not in ["value", "speed"]: + raise Exception("Invalid value_type") + + if len(self._points) == 0: + return 0.0 + elif len(self._points) == 1: + if value_type == "value": + return self._points[0].value + else: + return self._points[0].delta + else: + if x < self._splines[0].min: + x = self._splines[0].min + if x > self._splines[-1].max: + x = self._splines[-1].max + + for i in range(len(self._splines)): + if x >= self._splines[i].min and x <= self._splines[i].max: + xi = (x - self._splines[i].min) / ( + self._splines[i].max - self._splines[i].min + ) + if value_type == "value": + return self.polynom_value(xi, self._splines[i].poly) + elif value_type == "speed": + return self.polynom_diff(xi, self._splines[i].poly) + return 0.0 + + def polynom_value(self, t: float, p: Polynom): + return p.d + t * (t * (p.a * t + p.b) + p.c) + + def polynom_diff(self, t: float, p: Polynom): + return t * (3 * p.a * t + 2 * p.b) + p.c + + +if __name__ == "__main__": + poly_spline = PolySpline() + poly_spline.add_point(0.0, 0.0, 0.0) + poly_spline.add_point(1.0, 1.0, 0.0) + poly_spline.add_point(2.0, 0.0, 0.0) + import matplotlib.pyplot as plt + + x = np.linspace(-1, 3, 100) + y = [poly_spline.get(i) for i in x] + plt.plot(x, y) + plt.show() diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/rl_utils.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/rl_utils.py new file mode 100644 index 0000000..a5a0ae9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/rl_utils.py @@ -0,0 +1,120 @@ +# This is the joints order when loading using IsaacGymEnvs +# ['left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna', 'right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle'] +# This is the "standard" order (from mujoco) +# ['right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle', 'left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna'] +# +# We need to reorder the joints to match the IsaacGymEnvs order +# + +mujoco_joints_order = [ + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", +] + +isaac_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + + +def isaac_to_mujoco(joints): + new_joints = [ + # right leg + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + # left leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # head + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + ] + + return new_joints + + +def mujoco_to_isaac(joints): + new_joints = [ + # left leg + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + # head + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + # right leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + ] + return new_joints + + +def test(joints): + new_joints = [ + # right leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # head + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + # left leg + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + ] + return new_joints + + +def action_to_pd_targets(action, pd_action_offset, pd_action_scale): + return pd_action_offset + pd_action_scale * action diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/xbox_controller.py b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/xbox_controller.py new file mode 100644 index 0000000..bd0b640 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/xbox_controller.py @@ -0,0 +1,130 @@ +import math +import threading + +from inputs import get_gamepad + + +class XboxController(object): + MAX_TRIG_VAL = math.pow(2, 8) + MAX_JOY_VAL = math.pow(2, 15) + + def __init__(self): + + self.LeftJoystickY = 0 + self.LeftJoystickX = 0 + self.RightJoystickY = 0 + self.RightJoystickX = 0 + self.LeftTrigger = 0 + self.RightTrigger = 0 + self.LeftBumper = 0 + self.RightBumper = 0 + self.A = 0 + self.X = 0 + self.Y = 0 + self.B = 0 + self.LeftThumb = 0 + self.RightThumb = 0 + self.Back = 0 + self.Start = 0 + self.LeftDPad = 0 + self.RightDPad = 0 + self.UpDPad = 0 + self.DownDPad = 0 + + self._monitor_thread = threading.Thread( + target=self._monitor_controller, args=() + ) + self._monitor_thread.daemon = True + self._monitor_thread.start() + + def deadzone(self, val, range): + if abs(val) < range: + return 0 + return val + + def read(self): # return the buttons/triggers that you care about in this method + ret = { + "l_x": round(self.deadzone(self.LeftJoystickX, 0.15), 2), + "l_y": round(self.deadzone(self.LeftJoystickY, 0.15), 2), + "r_x": round(self.deadzone(self.RightJoystickX, 0.15), 2), + "r_y": round(self.deadzone(self.RightJoystickY, 0.15), 2), + "dpad_left": self.LeftDPad, + "dpad_right": self.RightDPad, + "dpad_up": self.UpDPad, + "dpad_down": self.DownDPad, + "l_trigger": round(self.LeftTrigger / 4.0, 2), + "r_trigger": round(self.RightTrigger / 4.0, 2), + "l_bumper": self.LeftBumper, + "r_bumper": self.RightBumper, + "a": self.A, + "x": self.X, + "y": self.Y, + "b": self.B, + "start": self.Start, + "back": self.Back, + } + + return ret + + def _monitor_controller(self): + while True: + events = get_gamepad() + for event in events: + if event.code == "ABS_Y": + self.LeftJoystickY = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_X": + self.LeftJoystickX = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RY": + self.RightJoystickY = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RX": + self.RightJoystickX = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_Z": + self.LeftTrigger = ( + event.state / XboxController.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "ABS_RZ": + self.RightTrigger = ( + event.state / XboxController.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "BTN_TL": + self.LeftBumper = event.state + elif event.code == "BTN_TR": + self.RightBumper = event.state + elif event.code == "BTN_SOUTH": + self.A = event.state + elif event.code == "BTN_NORTH": + self.X = event.state # previously switched with Y + elif event.code == "BTN_WEST": + self.Y = event.state # previously switched with X + elif event.code == "BTN_EAST": + self.B = event.state + elif event.code == "BTN_THUMBL": + self.LeftThumb = event.state + elif event.code == "BTN_THUMBR": + self.RightThumb = event.state + elif event.code == "BTN_SELECT": + self.Back = event.state + elif event.code == "BTN_START": + self.Start = event.state + elif event.code == "BTN_TRIGGER_HAPPY1": + self.LeftDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY2": + self.RightDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY3": + self.UpDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY4": + self.DownDPad = event.state + + +if __name__ == "__main__": + joy = XboxController() + while True: + print(joy.read()) diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/.gitignore b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/.gitignore new file mode 100644 index 0000000..9c9cd7b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/.gitignore @@ -0,0 +1 @@ +old_bdx \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/.gitignore~ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/.gitignore~ new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part new file mode 100644 index 0000000..f06679f --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MRjqS1rfFrE9mVebb", + "isStandardContent": false, + "name": "39281023.prt <1>", + "partId": "JFn", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl new file mode 100644 index 0000000..3ca5977 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part new file mode 100644 index 0000000..3d84899 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "Mme4otj1W/8+N1ByX", + "isStandardContent": false, + "name": "antenna <1>", + "partId": "RNCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl new file mode 100644 index 0000000..957ed1d Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.part new file mode 100644 index 0000000..c089398 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MnM0IlZpoFlIWBoQO", + "isStandardContent": false, + "name": "antenna_holder_left <1>", + "partId": "RSDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl new file mode 100644 index 0000000..c014a7b Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.part new file mode 100644 index 0000000..446e932 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MCFYWU6z+m7i7uuh3", + "isStandardContent": false, + "name": "antenna_holder_right <1>", + "partId": "R8BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl new file mode 100644 index 0000000..cb55a2d Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.part new file mode 100644 index 0000000..5673957 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MQd2hqMci0z3nsWR2", + "isStandardContent": false, + "name": "antenna_motor_holder <1>", + "partId": "RpCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl new file mode 100644 index 0000000..4510f41 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part new file mode 100644 index 0000000..28a1b78 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MDIl/o7j9ZkP0F1Lu", + "isStandardContent": false, + "name": "antenna_tip <1>", + "partId": "RQCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl new file mode 100644 index 0000000..10ffbe9 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.part new file mode 100644 index 0000000..90f64f3 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "Mq59OqMGpBdxKaLN8", + "isStandardContent": false, + "name": "axis_to_axis <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl new file mode 100644 index 0000000..8bd53af Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.part new file mode 100644 index 0000000..a8e0332 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "M9iQmeJTc2SjWPNZy", + "isStandardContent": false, + "name": "block_to_axis <1>", + "partId": "JND", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl new file mode 100644 index 0000000..b811f43 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part new file mode 100644 index 0000000..5adf5a2 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MqxaAqvh7r8oVUe/M", + "isStandardContent": false, + "name": "bms <1>", + "partId": "JSD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl new file mode 100644 index 0000000..4598b81 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part new file mode 100644 index 0000000..834b727 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "58e35a434b8265113623a1c6", + "fullConfiguration": "default", + "id": "McEQHWuSIpb4LzRnT", + "isStandardContent": false, + "name": "BNO055 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl new file mode 100644 index 0000000..047b620 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part new file mode 100644 index 0000000..dacfd4b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MTEKAmJCtStJHQZcz", + "isStandardContent": false, + "name": "Board <1>", + "partId": "JFf", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl new file mode 100644 index 0000000..ebdecfe Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part new file mode 100644 index 0000000..bf178fb --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MgtEBF9hk9yBKNuFr", + "isStandardContent": false, + "name": "body_part <1>", + "partId": "RUBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl new file mode 100644 index 0000000..cd219cd Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part new file mode 100644 index 0000000..3dcb855 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MtGjXc2STrbHqs0TF", + "isStandardContent": false, + "name": "cage_back <1>", + "partId": "RfBH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl new file mode 100644 index 0000000..4616af9 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part new file mode 100644 index 0000000..082d252 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "M3PrPkym5OxucEDLY", + "isStandardContent": false, + "name": "cage_bottom <1>", + "partId": "R5DD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl new file mode 100644 index 0000000..8cdbc2e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.part new file mode 100644 index 0000000..6476567 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MlZeSTv4GhNntgplW", + "isStandardContent": false, + "name": "cage_bottom_battery_hold <1>", + "partId": "R5DH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl new file mode 100644 index 0000000..2ac1ca6 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part new file mode 100644 index 0000000..c5c118b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MMhqY41MtSO0BMQmM", + "isStandardContent": false, + "name": "cage_top <1>", + "partId": "RRDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl new file mode 100644 index 0000000..1c35e18 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part new file mode 100644 index 0000000..0c5f048 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "Mx1gCi95bgVKmQGMH", + "isStandardContent": false, + "name": "cell <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl new file mode 100644 index 0000000..20e4cc3 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json new file mode 100644 index 0000000..49a9eac --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json @@ -0,0 +1,5 @@ +{ + "documentId": "a18ff8cc622a533762a3a6f5", + "outputFormat": "urdf", + "assemblyName": "BDX", +} diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.part new file mode 100644 index 0000000..2f84b4d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MeE9kg4oOZhPqrFKc", + "isStandardContent": false, + "name": "DC15_A01_CASE_B_DUMMY <1>", + "partId": "JFn", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl new file mode 100644 index 0000000..8bc42f3 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.part new file mode 100644 index 0000000..3aa9005 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MMeN4IWguTKJDjvpk", + "isStandardContent": false, + "name": "DC15_A01_CASE_F_DUMMY <1>", + "partId": "JFr", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl new file mode 100644 index 0000000..211a2ee Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.part new file mode 100644 index 0000000..a92d5fa --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MVGfJzCaRc159i+a2", + "isStandardContent": false, + "name": "DC15_A01_CASE_M_DUMMY <1>", + "partId": "JFv", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl new file mode 100644 index 0000000..7f4c118 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.part new file mode 100644 index 0000000..2240b09 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MS6UeJfX4Xsr7Qkr6", + "isStandardContent": false, + "name": "DC15_A01_HORN_DUMMY <1>", + "partId": "JFj", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl new file mode 100644 index 0000000..c56ae4a Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.part new file mode 100644 index 0000000..1e70692 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MvkpKlw0tvUizYc+v", + "isStandardContent": false, + "name": "DC15_A01_HORN_IDLE2_DUMMY <1>", + "partId": "JFT", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl new file mode 100644 index 0000000..d36e079 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part new file mode 100644 index 0000000..aa6358d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MGLU297Hzzd9cmJbM", + "isStandardContent": false, + "name": "double_U <1>", + "partId": "R2BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl new file mode 100644 index 0000000..c31e309 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part new file mode 100644 index 0000000..9b3b38b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "bdbd17662073b7a524b273fe", + "fullConfiguration": "default", + "id": "MabiF1g8dKex+GSYq", + "isStandardContent": false, + "name": "foot <1>", + "partId": "JwD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl new file mode 100644 index 0000000..360e25d Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part new file mode 100644 index 0000000..98cd544 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "bdbd17662073b7a524b273fe", + "fullConfiguration": "default", + "id": "MacgSWoiJ3m0BAOhr", + "isStandardContent": false, + "name": "foot_contact <1>", + "partId": "JwH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl new file mode 100644 index 0000000..0675f8b Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part new file mode 100644 index 0000000..366aa6d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MgOjcYXWnsH30ASqy", + "isStandardContent": false, + "name": "front_cover <1>", + "partId": "JmD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl new file mode 100644 index 0000000..6a26fd1 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part new file mode 100644 index 0000000..e5d1eee --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MJY1ejhDKCoaIDnR4", + "isStandardContent": false, + "name": "head <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl new file mode 100644 index 0000000..87e00df Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.part new file mode 100644 index 0000000..c1b82d8 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MQrACtZhtBd++0zIE", + "isStandardContent": false, + "name": "head_roll_pitch <1>", + "partId": "JlD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl new file mode 100644 index 0000000..b18e1a2 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.part new file mode 100644 index 0000000..0272703 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MZ4X5IZKAFwcUFWJL", + "isStandardContent": false, + "name": "hip_left_roll_to_pitch <1>", + "partId": "RmCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl new file mode 100644 index 0000000..ba4bf36 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.part new file mode 100644 index 0000000..4d2c985 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MqpX4oSaGt0fcUeEq", + "isStandardContent": false, + "name": "hip_right_roll_to_pitch <1>", + "partId": "JXD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl new file mode 100644 index 0000000..ea5c3a4 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part new file mode 100644 index 0000000..9224de8 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MhtdHOvIsBT6us1tg", + "isStandardContent": false, + "name": "holder <1>", + "partId": "JLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl new file mode 100644 index 0000000..0d3f382 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.part new file mode 100644 index 0000000..1288771 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MwzAB4u2uoHpcOUsB", + "isStandardContent": false, + "name": "holder_U2D2_power_hub <1>", + "partId": "RdED", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl new file mode 100644 index 0000000..446c015 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.part new file mode 100644 index 0000000..b23e220 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MqKlaiko0aCWNOvid", + "isStandardContent": false, + "name": "JST-B3B-EH-A.PRT.1 <2>", + "partId": "JFb", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl new file mode 100644 index 0000000..2262446 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.part new file mode 100644 index 0000000..62eca13 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MiQsWzqt1weSpH7Ee", + "isStandardContent": false, + "name": "JST-B3B-EH-A.PRT <1>", + "partId": "JFX", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl new file mode 100644 index 0000000..2262446 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.part new file mode 100644 index 0000000..53a4820 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mjo+l+YE4yXewvi5b", + "isStandardContent": false, + "name": "JST-B4B-EH-A.PRT.1 <2>", + "partId": "JFj", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl new file mode 100644 index 0000000..a957dc2 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.part new file mode 100644 index 0000000..ebdbae7 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mkf/5vUS2ezdkcZAV", + "isStandardContent": false, + "name": "JST-B4B-EH-A.PRT <1>", + "partId": "JFT", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl new file mode 100644 index 0000000..a957dc2 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.part new file mode 100644 index 0000000..3d5cc52 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mk62vUHK7F3maBAlW", + "isStandardContent": false, + "name": "JST-B4B-PH-K-S.PRT <1>", + "partId": "JFP", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl new file mode 100644 index 0000000..3f02215 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.part new file mode 100644 index 0000000..c0d975f --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mzqj3Q49Pss2LoqJX", + "isStandardContent": false, + "name": "MICRO_USB_2_0_CONNECTOR__AB_REC.PRT <1>", + "partId": "JFL", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl new file mode 100644 index 0000000..10c20a8 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.part new file mode 100644 index 0000000..6974d48 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MnAngNatw3dhkwz+s", + "isStandardContent": false, + "name": "PCB_U2D2.PRT <1>", + "partId": "JFH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl new file mode 100644 index 0000000..d620752 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part new file mode 100644 index 0000000..98053ba --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "8bfad62b97860b67f75e3376", + "fullConfiguration": "default", + "id": "M99VFeL41Pi5byGXl", + "isStandardContent": false, + "name": "power_switch <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl new file mode 100644 index 0000000..12a17d9 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part new file mode 100644 index 0000000..d14b889 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MzdR3wa2Pikx8l7un", + "isStandardContent": false, + "name": "rasp_spacer <3>", + "partId": "R3DD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl new file mode 100644 index 0000000..5cd723a Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part new file mode 100644 index 0000000..e93fb09 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "9f45233997c8070932aec904", + "fullConfiguration": "default", + "id": "MchLzfPC746ZAzlWi", + "isStandardContent": false, + "name": "RaspberryPiZeroW <2>", + "partId": "JFH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl new file mode 100644 index 0000000..93e9d8a Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.part new file mode 100644 index 0000000..50ef50a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "M9cyMohQTfT3UyQa8", + "isStandardContent": false, + "name": "renfort_head_link <1>", + "partId": "RJDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl new file mode 100644 index 0000000..d335e81 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part new file mode 100644 index 0000000..4ddbedc --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MEjG5RS5osemC0kEN", + "isStandardContent": false, + "name": "renfort_leg <1>", + "partId": "JUD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl new file mode 100644 index 0000000..63a1691 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf new file mode 100644 index 0000000..16f9f19 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf @@ -0,0 +1,2284 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml new file mode 100644 index 0000000..403e3ae --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml @@ -0,0 +1,305 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml new file mode 100644 index 0000000..01f1a02 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part new file mode 100644 index 0000000..cbe4d1f --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "349d7eaab34e22a2c029ddc9", + "fullConfiguration": "default", + "id": "MuCWOV8+Toix8DMeR", + "isStandardContent": false, + "name": "sg90 <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl new file mode 100644 index 0000000..581f49c Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part new file mode 100644 index 0000000..dba4834 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MpwO0az74Ch9pB7z/", + "isStandardContent": false, + "name": "spacer <1>", + "partId": "RCCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl new file mode 100644 index 0000000..3e96b26 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.part new file mode 100644 index 0000000..6859ce6 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MNRmb843uWiCvcYRP", + "isStandardContent": false, + "name": "U2D2_CASING.PRT <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl new file mode 100644 index 0000000..fb32481 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.part new file mode 100644 index 0000000..2955805 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "1a4a827101fb7d1d1b3cc8de", + "fullConfiguration": "default", + "id": "MOdHUYaAV7HrozYl7", + "isStandardContent": false, + "name": "usb_c_charger <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl new file mode 100644 index 0000000..b2483b9 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.part new file mode 100644 index 0000000..1a8f3a4 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MmXLbiyIJZ1T9tiX3", + "isStandardContent": false, + "name": "antenna <1>", + "partId": "RuOD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl new file mode 100644 index 0000000..07eb706 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.part new file mode 100644 index 0000000..c42dd15 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MvOQ01tQlVV+zSv63", + "isStandardContent": false, + "name": "battery_pack_lid <1>", + "partId": "R5OD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl new file mode 100644 index 0000000..076eb25 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.part new file mode 100644 index 0000000..d5e0fd2 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MqEGDvIKFH4SpUaCn", + "isStandardContent": false, + "name": "bms <1>", + "partId": "JSD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl new file mode 100644 index 0000000..4598b81 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.part new file mode 100644 index 0000000..31c1864 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "58e35a434b8265113623a1c6", + "fullConfiguration": "default", + "id": "MCK/voA1KPjJiuZRD", + "isStandardContent": false, + "name": "BNO055 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl new file mode 100644 index 0000000..6de1ee7 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.part new file mode 100644 index 0000000..dbd8cbc --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "fbc3ecd1071c724f4d859dd6", + "fullConfiguration": "default", + "id": "MxRZVYwGEp1Sig2/i", + "isStandardContent": false, + "name": "Board <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl new file mode 100644 index 0000000..f2f3da5 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.part new file mode 100644 index 0000000..0cac428 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MfCA08JspCUYCoOzy", + "isStandardContent": false, + "name": "body_back <1>", + "partId": "RqKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl new file mode 100644 index 0000000..b6692ac Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.part new file mode 100644 index 0000000..6bcf261 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MAILAHna+4EMDjwEB", + "isStandardContent": false, + "name": "body_front <1>", + "partId": "RbKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl new file mode 100644 index 0000000..fa76bb4 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.part new file mode 100644 index 0000000..93a8273 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mi7/ZBgz7j8FhNN8n", + "isStandardContent": false, + "name": "body_middle_bottom <1>", + "partId": "R/KH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl new file mode 100644 index 0000000..9c8ccee Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.part new file mode 100644 index 0000000..b39e827 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MwLs2m7WqJdIp5rCM", + "isStandardContent": false, + "name": "body_middle_top <1>", + "partId": "R/KD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl new file mode 100644 index 0000000..5530f0b Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.part new file mode 100644 index 0000000..c7c9417 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "Mx1gCi95bgVKmQGMH", + "isStandardContent": false, + "name": "cell <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl new file mode 100644 index 0000000..20e4cc3 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/config.json b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/config.json new file mode 100644 index 0000000..a1032a4 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/config.json @@ -0,0 +1,5 @@ +{ + "documentId": "64074dfcfa379b37d8a47762", + "outputFormat": "urdf", + "assemblyName": "Open_Duck_Mini_v2", +} diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.part new file mode 100644 index 0000000..b940a1a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MHjgEclEhEZQK/p6r", + "isStandardContent": false, + "name": "drive_palonier <1>", + "partId": "JJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl new file mode 100644 index 0000000..66e60e4 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.part new file mode 100644 index 0000000..e087dd5 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MbWaXbNFhwXrvfPr1", + "isStandardContent": false, + "name": "foot_bottom_pla <1>", + "partId": "R7GH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl new file mode 100644 index 0000000..66feed7 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.part new file mode 100644 index 0000000..22b27eb --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MSRJjnMROc2yqGlBV", + "isStandardContent": false, + "name": "foot_bottom_tpu <1>", + "partId": "R7GD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl new file mode 100644 index 0000000..4f05326 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.part new file mode 100644 index 0000000..55842e9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M9QvdrmpVd96BQQ3J", + "isStandardContent": false, + "name": "foot_side <1>", + "partId": "RsGD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl new file mode 100644 index 0000000..20a434e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.part new file mode 100644 index 0000000..8981480 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MryNg763VjsN4xBWU", + "isStandardContent": false, + "name": "foot_top <1>", + "partId": "RsGH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl new file mode 100644 index 0000000..0d08c19 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.part new file mode 100644 index 0000000..1395a55 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MGPnRRHSCLRuHbf9s", + "isStandardContent": false, + "name": "head <1>", + "partId": "RXMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl new file mode 100644 index 0000000..2a101fd Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.part new file mode 100644 index 0000000..4184b67 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MB88Zh5nm8uQRQJB5", + "isStandardContent": false, + "name": "head_bot_sheet <1>", + "partId": "RxMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl new file mode 100644 index 0000000..c565ba6 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.part new file mode 100644 index 0000000..7892e94 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MMYrJ8GU6Tulh9Ezl", + "isStandardContent": false, + "name": "head_pitch_to_yaw <1>", + "partId": "RGCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl new file mode 100644 index 0000000..1de8aef Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.part new file mode 100644 index 0000000..01b0ff2 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MRmftgtax43WG3TM4", + "isStandardContent": false, + "name": "head_roll_mount <1>", + "partId": "RMND", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl new file mode 100644 index 0000000..e6efec9 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.part new file mode 100644 index 0000000..df7aa08 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MSSQrz89LHCpx23/c", + "isStandardContent": false, + "name": "head_yaw_to_roll <1>", + "partId": "RyCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl new file mode 100644 index 0000000..14a2122 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.part new file mode 100644 index 0000000..cd32e37 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MhtdHOvIsBT6us1tg", + "isStandardContent": false, + "name": "holder <1>", + "partId": "JLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl new file mode 100644 index 0000000..0d3f382 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.part new file mode 100644 index 0000000..b56531f --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MqpShx/iKn+WMc6nz", + "isStandardContent": false, + "name": "left_antenna_holder <1>", + "partId": "RfPL", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl new file mode 100644 index 0000000..5b20ae7 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.part new file mode 100644 index 0000000..b68c120 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mh206kD+LDwOu9+3I", + "isStandardContent": false, + "name": "left_cache <1>", + "partId": "RELD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl new file mode 100644 index 0000000..a927bc9 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.part new file mode 100644 index 0000000..c5f56e0 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MkexR4anxdIDvoNnP", + "isStandardContent": false, + "name": "left_knee_to_ankle_left_sheet <1>", + "partId": "RyDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.stl new file mode 100644 index 0000000..e46725d Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.part new file mode 100644 index 0000000..72193f8 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MFSYykuSOyFPl8MEE", + "isStandardContent": false, + "name": "left_knee_to_ankle_right_sheet <1>", + "partId": "RyDH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.stl new file mode 100644 index 0000000..a1fc36e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.part new file mode 100644 index 0000000..8036293 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mg7gXWiWLFdTkX/WZ", + "isStandardContent": false, + "name": "left_roll_to_pitch <1>", + "partId": "RRFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl new file mode 100644 index 0000000..11ee400 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.part new file mode 100644 index 0000000..7ca518f --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M1AJs3nTP2bD6DgAF", + "isStandardContent": false, + "name": "leg_spacer <1>", + "partId": "RfED", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl new file mode 100644 index 0000000..336c0b3 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.part new file mode 100644 index 0000000..db821c4 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M8Je0JZqWOimCS7cY", + "isStandardContent": false, + "name": "neck_left_sheet <1>", + "partId": "RgLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl new file mode 100644 index 0000000..04825be Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.part new file mode 100644 index 0000000..422d393 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M8fJckSPn4u+ct8pO", + "isStandardContent": false, + "name": "neck_right_sheet <1>", + "partId": "RgLH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl new file mode 100644 index 0000000..867149e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.part new file mode 100644 index 0000000..c3fe8bf --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MtwZA0wRrU1lvONjK", + "isStandardContent": false, + "name": "passive_palonier <1>", + "partId": "JVD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl new file mode 100644 index 0000000..234dd7e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.part new file mode 100644 index 0000000..65df91a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "8bfad62b97860b67f75e3376", + "fullConfiguration": "default", + "id": "MoxN2/6DvG8o5Khzi", + "isStandardContent": false, + "name": "power_switch <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl new file mode 100644 index 0000000..a72c233 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.part new file mode 100644 index 0000000..769abe7 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "9f45233997c8070932aec904", + "fullConfiguration": "default", + "id": "MT4+RWBYZDvB/ifWw", + "isStandardContent": false, + "name": "RaspberryPiZeroW <1>", + "partId": "JFH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl new file mode 100644 index 0000000..c47c605 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.part new file mode 100644 index 0000000..43bbafc --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M/p8PGArLA1nrXwe8", + "isStandardContent": false, + "name": "right_antenna_holder <1>", + "partId": "RcOD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl new file mode 100644 index 0000000..3bf85d6 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.part new file mode 100644 index 0000000..3f165a0 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MBtFsLan/52XkruX7", + "isStandardContent": false, + "name": "right_cache <1>", + "partId": "RfPH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl new file mode 100644 index 0000000..fa40ec6 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.part new file mode 100644 index 0000000..5480194 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mvz4SlAytL1YUO5L4", + "isStandardContent": false, + "name": "right_roll_to_pitch <1>", + "partId": "RfPD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl new file mode 100644 index 0000000..38dd671 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.urdf b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.urdf new file mode 100644 index 0000000..2b90f41 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.urdf @@ -0,0 +1,2269 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml new file mode 100644 index 0000000..05b788c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml @@ -0,0 +1,303 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot_motors.xml b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot_motors.xml new file mode 100644 index 0000000..d4ccdb3 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot_motors.xml @@ -0,0 +1,1086 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.part new file mode 100644 index 0000000..566900c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Ma27pDNdkzSRCakAZ", + "isStandardContent": false, + "name": "roll_bearing <1>", + "partId": "RtHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl new file mode 100644 index 0000000..3210d4e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.part new file mode 100644 index 0000000..a824c1a --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MjsDdlagt/LopvMjR", + "isStandardContent": false, + "name": "roll_motor_bottom <1>", + "partId": "RGHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl new file mode 100644 index 0000000..a36dff5 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.part new file mode 100644 index 0000000..7233b6c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MoZYOvTkbbGLGW0df", + "isStandardContent": false, + "name": "roll_motor_top <1>", + "partId": "RyHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl new file mode 100644 index 0000000..99f314c Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml new file mode 100644 index 0000000..d4f7b0c --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml new file mode 100644 index 0000000..31b5f63 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.part new file mode 100644 index 0000000..c02443b --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "349d7eaab34e22a2c029ddc9", + "fullConfiguration": "default", + "id": "MkrkdH7oZvjAepXlL", + "isStandardContent": false, + "name": "sg90 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl new file mode 100644 index 0000000..bac6639 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.part new file mode 100644 index 0000000..2929658 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MMYHrJ4hqH24cpa8I", + "isStandardContent": false, + "name": "trunk_bottom <1>", + "partId": "RvJH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl new file mode 100644 index 0000000..48afbc7 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.part new file mode 100644 index 0000000..f5fad37 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MNg8avQSkDpa0UkRX", + "isStandardContent": false, + "name": "trunk_top <1>", + "partId": "RvJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl new file mode 100644 index 0000000..760665c Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.part new file mode 100644 index 0000000..8cb8bd9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "1a4a827101fb7d1d1b3cc8de", + "fullConfiguration": "default", + "id": "MXjBB69uv3FCgauqS", + "isStandardContent": false, + "name": "usb_c_charger <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl new file mode 100644 index 0000000..2760f99 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.part new file mode 100644 index 0000000..342f518 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MOFyAb/RZuW0kTpzn", + "isStandardContent": false, + "name": "WJ-WK00-0122TOPCABINETCASE_95 <1>", + "partId": "JFb", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl new file mode 100644 index 0000000..984d136 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.part new file mode 100644 index 0000000..ffcc8ab --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "M1HwhnHIG/iEEwnW3", + "isStandardContent": false, + "name": "WJ-WK00-0123MIDDLECASE_56 <1>", + "partId": "JFP", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl new file mode 100644 index 0000000..098c62c Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.part b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.part new file mode 100644 index 0000000..3497c02 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MOFR6bFTIen7rmARW", + "isStandardContent": false, + "name": "WJ-WK00-0124BOTTOMCASE_45 <1>", + "partId": "JFT", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl new file mode 100644 index 0000000..b5c7064 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/battery_pack_lid.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/battery_pack_lid.stl new file mode 100644 index 0000000..c2ba5f8 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/battery_pack_lid.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_back.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_back.stl new file mode 100644 index 0000000..564b8c2 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_back.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_front.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_front.stl new file mode 100644 index 0000000..ec3a048 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_front.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_middle_bottom.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_middle_bottom.stl new file mode 100644 index 0000000..b4207cc Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_middle_bottom.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_middle_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_middle_top.stl new file mode 100644 index 0000000..96dd935 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/body_middle_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/bulb.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/bulb.stl new file mode 100644 index 0000000..93f7d48 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/bulb.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/flash_light_module.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/flash_light_module.stl new file mode 100644 index 0000000..97c3f49 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/flash_light_module.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/flash_reflector_interface.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/flash_reflector_interface.stl new file mode 100644 index 0000000..1919e29 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/flash_reflector_interface.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_bottom_pla.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_bottom_pla.stl new file mode 100644 index 0000000..b53e0d5 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_bottom_pla.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_bottom_tpu.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_bottom_tpu.stl new file mode 100644 index 0000000..2aed4b8 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_bottom_tpu.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_side.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_side.stl new file mode 100644 index 0000000..55bb289 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_side.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_top.stl new file mode 100644 index 0000000..134284e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/foot_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head.stl new file mode 100644 index 0000000..ccf7211 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_bot_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_bot_sheet.stl new file mode 100644 index 0000000..95c939e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_bot_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl new file mode 100644 index 0000000..e64ad2f Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_roll_mount.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_roll_mount.stl new file mode 100644 index 0000000..93aec87 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_roll_mount.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_yaw_to_roll.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_yaw_to_roll.stl new file mode 100644 index 0000000..29edd44 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/head_yaw_to_roll.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl new file mode 100644 index 0000000..e8a82ef Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl new file mode 100644 index 0000000..8242f8c Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_antenna_holder.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_antenna_holder.stl new file mode 100644 index 0000000..2df311c Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_cache.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_cache.stl new file mode 100644 index 0000000..0ce5ca2 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_cache.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_eye.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_eye.stl new file mode 100644 index 0000000..6281ea1 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_eye.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_roll_to_pitch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_roll_to_pitch.stl new file mode 100644 index 0000000..9a60364 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/leg_spacer.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/leg_spacer.stl new file mode 100644 index 0000000..c57f8d2 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/leg_spacer.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/neck_left_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/neck_left_sheet.stl new file mode 100644 index 0000000..e91778a Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/neck_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/neck_right_sheet.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/neck_right_sheet.stl new file mode 100644 index 0000000..c05919e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/neck_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_antenna_holder.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_antenna_holder.stl new file mode 100644 index 0000000..3797e0c Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_cache.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_cache.stl new file mode 100644 index 0000000..16c24f0 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_cache.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_eye.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_eye.stl new file mode 100644 index 0000000..185500a Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_eye.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_roll_to_pitch.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_roll_to_pitch.stl new file mode 100644 index 0000000..d57105d Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/roll_motor_bottom.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/roll_motor_bottom.stl new file mode 100644 index 0000000..a8dcb05 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/roll_motor_bottom.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/roll_motor_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/roll_motor_top.stl new file mode 100644 index 0000000..91ac599 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/roll_motor_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/speaker_interface.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/speaker_interface.stl new file mode 100644 index 0000000..dd7225e Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/speaker_interface.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/speaker_stand.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/speaker_stand.stl new file mode 100644 index 0000000..4265223 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/speaker_stand.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/trunk_bottom.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/trunk_bottom.stl new file mode 100644 index 0000000..a9545d1 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/trunk_bottom.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/print/trunk_top.stl b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/trunk_top.stl new file mode 100644 index 0000000..4fd8711 Binary files /dev/null and b/Open_Duck_Mini-2/Open_Duck_Mini-2/print/trunk_top.stl differ diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/pyproject.toml b/Open_Duck_Mini-2/Open_Duck_Mini-2/pyproject.toml new file mode 100644 index 0000000..7fd26b9 --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" \ No newline at end of file diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/setup.cfg b/Open_Duck_Mini-2/Open_Duck_Mini-2/setup.cfg new file mode 100644 index 0000000..a50de1d --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/setup.cfg @@ -0,0 +1,41 @@ +[metadata] +name = mini-bdx +version = 0.0.1 +author = Antoine Pirrone +author_email = antoine.pirrone@gmail.com +url = https://github.com/apirrone/mini_BDX +description = Making a mini version of the BDX droid +long_description = file: README.md +long_description_content_type = text/markdown + + +[options] +packages = find: +zip_safe = True +include_package_data = True +package_dir= + =mini_bdx +install_requires = +[options.packages.find] +where=mini_bdx + +[options.package_data] + +[options.extras_require] +all = + mujoco==3.1.5 + mujoco-python-viewer==0.1.4 + onshape-to-robot==0.3.25 + gymnasium[mujoco] == 0.29.1 + stable-baselines3[extra]==2.3.2 + sb3_contrib==2.3.0 + placo==0.5.0 + FramesViewer + inputs==0.5 + imitation==1.0.0 + h5py==3.11.0 +robot = placo==0.5.0 + pypot @ git+https://github.com/apirrone/pypot@support_4B_registers + +[options.entry_points] +console_scripts = diff --git a/Open_Duck_Mini-2/Open_Duck_Mini-2/thanks.md b/Open_Duck_Mini-2/Open_Duck_Mini-2/thanks.md new file mode 100644 index 0000000..30f984e --- /dev/null +++ b/Open_Duck_Mini-2/Open_Duck_Mini-2/thanks.md @@ -0,0 +1,10 @@ +People involved in the project. (I'll try to update this list when I think of a name, so not to forget anyone) + +- Mimir Reynisson +- Grégoire Passault +- Augustin Crampette +- Michel Aractingi +- Mankaran Singh +- Steve N'Guyen +- Pierre Rouanet +- Thomas Wolf diff --git a/Open_Duck_Mini-2/README.md b/Open_Duck_Mini-2/README.md new file mode 100644 index 0000000..0becdbe --- /dev/null +++ b/Open_Duck_Mini-2/README.md @@ -0,0 +1,97 @@ +# Open Duck Mini v2 + + + + + + + +
1 2 3
+ +We are making a miniature version of the BDX Droid by Disney. It is about 42 centimeters tall with its legs extended. +The full BOM cost should be under $400 ! + +This repo is kind of a hub where we centralize all resources related to this project. This is a working repo, so there are a lot of undocumented scripts :) We'll try to clean things up at some point. + + +# State of sim2real + +https://github.com/user-attachments/assets/58721d0f-2f95-4088-8900-a5d02f41bba7 + +https://github.com/user-attachments/assets/4129974a-9d97-4651-9474-c078043bb182 + +https://github.com/user-attachments/assets/a0afcd38-15d8-40c6-8171-a619107406b8 + + +# Updates + +> Update 02/04/2024: You can try two policies we trained : [this one](BEST_WALK_ONNX.onnx) and [this one](BEST_WALK_ONNX_2.onnx) +> Run with the following arguments : +> python v2_rl_walk_mujoco.py --onnx_model_path ~/BEST_WALK_ONNX_2.onnx + +> Update 15/03/2025: join our discord server to get help or show us your duck :) https://discord.gg/UtJZsgfQGe + +> Update 07/02/2025: Big progress on sim2real, see videos above :) + +> Update 24/02/2025: Working hard on sim2real ! + +> Update 07/02/2025 : We are writing documentation on the go, but the design and BOM should not change drastically. Still missing the "expression" features, but they can be added after building the robot! + +> Update 22/01/2025 : The mechanical design is pretty much finalized (fixing some mistakes here and there). The current version does not include all the "expression" features we want to include in the final robot (LEDs for the eyes, a camera, a speaker and a microphone). We are now working on making it walk with reinforcement learning ! + +# Community + +![duck_collage](https://github.com/user-attachments/assets/e240c06e-769f-4c87-b65f-189a442cf1e9) + +Join our discord community ! https://discord.gg/UtJZsgfQGe + +# CAD + +https://cad.onshape.com/documents/64074dfcfa379b37d8a47762/w/3650ab4221e215a4f65eb7fe/e/0505c262d882183a25049d05 + +See [this document](docs/prepare_robot.md) for getting from a onshape design to a simulated robot in MuJoCo (Warning, outdated. Has not been updated in a while) + +# RL stuff + +We are switching to Mujoco Playground, see this [repo](https://github.com/apirrone/Open_Duck_Playground) + +https://github.com/user-attachments/assets/037a1790-7ac1-4140-b154-2c901d20d5f5 + + +## Reference motion generation for imitation learning + +https://github.com/user-attachments/assets/4cb52e17-99a5-47a8-b841-4141596b7afb + +See [this repo](https://github.com/apirrone/Open_Duck_reference_motion_generator) + +## Actuator identification + +We used Rhoban's [BAM](https://github.com/Rhoban/bam) + +# BOM + +https://docs.google.com/spreadsheets/d/1gq4iWWHEJVgAA_eemkTEsshXqrYlFxXAPwO515KpCJc/edit?usp=sharing + +Chinese: https://zihao-ai.feishu.cn/wiki/AfAtw69vRigXaRk5UkbcrAiLnJw?from=from_copylink + +# Build Guide + +Chinese: https://zihao-ai.feishu.cn/wiki/space/7488517034406625281 + +## Print Guide + +See [print_guide](docs/print_guide.md). + +## Assembly Guide + +See [assembly guide (incomplete)](docs/assembly_guide.md). + +# Embedded runtime + +This repo contains the code to run the policies on the onboard computer (Raspberry pi zero 2w) https://github.com/apirrone/Open_Duck_Mini_Runtime + +# Training your own policies + +If you want to train your own policies, and contribute to making the ducks walk nicely, see [this document](docs/sim2real.md) + +> Thanks a lot to HuggingFace and Pollen Robotics for sponsoring this project ! diff --git a/Open_Duck_Mini-2/docs/assembly_guide.md b/Open_Duck_Mini-2/docs/assembly_guide.md new file mode 100644 index 0000000..e264fe5 --- /dev/null +++ b/Open_Duck_Mini-2/docs/assembly_guide.md @@ -0,0 +1,251 @@ +# Assembly guide + +> Before assembling the duck, you should first [configure your motors](./configure_motors.md) + +## Requirements : + +You will need : +- A soldering iron, and basic electronics tools and skills +- X m3 screws (TODO : add the exact number) +- Some wire +- Loctite Threadlocker blue 243 + +> General note : Everytime you screw something in the motors metal against metal, you want to use a little loctite threadlocker. This will prevent the screws from coming loose due to the vibrations during the operation of the robot. It adds a little time to to the build, but you'll be glad you took the time ;) +> +> Don't use loctite with the plastic screws + +> At any time, you can refer to the CAD here : https://cad.onshape.com/documents/64074dfcfa379b37d8a47762/w/3650ab4221e215a4f65eb7fe/e/0505c262d882183a25049d05 + +## Steps : + +### Assemble the trunk + +Place the bearings in `trunk_bottom` like so, and insert M3 inserts in these holes. It's also a good time to insert the 4 M3 inserts in the bottom of this part to mount body parts later on. + +1 + +Then assamble `trunk_bottom` and `trunk_top`, and screw them together with 2 `M3x10` screws through these holes + +1 + +Mount the middle motor like so and screw it with the plastic screws that came with the motors : + +1 + +Insert `roll_motor_bottom` like this + +1 + + +### Assemble the feet + +Both feet are the same. + +First, assemble `foot_bottom_tpu` with `foot_bottom_pla`. Insert M3 inserts in these holes : + +1 + +And screw the two parts together with two `M3x6` screws. + +Then, insert M3 inserts in these holes in `foot_top` here : + +1 + +And assemble everything like so. Make sure the driver side of the motor is on the `foot_top` part side : + + + + + + +
1 1
+ +You can add the foot switches like this too : + +> You press fit them so that the switch is activated when the foot touches the ground + + + + + + +
1 1
+ + +### Assemble the shins + +Insert M3 Inserts in these holes of `leg_spacer` (on both sides. Insert 4 M3 inserts in total) : + +1 + +Then, first plug the motor cable in the foot's motor, and make it go through the `right_sheet` like so + +1 + +Then assemble like below: + +1 + +### Assemble the thighs + +The thigh is pretty much the same thing, except the `hip_pitch` motor is mounted this way (important for the zero position) + +1 + +### Assemble the hips + +Mount `left_roll_to_pitch` or `right_roll_to_pitch`, here the parts are symmetrical so you have to use the right one. + +1 + +Mount `roll_motor_top` to the `hip_yaw` servo (screw from the bottom). Don't mount the servo to the trunk yet. + + +1 + +Then mount `hip_roll` like this + +1 + +And insert the sub assembly like this + +1 + +Screw everything you can (with the plastic screws provided with the servos) + +You can now mount the leg like this : + +1 + +And do the same for the other leg :) + +Your duck should now look like this + +1 + +### Assemble the neck + +You know the drill + +1 + +### Assemble the head mechanism + +First, mount `head_pitch_to_yaw` like this + +1 + +Then, independently mount `head_yaw_to_roll` and `head_roll_mount` to `head_roll dof` + +1 + +(You can insert `head_bot_plate` and `body_middle_top` now too to avoid having to disassamble the head later) + +Then + +1 + +Then + +1 + +Your duck should now look like this + +1 + +### Mount the servo driver board + +TODO take a photo + +### Mount the IMU + +Like this + +> It's actually better to mount the IMU with the correct natural orientation, which would be flipped along the X axis compared to the pictures below +> In the picture below, the IMU is mounted upside down. +> It probably doesn't really matter a lot if you mount it upside down or not. You can configure how you mounted it later + + + + + + +
1 2
+ +## Electronics + +Here is the global electonics schematic for reference + + + + + + +
1 2
+ +Here is how to wire the feet + +![feet](https://github.com/user-attachments/assets/d3376494-4690-4484-8352-132e6284731a) + + + +### Battery pack + +> To be safe, make sure your cells are charged to the same voltage before placing them in the holder. + + + + + + +
1 2
+ + +### Head + +First, insert the M3 inserts in all these holes + +![image](https://github.com/user-attachments/assets/ef4cd513-6b8d-41fa-9cc3-149fc8333d3e) + +> TODO add instructions for expression features (camera, antennas, eye leds, projector and speaker) + +Then insert the bearing, mount the ear motors and the raspberry pi zero 2w. + +For reference, the inside of the head looks like this now + +![image](https://github.com/user-attachments/assets/91284081-a563-4c02-bb3d-194b3afbc25c) + + +Then assemble the neck with the head like this + +![image](https://github.com/user-attachments/assets/96fd5347-bff7-47e9-a7bd-fde17ab1bcd2) + + +## Body + +First screw on `body_middle_bottom` + +![Capture d’écran du 2025-02-09 12-10-00](https://github.com/user-attachments/assets/081d8840-8e88-4938-9d9a-4d97614e6261) + +Then insert the M3 inserts in all the holes of `body_middle_bottom` and `body_middle_top` on which we'll mount the battery pack and `body_front`. + +Then mount `body_middle_top`, `body_front` and the battery pack + + + + + + +
1 2
+ +Et voila :) + + + + + + +
1 2
+ + +> Now that your duck is fully assembled, you setup the raspberry pi and the runtime software [here](https://github.com/apirrone/Open_Duck_Mini_Runtime) diff --git a/Open_Duck_Mini-2/docs/configure_motors.md b/Open_Duck_Mini-2/docs/configure_motors.md new file mode 100644 index 0000000..a2bf15a --- /dev/null +++ b/Open_Duck_Mini-2/docs/configure_motors.md @@ -0,0 +1,40 @@ +# Configure the motors + +> This sould be done independently on each motor *before* builiding the duck. +> +> During the process, the motor will move to its zero position. You can then install the horn while trying to align it the best you can like in the photo below. (Don't worry if it's not perfect, we will compensate for that later) + +![Capture d’écran du 2025-04-25 09-55-34](https://github.com/user-attachments/assets/e3c4aefa-5e0a-4d4e-89f4-82df9bf30e29) + + +Clone and install (`pip install -e .`) the runtime repo on the `v2` branch : `https://github.com/apirrone/Open_Duck_Mini_Runtime` + +You can either install it on your own computer or on the raspberry pi for the configuration, as you want. You'll just want a way to power the servos, for example, the battery pack. + + +Then for each motor, run the following command : + +```bash +python configure_motor.py --id +``` + +The motors ids are : + +```python +{ + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} +``` diff --git a/Open_Duck_Mini-2/docs/feetech_identification.md b/Open_Duck_Mini-2/docs/feetech_identification.md new file mode 100644 index 0000000..c9dbd45 --- /dev/null +++ b/Open_Duck_Mini-2/docs/feetech_identification.md @@ -0,0 +1,29 @@ +$\theta_d^f$ : $\theta$ requested in firmware units + +$\epsilon = \theta_d^f - \theta^f$ + +### Error in firmware units + +> $\lambda$ is the duty cycle, aka the **PWM** + + + +$\epsilon^f = \epsilon \frac{4096}{2\pi}$ + +> 12 bits + +### Firmware duty cycle + + +$\lambda^f = K_p \epsilon^f$ + +$\lambda = K_p K_g\epsilon$ + + +$R = 2.5\Omega$ + +------ + +$\lambda = \epsilon K_p K_g$ + +$K_g = \frac{\lambda}{\epsilon K_p }$ \ No newline at end of file diff --git a/Open_Duck_Mini-2/docs/head_schematic.xcf b/Open_Duck_Mini-2/docs/head_schematic.xcf new file mode 100644 index 0000000..0141713 Binary files /dev/null and b/Open_Duck_Mini-2/docs/head_schematic.xcf differ diff --git a/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png b/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png new file mode 100644 index 0000000..3b39e10 Binary files /dev/null and b/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png differ diff --git a/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio b/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio new file mode 100644 index 0000000..c60ad8c --- /dev/null +++ b/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio @@ -0,0 +1,396 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png b/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png new file mode 100644 index 0000000..d90ebe3 Binary files /dev/null and b/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png differ diff --git a/Open_Duck_Mini-2/docs/prepare_robot.md b/Open_Duck_Mini-2/docs/prepare_robot.md new file mode 100644 index 0000000..4a4f0c7 --- /dev/null +++ b/Open_Duck_Mini-2/docs/prepare_robot.md @@ -0,0 +1,123 @@ +# Preparing a robot for simulation in MuJoCo + +## If you designed your robot in OnShape + +### Make sure to design you robot according to onshape-to-robot constraints +https://onshape-to-robot.readthedocs.io/en/latest/design.html + + +The urdf will contain frames named `closing_<...>_1` and `closing_<...>_2` that you can use to close the loops in the mjcf file. + +### Get get robot urdf from onshape + +Run + +```bash +$ onshape-to-robot robots/bd1/ +``` + +#### (Optional) If you have closing loops, follow the instructions for handling them in the documentation of onshape-to-robotn then: + +In `robot.urdf`, add : +```xml + + + + + ... + +``` + +# Convert URDF to MJCF (MuJoCo) + +## Get MuJoCo binaries + +Download mujoco binaries somewhere https://github.com/google-deepmind/mujoco/releases + +unpack and run + +```bash +$ ./compile robot.urdf robot.xml +``` + +## In robot.xml, add actuators : +Example : +```xml + + + + + + + + + + + + + + + +``` + +## Add a freejoint + +encapsulate the body in a freejoint +```xml + + + + ... + ... + + +``` + +## (Optional) Constrain closing loop + +Add the following to the mjcf file +```xml + + + +``` + +the x, y, z values can be found in the .urdf + +```xml + + + ... + +``` + +## Setup collision groups, damping and friction +/!\ remove actuatorfrcrange in joints +Put that inside the bracket +```xml + + + + + + + ... + ... + +``` + +still need to add : +- change frames to sites + + +## Visualize + +```bash +$ python3 -m mujoco.viewer --mjcf=/scene.xml +``` + +or + +```bash +$ /bin/simulate /scene.xml +``` diff --git a/Open_Duck_Mini-2/docs/print_guide.md b/Open_Duck_Mini-2/docs/print_guide.md new file mode 100644 index 0000000..aa64eba --- /dev/null +++ b/Open_Duck_Mini-2/docs/print_guide.md @@ -0,0 +1,43 @@ +# Print guide + +You can find the `.stl` files under the `print/` directory at the root of this repo. + +All the parts are printed in standard PLA with 15% infill, except for `foot_bottom_tpu.stl`, which is to be printed in TPU at 40% infill. + +## Parts to print +- foot_top.stl x2 +- foot_side.stl x2 +- foot_bottom_pla.stl x2 +- foot_bottom_tpu.stl x2 (TPU) +- knee_to_ankle_left_sheet.stl x4 +- knee_to_ankle_right_sheet.stl x4 +- leg_spacer.stl x4 +- left_roll_to_pitch.stl x1 +- right_roll_to_pitch.stl x1 +- roll_motor_bottom.stl x2 +- roll_motor_top.stl x2 +- trunk_bottom.stl x1 +- trunk_top.stl x1 +- neck_left_sheet.stl x1 +- neck_right_sheet.stl x1 +- head_pitch_to_yaw.stl x1 +- head_yaw_to_roll.stl x1 +- head_roll_mount.stl x1 +- head.stl x1 +- head_bot_sheet.stl x1 +- left_antenna_holder.stl x1 +- right_antenna_holder.stl x1 +- left_cache.stl x1 +- right_cache.stl x1 +- body_front.stl x1 +- body_middle_bottom.stl x1 +- body_middle_top.stl x1 +- body_back.stl x1 +- battery_pack_lid.stl x1 +- bulb.stl x1 +- flash_light_module.stl x1 +- flash_reflector_interface.stl x1 +- left_eye.stl x1 +- right_eye.stl x1 +- speaker_interface.stl x1 +- speaker_stand.stl x1 diff --git a/Open_Duck_Mini-2/docs/sim2real.md b/Open_Duck_Mini-2/docs/sim2real.md new file mode 100644 index 0000000..1cce56b --- /dev/null +++ b/Open_Duck_Mini-2/docs/sim2real.md @@ -0,0 +1,45 @@ +> Not finalized yet + +# Training policies that transfer to the real robot (sim2real) + +We want to train policies that transfer well to the real robot. This is the sim2real problem. It's a hard problem, especially for us since we are using cheap servomotors that are hard to model and not overly powerful. + +Below, I'll roughly explain the steps we went through to get there, but you won't have to do everything again yourself since we provide the results of each process. + +## Make an accurate model of the robot (URDF/MJCF) + +### Robot structure + +In the [Onhape document](https://cad.onshape.com/documents/64074dfcfa379b37d8a47762/w/3650ab4221e215a4f65eb7fe/e/0505c262d882183a25049d05), we specify the material of each part. But to be more accurate, because we print with infill we override the mass of the parts with the (pretty accurate) estimation of the slicer. + +We use [onshape-to-robot](https://github.com/Rhoban/onshape-to-robot) to export URDF/MJCF descriptions. For MJX, we have to make a lightweight model, see our [config.json](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/xmls/config.json). + +This gives us a MJCF (Mujoco format) [description of the robot](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/xmls/open_duck_mini_v2.xml) which describes the masses and moments of inertia of the full robot. + +### Motors + +Another very important part of having an accurate model is modeling the motors's behavior. We use [BAM](https://github.com/Rhoban/bam/) for that. You don't have to go through the identification process yourself, we provide the results [here](https://github.com/Rhoban/bam/tree/main/params/feetech_sts3215_7_4V) + +It's critical that the simulator simulates the motors accurately, because we will train a policy (a neural network) to output motor positions based on sensory inputs (motors positions/speeds, imu and feet sensors). If the motors behave differently in the simulation than in the real world, the policy won't work, or at worst, produce chaotic movements. + +`BAM` allows us to export the main identified parameters to mujoco units (using `bam.to_mujoco`). These values are the ones we set in out mjcf model for the actuators and joints properties. + +- damping +- kp +- frictionloss +- armature +- forcerange + +## Training policies + +We use our own [mujoco playground](https://github.com/google-deepmind/mujoco_playground) based framework, [Open Duck Playground](https://github.com/apirrone/Open_Duck_Playground) + +In the [joystick](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/joystick.py) env, you can try to enable/disable different rewards, write your own, play with the weights, noise, randomization etc. + +We obtained good results by implementing the imitation reward described by Disney in their [BDX paper](https://github.com/apirrone/Open_Duck_Playground/blob/main/playground/open_duck_mini_v2/joystick.py). + +To use this reward, we need reference motion. We made [this repo](https://github.com/apirrone/Open_Duck_reference_motion_generator) to generate such motion using a parametric walk engine. Following the instructions there, you can generate a `polynomial_coefficients.pkl` file which contains the reference motions. There is already such a file in the playground repo under the `data/` directory. + +Once your policy is trained, you can try to run it on the real robot using [this script](https://github.com/apirrone/Open_Duck_Mini_Runtime/blob/v2/scripts/v2_rl_walk_mujoco.py) in the runtime repo. Make sure you completed all the steps in the [checklist](https://github.com/apirrone/Open_Duck_Mini_Runtime/blob/v2/checklist.md) before running this. + + diff --git a/Open_Duck_Mini-2/docs/wiring.drawio b/Open_Duck_Mini-2/docs/wiring.drawio new file mode 100644 index 0000000..f533332 --- /dev/null +++ b/Open_Duck_Mini-2/docs/wiring.drawio @@ -0,0 +1,503 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/docs/wiring.png b/Open_Duck_Mini-2/docs/wiring.png new file mode 100644 index 0000000..2c60f39 Binary files /dev/null and b/Open_Duck_Mini-2/docs/wiring.png differ diff --git a/Open_Duck_Mini-2/experiments/LeRobot/README.md b/Open_Duck_Mini-2/experiments/LeRobot/README.md new file mode 100644 index 0000000..3ce568b --- /dev/null +++ b/Open_Duck_Mini-2/experiments/LeRobot/README.md @@ -0,0 +1,4 @@ +# Current state of things + +Tried to clean up `record_episodes_hdf5.py` by using `bdx_mujoco_server.py` but got weird behavior with the walk engine. + diff --git a/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py b/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py new file mode 100644 index 0000000..c80de03 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py @@ -0,0 +1,166 @@ +import argparse +import os +import time +from glob import glob +from typing import Dict, List + +import h5py +import mujoco +import mujoco.viewer +import numpy as np +import placo +from scipy.spatial.transform import Rotation as R + +from mini_bdx.bdx_mujoco_server import BDXMujocoServer +from mini_bdx.utils.mujoco_utils import check_contact + +# from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser("Record episodes") +parser.add_argument( + "-n", + "--session_name", + type=str, + required=True, +) +parser.add_argument( + "-r", + "--sampling_rate", + type=int, + required=False, + default=30, + help="Sampling rate in Hz", +) +parser.add_argument( + "-l", + "--episode_length", + type=int, + required=False, + default=10, + help="Episode length in seconds", +) +args = parser.parse_args() + +session_name = args.session_name + "_raw" +session_path = os.path.join("data", session_name) +os.makedirs(session_path, exist_ok=True) + +bdx_mujoco_server = BDXMujocoServer( + model_path="/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx" +) + +recording = False +data_dict: Dict[str, List[float]] = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], +} + + +def key_callback(keycode): + if keycode == 257: # enter + start_stop_recording() + + +def start_stop_recording(): + global recording, data_dict + recording = not recording + if recording: + print("Recording started") + pass + else: + print("Recording stopped") + episode_id = len(glob(f"{session_path}/*.hdf5")) + episode_path = os.path.join(session_path, f"episode_{episode_id}.hdf5") + print(f"Saving episode in {episode_path} ...") + max_timesteps = len(data_dict["/action"]) + with h5py.File( + episode_path, + "w", + rdcc_nbytes=1024**2 * 2, + ) as root: + obs = root.create_group("observations") + obs.create_dataset("qpos", (max_timesteps, 20)) + obs.create_dataset("qvel", (max_timesteps, 19)) + root.create_dataset("action", (max_timesteps, 13)) + + for name, array in data_dict.items(): + root[name][...] = array + print("Done") + data_dict = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], + } + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +walking = True + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine(robot) + +bdx_mujoco_server.start() + +try: + prev = bdx_mujoco_server.data.time + last = bdx_mujoco_server.data.time + episode_start = bdx_mujoco_server.data.time + # start_stop_recording() + while True: + dt = bdx_mujoco_server.data.time - prev + + # if bdx_mujoco_server.data.time - episode_start > args.episode_length: + # start_stop_recording() + # episode_start = bdx_mujoco_server.data.time + # start_stop_recording() + + # Update the walk engine + right_contact, left_contact = bdx_mujoco_server.get_feet_contact() + gyro, accelerometer = bdx_mujoco_server.get_imu() + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + # Get the angles from the walk engine + angles = walk_engine.get_angles() + bdx_mujoco_server.send_action(list(angles.values())) + + # if recording and bdx_mujoco_server.data.time - last > (1 / args.sampling_rate): + # last = bdx_mujoco_server.data.time + # action = list(angles.values()) + # qpos, qvel = bdx_mujoco_server.get_state() + + # data_dict["/action"].append(action) + # data_dict["/observations/qpos"].append(qpos) + # data_dict["/observations/qvel"].append(qvel) + + prev = bdx_mujoco_server.data.time + time.sleep(0.0001) + +except KeyboardInterrupt: + print("stop") + exit() diff --git a/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py b/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py new file mode 100644 index 0000000..c7f56ff --- /dev/null +++ b/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py @@ -0,0 +1,232 @@ +import argparse +import os +import time +from glob import glob +from typing import Dict, List + +import h5py +import mujoco +import mujoco.viewer +import numpy as np +import placo +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.xbox_controller import XboxController + +# from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser("Record episodes") +parser.add_argument( + "-n", + "--session_name", + type=str, + required=True, +) +parser.add_argument( + "-r", + "--sampling_rate", + type=int, + required=False, + default=30, + help="Sampling rate in Hz", +) +parser.add_argument( + "-l", + "--episode_length", + type=int, + required=False, + default=10, + help="Episode length in seconds", +) +args = parser.parse_args() + +session_name = args.session_name + "_raw" +session_path = os.path.join("data", session_name) +os.makedirs(session_path, exist_ok=True) + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + +recording = False +data_dict: Dict[str, List[float]] = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], + "/observations/target": [], # [target_step_size_x, target_step_size_y, target_yaw, target_head_pitch, target_head_yaw, target_head_z_offset] + "/observations/feet_contact": [], # [right_contact, left_contact] +} + + +def key_callback(keycode): + if keycode == 257: # enter + start_stop_recording() + + +def start_stop_recording(): + global recording, data_dict + recording = not recording + if recording: + print("Recording started") + pass + else: + print("Recording stopped") + episode_id = len(glob(f"{session_path}/*.hdf5")) + episode_path = os.path.join(session_path, f"episode_{episode_id}.hdf5") + print(f"Saving episode in {episode_path} ...") + max_timesteps = len(data_dict["/action"]) + with h5py.File( + episode_path, + "w", + rdcc_nbytes=1024**2 * 2, + ) as root: + obs = root.create_group("observations") + obs.create_dataset("qpos", (max_timesteps, 20)) + obs.create_dataset("qvel", (max_timesteps, 19)) + obs.create_dataset("target", (max_timesteps, 6)) + obs.create_dataset("feet_contact", (max_timesteps, 2)) + root.create_dataset("action", (max_timesteps, 13)) + + for name, array in data_dict.items(): + root[name][...] = array + print("Done") + data_dict = { + "/action": [], + "/observations/qpos": [], + "/observations/qvel": [], + "/observations/target": [], + "/observations/feet_contact": [], + } + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +walking = True +xbox = XboxController() + + +def xbox_input(): + global target_velocity, target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.5: + target_head_pitch = inputs["r_y"] * np.deg2rad(45) + target_head_yaw = -inputs["r_x"] * np.deg2rad(120) + target_head_z_offset = inputs["r_trigger"] * 0.08 + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + target_velocity = np.array([-inputs["l_y"], inputs["l_x"], -inputs["r_x"]]) + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine(robot) + + +def get_imu(data): + + rot_mat = np.array(data.body("base").xmat).reshape(3, 3) + gyro = R.from_matrix(rot_mat).as_euler("xyz") + + accelerometer = np.array(data.body("base").cvel)[3:] + + return gyro, accelerometer + + +def get_feet_contact(data, model): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +try: + prev = data.time + last = data.time + episode_start = data.time + # start_stop_recording() + while True: + dt = data.time - prev + xbox_input() + + # if data.time - episode_start > args.episode_length: + # start_stop_recording() + # episode_start = data.time + # start_stop_recording() + + # Update the walk engine + right_contact, left_contact = get_feet_contact(data, model) + gyro, accelerometer = get_imu(data) + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + # Get the angles from the walk engine + angles = walk_engine.get_angles() + + # Apply the angles to the robot + data.ctrl[:] = list(angles.values()) + + if recording and data.time - last > (1 / args.sampling_rate): + last = data.time + action = list(angles.values()) + qpos = data.qpos.flat.copy() + qvel = data.qvel.flat.copy() + + # TODO merge all observations into one array "state" ? + # Don't understand very well how it is handled in lerobot + data_dict["/action"].append(action) + data_dict["/observations/qpos"].append(qpos) + data_dict["/observations/qvel"].append(qvel) + data_dict["/observations/target"].append( + [ + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + ] + ) + data_dict["/observations/feet_contact"].append( + [right_contact, left_contact] + ) + + prev = data.time + mujoco.mj_step(model, data) + viewer.sync() + # time.sleep(model.opt.timestep) + time.sleep(0.001) + +except KeyboardInterrupt: + print("stop") + exit() diff --git a/Open_Duck_Mini-2/experiments/README.md b/Open_Duck_Mini-2/experiments/README.md new file mode 100644 index 0000000..6e99a1d --- /dev/null +++ b/Open_Duck_Mini-2/experiments/README.md @@ -0,0 +1,3 @@ +# Experiments + +These are undocumented experiments, some of them very old. I wouldn't recommend trying to run them :) \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/RL/env.py b/Open_Duck_Mini-2/experiments/RL/env.py new file mode 100644 index 0000000..de51cc4 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/env.py @@ -0,0 +1,401 @@ +import numpy as np +import placo +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +from mini_bdx.walk_engine import WalkEngine + +init_pos = { + "right_hip_yaw": 0, + "right_hip_roll": 0, + "right_hip_pitch": np.deg2rad(45), + "right_knee_pitch": np.deg2rad(-90), + "right_ankle_pitch": np.deg2rad(45), + "left_hip_yaw": 0, + "left_hip_roll": 0, + "left_hip_pitch": np.deg2rad(45), + "left_knee_pitch": np.deg2rad(-90), + "left_ankle_pitch": np.deg2rad(45), + "head_pitch1": np.deg2rad(-45), + "head_pitch2": np.deg2rad(-45), + "head_yaw": 0, +} + + +dofs = { + "right_hip_yaw": 0, + "right_hip_roll": 1, + "right_hip_pitch": 2, + "right_knee_pitch": 3, + "right_ankle_pitch": 4, + "left_hip_yaw": 5, + "left_hip_roll": 6, + "left_hip_pitch": 7, + "left_knee_pitch": 8, + "left_ankle_pitch": 9, + "head_pitch1": 10, + "head_pitch2": 11, + "head_yaw": 12, +} + + +class BDXEnv(MujocoEnv, utils.EzPickle): + """ + ## Action space + + | Num | Action | Control Min | Control Max | Name (in corresponding XML file) | Joint | Unit | + | ---- | ----------------------------------------------------------------- | ----------- | ----------- | -------------------------------- | -------- |------------- | + | 0 | Set position of right_hip_yaw | -0.58TODO | 0.58TODO | right_hip_yaw | cylinder | pos (rad) | + | 1 | Set position of right_hip_roll | -0.58TODO | 0.58TODO | right_hip_roll | cylinder | pos (rad) | + | 2 | Set position of right_hip_pitch | -0.58TODO | 0.58TODO | right_hip_pitch | cylinder | pos (rad) | + | 3 | Set position of right_knee_pitch | -0.58TODO | 0.58TODO | right_knee_pitch | cylinder | pos (rad) | + | 4 | Set position of right_ankle_pitch | -0.58TODO | 0.58TODO | right_ankle_pitch | cylinder | pos (rad) | + | 5 | Set position of left_hip_yaw | -0.58TODO | 0.58TODO | left_hip_yaw | cylinder | pos (rad) | + | 6 | Set position of left_hip_roll | -0.58TODO | 0.58TODO | left_hip_roll | cylinder | pos (rad) | + | 7 | Set position of left_hip_pitch | -0.58TODO | 0.58TODO | left_hip_pitch | cylinder | pos (rad) | + | 8 | Set position of left_knee_pitch | -0.58TODO | 0.58TODO | left_knee_pitch | cylinder | pos (rad) | + | 9 | Set position of left_ankle_pitch | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch1 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch2 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_yaw | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + + + ## Observation space + + | Num | Observation | Min | Max | Name (in corresponding XML file) | Joint | Unit | + | --- | -------------------------------------------------------- | ---- | --- | -------------------------------- | -------- | ------------------------ | + | 0 | Rotation right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | angle (rad) | + | 1 | Rotation right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | angle (rad) | + | 2 | Rotation right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | angle (rad) | + | 3 | Rotation right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | angle (rad) | + | 4 | Rotation right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | angle (rad) | + | 5 | Rotation left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | angle (rad) | + | 6 | Rotation left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | angle (rad) | + | 7 | Rotation left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | angle (rad) | + | 8 | Rotation left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | angle (rad) | + | 9 | Rotation left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | angle (rad) | + | 10 | Rotation head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | angle (rad) | + | 11 | Rotation head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | angle (rad) | + | 12 | Rotation head_yaw | -Inf | Inf | head_yaw | cylinder | angle (rad) | + | 13 | Velocity of right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | speed (rad/s) | + | 14 | Velocity of right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | speed (rad/s) | + | 15 | Velocity of right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | speed (rad/s) | + | 16 | Velocity of right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | speed (rad/s) | + | 17 | Velocity of right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | speed (rad/s) | + | 18 | Velocity of left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | speed (rad/s) | + | 19 | Velocity of left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | speed (rad/s) | + | 20 | Velocity of left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | speed (rad/s) | + | 21 | Velocity of left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | speed (rad/s) | + | 22 | Velocity of left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | speed (rad/s) | + | 23 | Velocity of head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | speed (rad/s) | + | 24 | Velocity of head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | speed (rad/s) | + | 25 | Velocity of head_yaw | -Inf | Inf | head_yaw | cylinder | speed (rad/s) | + + + | 26 | roll angular velocity | -Inf | Inf | | | |e + | 27 | pitch angular velocity | -Inf | Inf | | | | + | 28 | yaw angular velocity | -Inf | Inf | | | | + | 29 | current x linear velocity | -Inf | Inf | | | | + | 30 | current y linear velocity | -Inf | Inf | | | | + | 31 | current z linear velocity | -Inf | Inf | | | | + | 32 | current x target linear velocity | -Inf | Inf | | | | + | 33 | current y target linear velocity | -Inf | Inf | | | | + | 34 | current yaw target angular velocity | -Inf | Inf | | | | + + | 35 | t-1 right_hip_yaw rotation error | -Inf | Inf | | | | + | 36 | t-1 right_hip_roll rotation error | -Inf | Inf | | | | + | 37 | t-1 right_hip_pitch rotation error | -Inf | Inf | | | | + | 38 | t-1 right_knee_pitch rotation error | -Inf | Inf | | | | + | 39 | t-1 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 40 | t-1 left_hip_yaw rotation error | -Inf | Inf | | | | + | 41 | t-1 left_hip_roll rotation error | -Inf | Inf | | | | + | 42 | t-1 left_hip_pitch rotation error | -Inf | Inf | | | | + | 43 | t-1 left_knee_pitch rotation error | -Inf | Inf | | | | + | 44 | t-1 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 45 | t-1 head_pitch1 rotation error | -Inf | Inf | | | | + | 46 | t-1 head_pitch2 rotation error | -Inf | Inf | | | | + | 47 | t-1 head_yaw rotation error | -Inf | Inf | | | | + | 48 | t-2 right_hip_yaw rotation error | -Inf | Inf | | | | + | 49 | t-2 right_hip_roll rotation error | -Inf | Inf | | | | + | 50 | t-2 right_hip_pitch rotation error | -Inf | Inf | | | | + | 51 | t-2 right_knee_pitch rotation error | -Inf | Inf | | | | + | 52 | t-2 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 53 | t-2 left_hip_yaw rotation error | -Inf | Inf | | | | + | 54 | t-2 left_hip_roll rotation error | -Inf | Inf | | | | + | 55 | t-2 left_hip_pitch rotation error | -Inf | Inf | | | | + | 56 | t-2 left_knee_pitch rotation error | -Inf | Inf | | | | + | 57 | t-2 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 58 | t-2 head_pitch1 rotation error | -Inf | Inf | | | | + | 59 | t-2 head_pitch2 rotation error | -Inf | Inf | | | | + | 60 | t-2 head_yaw rotation error | -Inf | Inf | | | | + | 61 | t-3 right_hip_yaw rotation error | -Inf | Inf | | | | + | 62 | t-3 right_hip_roll rotation error | -Inf | Inf | | | | + | 63 | t-3 right_hip_pitch rotation error | -Inf | Inf | | | | + | 64 | t-3 right_knee_pitch rotation error | -Inf | Inf | | | | + | 65 | t-3 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 66 | t-3 left_hip_yaw rotation error | -Inf | Inf | | | | + | 67 | t-3 left_hip_roll rotation error | -Inf | Inf | | | | + | 68 | t-3 left_hip_pitch rotation error | -Inf | Inf | | | | + | 69 | t-3 left_knee_pitch rotation error | -Inf | Inf | | | | + | 70 | t-3 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 71 | t-3 head_pitch1 rotation error | -Inf | Inf | | | | + | 72 | t-3 head_pitch2 rotation error | -Inf | Inf | | | | + | 73 | t-3 head_yaw rotation error | -Inf | Inf | | | | + + | 74 | left foot in contact with the floor | -Inf | Inf | | | | + | 75 | right foot in contact with the floor | -Inf | Inf | | | | + | 76 | t | -Inf | Inf | | | | + + | x74 | sinus | -Inf | Inf | | | | + + """ + + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 100, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + observation_space = Box(low=-np.inf, high=np.inf, shape=(77,), dtype=np.float64) + self.target_velocity = np.asarray([1, 0, 0]) # x, y, yaw + self.joint_history_length = 3 + self.joint_error_history = self.joint_history_length * [13 * [0]] + self.joint_ctrl_history = self.joint_history_length * [13 * [0]] + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + + self.prev_t = 0 + + robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions + ) + self.walk_engine = WalkEngine(robot) + + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + 5, + observation_space=observation_space, + **kwargs, + ) + # self.frame_skip = 30 + + def check_contact(self, body1_name, body2_name): + body1_id = self.data.body(body1_name).id + body2_id = self.data.body(body2_name).id + + for i in range(self.data.ncon): + contact = self.data.contact[i] + + if ( + self.model.geom_bodyid[contact.geom1] == body1_id + and self.model.geom_bodyid[contact.geom2] == body2_id + ) or ( + self.model.geom_bodyid[contact.geom1] == body2_id + and self.model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + def smoothness_reward(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(13): + smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + t0[i] - 2 * t_minus1[i] + t_minus2[i] + ) + + return -smooth + + def feet_contact_reward(self): + + return self.right_foot_in_contact + self.left_foot_in_contact + + def velocity_tracking_reward(self): + base_velocity = list(self.data.body("base").cvel[3:][:2]) + [ + self.data.body("base").cvel[:3][2] + ] + base_velocity = np.asarray(base_velocity) + return np.exp(-np.square(base_velocity - self.target_velocity).sum()) + + def joint_angle_deviation_reward(self): + current_ctrl = self.data.ctrl + init_ctrl = np.array(list(init_pos.values())) + return -np.square(current_ctrl - init_ctrl).sum() + + def walking_height_reward(self): + return ( + -np.square((self.get_body_com("base")[2] - 0.14)) * 100 + ) # "normal" walking height is about 0.14m + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def is_terminated(self) -> bool: + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + upright = np.array([0, 0, 1]) + return ( + self.data.body("base").xpos[2] < 0.08 or np.dot(upright, Z_vec) <= 0 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def follow_walk_engine_reward(self, dt): + self.walk_engine.update( + True, + [0, 0, 0], + [0, 0, 0], + self.left_foot_in_contact, + self.right_foot_in_contact, + 0.03, + 0, + 0, + 0, + 0, + 0, + dt, + ignore_feet_contact=True, + ) + angles = self.walk_engine.get_angles() + return -np.square(self.data.ctrl - list(angles.values())).sum() + + def step(self, a): + # https://www.nature.com/articles/s41598-023-38259-7.pdf + + # add random force + # if np.random.rand() < 0.05: + # self.data.xfrc_applied[self.data.body("base").id][:3] = [ + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # ] # absolute + + t = self.data.time + dt = t - self.prev_t + + self.do_simulation(a, 1) + # self.do_simulation( + # a, self.frame_skip + # ) # TODO maybe set frame_skip to 1 when bootstrapping with walk engine + + self.right_foot_in_contact = self.check_contact("foot_module", "floor") + self.left_foot_in_contact = self.check_contact("foot_module_2", "floor") + + reward = ( + 0.005 # time reward + # + 0.1 * self.walking_height_reward() + # + 0.1 * self.upright_reward() + # + 0.1 * self.velocity_tracking_reward() + # + 0.1 * self.smoothness_reward() + # + 0.1 * self.feet_contact_reward() + # + 0.1 * self.joint_angle_deviation_reward() + # + 0.01 * self.follow_walk_engine_reward(dt) + ) + + # print("time reward", 0.005) + # print("walking height reward", 0.1 * self.walking_height_reward()) + # print("upright reward", 0.1 * self.upright_reward()) + # print("velocity tracking reward", 0.1 * self.velocity_tracking_reward()) + # # print("smoothness reward", 0.1 * self.smoothness_reward()) + # print("feet contact reward", 0.1 * self.feet_contact_reward()) + # # print("joint angle deviation reward", 0.1 * self.joint_angle_deviation_reward()) + # # print("follow walk engine reward", 0.01 * self.follow_walk_engine_reward(dt)) + # print("reward", reward) + # print("===") + + # if self.is_terminated(): + # reward = -10 + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + self.prev_t = t + + return ( + ob, + reward, + self.is_terminated(), # terminated + False, # truncated + dict( + time_reward=0.5, + walking_height_reward=0.5 * self.walking_height_reward(), + upright_reward=0.5 * self.upright_reward(), + velocity_tracking_reward=1.0 * self.velocity_tracking_reward(), + smoothness_reward=0.1 * self.smoothness_reward(), + feet_contact_reward=0.2 * self.feet_contact_reward(), + # joint_angle_deviation_reward=0.1 * self.joint_angle_deviation_reward(), + ), + ) + + def reset_model(self): + self.prev_t = self.data.time + + # self.model.opt.gravity[:] = [0, 0, 0] # no gravity + + qpos = self.data.qpos + + # LATEST + # added randomization to the initial position + for i in range(7, len(qpos)): + qpos[i] = np.random.uniform(-0.3, 0.3) + + self.init_qpos = qpos.copy().flatten() + + # Randomize later + # self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.target_velocity = np.asarray([2, 0, 0]) # x, y, yaw + + self.set_state(qpos, self.init_qvel) + return self._get_obs() + + def goto_init(self): + self.data.qpos[:] = self.init_qpos[:] + for i, value in enumerate(init_pos.values()): + self.data.qpos[i + 7] = value + + def _get_obs(self): + + joints_rotations = self.data.qpos[7 : 7 + 13] + joints_velocities = self.data.qvel[6 : 6 + 13] + + joints_error = self.data.ctrl - self.data.qpos[7 : 7 + 13] + self.joint_error_history.append(joints_error) + self.joint_error_history = self.joint_error_history[ + -self.joint_history_length : + ] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.joint_ctrl_history.append(self.data.ctrl.copy()) + self.joint_ctrl_history = self.joint_ctrl_history[-self.joint_history_length :] + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocity, + np.array(self.joint_error_history).flatten(), + [self.left_foot_in_contact, self.right_foot_in_contact], + [self.data.time], + ] + ) diff --git a/Open_Duck_Mini-2/experiments/RL/env_humanoid.py b/Open_Duck_Mini-2/experiments/RL/env_humanoid.py new file mode 100644 index 0000000..0b68c2c --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/env_humanoid.py @@ -0,0 +1,218 @@ +# Mimicking the humanoidv4 reward + +import numpy as np +import placo +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +from mini_bdx.walk_engine import WalkEngine + + +def mass_center(model, data): + mass = np.expand_dims(model.body_mass, axis=1) + xpos = data.xipos + return (np.sum(mass * xpos, axis=0) / np.sum(mass))[0:2].copy() + + +class BDXEnv(MujocoEnv, utils.EzPickle): + """ + ## Action space + + | Num | Action | Control Min | Control Max | Name (in corresponding XML file) | Joint | Unit | + | ---- | ----------------------------------------------------------------- | ----------- | ----------- | -------------------------------- | -------- |------------- | + | 0 | Set position of right_hip_yaw | -0.58TODO | 0.58TODO | right_hip_yaw | cylinder | pos (rad) | + | 1 | Set position of right_hip_roll | -0.58TODO | 0.58TODO | right_hip_roll | cylinder | pos (rad) | + | 2 | Set position of right_hip_pitch | -0.58TODO | 0.58TODO | right_hip_pitch | cylinder | pos (rad) | + | 3 | Set position of right_knee_pitch | -0.58TODO | 0.58TODO | right_knee_pitch | cylinder | pos (rad) | + | 4 | Set position of right_ankle_pitch | -0.58TODO | 0.58TODO | right_ankle_pitch | cylinder | pos (rad) | + | 5 | Set position of left_hip_yaw | -0.58TODO | 0.58TODO | left_hip_yaw | cylinder | pos (rad) | + | 6 | Set position of left_hip_roll | -0.58TODO | 0.58TODO | left_hip_roll | cylinder | pos (rad) | + | 7 | Set position of left_hip_pitch | -0.58TODO | 0.58TODO | left_hip_pitch | cylinder | pos (rad) | + | 8 | Set position of left_knee_pitch | -0.58TODO | 0.58TODO | left_knee_pitch | cylinder | pos (rad) | + | 9 | Set position of left_ankle_pitch | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch1 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch2 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_yaw | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + + + ## Observation space + OBSOLETE + + | Num | Observation | Min | Max | Name (in corresponding XML file) | Joint | Unit | + | --- | -------------------------------------------------------- | ---- | --- | -------------------------------- | -------- | ------------------------ | + | 0 | Rotation right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | angle (rad) | + | 1 | Rotation right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | angle (rad) | + | 2 | Rotation right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | angle (rad) | + | 3 | Rotation right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | angle (rad) | + | 4 | Rotation right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | angle (rad) | + | 5 | Rotation left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | angle (rad) | + | 6 | Rotation left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | angle (rad) | + | 7 | Rotation left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | angle (rad) | + | 8 | Rotation left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | angle (rad) | + | 9 | Rotation left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | angle (rad) | + | 10 | Rotation head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | angle (rad) | + | 11 | Rotation head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | angle (rad) | + | 12 | Rotation head_yaw | -Inf | Inf | head_yaw | cylinder | angle (rad) | + | 13 | Velocity of right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | speed (rad/s) | + | 14 | Velocity of right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | speed (rad/s) | + | 15 | Velocity of right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | speed (rad/s) | + | 16 | Velocity of right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | speed (rad/s) | + | 17 | Velocity of right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | speed (rad/s) | + | 18 | Velocity of left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | speed (rad/s) | + | 19 | Velocity of left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | speed (rad/s) | + | 20 | Velocity of left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | speed (rad/s) | + | 21 | Velocity of left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | speed (rad/s) | + | 22 | Velocity of left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | speed (rad/s) | + | 23 | Velocity of head_pitch1 | -Inf | Inf | head_pitch1 | cylinder | speed (rad/s) | + | 24 | Velocity of head_pitch2 | -Inf | Inf | head_pitch2 | cylinder | speed (rad/s) | + | 25 | Velocity of head_yaw | -Inf | Inf | head_yaw | cylinder | speed (rad/s) | + | 26 | roll angular velocity | -Inf | Inf | | | | + | 27 | pitch angular velocity | -Inf | Inf | | | | + | 28 | yaw angular velocity | -Inf | Inf | | | | + | 29 | current x linear velocity | -Inf | Inf | | | | + | 30 | current y linear velocity | -Inf | Inf | | | | + | 31 | current z linear velocity | -Inf | Inf | | | | + + | 32 | left foot in contact with the floor | -Inf | Inf | | | | + | 33 | right foot in contact with the floor | -Inf | Inf | | | | + """ + + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 100, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + observation_space = Box(low=-np.inf, high=np.inf, shape=(41,), dtype=np.float64) + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + + self._healthy_z_range = (0.1, 0.2) + + self._forward_reward_weight = 1.25 + self._ctrl_cost_weight = 0.3 + self.healthy_reward = 5.0 + + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + 5, # frame_skip TODO set to 1 to test + observation_space=observation_space, + **kwargs, + ) + + def check_contact(self, body1_name, body2_name): + body1_id = self.data.body(body1_name).id + body2_id = self.data.body(body2_name).id + + for i in range(self.data.ncon): + contact = self.data.contact[i] + + if ( + self.model.geom_bodyid[contact.geom1] == body1_id + and self.model.geom_bodyid[contact.geom2] == body2_id + ) or ( + self.model.geom_bodyid[contact.geom1] == body2_id + and self.model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + def control_cost(self): + control_cost = self._ctrl_cost_weight * np.sum(np.square(self.data.ctrl)) + return control_cost + + @property + def is_healthy(self): + min_z, max_z = self._healthy_z_range + is_healthy = min_z < self.data.qpos[2] < max_z + + return is_healthy + + @property + def terminated(self): + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + upright = np.array([0, 0, 1]) + return not self.is_healthy or np.dot(upright, Z_vec) <= 0 + + def step(self, a): + xy_position_before = mass_center(self.model, self.data) + self.do_simulation(a, self.frame_skip) + xy_position_after = mass_center(self.model, self.data) + xy_velocity = (xy_position_after - xy_position_before) / self.dt + x_velocity, y_velocity = xy_velocity + + self.right_foot_in_contact = self.check_contact("foot_module", "floor") + self.left_foot_in_contact = self.check_contact("foot_module_2", "floor") + + ctrl_cost = self.control_cost() + moving_cost = 1.0 * (abs(x_velocity) + np.abs(x_velocity)) + + forward_reward = self._forward_reward_weight * x_velocity + healthy_reward = self.healthy_reward + + # rewards = forward_reward + healthy_reward + rewards = healthy_reward + reward = rewards - ctrl_cost - moving_cost + + # print("healthy", healthy_reward) + # print("ctrl", ctrl_cost) + # print("moving", moving_cost) + # print(("total reward", reward)) + # print("-") + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + return (ob, reward, self.terminated, False, {}) + + def reset_model(self): + noise_low = -1e-2 + noise_high = 1e-2 + + qpos = self.init_qpos + self.np_random.uniform( + low=noise_low, high=noise_high, size=self.model.nq + ) + qvel = self.init_qvel + self.np_random.uniform( + low=noise_low, high=noise_high, size=self.model.nv + ) + self.set_state(qpos, qvel) + + observation = self._get_obs() + return observation + + def _get_obs(self): + + position = ( + self.data.qpos.flat.copy() + ) # position/rotation of trunk + position of all joints + velocity = ( + self.data.qvel.flat.copy() + ) # positional/angular velocity of trunk + of all joints + + # com_inertia = self.data.cinert.flat.copy() + # com_velocity = self.data.cvel.flat.copy() + + # actuator_forces = self.data.qfrc_actuator.flat.copy() + # external_contact_forces = self.data.cfrc_ext.flat.copy() + + obs = np.concatenate( + [ + position, + velocity, + [self.left_foot_in_contact, self.right_foot_in_contact], + ] + ) + # print("OBS SIZE", len(obs)) + return obs diff --git a/Open_Duck_Mini-2/experiments/RL/new/.gitignore b/Open_Duck_Mini-2/experiments/RL/new/.gitignore new file mode 100644 index 0000000..314f02b --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/.gitignore @@ -0,0 +1 @@ +*.txt \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/RL/new/env.py b/Open_Duck_Mini-2/experiments/RL/new/env.py new file mode 100644 index 0000000..16bd3fb --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/env.py @@ -0,0 +1,389 @@ +import numpy as np +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +FRAME_SKIP = 10 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + """ + ## Action space + + | Num | Action | Control Min | Control Max | Name (in corresponding XML file) | Joint | Unit | + | ---- | ----------------------------------------------------------------- | ----------- | ----------- | -------------------------------- | -------- |------------- | + | 0 | Set position of right_hip_yaw | -0.58TODO | 0.58TODO | right_hip_yaw | cylinder | pos (rad) | + | 1 | Set position of right_hip_roll | -0.58TODO | 0.58TODO | right_hip_roll | cylinder | pos (rad) | + | 2 | Set position of right_hip_pitch | -0.58TODO | 0.58TODO | right_hip_pitch | cylinder | pos (rad) | + | 3 | Set position of right_knee_pitch | -0.58TODO | 0.58TODO | right_knee_pitch | cylinder | pos (rad) | + | 4 | Set position of right_ankle_pitch | -0.58TODO | 0.58TODO | right_ankle_pitch | cylinder | pos (rad) | + | 5 | Set position of left_hip_yaw | -0.58TODO | 0.58TODO | left_hip_yaw | cylinder | pos (rad) | + | 6 | Set position of left_hip_roll | -0.58TODO | 0.58TODO | left_hip_roll | cylinder | pos (rad) | + | 7 | Set position of left_hip_pitch | -0.58TODO | 0.58TODO | left_hip_pitch | cylinder | pos (rad) | + | 8 | Set position of left_knee_pitch | -0.58TODO | 0.58TODO | left_knee_pitch | cylinder | pos (rad) | + | 9 | Set position of left_ankle_pitch | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch1 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_pitch2 | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + | 9 | Set position of head_yaw | -0.58TODO | 0.58TODO | left_ankle_pitch | cylinder | pos (rad) | + + + ## Observation space + + | Num | Observation | Min | Max | Name (in corresponding XML file) | Joint | Unit | + | --- | -------------------------------------------------------- | ---- | --- | -------------------------------- | -------- | ------------------------ | + | 0 | Rotation right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | angle (rad) | + | 1 | Rotation right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | angle (rad) | + | 2 | Rotation right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | angle (rad) | + | 3 | Rotation right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | angle (rad) | + | 4 | Rotation right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | angle (rad) | + | 5 | Rotation left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | angle (rad) | + | 6 | Rotation left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | angle (rad) | + | 7 | Rotation left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | angle (rad) | + | 8 | Rotation left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | angle (rad) | + | 9 | Rotation left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | angle (rad) | + | 10 | Rotation neck_pitch | -Inf | Inf | head_pitch1 | cylinder | angle (rad) | + | 11 | Rotation head_pitch | -Inf | Inf | head_pitch2 | cylinder | angle (rad) | + | 12 | Rotation head_yaw | -Inf | Inf | head_yaw | cylinder | angle (rad) | + | 13 | Velocity of right_hip_yaw | -Inf | Inf | right_hip_yaw | cylinder | speed (rad/s) | + | 14 | Velocity of right_hip_roll | -Inf | Inf | right_hip_roll | cylinder | speed (rad/s) | + | 15 | Velocity of right_hip_pitch | -Inf | Inf | right_hip_pitch | cylinder | speed (rad/s) | + | 16 | Velocity of right_knee_pitch | -Inf | Inf | right_knee_pitch | cylinder | speed (rad/s) | + | 17 | Velocity of right_ankle_pitch | -Inf | Inf | right_ankle_pitch | cylinder | speed (rad/s) | + | 18 | Velocity of left_hip_yaw | -Inf | Inf | left_hip_yaw | cylinder | speed (rad/s) | + | 19 | Velocity of left_hip_roll | -Inf | Inf | left_hip_roll | cylinder | speed (rad/s) | + | 20 | Velocity of left_hip_pitch | -Inf | Inf | left_hip_pitch | cylinder | speed (rad/s) | + | 21 | Velocity of left_knee_pitch | -Inf | Inf | left_knee_pitch | cylinder | speed (rad/s) | + | 22 | Velocity of left_ankle_pitch | -Inf | Inf | left_ankle_pitch | cylinder | speed (rad/s) | + | 23 | Velocity of neck_pitch | -Inf | Inf | head_pitch1 | cylinder | speed (rad/s) | + | 24 | Velocity of head_pitch | -Inf | Inf | head_pitch2 | cylinder | speed (rad/s) | + | 25 | Velocity of head_yaw | -Inf | Inf | head_yaw | cylinder | speed (rad/s) | + + + | 26 | roll angular velocity | -Inf | Inf | | | |e + | 27 | pitch angular velocity | -Inf | Inf | | | | + | 28 | yaw angular velocity | -Inf | Inf | | | | + | 29 | current x linear velocity | -Inf | Inf | | | | + | 30 | current y linear velocity | -Inf | Inf | | | | + | 31 | current z linear velocity | -Inf | Inf | | | | + | 32 | current x target linear velocity | -Inf | Inf | | | | + | 33 | current y target linear velocity | -Inf | Inf | | | | + | 34 | current yaw target angular velocity | -Inf | Inf | | | | + + Changed to control history + | 35 | t-1 right_hip_yaw rotation error | -Inf | Inf | | | | + | 36 | t-1 right_hip_roll rotation error | -Inf | Inf | | | | + | 37 | t-1 right_hip_pitch rotation error | -Inf | Inf | | | | + | 38 | t-1 right_knee_pitch rotation error | -Inf | Inf | | | | + | 39 | t-1 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 40 | t-1 left_hip_yaw rotation error | -Inf | Inf | | | | + | 41 | t-1 left_hip_roll rotation error | -Inf | Inf | | | | + | 42 | t-1 left_hip_pitch rotation error | -Inf | Inf | | | | + | 43 | t-1 left_knee_pitch rotation error | -Inf | Inf | | | | + | 44 | t-1 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 45 | t-1 neck_pitch rotation error | -Inf | Inf | | | | + | 46 | t-1 head_pitch rotation error | -Inf | Inf | | | | + | 47 | t-1 head_yaw rotation error | -Inf | Inf | | | | + + | 48 | t-2 right_hip_yaw rotation error | -Inf | Inf | | | | + | 49 | t-2 right_hip_roll rotation error | -Inf | Inf | | | | + | 50 | t-2 right_hip_pitch rotation error | -Inf | Inf | | | | + | 51 | t-2 right_knee_pitch rotation error | -Inf | Inf | | | | + | 52 | t-2 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 53 | t-2 left_hip_yaw rotation error | -Inf | Inf | | | | + | 54 | t-2 left_hip_roll rotation error | -Inf | Inf | | | | + | 55 | t-2 left_hip_pitch rotation error | -Inf | Inf | | | | + | 56 | t-2 left_knee_pitch rotation error | -Inf | Inf | | | | + | 57 | t-2 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 58 | t-2 neck_pitch rotation error | -Inf | Inf | | | | + | 59 | t-2 head_pitch rotation error | -Inf | Inf | | | | + | 60 | t-2 head_yaw rotation error | -Inf | Inf | | | | + | 61 | t-3 right_hip_yaw rotation error | -Inf | Inf | | | | + | 62 | t-3 right_hip_roll rotation error | -Inf | Inf | | | | + | 63 | t-3 right_hip_pitch rotation error | -Inf | Inf | | | | + | 64 | t-3 right_knee_pitch rotation error | -Inf | Inf | | | | + | 65 | t-3 right_ankle_pitch rotation error | -Inf | Inf | | | | + | 66 | t-3 left_hip_yaw rotation error | -Inf | Inf | | | | + | 67 | t-3 left_hip_roll rotation error | -Inf | Inf | | | | + | 68 | t-3 left_hip_pitch rotation error | -Inf | Inf | | | | + | 69 | t-3 left_knee_pitch rotation error | -Inf | Inf | | | | + | 70 | t-3 left_ankle_pitch rotation error | -Inf | Inf | | | | + | 71 | t-3 neck_pitch rotation error | -Inf | Inf | | | | + | 72 | t-3 head_pitch rotation error | -Inf | Inf | | | | + | 73 | t-3 head_yaw rotation error | -Inf | Inf | | | | + + | 74 | left foot in contact with the floor | -Inf | Inf | | | | + | 75 | right foot in contact with the floor | -Inf | Inf | | | | + + | x76 | t | -Inf | Inf | | | | + | x74 | sinus | -Inf | Inf | | | | + + """ + + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 50, + } + + # Ideas + # Low pass filter on the joint angles + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + observation_space = Box(low=-np.inf, high=np.inf, shape=(86,), dtype=np.float64) + self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.joint_history_length = 3 + self.joint_error_history = self.joint_history_length * [15 * [0]] + self.joint_ctrl_history = self.joint_history_length * [15 * [0]] + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + self.startup_cooldown = 1.0 + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def check_contact(self, body1_name, body2_name): + body1_id = self.data.body(body1_name).id + body2_id = self.data.body(body2_name).id + + for i in range(self.data.ncon): + contact = self.data.contact[i] + + if ( + self.model.geom_bodyid[contact.geom1] == body1_id + and self.model.geom_bodyid[contact.geom2] == body2_id + ) or ( + self.model.geom_bodyid[contact.geom1] == body2_id + and self.model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + def feet_contact_reward(self): + return self.right_foot_in_contact + self.left_foot_in_contact + + def velocity_tracking_reward(self): + base_velocity = list(self.data.body("base").cvel[3:][:2]) + [ + self.data.body("base").cvel[:3][2] + ] + base_velocity = np.asarray(base_velocity) + return np.exp(-np.square(base_velocity - self.target_velocity).sum()) + + def smoothness_reward(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(15): + smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + t0[i] - 2 * t_minus1[i] + t_minus2[i] + ) + + return -smooth + + def smoothness_reward2(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(15): + smooth += np.square(t0[i] - t_minus1[i]) + np.square( + t_minus1[i] - t_minus2[i] + ) + # smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + # t0[i] - 2 * t_minus1[i] + t_minus2[i] + # ) + + return -smooth + + def joint_velocity_reward(self): + return -np.square(self.data.qvel[:]).sum() + + def walking_height_reward(self): + return ( + -np.square((self.get_body_com("base")[2] - 0.15)) * 100 + ) # "normal" walking height is about 0.15m + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def init_position_reward(self): + return -np.square(self.data.qpos[7 : 7 + 15] - self.init_pos).sum() + + def is_terminated(self) -> bool: + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + upright = np.array([0, 0, 1]) + return ( + self.data.body("base").xpos[2] < 0.08 or np.dot(upright, Z_vec) <= 0 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def action_LFP(self, action): + # Low pass filter for the actions + action_tminus1 = self.joint_ctrl_history[0] + action_tminus2 = self.joint_ctrl_history[1] + action_tminus3 = self.joint_ctrl_history[2] + # return action * 0.5 + action_tminus1 * 0.5 + return ( + action * 0.5 + + action_tminus1 * 0.3 + + action_tminus2 * 0.15 + + action_tminus3 * 0.05 + ) + + d_action = action - action_tminus1 + + action = [ + a if abs(d) < 0.1 else b + for a, b, d in zip(action, action_tminus1, d_action) + ] + return action + + def step(self, a): + # https://www.nature.com/articles/s41598-023-38259-7.pdf + + # add random force + # if np.random.rand() < 0.05: + # self.data.xfrc_applied[self.data.body("base").id][:3] = [ + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # np.random.randint(-5, 5), + # ] # absolute + + t = self.data.time + dt = t - self.prev_t + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + + if self.startup_cooldown > 0: + self.do_simulation(self.init_pos, FRAME_SKIP) + reward = 0 + else: + self.do_simulation(a, FRAME_SKIP) + # self.do_simulation(self.action_LFP(a), 4) + self.right_foot_in_contact = self.check_contact("foot_module", "floor") + self.left_foot_in_contact = self.check_contact("foot_module_2", "floor") + + reward = ( + 0.05 # time reward + # + 0.1 * self.walking_height_reward() + # + 0.1 * self.upright_reward() + # + 0.1 * self.velocity_tracking_reward() + + 0.01 * self.smoothness_reward2() + + 0.1 * self.init_position_reward() + # + 0.1 * self.feet_contact_reward() + # + 0.1 * self.joint_angle_deviation_reward() + # + 0.01 * self.follow_walk_engine_reward(dt) + # + 0.001 * self.joint_velocity_reward() + ) + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + self.prev_t = t + + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + + # self.model.opt.gravity[:] = [0, 0, 0] # no gravity + + self.goto_init() + + self.joint_error_history = self.joint_history_length * [15 * [0]] + self.joint_ctrl_history = self.joint_history_length * [15 * [0]] + # qpos = self.data.qpos + + # LATEST + # added randomization to the initial position + # for i in range(7, len(qpos)): + # qpos[i] = np.random.uniform(-0.3, 0.3) + + # self.init_qpos = qpos.copy().flatten() + + # Randomize later + # self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qpos[:] = np.zeros(len(self.data.qpos[:])) + self.data.qpos[2] = 0.15 + # self.data.qpos[3 : 3 + 4] = [1, 0, 0, 0] + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + self.data.qpos[7 : 7 + 15] = self.init_pos + self.data.ctrl[:] = self.init_pos + + def _get_obs(self): + joints_rotations = self.data.qpos[7 : 7 + 15] + joints_velocities = self.data.qvel[6 : 6 + 15] + + # joints_error = self.data.ctrl - self.data.qpos[7 : 7 + 13] + # self.joint_error_history.append(joints_error) + # self.joint_error_history = self.joint_error_history[ + # -self.joint_history_length : + # ] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.joint_ctrl_history.append(self.data.ctrl.copy()) + self.joint_ctrl_history = self.joint_ctrl_history[-self.joint_history_length :] + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocity, + np.array(self.joint_ctrl_history).flatten(), + [self.left_foot_in_contact, self.right_foot_in_contact], + ] + ) diff --git a/Open_Duck_Mini-2/experiments/RL/new/eval.py b/Open_Duck_Mini-2/experiments/RL/new/eval.py new file mode 100644 index 0000000..8085b15 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/eval.py @@ -0,0 +1,130 @@ +import argparse +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import gymnasium as gym +import mujoco +import numpy as np +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + +register( + id="BDX_env", + entry_point="footsteps_env:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + + +def draw_clock(clock): + # clock [a, b] + clock_radius = 100 + im = np.zeros((clock_radius * 2, clock_radius * 2, 3), np.uint8) + im = cv2.circle(im, (clock_radius, clock_radius), clock_radius, (255, 255, 255), -1) + im = cv2.line( + im, + (clock_radius, clock_radius), + ( + int(clock_radius + clock_radius * clock[0]), + int(clock_radius + clock_radius * clock[1]), + ), + (0, 0, 255), + 2, + ) + cv2.imshow("clock", im) + cv2.waitKey(1) + + +def draw_frame(pose, i, env): + pose = fv_utils.rotateInSelf(pose, [0, 90, 0]) + # env.mujoco_renderer._get_viewer(render_mode="human") + env.mujoco_renderer._get_viewer(render_mode="human").add_marker( + pos=pose[:3, 3], + mat=pose[:3, :3], + size=[0.005, 0.005, 0.1], + type=mujoco.mjtGeom.mjGEOM_ARROW, + rgba=[1, 0, 0, 1], + label=str(i), + ) + + +def test(env, sb3_algo, path_to_model): + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + + print("Using latest model: ", latest_model_path) + + path_to_model = latest_model_path + + match sb3_algo: + case "SAC": + model = SAC.load(path_to_model, env=env) + case "TD3": + model = TD3.load(path_to_model, env=env) + case "A2C": + model = A2C.load(path_to_model, env=env) + case "TQC": + model = TQC.load(path_to_model, env=env) + case "PPO": + model = PPO.load(path_to_model, env=env) + case _: + print("Algorithm not found") + return + + obs = env.reset()[0] + done = False + extra_steps = 500 + while True: + action, _ = model.predict(obs) + obs, _, done, _, _ = env.step(action) + footsteps = env.next_footsteps + base_target_2D = np.mean( + [footsteps[2][:3, 3][:2], footsteps[3][:3, 3][:2]], axis=0 + ) + base_target_frame = np.eye(4) + base_target_frame[:3, 3][:2] = base_target_2D + draw_frame(base_target_frame, "base target", env) + base_pos_2D = env.data.body("base").xpos[:2] + base_pos_frame = np.eye(4) + base_pos_frame[:3, 3][:2] = base_pos_2D + draw_frame(base_pos_frame, "base pos", env) + + # draw_clock(env.get_clock_signal()) + + for i, footstep in enumerate(footsteps[2:]): + draw_frame(footstep, i, env) + + if done: + extra_steps -= 1 + + if extra_steps < 0: + break + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + gymenv = gym.make("BDX_env", render_mode="human") + test(gymenv, args.algo, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py b/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py new file mode 100644 index 0000000..c168b41 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py @@ -0,0 +1,142 @@ +import argparse +from scipy.spatial.transform import Rotation as R +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import gymnasium as gym +import mujoco +import numpy as np +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + +register( + id="BDX_env", + entry_point="simple_env:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + + +def draw_clock(clock): + # clock [a, b] + clock_radius = 100 + im = np.zeros((clock_radius * 2, clock_radius * 2, 3), np.uint8) + im = cv2.circle(im, (clock_radius, clock_radius), clock_radius, (255, 255, 255), -1) + im = cv2.line( + im, + (clock_radius, clock_radius), + ( + int(clock_radius + clock_radius * clock[0]), + int(clock_radius + clock_radius * clock[1]), + ), + (0, 0, 255), + 2, + ) + cv2.imshow("clock", im) + cv2.waitKey(1) + + +def draw_frame(pose, i, env): + pose = fv_utils.rotateInSelf(pose, [0, 90, 0]) + # env.mujoco_renderer._get_viewer(render_mode="human") + env.mujoco_renderer._get_viewer(render_mode="human").add_marker( + pos=pose[:3, 3], + mat=pose[:3, :3], + size=[0.005, 0.005, 0.1], + type=mujoco.mjtGeom.mjGEOM_ARROW, + rgba=[1, 0, 0, 1], + label=str(i), + ) + + +def draw_velocities(robot_orig_xy, velocities_xytheta, env): + horizon = 10 # seconds + + robot_orig_xyz = np.array([robot_orig_xy[0], robot_orig_xy[1], 0]) + for i in range(horizon): + j = i * 0.1 + frame = np.eye(4) + frame[:3, 3] = robot_orig_xyz + np.array( + [velocities_xytheta[0] * j, velocities_xytheta[1] * j, 0] + ) + # rotate frame to point in the direction of the velocity + + frame = fv_utils.rotateAbout( + frame, + [0, 0, velocities_xytheta[2] * j], + center=robot_orig_xyz, + degrees=False, + ) + + draw_frame(frame, i, env) + + +def test(env, sb3_algo, path_to_model): + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + + print("Using latest model: ", latest_model_path) + + path_to_model = latest_model_path + + match sb3_algo: + case "SAC": + model = SAC.load(path_to_model, env=env) + case "TD3": + model = TD3.load(path_to_model, env=env) + case "A2C": + model = A2C.load(path_to_model, env=env) + case "TQC": + model = TQC.load(path_to_model, env=env) + case "PPO": + model = PPO.load(path_to_model, env=env) + + # model = PPO("MlpPolicy", env) + # model.policy.load(path_to_model) + case _: + print("Algorithm not found") + return + + obs = env.reset()[0] + done = False + extra_steps = 500 + while True: + action, _ = model.predict(obs) + obs, _, done, _, _ = env.step(action) + + draw_velocities(env.data.body("base").xpos[:2], env.target_velocities, env) + + if done: + extra_steps -= 1 + + if extra_steps < 0: + break + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + gymenv = gym.make("BDX_env", render_mode="human") + test(gymenv, args.algo, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py b/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py new file mode 100644 index 0000000..d8d9e91 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py @@ -0,0 +1,384 @@ +import numpy as np +import placo +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact, get_contact_force + +FRAME_SKIP = 4 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + # Inspired by https://arxiv.org/pdf/2207.12644 + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 125, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + self.nb_dofs = 15 + + observation_space = Box( + np.array( + [ + *(-np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(-10 * np.ones(self.nb_dofs)), # joints_velocities + *(-10 * np.ones(3)), # angular_velocity + *(-10 * np.ones(3)), # linear_velocity + *(-10 * np.ones(8)), # next two footsteps 2*[x, y, z, theta] + *(-np.pi * np.ones(2)), # clock signal + ] + ), + np.array( + [ + *(np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(10 * np.ones(self.nb_dofs)), # joints_velocities + *(10 * np.ones(3)), # angular_velocity + *(10 * np.ones(3)), # linear_velocity + *(10 * np.ones(8)), # next two footsteps 2*[x, y, z, theta] + *(np.pi * np.ones(2)), # clock signal + ] + ), + ) + + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + + self.pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/robot.urdf", + ignore_feet_contact=True, + ) + + self.startup_cooldown = -self.pwe.initial_delay + self.next_footsteps = self.pwe.get_footsteps_in_world().copy() + for i in range(len(self.next_footsteps)): + self.next_footsteps[i][:3, 3][2] = 0 + + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def is_terminated(self) -> bool: + left_antenna_contact = check_contact( + self.data, self.model, "left_antenna_assembly", "floor" + ) + right_antenna_contact = check_contact( + self.data, self.model, "right_antenna_assembly", "floor" + ) + body_contact = check_contact(self.data, self.model, "body_module", "floor") + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + Z_vec /= np.linalg.norm(Z_vec) + upright = np.array([0, 0, 1]) + + base_pos_2D = self.data.body("base").xpos[:2] + upcoming_footstep = self.next_footsteps[2] + second_footstep = self.next_footsteps[3] + base_target_2D = np.mean( + [upcoming_footstep[:3, 3][:2], second_footstep[:3, 3][:2]], axis=0 + ) + base_dist_to_target = np.linalg.norm(base_pos_2D - base_target_2D) + if base_dist_to_target > 0.3: + print("TERMINATED BECAUSE TOO FAR FROM TARGET") + return ( + self.data.body("base").xpos[2] < 0.08 + or np.dot(upright, Z_vec) <= 0.4 + or left_antenna_contact + or right_antenna_contact + or body_contact + or base_dist_to_target > 0.3 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def gait_reward(self): + # During single support: + # - reward force on the supporting foot and speed on the flying foot + # - penalize force on the flying foot and speed on the supporting foot + # During double support: + # - reward force on both feet + # - penalize speed on both feet + + support_phase = self.pwe.get_current_support_phase() # left, right, both + right_contact_force = np.sum( + get_contact_force(self.data, self.model, "right_foot", "floor") + ) + left_contact_force = np.sum( + get_contact_force(self.data, self.model, "left_foot", "floor") + ) + right_speed = self.data.body("right_foot").cvel[3:] # [rot:vel] size 6 + left_speed = self.data.body("left_foot").cvel[3:] # [rot:vel] size 6 + + if support_phase == "both": + return ( + right_contact_force + + left_contact_force + - np.linalg.norm(right_speed) + - np.linalg.norm(left_speed) + ) + elif support_phase is placo.HumanoidRobot_Side.left: + return ( + left_contact_force + - np.linalg.norm(left_speed) + + right_contact_force + + np.linalg.norm(right_speed) + ) + elif support_phase is placo.HumanoidRobot_Side.right: + return ( + right_contact_force + - np.linalg.norm(right_speed) + + left_contact_force + + np.linalg.norm(left_speed) + ) + + def support_flying_reward(self): + # Idea : reward when there is a support foot and a flying foot + # penalize when both feet are in the air or both feet are on the ground + right_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "right_foot", "floor")) + ) + left_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "left_foot", "floor")) + ) + right_speed = np.linalg.norm( + self.data.body("right_foot").cvel[3:] + ) # [rot:vel] size 6 + left_speed = np.linalg.norm( + self.data.body("left_foot").cvel[3:] + ) # [rot:vel] size 6 + + return left_contact_force - right_contact_force + right_speed - left_speed + + def step_reward(self): + # Incentivize the robot to step and orient the body toward targets + # dfoot : distance of the closest foot to the upcoming footstep + # hit reward : reward any foot that hits the upcoming footstep. Only when either or both feet are within a radius of the target + # progress reward : encourage the moving base to move toward he target + + # Using absolute positions here, while in the paper they use relative + + target_radius = ( + 0.05 # Only when either or both feet are within a radius of the target ?? + ) + + base_pos_2D = self.data.body("base").xpos[:2] + upcoming_footstep = self.next_footsteps[2] + second_footstep = self.next_footsteps[3] + + base_target_2D = np.mean( + [upcoming_footstep[:3, 3][:2], second_footstep[:3, 3][:2]], axis=0 + ) + + pos = self.data.body("base").xpos + mat = self.data.body("base").xmat + T_world_body = np.eye(4) + T_world_body[:3, :3] = mat.reshape(3, 3) + T_world_body[:3, 3] = pos + + T_world_rightFoot = np.eye(4) + T_world_rightFoot[:3, 3] = self.data.body("right_foot").xpos + T_world_rightFoot[:3, :3] = self.data.body("right_foot").xmat.reshape(3, 3) + + T_world_leftFoot = np.eye(4) + T_world_leftFoot[:3, 3] = self.data.body("left_foot").xpos + T_world_leftFoot[:3, :3] = self.data.body("left_foot").xmat.reshape(3, 3) + + right_foot_dist = np.linalg.norm( + upcoming_footstep[:3, 3] - T_world_rightFoot[:3, 3] + ) + left_foot_dist = np.linalg.norm( + upcoming_footstep[:3, 3] - T_world_leftFoot[:3, 3] + ) + + dfoot = min(right_foot_dist, left_foot_dist) + droot = np.linalg.norm(base_pos_2D - base_target_2D) + if dfoot > target_radius: + dfoot = 2 # penalize if the foot is too far from the target + + khit = 0.8 + return khit * np.exp(-dfoot / 0.25) + (1 - khit) * np.exp(-droot / 2) + + def orient_reward(self): + euler = R.from_matrix(self.pwe.robot.get_T_world_fbase()[:3, :3]).as_euler( + "xyz" + ) + desired_yaw = euler[2] + current_yaw = R.from_matrix( + np.array(self.data.body("base").xmat).reshape(3, 3) + ).as_euler("xyz")[2] + return -((abs(desired_yaw) - abs(current_yaw)) ** 2) + + def height_reward(self): + current_height = self.data.body("base").xpos[2] + return np.exp(-40 * (0.15 - current_height) ** 2) + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def action_reward(self, a): + current_action = a.copy() + + # This can explode, don't understand why + return min( + 2, np.exp(-5 * np.sum((self.prev_action - current_action)) / self.nb_dofs) + ) + + def torque_reward(self): + current_torque = self.data.qfrc_actuator + return np.exp( + -0.25 * np.sum((self.prev_torque - current_torque)) / self.nb_dofs + ) + + def step(self, a): + t = self.data.time + dt = t - self.prev_t + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + + if self.startup_cooldown > 0: + self.do_simulation(self.init_pos, FRAME_SKIP) + reward = 0 + else: + # We want to learn deltas from the initial position + a += self.init_pos + + # Maybe use that too :) + current_ctrl = self.data.ctrl.copy() + delta_max = 0.05 + a = np.clip(a, current_ctrl - delta_max, current_ctrl + delta_max) + + self.do_simulation(a, FRAME_SKIP) + + self.pwe.tick(dt) + + # TODO penalize auto collisions + # check all collisions and penalize all that is not foot on the ground. + # Need to ignore the auto collisions that occur because of the robot's assembly. + # Tried contact exclude but does not seem to work + # https://github.com/google-deepmind/mujoco/issues/104 + + reward = ( + # 0.30 * self.gait_reward() + 0.30 * self.support_flying_reward() + + 0.6 * self.step_reward() + + 0.05 * self.orient_reward() + + 0.15 * self.height_reward() + + 0.05 * self.upright_reward() + + 0.05 * self.action_reward(a) + + 0.05 * self.torque_reward() + ) + + ob = self._get_obs() + + if self.render_mode == "human": + if self.startup_cooldown <= 0: + print("support flying reward: ", 0.30 * self.support_flying_reward()) + # print("Gait reward: ", 0.30 * self.gait_reward()) + print("Step reward: ", 0.6 * self.step_reward()) + print("Orient reward: ", 0.05 * self.orient_reward()) + print("Height reward: ", 0.15 * self.height_reward()) + print("Upright reward: ", 0.05 * self.upright_reward()) + print("Action reward: ", 0.05 * self.action_reward(a)) + print("Torque reward: ", 0.05 * self.torque_reward()) + print("===") + self.render() + + self.prev_t = t + self.prev_action = a.copy() + self.prev_torque = self.data.qfrc_actuator.copy() + + # self.viz.display(self.pwe.robot.state.q) + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + self.pwe.reset() + + d_x = np.random.uniform(0.01, 0.03) + d_y = np.random.uniform(-0.01, 0.01) + d_theta = np.random.uniform(-0.1, 0.1) + # self.pwe.set_traj(d_x, d_y, d_theta) + self.pwe.set_traj(0.03, 0, 0.001) + + self.goto_init() + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + noise = np.random.uniform(-0.01, 0.01, self.nb_dofs) + self.data.qpos[7 : 7 + self.nb_dofs] = self.init_pos + noise + self.data.qpos[2] = 0.15 + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.ctrl[:] = self.init_pos + + def get_clock_signal(self): + a = np.sin(2 * np.pi * (self.pwe.t % self.pwe.period) / self.pwe.period) + b = np.cos(2 * np.pi * (self.pwe.t % self.pwe.period) / self.pwe.period) + return [a, b] + + def _get_obs(self): + joints_rotations = self.data.qpos[7 : 7 + self.nb_dofs] + joints_velocities = self.data.qvel[6 : 6 + self.nb_dofs] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.next_footsteps = self.pwe.get_footsteps_in_world().copy() + for i in range(len(self.next_footsteps)): + self.next_footsteps[i][:3, 3][2] = 0 + next_two_footsteps = [] # 2*[x, y, z, theta] + for footstep in self.next_footsteps[2:4]: + yaw = R.from_matrix(footstep[:3, :3]).as_euler("xyz")[2] + next_two_footsteps.append(list(footstep[:3, 3]) + [yaw]) + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + np.array(next_two_footsteps).flatten(), + self.get_clock_signal(), + ] + ) diff --git a/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh b/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh new file mode 100644 index 0000000..4f12c82 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh @@ -0,0 +1,3 @@ +last=$(ssh -p4242 apirrone@s-nguyen.net "cd /home/apirrone/MISC/mini_BDX/experiments/RL/new/$1/ ; ls -lt | sed -n '2 p' | grep -io '[$1]*_[0123456789]*.zip'") + +scp -p4242 apirrone@s-nguyen.net:/home/apirrone/MISC/mini_BDX/experiments/RL/new/$1/$last ./$1/ \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py b/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py new file mode 100644 index 0000000..5a2dfa2 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py @@ -0,0 +1,258 @@ +import numpy as np +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact + +# from placo_utils.visualization import robot_viz + + +FRAME_SKIP = 4 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 125, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + self.nb_dofs = 15 + + self.target_velocity = np.asarray([0, 0, 0]) # x, y, yaw + self.joint_history_length = 3 + self.joint_error_history = self.joint_history_length * [self.nb_dofs * [0]] + self.joint_ctrl_history = self.joint_history_length * [self.nb_dofs * [0]] + + # observation_space = Box( + # low=-np.inf, high=np.inf, shape=(101,), dtype=np.float64 + # ) + + observation_space = Box( + np.array( + [ + *(-np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(-10 * np.ones(self.nb_dofs)), # joints_velocities + *(-10 * np.ones(3)), # angular_velocity + *(-10 * np.ones(3)), # linear_velocity + *(-10 * np.ones(3)), # target_velocity + *( + -np.pi * np.ones(self.nb_dofs * self.joint_history_length) + ), # joint_ctrl_history + *(np.zeros(2)), # feet_contact + *(-np.pi * np.ones(self.nb_dofs)), # placo_angles + ] + ), + np.array( + [ + *(np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(10 * np.ones(self.nb_dofs)), # joints_velocities + *(10 * np.ones(3)), # angular_velocity + *(10 * np.ones(3)), # linear_velocity + *(10 * np.ones(3)), # target_velocity + *( + np.pi * np.ones(self.nb_dofs * self.joint_history_length) + ), # joint_ctrl_history + *(np.ones(2)), # feet_contact + *(np.pi * np.ones(self.nb_dofs)), # placo_angles + ] + ), + ) + + self.left_foot_in_contact = 0 + self.right_foot_in_contact = 0 + self.startup_cooldown = 1.0 + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + + self.pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/robot.urdf", + ignore_feet_contact=True, + ) + + # self.viz = robot_viz(self.pwe.robot) + MujocoEnv.__init__( + self, + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def is_terminated(self) -> bool: + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + Z_vec /= np.linalg.norm(Z_vec) + upright = np.array([0, 0, 1]) + return ( + self.data.body("base").xpos[2] < 0.08 or np.dot(upright, Z_vec) <= 0.2 + ) # base z is below 0.08m or base has more than 90 degrees of tilt + + def get_feet_contact(self): + right_contact = check_contact(self.data, self.model, "foot_module", "floor") + left_contact = check_contact(self.data, self.model, "foot_module_2", "floor") + return right_contact, left_contact + + def follow_placo_reward(self): + current_pos = self.data.qpos[7 : 7 + self.nb_dofs] + placo_angles = list(self.pwe.get_angles().values()) + # print(np.around(placo_angles, 2)) + error = np.linalg.norm(placo_angles - current_pos) + return -np.square(error) + + def walking_height_reward(self): + return ( + -np.square((self.get_body_com("base")[2] - 0.14)) * 100 + ) # "normal" walking height is about 0.14m + + def velocity_tracking_reward(self): + base_velocity = list(self.data.body("base").cvel[3:][:2]) + [ + self.data.body("base").cvel[:3][2] + ] + base_velocity = np.asarray(base_velocity) + return np.exp(-np.square(base_velocity - self.target_velocity).sum()) + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def smoothness_reward2(self): + # Warning, this function only works if the history is 3 :) + smooth = 0 + t0 = self.joint_ctrl_history[0] + t_minus1 = self.joint_ctrl_history[1] + t_minus2 = self.joint_ctrl_history[2] + + for i in range(15): + smooth += np.square(t0[i] - t_minus1[i]) + np.square( + t_minus1[i] - t_minus2[i] + ) + # smooth += 2.5 * np.square(t0[i] - t_minus1[i]) + 1.5 * np.square( + # t0[i] - 2 * t_minus1[i] + t_minus2[i] + # ) + + return -smooth + + def step(self, a): + + t = self.data.time + dt = t - self.prev_t + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + + if self.startup_cooldown > 0: + self.do_simulation(self.init_pos, FRAME_SKIP) + reward = 0 + else: + + current_ctrl = self.data.ctrl.copy() + # Limiting the control + delta_max = 0.1 + # print("a before clipping", a) + a = np.clip(a, current_ctrl - delta_max, current_ctrl + delta_max) + # print("a after clipping", a) + + self.do_simulation(a, FRAME_SKIP) + + # self.right_foot_in_contact, self.left_foot_in_contact = ( + # self.get_feet_contact() + # ) + + self.pwe.tick( + dt + ) # , self.left_foot_in_contact, self.right_foot_in_contact) + + reward = ( + 0.05 # time reward + + 0.1 * self.walking_height_reward() + + 0.1 * self.upright_reward() + + 0.1 * self.velocity_tracking_reward() + + 0.01 * self.smoothness_reward2() + ) + # print(self.follow_placo_reward(a)) + + ob = self._get_obs() + + if self.render_mode == "human": + self.render() + + self.prev_t = t + + # self.viz.display(self.pwe.robot.state.q) + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + self.pwe.reset() + + self.goto_init() + + self.joint_error_history = self.joint_history_length * [self.nb_dofs * [0]] + self.joint_ctrl_history = self.joint_history_length * [self.nb_dofs * [0]] + + self.target_velocity = np.asarray([0.2, 0, 0]) # x, y, yaw + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + self.data.qpos[7 : 7 + self.nb_dofs] = self.init_pos + self.data.qpos[2] = 0.15 + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.ctrl[:] = self.init_pos + + def _get_obs(self): + + joints_rotations = self.data.qpos[7 : 7 + self.nb_dofs] + joints_velocities = self.data.qvel[6 : 6 + self.nb_dofs] + + angular_velocity = self.data.body("base").cvel[ + :3 + ] # TODO this is imu, add noise to it later + linear_velocity = self.data.body("base").cvel[3:] + + self.joint_ctrl_history.append(self.data.ctrl.copy()) + self.joint_ctrl_history = self.joint_ctrl_history[-self.joint_history_length :] + placo_angles = list(self.pwe.get_angles().values()) + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocity, + np.array(self.joint_ctrl_history).flatten(), + [self.left_foot_in_contact, self.right_foot_in_contact], + placo_angles, + ] + ) diff --git a/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py b/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py new file mode 100644 index 0000000..126ec58 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py @@ -0,0 +1,41 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms import bc +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="simple_env:BDXEnv") + +env = gym.make("BDX_env", render_mode=None) + +rng = np.random.default_rng(0) + +bc_trainer = bc.BC( + observation_space=env.observation_space, + action_space=env.action_space, + demonstrations=dataset, + rng=rng, + device="cpu", + policy=PPO( + "MlpPolicy", env, policy_kwargs=dict(net_arch=[400, 300]) + ).policy, # not working with SAC for some reason +) +bc_trainer.train(n_epochs=100) + +bc_trainer.policy.save("bc_policy_ppo.zip") + +# reward, _ = evaluate_policy(bc_trainer.policy, env, 1) +# print(reward) diff --git a/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py b/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py new file mode 100644 index 0000000..9a8ca50 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py @@ -0,0 +1,90 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms.adversarial.gail import GAIL +from imitation.data.wrappers import RolloutInfoWrapper +from imitation.rewards.reward_nets import BasicRewardNet +from imitation.util.networks import RunningNorm +from imitation.util.util import make_vec_env +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy +from stable_baselines3.ppo import MlpPolicy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="simple_env:BDXEnv") + +SEED = 42 +rng = np.random.default_rng(SEED) +# env = gym.make("BDX_env", render_mode=None) +env = make_vec_env( + "BDX_env", + rng=rng, + n_envs=8, + post_wrappers=[lambda env, _: RolloutInfoWrapper(env)], # to compute rollouts +) + + +learner = PPO( + env=env, + policy=MlpPolicy, + batch_size=64, + ent_coef=0.0, + learning_rate=0.0004, + gamma=0.95, + n_epochs=5, + seed=SEED, + tensorboard_log="logs", +) +reward_net = BasicRewardNet( + observation_space=env.observation_space, + action_space=env.action_space, + normalize_input_layer=RunningNorm, +) +gail_trainer = GAIL( + demonstrations=dataset, + demo_batch_size=1024, + gen_replay_buffer_capacity=512, + n_disc_updates_per_round=8, + venv=env, + gen_algo=learner, + reward_net=reward_net, + allow_variable_horizon=True, +) + +# print("evaluate the learner before training") +# env.seed(SEED) +# learner_rewards_before_training, _ = evaluate_policy( +# learner, +# env, +# 10, +# return_episode_rewards=True, +# ) + +print("train the learner and evaluate again") +env.seed(SEED) +gail_trainer.train(2000000) # Train for 800_000 steps to match expert. + +# env.seed(SEED) +# learner_rewards_after_training, _ = evaluate_policy( +# learner, +# env, +# 10, +# return_episode_rewards=True, +# ) + +# print("mean episode reward before training:", np.mean(learner_rewards_before_training)) +# print("mean episode reward after training:", np.mean(learner_rewards_after_training)) + +print("Save the policy") +gail_trainer.policy.save("gail_policy_ppo.zip") diff --git a/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py b/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py new file mode 100644 index 0000000..e898ddc --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py @@ -0,0 +1,79 @@ +import argparse +import pickle +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import gymnasium as gym +import mujoco +import numpy as np +from gymnasium.envs.registration import register +from mini_bdx.placo_walk_engine import PlacoWalkEngine + +register( + id="BDX_env", + entry_point="simple_env:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + +pwe = PlacoWalkEngine( + "../../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True +) + +EPISODE_LENGTH = 20 +NB_EPISODES_TO_RECORD = 100 + + +def run(env): + episodes = [] + current_episode = {"observations": [], "actions": []} + while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + obs = env.reset()[0] + done = False + prev = env.data.time + start = env.data.time + while not done: + t = env.data.time + dt = t - prev + pwe.tick(dt) + angles = pwe.get_angles() + action = list(angles.values()) + action -= env.init_pos + action = np.array(action) + + obs, _, done, _, _ = env.step(action) + current_episode["observations"].append(list(obs)) + current_episode["actions"].append(list(action)) + + if env.data.time - start > EPISODE_LENGTH: + print("Episode done") + current_episode["observations"].append(list(env.reset()[0])) + + episode = Trajectory( + np.array(current_episode["observations"]), + np.array(current_episode["actions"]), + None, + True, + ) + episodes.append(episode) + + with open("dataset.pkl", "wb") as f: + pickle.dump(episodes, f) + + current_episode = {"observations": [], "actions": []} + done = True + + prev = t + + +if __name__ == "__main__": + gymenv = gym.make("BDX_env", render_mode="human") + run(gymenv) diff --git a/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py b/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py new file mode 100644 index 0000000..b4eb728 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py @@ -0,0 +1,152 @@ +import argparse +import json +import os +import time +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco_viewer +import numpy as np +from imitation.data.types import Trajectory +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.rl_utils import mujoco_to_isaac +from scipy.spatial.transform import Rotation as R + +import mujoco + +pwe = PlacoWalkEngine("../../../mini_bdx/robots/bdx/robot.urdf") + +EPISODE_LENGTH = 100 +NB_EPISODES_TO_RECORD = 1 +FPS = 60 + +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + + +episodes = [] + +current_episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], +} + +model = mujoco.MjModel.from_xml_path("../../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +mujoco_init_pos = np.array( + [ + # right_leg + -0.014, + 0.08, + 0.53, + -1.62, + 0.91, + # left leg + 0.013, + 0.077, + 0.59, + -1.63, + 0.86, + # head + -0.17, + -0.17, + 0.0, + 0.0, + 0.0, + ] +) + +data.qpos[3 : 3 + 4] = [1, 0, 0.08, 1] +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos + +while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + done = False + prev = time.time() + start = time.time() + last_record = time.time() + pwe.set_traj(0.0, 0.0, 0.001) + while not done: + t = time.time() + dt = t - prev + + # qpos = env.data.qpos[:3].copy() + # qpos[2] = 0.15 + # env.data.qpos[:3] = qpos + right_contact, left_contact = get_feet_contact() + + pwe.tick(dt, left_contact, right_contact) + # if pwe.t < 0: # for stand + angles = pwe.get_angles() + action = list(angles.values()) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 7) # 4 seems good + + if pwe.t <= 0: + start = time.time() + print("waiting ...") + prev = t + viewer.render() + continue + + if t - last_record >= 1 / FPS: + root_position = list(np.around(data.body("base").xpos, 3)) + root_orientation = list(np.around(data.body("base").xquat, 3)) # w, x, y, z + + # convert to x, y, z, w + root_orientation = [ + root_orientation[1], + root_orientation[2], + root_orientation[3], + root_orientation[0], + ] + + joints_positions = list(np.around(data.qpos[7 : 7 + 15], 3)) + + joints_positions = mujoco_to_isaac(joints_positions) + + current_episode["Frames"].append( + root_position + root_orientation + joints_positions + ) + last_record = time.time() + + if time.time() - start > EPISODE_LENGTH * 2: + print("Episode done") + print(len(current_episode["Frames"])) + episodes.append(current_episode) + + # save json as bdx_walk.txt + with open("bdx_walk.txt", "w") as f: + json.dump(current_episode, f) + + current_episode = { + "LoopMode": "Wrap", + "FrameDuration": 0.01667, + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], + } + done = True + viewer.render() + prev = t diff --git a/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py b/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py new file mode 100644 index 0000000..3a80d5d --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py @@ -0,0 +1,132 @@ +import argparse +import mujoco_viewer +import time +from mini_bdx.utils.rl_utils import mujoco_to_isaac +import json +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R +import os +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco +import numpy as np +from mini_bdx.placo_walk_engine import PlacoWalkEngine + +pwe = PlacoWalkEngine( + "../../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True +) + +EPISODE_LENGTH = 60 +NB_EPISODES_TO_RECORD = 1 +FPS = 60 + +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + + +episodes = [] + +current_episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], +} + +model = mujoco.MjModel.from_xml_path("../../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + +while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + done = False + prev = time.time() + start = time.time() + last_record = time.time() + pwe.set_traj(0.02, 0.0, 0.001) + while not done: + t = time.time() + dt = t - prev + + # qpos = env.data.qpos[:3].copy() + # qpos[2] = 0.15 + # env.data.qpos[:3] = qpos + # if pwe.t <= 0: # for stand + pwe.tick(dt) + angles = pwe.get_angles() + action = list(angles.values()) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 10) # 4 seems good + + if pwe.t <= 0: + start = time.time() + print("waiting ...") + prev = t + continue + + if t - last_record >= 1 / FPS: + root_position = list(np.around(data.body("base").xpos, 3)) + root_orientation = list(np.around(data.body("base").xquat, 3)) # w, x, y, z + + # convert to x, y, z, w + root_orientation = [ + root_orientation[1], + root_orientation[2], + root_orientation[3], + root_orientation[0], + ] + + joints_positions = list(np.around(data.qpos[7 : 7 + 15], 3)) + + # joints_positions = [ + # joints_positions[0], + # joints_positions[1], + # joints_positions[2], + # joints_positions[3], + # joints_positions[4], + # joints_positions[10], + # joints_positions[11], + # joints_positions[12], + # joints_positions[13], + # joints_positions[14], + # joints_positions[5], + # joints_positions[6], + # joints_positions[7], + # joints_positions[8], + # joints_positions[9], + # ] + joints_positions = mujoco_to_isaac(joints_positions) + + current_episode["Frames"].append( + root_position + root_orientation + joints_positions + ) + last_record = time.time() + + if time.time() - start > EPISODE_LENGTH * 2: + print("Episode done") + print(len(current_episode["Frames"])) + episodes.append(current_episode) + + # save json as bdx_walk.txt + with open("bdx_walk.txt", "w") as f: + json.dump(current_episode, f) + + current_episode = { + "LoopMode": "Wrap", + "FrameDuration": 0.01667, + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], + } + done = True + viewer.render() + prev = t diff --git a/Open_Duck_Mini-2/experiments/RL/new/simple_env.py b/Open_Duck_Mini-2/experiments/RL/new/simple_env.py new file mode 100644 index 0000000..8d600ec --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/simple_env.py @@ -0,0 +1,320 @@ +import numpy as np +from gymnasium import utils +from gymnasium.envs.mujoco import MujocoEnv +from gymnasium.spaces import Box +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact, get_contact_force + +FRAME_SKIP = 4 + + +class BDXEnv(MujocoEnv, utils.EzPickle): + metadata = { + "render_modes": [ + "human", + "rgb_array", + "depth_array", + ], + "render_fps": 125, + } + + def __init__(self, **kwargs): + utils.EzPickle.__init__(self, **kwargs) + self.nb_dofs = 15 + + observation_space = Box( + np.array( + [ + *(-np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(-10 * np.ones(self.nb_dofs)), # joints_velocities + *(-10 * np.ones(3)), # angular_velocity + *(-10 * np.ones(3)), # linear_velocity + *(-10 * np.ones(3)), # target velocity [x, y, theta] + *(0 * np.ones(2)), # feet contact [left, right] + *(-np.pi * np.ones(2)), # clock signal + ] + ), + np.array( + [ + *(np.pi * np.ones(self.nb_dofs)), # joints_rotations + *(10 * np.ones(self.nb_dofs)), # joints_velocities + *(10 * np.ones(3)), # angular_velocity + *(10 * np.ones(3)), # linear_velocity + *(10 * np.ones(3)), # target velocity [x, y, theta] + *(1 * np.ones(2)), # feet contact [left, right] + *(np.pi * np.ones(2)), # clock signal + ] + ), + ) + + self.right_foot_contact = True + self.left_foot_contact = True + + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + + self.prev_t = 0 + self.init_pos = np.array( + [ + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] + ) + + self.startup_cooldown = 1.0 + self.walk_period = 1.0 + self.target_velocities = np.asarray([0, 0, 0]) # x, y, yaw + self.cumulated_reward = 0.0 + self.last_time_both_feet_on_the_ground = 0 + self.init_pos_noise = np.zeros(self.nb_dofs) + + MujocoEnv.__init__( + self, + "../../../mini_bdx/robots/bdx/scene.xml", + FRAME_SKIP, + observation_space=observation_space, + **kwargs, + ) + + def is_terminated(self) -> bool: + left_antenna_contact = check_contact( + self.data, self.model, "left_antenna_assembly", "floor" + ) + right_antenna_contact = check_contact( + self.data, self.model, "right_antenna_assembly", "floor" + ) + body_contact = check_contact(self.data, self.model, "body_module", "floor") + rot = np.array(self.data.body("base").xmat).reshape(3, 3) + Z_vec = rot[:, 2] + Z_vec /= np.linalg.norm(Z_vec) + upright = np.array([0, 0, 1]) + + return ( + self.data.body("base").xpos[2] < 0.08 + or np.dot(upright, Z_vec) <= 0.4 + or left_antenna_contact + or right_antenna_contact + or body_contact + ) + + def support_flying_reward(self): + # Idea : reward when there is a support foot and a flying foot + # penalize when both feet are in the air or both feet are on the ground + right_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "right_foot", "floor")) + ) + left_contact_force = abs( + np.sum(get_contact_force(self.data, self.model, "left_foot", "floor")) + ) + right_speed = np.linalg.norm( + self.data.body("right_foot").cvel[3:] + ) # [rot:vel] size 6 + left_speed = np.linalg.norm( + self.data.body("left_foot").cvel[3:] + ) # [rot:vel] size 6 + + return abs(left_contact_force - right_contact_force) + abs( + right_speed - left_speed + ) + + def orient_reward(self): + desired_yaw = self.target_velocities[2] + current_yaw = R.from_matrix( + np.array(self.data.body("base").xmat).reshape(3, 3) + ).as_euler("xyz")[2] + + return -((abs(desired_yaw) - abs(current_yaw)) ** 2) + + def follow_xy_target_reward(self): + x_velocity = self.data.body("base").cvel[3:][0] + y_velocity = self.data.body("base").cvel[3:][1] + x_error = abs(self.target_velocities[0] - x_velocity) + y_error = abs(self.target_velocities[1] - y_velocity) + return -(x_error + y_error) + + def follow_yaw_target_reward(self): + yaw_velocity = self.data.body("base").cvel[:3][2] + yaw_error = abs(self.target_velocities[2] - yaw_velocity) + return -yaw_error + + def height_reward(self): + current_height = self.data.body("base").xpos[2] + return np.exp(-40 * (0.15 - current_height) ** 2) + + def upright_reward(self): + # angular distance to upright position in reward + Z_vec = np.array(self.data.body("base").xmat).reshape(3, 3)[:, 2] + return np.square(np.dot(np.array([0, 0, 1]), Z_vec)) + + def action_reward(self, a): + current_action = a.copy() + + # This can explode, don't understand why + return min( + 2, np.exp(-5 * np.sum((self.prev_action - current_action)) / self.nb_dofs) + ) + + def torque_reward(self): + current_torque = self.data.qfrc_actuator + return np.exp( + -0.25 * np.sum((self.prev_torque - current_torque)) / self.nb_dofs + ) + + def feet_spacing_reward(self): + target_spacing = 0.12 + left_pos = self.data.body("left_foot").xpos + right_pos = self.data.body("right_foot").xpos + spacing = np.linalg.norm(left_pos - right_pos) + return np.exp(-10 * (spacing - target_spacing) ** 2) + + def both_feet_on_the_ground_penalty(self): + elapsed = self.data.time - self.last_time_both_feet_on_the_ground + if elapsed > self.walk_period: + return -1 + else: + return 0 + + def step(self, a): + t = self.data.time + dt = t - self.prev_t + + if self.startup_cooldown > 0: + self.startup_cooldown -= dt + self.do_simulation(self.init_pos + self.init_pos_noise, FRAME_SKIP) + reward = 0 + self.last_time_both_feet_on_the_ground = t + else: + self.right_foot_contact = check_contact( + self.data, self.model, "right_foot", "floor" + ) + self.left_foot_contact = check_contact( + self.data, self.model, "left_foot", "floor" + ) + + if self.right_foot_contact and self.left_foot_contact: + self.last_time_both_feet_on_the_ground = t + + # We want to learn deltas from the initial position + a += self.init_pos + + # Maybe use that too :) + current_ctrl = self.data.ctrl.copy() + delta_max = 0.05 + a = np.clip(a, current_ctrl - delta_max, current_ctrl + delta_max) + a[10:] = self.init_pos[10:] # Only control the legs + + self.do_simulation(a, FRAME_SKIP) + + # IDEA : normalize reward by the episode length ? + reward = ( + # 0.1 * self.support_flying_reward() + 0.15 * self.follow_xy_target_reward() + + 0.15 * self.follow_yaw_target_reward() + # + 0.15 * self.height_reward() + # + 0.05 * self.upright_reward() + # + 0.05 * self.action_reward(a) + + 0.05 * self.torque_reward() + # + 0.05 * self.feet_spacing_reward() + # + 0.05 * self.both_feet_on_the_ground_penalty() + ) + + self.cumulated_reward += reward + + ob = self._get_obs() + + if self.render_mode == "human": + if self.startup_cooldown <= 0: + # print("support flying reward: ", 0.1 * self.support_flying_reward()) + # print( + # "Follow xy target reward: ", 0.15 * self.follow_xy_target_reward() + # ) + # print( + # "Follow yaw target reward: ", 0.15 * self.follow_yaw_target_reward() + # ) + # print("Height reward: ", 0.15 * self.height_reward()) + # print("Upright reward: ", 0.05 * self.upright_reward()) + # print("Action reward: ", 0.05 * self.action_reward(a)) + # print("Torque reward: ", 0.05 * self.torque_reward()) + # print("Feet spacing reward: ", 0.05 * self.feet_spacing_reward()) + # print(s + # print("TARGET : ", self.target_velocities) + # print("===") + pass + self.render() + + self.prev_t = t + self.prev_action = a.copy() + self.prev_torque = self.data.qfrc_actuator.copy() + + return (ob, reward, self.is_terminated(), False, {}) # terminated # truncated + + def reset_model(self): + # self.model.opt.gravity[:] = [0, 0, 0] # no gravity + self.prev_t = self.data.time + self.startup_cooldown = 1.0 + print("CUMULATED REWARD: ", self.cumulated_reward) + self.cumulated_reward = 0.0 + self.last_time_both_feet_on_the_ground = self.data.time + + v_x = np.random.uniform(0.0, 0.05) + v_y = np.random.uniform(-0.03, 0.03) + v_theta = np.random.uniform(-0.1, 0.1) + # self.target_velocities = np.asarray([v_x, v_y, v_theta]) # x, y, yaw + self.target_velocities = np.asarray([0.05, 0, 0]) # x, y, yaw + + self.prev_action = np.zeros(self.nb_dofs) + self.prev_torque = np.zeros(self.nb_dofs) + + self.goto_init() + + self.set_state(self.data.qpos, self.data.qvel) + return self._get_obs() + + def goto_init(self): + self.data.qvel[:] = np.zeros(len(self.data.qvel[:])) + # self.init_pos_noise = np.random.uniform(-0.01, 0.01, self.nb_dofs) + self.init_pos_noise = np.zeros(self.nb_dofs) + self.data.qpos[7 : 7 + self.nb_dofs] = self.init_pos + self.init_pos_noise + self.data.qpos[2] = 0.15 + self.data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + + self.data.ctrl[:] = self.init_pos + + def get_clock_signal(self): + a = np.sin(2 * np.pi * (self.data.time % self.walk_period) / self.walk_period) + b = np.cos(2 * np.pi * (self.data.time % self.walk_period) / self.walk_period) + return [a, b] + + def _get_obs(self): + joints_rotations = self.data.qpos[7 : 7 + self.nb_dofs] + joints_velocities = self.data.qvel[6 : 6 + self.nb_dofs] + + # TODO this is imu, add noise to it later + angular_velocity = self.data.body("base").cvel[:3] + linear_velocity = self.data.body("base").cvel[3:] + + return np.concatenate( + [ + joints_rotations, + joints_velocities, + angular_velocity, + linear_velocity, + self.target_velocities, + [self.left_foot_contact, self.right_foot_contact], + self.get_clock_signal(), + ] + ) diff --git a/Open_Duck_Mini-2/experiments/RL/new/train.py b/Open_Duck_Mini-2/experiments/RL/new/train.py new file mode 100644 index 0000000..fb7dee7 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/new/train.py @@ -0,0 +1,151 @@ +import argparse +import os +from datetime import datetime + +import gymnasium as gym +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + + +def train(env, sb3_algo, model_dir, log_dir, pretrained=None, device="cuda"): + # SAC parameters found here https://github.com/hill-a/stable-baselines/issues/840#issuecomment-623171534 + if pretrained is None: + match sb3_algo: + case "SAC": + model = SAC( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "TD3": + model = TD3( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "A2C": + model = A2C( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "TQC": + model = TQC( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "PPO": + model = PPO( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case _: + print("Algorithm not found") + return + else: + match sb3_algo: + case "SAC": + model = SAC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TD3": + model = TD3.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "A2C": + model = A2C.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TQC": + model = TQC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "PPO": + model = PPO( + "MlpPolicy", env, verbose=1, device="cuda", tensorboard_log=log_dir + ) + model.policy.load(pretrained) + # model = PPO.load( + # pretrained, + # env=env, + # verbose=1, + # device="cuda", + # tensorboard_log=log_dir, + # ) + case _: + print("Algorithm not found") + return + + TIMESTEPS = 10000 + iters = 0 + while True: + iters += 1 + + model.learn( + total_timesteps=TIMESTEPS, + reset_num_timesteps=False, + progress_bar=True, + ) + model.save(f"{model_dir}/{sb3_algo}_{TIMESTEPS*iters}") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser(description="Train BDX") + parser.add_argument( + "-a", + "--algo", + type=str, + choices=["SAC", "TD3", "A2C", "TQC", "PPO"], + default="SAC", + ) + parser.add_argument("-p", "--pretrained", type=str, required=False) + parser.add_argument("-d", "--device", type=str, required=False, default="cuda") + + parser.add_argument( + "-n", + "--name", + type=str, + required=False, + default=datetime.now().strftime("%Y-%m-%d_%H-%M-%S"), + help="Name of the experiment", + ) + + args = parser.parse_args() + + register( + id="BDX_env", + entry_point="simple_env:BDXEnv", + max_episode_steps=None, # formerly 500 + autoreset=True, + ) + # register( + # id="BDX_env", + # entry_point="env:BDXEnv", + # max_episode_steps=None, # formerly 500 + # autoreset=True, + # ) + + env = gym.make("BDX_env", render_mode=None) + # Create directories to hold models and logs + model_dir = args.name + log_dir = "logs/" + args.name + os.makedirs(model_dir, exist_ok=True) + os.makedirs(log_dir, exist_ok=True) + + train( + env, + args.algo, + pretrained=args.pretrained, + model_dir=model_dir, + log_dir=log_dir, + device=args.device, + ) diff --git a/Open_Duck_Mini-2/experiments/RL/old_test.py b/Open_Duck_Mini-2/experiments/RL/old_test.py new file mode 100644 index 0000000..a28bcdc --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/old_test.py @@ -0,0 +1,83 @@ +import argparse +import os +from glob import glob + +import gymnasium as gym +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 + +register( + id="BDX_env", + entry_point="env_humanoid:BDXEnv", + autoreset=True, + # max_episode_steps=200, +) + + +def test(env, sb3_algo, path_to_model): + + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + + print("Using latest model: ", latest_model_path) + + path_to_model = latest_model_path + + match sb3_algo: + case "SAC": + model = SAC.load(path_to_model, env=env) + case "TD3": + model = TD3.load(path_to_model, env=env) + case "A2C": + model = A2C.load(path_to_model, env=env) + case "TQC": + model = TQC.load(path_to_model, env=env) + case "PPO": + model = PPO("MlpPolicy", env) + model.policy.load(path_to_model) + # model = PPO.load(path_to_model, env=env) + + case _: + print("Algorithm not found") + return + + obs = env.reset()[0] + done = False + extra_steps = 500 + while True: + action, _ = model.predict(obs) + obs, _, done, _, _ = env.step(action) + + if done: + extra_steps -= 1 + + if extra_steps < 0: + break + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + gymenv = gym.make("BDX_env", render_mode="human") + test(gymenv, args.algo, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/experiments/RL/play_policy.py b/Open_Duck_Mini-2/experiments/RL/play_policy.py new file mode 100644 index 0000000..f62c360 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/play_policy.py @@ -0,0 +1,118 @@ +import argparse +import time +from glob import glob + +import gymnasium as gym +import mujoco +import mujoco.viewer +import numpy as np +from gymnasium.envs.registration import register +from stable_baselines3 import PPO, SAC + +from mini_bdx.utils.mujoco_utils import check_contact + + +def get_observation(data, left_contact, right_contact): + + position = ( + data.qpos.flat.copy() + ) # position/rotation of trunk + position of all joints + velocity = ( + data.qvel.flat.copy() + ) # positional/angular velocity of trunk + of all joints + + obs = np.concatenate( + [ + position, + velocity, + [left_contact, right_contact], + ] + ) + # print("OBS SIZE", len(obs)) + return obs + + +def key_callback(keycode): + pass + + +def get_model_from_dir(path_to_model): + + if not path_to_model.endswith(".zip"): + models_paths = glob(path_to_model + "/*.zip") + latest_model_id = 0 + latest_model_path = None + for model_path in models_paths: + model_id = model_path.split("/")[-1][: -len(".zip")].split("_")[-1] + if int(model_id) > latest_model_id: + latest_model_id = int(model_id) + latest_model_path = model_path + + if latest_model_path is None: + print("No models found in directory: ", path_to_model) + return + else: + latest_model_path = path_to_model + + return latest_model_path + + +def get_feet_contact(data, model): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +def play(env, path_to_model): + model_path = get_model_from_dir(path_to_model) + + model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") + data = mujoco.MjData(model) + + left_contact = False + right_contact = False + + viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + + # nn_model = SAC.load(model_path, env) + + nn_model = PPO("MlpPolicy", env) + nn_model.policy.load(model_path) + + try: + while True: + + right_contact, left_contact = get_feet_contact(data, model) + obs = get_observation( + data, + left_contact, + right_contact, + ) + action, _ = nn_model.predict(obs) + data.ctrl[:] = action + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep / 2.5) + + except KeyboardInterrupt: + viewer.close() + + viewer.close() + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="Test model") + parser.add_argument( + "-p", + "--path", + metavar="path_to_model", + help="Path to the model. If directory, will use the latest model.", + ) + parser.add_argument("-a", "--algo", default="SAC") + args = parser.parse_args() + + register(id="BDX_env", entry_point="env_humanoid:BDXEnv") + env = gym.make("BDX_env", render_mode=None) + play(env, path_to_model=args.path) diff --git a/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py b/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py new file mode 100644 index 0000000..9dc98b0 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py @@ -0,0 +1,41 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms import bc +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="env_humanoid:BDXEnv") + +env = gym.make("BDX_env", render_mode=None) + +rng = np.random.default_rng(0) + +bc_trainer = bc.BC( + observation_space=env.observation_space, + action_space=env.action_space, + demonstrations=dataset, + rng=rng, + device="cpu", + policy=PPO( + "MlpPolicy", env, policy_kwargs=dict(net_arch=[400, 300]) + ).policy, # not working with SAC for some reason +) +bc_trainer.train(n_epochs=10) + +bc_trainer.policy.save("bc_policy_ppo.zip") + +# reward, _ = evaluate_policy(bc_trainer.policy, env, 1) +# print(reward) diff --git a/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py b/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py new file mode 100644 index 0000000..3aaae72 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py @@ -0,0 +1,64 @@ +import argparse +import pickle +import pprint + +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms import density as db +from imitation.data import serialize +from imitation.util import util +from stable_baselines3 import PPO +from stable_baselines3.common.policies import ActorCriticPolicy + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + +rng = np.random.default_rng(0) + +register(id="BDX_env", entry_point="env:BDXEnv") +env = util.make_vec_env("BDX_env", rng=rng, n_envs=2) + +dataset = pickle.load(open(args.dataset, "rb")) + +imitation_trainer = PPO( + ActorCriticPolicy, env, learning_rate=3e-4, gamma=0.95, ent_coef=1e-4, n_steps=2048 +) +density_trainer = db.DensityAlgorithm( + venv=env, + rng=rng, + demonstrations=dataset, + rl_algo=imitation_trainer, + density_type=db.DensityType.STATE_ACTION_DENSITY, + is_stationary=True, + kernel="gaussian", + kernel_bandwidth=0.4, + standardise_inputs=True, + allow_variable_horizon=True, +) +density_trainer.train() + + +def print_stats(density_trainer, n_trajectories): + stats = density_trainer.test_policy(n_trajectories=n_trajectories) + print("True reward function stats:") + pprint.pprint(stats) + stats_im = density_trainer.test_policy( + true_reward=False, n_trajectories=n_trajectories + ) + print("Imitation reward function stats:") + pprint.pprint(stats_im) + + +print("Stats before training:") +print_stats(density_trainer, 1) + +density_trainer.train_policy( + 1000000, + progress_bar=True, +) # Train for 1_000_000 steps to approach expert performance. + +print("Stats after training:") +print_stats(density_trainer, 1) + +density_trainer.policy.save("density_policy_ppo.zip") diff --git a/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py b/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py new file mode 100644 index 0000000..7007671 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py @@ -0,0 +1,89 @@ +import argparse +import pickle + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from imitation.algorithms.adversarial.gail import GAIL +from imitation.data.wrappers import RolloutInfoWrapper +from imitation.rewards.reward_nets import BasicRewardNet +from imitation.util.networks import RunningNorm +from imitation.util.util import make_vec_env +from stable_baselines3 import PPO +from stable_baselines3.common.evaluation import evaluate_policy +from stable_baselines3.ppo import MlpPolicy + +# Check this out https://imitation.readthedocs.io/en/latest/algorithms/bc.html + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + + +dataset = pickle.load(open(args.dataset, "rb")) + +register(id="BDX_env", entry_point="env:BDXEnv") + +SEED = 42 +rng = np.random.default_rng(SEED) +# env = gym.make("BDX_env", render_mode=None) +env = make_vec_env( + "BDX_env", + rng=rng, + n_envs=8, + post_wrappers=[lambda env, _: RolloutInfoWrapper(env)], # to compute rollouts +) + + +learner = PPO( + env=env, + policy=MlpPolicy, + batch_size=64, + ent_coef=0.0, + learning_rate=0.0004, + gamma=0.95, + n_epochs=5, + seed=SEED, + tensorboard_log="logs", +) +reward_net = BasicRewardNet( + observation_space=env.observation_space, + action_space=env.action_space, + normalize_input_layer=RunningNorm, +) +gail_trainer = GAIL( + demonstrations=dataset, + demo_batch_size=1024, + gen_replay_buffer_capacity=512, + n_disc_updates_per_round=8, + venv=env, + gen_algo=learner, + reward_net=reward_net, + allow_variable_horizon=True, +) + +print("evaluate the learner before training") +env.seed(SEED) +learner_rewards_before_training, _ = evaluate_policy( + learner, + env, + 100, + return_episode_rewards=True, +) + +print("train the learner and evaluate again") +gail_trainer.train(500000) # Train for 800_000 steps to match expert. + +env.seed(SEED) +learner_rewards_after_training, _ = evaluate_policy( + learner, + env, + 100, + return_episode_rewards=True, +) + +print("mean episode reward before training:", np.mean(learner_rewards_before_training)) +print("mean episode reward after training:", np.mean(learner_rewards_after_training)) + +print("Save the policy") +gail_trainer.policy.save("gail_policy_ppo.zip") diff --git a/Open_Duck_Mini-2/experiments/RL/record_episodes.py b/Open_Duck_Mini-2/experiments/RL/record_episodes.py new file mode 100644 index 0000000..0fa2250 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/record_episodes.py @@ -0,0 +1,194 @@ +import pickle +import time + +import mujoco +import mujoco.viewer +import numpy as np +import placo +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact + +# from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +# xbox = XboxController() + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + +EPISODE_LENGTH = 2000 + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +walking = True +recording = False +episodes = [] +current_episode = {"observations": [], "actions": []} + + +left_contact = False +right_contact = False + +start_button_timeout = time.time() + + +def xbox_input(): + global target_velocity, target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.5: + target_head_pitch = inputs["r_y"] * np.deg2rad(45) + target_head_yaw = -inputs["r_x"] * np.deg2rad(120) + target_head_z_offset = inputs["r_trigger"] * 0.08 + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + target_velocity = np.array([-inputs["l_y"], inputs["l_x"], -inputs["r_x"]]) + + +def key_callback(keycode): + global recording, walking, target_step_size_x, target_step_size_y, target_yaw, walk_engine, data, t + if keycode == 257: # enter + start_stop_recording() + if keycode == 261: # delete + walking = False + target_step_size_x = 0 + target_step_size_y = 0 + target_yaw = 0 + walk_engine.reset() + data.qpos[:7] = 0 + data.qpos[2] = 0.19 + data.ctrl[:] = 0 + + +def get_observation(): + global left_contact, right_contact, data + + position = ( + data.qpos.flat.copy() + ) # position/rotation of trunk + position of all joints + velocity = ( + data.qvel.flat.copy() + ) # positional/angular velocity of trunk + of all joints + + obs = np.concatenate( + [ + position, + velocity, + [left_contact, right_contact], + ] + ) + # print("OBS SIZE", len(obs)) + return obs + + +def start_stop_recording(): + global recording, current_episode + recording = not recording + if not recording: + print("Stop recording") + # store one last observation here + current_episode["observations"].append(list(get_observation())) + + episode = Trajectory( + np.array(current_episode["observations"]), + np.array(current_episode["actions"]), + None, + True, + ) + episodes.append(episode) + with open("dataset.pkl", "wb") as f: + pickle.dump(episodes, f) + else: + print("Start recording") + current_episode = {"observations": [], "actions": []} + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine(robot) + + +def get_imu(data): + + rot_mat = np.array(data.body("base").xmat).reshape(3, 3) + gyro = R.from_matrix(rot_mat).as_euler("xyz") + + accelerometer = np.array(data.body("base").cvel)[3:] + + return gyro, accelerometer + + +def get_feet_contact(data, model): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +start_stop_recording() # start recording +prev = data.time +try: + while True: + dt = data.time - prev + + # xbox_input() + + # Get sensor data + right_contact, left_contact = get_feet_contact(data, model) + gyro, accelerometer = get_imu(data) + + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + angles = walk_engine.get_angles() + + # store obs here + if recording: + current_episode["observations"].append(list(get_observation())) + current_episode["actions"].append(list(angles.values())) + + if len(current_episode["observations"]) > EPISODE_LENGTH: + start_stop_recording() # stop recording + start_stop_recording() # start recording + + # apply the angles to the robot + data.ctrl[:] = list(angles.values()) + + prev = data.time + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep / 2.5) + +except KeyboardInterrupt: + viewer.close() diff --git a/Open_Duck_Mini-2/experiments/RL/replay_episodes.py b/Open_Duck_Mini-2/experiments/RL/replay_episodes.py new file mode 100644 index 0000000..2b09382 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/replay_episodes.py @@ -0,0 +1,54 @@ +import argparse +import pickle +import time + +import mujoco +import mujoco.viewer + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--dataset", type=str, required=True) +args = parser.parse_args() + +episodes = pickle.load(open(args.dataset, "rb")) + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + + +def key_callback(keycode): + pass + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +current_episode_id = 0 +current_episode = episodes[current_episode_id] + +prev = data.time +try: + while True: + for i in range(len(current_episode.acts)): + angles = current_episode.acts[i] + # obs = current_episode.obs[i] + # print(len(obs)) + dt = data.time - prev + + data.ctrl[:] = angles + + prev = data.time + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(model.opt.timestep / 2.5) + print("new episode") + current_episode_id += 1 + if current_episode_id >= len(episodes): + print("saw all episodes, restarting") + current_episode_id = 0 + + current_episode = episodes[current_episode_id] + + +except KeyboardInterrupt: + viewer.close() + +viewer.close() diff --git a/Open_Duck_Mini-2/experiments/RL/train.py b/Open_Duck_Mini-2/experiments/RL/train.py new file mode 100644 index 0000000..ffa1f8d --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/train.py @@ -0,0 +1,150 @@ +import argparse +import os +from datetime import datetime + +import gymnasium as gym +import numpy as np +from gymnasium.envs.registration import register +from sb3_contrib import TQC +from stable_baselines3 import A2C, PPO, SAC, TD3 +from stable_baselines3.common.noise import NormalActionNoise + + +def train(env, sb3_algo, model_dir, log_dir, pretrained=None, device="cuda"): + n_actions = env.action_space.shape[-1] + # SAC parameters found here https://github.com/hill-a/stable-baselines/issues/840#issuecomment-623171534 + if pretrained is None: + match sb3_algo: + case "SAC": + model = SAC( + "MlpPolicy", + env, + verbose=1, + device=device, + tensorboard_log=log_dir, + # action_noise=NormalActionNoise( + # mean=np.zeros(n_actions), sigma=0.1 * np.ones(n_actions) + # ), + # learning_starts=10000, + # batch_size=100, + # learning_rate=1e-3, + # train_freq=1000, + # gradient_steps=1000, + # policy_kwargs=dict(net_arch=[400, 300]), + ) + case "TD3": + model = TD3( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "A2C": + model = A2C( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "TQC": + model = TQC( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case "PPO": + model = PPO( + "MlpPolicy", env, verbose=1, device=device, tensorboard_log=log_dir + ) + case _: + print("Algorithm not found") + return + else: + match sb3_algo: + case "SAC": + model = SAC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TD3": + model = TD3.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "A2C": + model = A2C.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case "TQC": + model = TQC.load( + pretrained, + env=env, + verbose=1, + device="cuda", + tensorboard_log=log_dir, + ) + case _: + print("Algorithm not found") + return + + TIMESTEPS = 10000 + iters = 0 + while True: + iters += 1 + + model.learn( + total_timesteps=TIMESTEPS, + reset_num_timesteps=False, + progress_bar=True, + ) + model.save(f"{model_dir}/{sb3_algo}_{TIMESTEPS*iters}") + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser(description="Train BDX") + parser.add_argument( + "-a", + "--algo", + type=str, + choices=["SAC", "TD3", "A2C", "TQC", "PPO"], + default="SAC", + ) + parser.add_argument("-p", "--pretrained", type=str, required=False) + parser.add_argument("-d", "--device", type=str, required=False, default="cuda") + + parser.add_argument( + "-n", + "--name", + type=str, + required=False, + default=datetime.now().strftime("%Y-%m-%d_%H-%M-%S"), + help="Name of the experiment", + ) + + args = parser.parse_args() + + register( + id="BDX_env", + entry_point="env_humanoid:BDXEnv", + max_episode_steps=None, # formerly 500 + autoreset=True, + ) + + env = gym.make("BDX_env", render_mode=None) + # Create directories to hold models and logs + model_dir = args.name + log_dir = "logs/" + args.name + os.makedirs(model_dir, exist_ok=True) + os.makedirs(log_dir, exist_ok=True) + + train( + env, + args.algo, + pretrained=args.pretrained, + model_dir=model_dir, + log_dir=log_dir, + device=args.device, + ) diff --git a/Open_Duck_Mini-2/experiments/RL/view_hdf5.py b/Open_Duck_Mini-2/experiments/RL/view_hdf5.py new file mode 100644 index 0000000..27e7086 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/RL/view_hdf5.py @@ -0,0 +1,11 @@ +import h5py + +data = h5py.File( + "/home/antoine/MISC/mini_BDX/experiments/RL/data/test_raw/episode_0.hdf5", "r" +) +print(len(data["/action"])) +exit() +for i in range(len(data["/action"])): + print(data["/action"][i]) + print(data["/observations/qpos"][i]) + print(data["/observations/qvel"][i]) diff --git a/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py b/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py new file mode 100644 index 0000000..b65c0fe --- /dev/null +++ b/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py @@ -0,0 +1,116 @@ +import placo +from placo_utils.visualization import robot_viz + +import numpy as np +import time + +from mini_bdx_runtime.io_330 import Dxl330IO + +dxl_io = Dxl330IO("/dev/ttyUSB0", baudrate=3000000, use_sync_read=True) + +# Based on performance graph here https://emanual.robotis.com/docs/en/dxl/x/xc330-m288/ +A = 2.69 +B = 0.19 +current_limit = 2.3 +torque_limit = 1.0 + + +def current_to_torque(current: float) -> float: + """ + Input current in A + Output torque in Nm + """ + torque = (current - B) / A + return min(torque_limit, max(-torque_limit, torque)) + + +def torque_to_current(torque: float) -> float: + """ + Input torque in Nm + Output current in A + """ + current = A * torque + B + return min(current_limit, max(-current_limit, current)) + + +def torque_to_current2(torque: float) -> float: + """ + Input torque in Nm + Output current in A + """ + kt = 2.73 + return torque / kt + + +joints = { + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} + +dxl_io.set_operating_mode({id: 0x0 for id in joints.values()}) # set in current mode + + +def get_right_leg_position(): + present_positions_list = dxl_io.get_present_position(joints.values()) + present_positions = { + joint: -np.deg2rad(position) + for joint, position in zip(joints, present_positions_list) + } + return present_positions + + +robot = placo.HumanoidRobot("../../mini_bdx/robots/bdx/robot.urdf") + +input("press any key to record position") +right_leg_position = get_right_leg_position() +for joint, position in right_leg_position.items(): + robot.set_joint(joint, position) + + +# viz = robot_viz(robot) +# while True: +# viz.display(robot.state.q) +# time.sleep(1 / 20) +# exit() +def get_target_current(): + target_torques = robot.static_gravity_compensation_torques_dict("trunk") + right_leg_target_torques = {} + for joint, torque in target_torques.items(): + if joint in list(joints.keys()): + right_leg_target_torques[joint] = torque + + print("target torque", right_leg_target_torques) + + right_leg_target_current = {} + for joint, torque in right_leg_target_torques.items(): + right_leg_target_current[joint] = -torque_to_current2(torque) * 1000 + + print("target current", right_leg_target_current) + right_leg_target_current_id = { + joints[joint]: round(current) + for joint, current in right_leg_target_current.items() + } + print("target current id", right_leg_target_current_id) + return right_leg_target_current_id + + +time.sleep(1) +input("press enter to set torques") +# exit() +dxl_io.enable_torque(joints.values()) +dxl_io.set_goal_current(get_target_current()) +try: + while True: + right_leg_position = get_right_leg_position() + for joint, position in right_leg_position.items(): + robot.set_joint(joint, position) + dxl_io.set_goal_current(get_target_current()) + print("running") + time.sleep(1.0) +except KeyboardInterrupt: + print("STOP") + dxl_io.disable_torque(joints.values()) + pass diff --git a/Open_Duck_Mini-2/experiments/identification/check_speed.py b/Open_Duck_Mini-2/experiments/identification/check_speed.py new file mode 100644 index 0000000..ce20a01 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/identification/check_speed.py @@ -0,0 +1,59 @@ +from mini_bdx_runtime.hwi import HWI +from mini_bdx_runtime.rl_utils import make_action_dict, mujoco_joints_order +import time +import numpy as np +import mujoco +import mujoco_viewer +import pickle + +hwi = HWI("/dev/ttyUSB0") + +hwi.turn_on() +hwi.set_pid_all([1100, 0, 0]) +# hwi.set_pid([500, 0, 0], "neck_pitch") +# hwi.set_pid([500, 0, 0], "head_pitch") +# hwi.set_pid([500, 0, 0], "head_yaw") + +dt = 0.0001 + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +init_pos = list(hwi.init_pos.values()) +# init_pos += [0, 0] +data.qpos[:13] = init_pos +data.ctrl[:13] = init_pos + +dof = 7 +a = 0.3 +f = 3 + +recording = {} +recording["mujoco_vel"] = [] +recording["robot_vel"] = [] +try: + while True: + target = a * np.sin(2 * np.pi * f * time.time()) + full_target = init_pos.copy() + full_target[dof] += target + + # + data.ctrl[:13] = full_target + action_dict = make_action_dict(full_target, mujoco_joints_order) + hwi.set_position_all(action_dict) + + mujoco.mj_step(model, data, 50) + + mujoco_vel = data.qvel[dof] + robot_vel = hwi.get_present_velocities()[dof] + + recording["mujoco_vel"].append(mujoco_vel) + recording["robot_vel"].append(robot_vel) + + viewer.render() +except KeyboardInterrupt: + pickle.dump(recording, open("speeds.pkl", "wb")) diff --git a/Open_Duck_Mini-2/experiments/identification/get_data.py b/Open_Duck_Mini-2/experiments/identification/get_data.py new file mode 100644 index 0000000..0fdca7a --- /dev/null +++ b/Open_Duck_Mini-2/experiments/identification/get_data.py @@ -0,0 +1,146 @@ +# run sin motion joints, get command vs data +# in mujoco and on real robot + +import argparse +import os +import pickle +import time + +import mujoco +import mujoco_viewer +import numpy as np +from mini_bdx_runtime.hwi import HWI +from mini_bdx_runtime.rl_utils import ( + ActionFilter, + make_action_dict, + mujoco_joints_order, +) +from utils import dof_to_id, id_to_dof, mujoco_init_pos + +parser = argparse.ArgumentParser() +parser.add_argument("--dof", type=str, default="left_ankle") +parser.add_argument("--move_freq", type=float, default=10) +parser.add_argument("--move_amp", type=float, default=0.5) +parser.add_argument("--ctrl_freq", type=float, default=30) +parser.add_argument("--sampling_freq", type=float, default=100) +parser.add_argument("--duration", type=float, default=5) +parser.add_argument("--save_dir", type=str, default="./data") +parser.add_argument("--saved_actions", type=str, required=False) +args = parser.parse_args() + +if args.saved_actions is not None: + saved_actions = pickle.loads(open(args.saved_actions, "rb").read()) + +os.makedirs(args.save_dir, exist_ok=True) + +dt = 0.001 + +assert args.dof in id_to_dof.values() + + +## === Init mujoco === +# Commented freejoint +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) +data.qpos = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos +mujoco_command_value = [] + +## === Init robot === +hwi = HWI(usb_port="/dev/ttyUSB0") +time.sleep(1) +hwi.turn_on() + +pid = [500, 0, 500] +hwi.set_pid_all(pid) +time.sleep(3) +robot_command_value = [] + +action_filter = ActionFilter(window_size=10) + + +prev = time.time() +last_control = time.time() +last_sample = time.time() +start = time.time() +if args.saved_actions is None: + last_target = 0 +else: + last_target = np.zeros(15) +i = 0 +while True: + t = time.time() + dt = t - prev + if t - last_control > 1 / args.ctrl_freq: + last_control = t + if args.saved_actions is None: + last_target = ( + mujoco_init_pos[dof_to_id[args.dof]] + + np.sin(2 * np.pi * args.move_freq * t) * args.move_amp + ) + data.ctrl[dof_to_id[args.dof]] = last_target + + action_filter.push(last_target) + filtered_action = action_filter.get_filtered_action() + hwi.set_position(args.dof, filtered_action) + else: + last_target = saved_actions[i] + data.ctrl[:] = last_target + action_dict = make_action_dict(last_target, mujoco_joints_order) + hwi.set_position_all(action_dict) + mujoco.mj_step(model, data, 5) + + if t - last_sample > 1 / args.sampling_freq: + last_sample = t + + mujoco_command_value.append( + [ + data.ctrl[:].copy(), + data.qpos[:].copy(), + ] + ) + if args.saved_actions is None: + last_robot_command = np.zeros(15) + print(last_target) + last_robot_command[dof_to_id[args.dof]] = last_target + else: + last_robot_command = last_target + robot_command_value.append( + [ + last_robot_command, + list(hwi.get_present_positions()) + [0, 0], + ] + ) + + mujoco_dof_vel = np.around(data.qvel[dof_to_id[args.dof]], 3) + robot_dof_vel = np.around(hwi.get_present_velocities()[dof_to_id[args.dof]], 3) + print(mujoco_dof_vel, robot_dof_vel) + i += 1 + + if t - start > args.duration: + break + if args.saved_actions is not None: + if i > len(saved_actions) - 1: + break + + viewer.render() + prev = t + +path = os.path.join(args.save_dir, f"{args.dof}.pkl") +data_dict = { + "config": { + "dof": args.dof, + "move_freq": args.move_freq, + "move_amp": args.move_amp, + "ctrl_freq": args.ctrl_freq, + "sampling_freq": args.sampling_freq, + "duration": args.duration, + }, + "mujoco": mujoco_command_value, + "robot": robot_command_value, +} +pickle.dump(data_dict, open(path, "wb")) +print("saved to", path) diff --git a/Open_Duck_Mini-2/experiments/identification/plot.py b/Open_Duck_Mini-2/experiments/identification/plot.py new file mode 100644 index 0000000..0772021 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/identification/plot.py @@ -0,0 +1,71 @@ +import argparse +import pickle + +import matplotlib.pyplot as plt +import numpy as np + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--data", type=str, required=True) +args = parser.parse_args() +data = pickle.load(open(args.data, "rb")) +if "robot" in data and "mujoco" in data: + res = None + while res is None or res not in ["1", "2"]: + res = input("plot robot (1) or mujoco (2) ? ") + if res == "1": + command_value = data["robot"] + title = "Robot" + else: + command_value = data["mujoco"] + title = "Mujoco" +elif "mujoco" in data: + command_value = data["mujoco"] + title = "Mujoco" +elif "robot" in data: + command_value = data["robot"] + title = "Robot" +else: + print("NO DATA") + exit() + + +dofs = { + 0: "right_hip_yaw", + 1: "right_hip_roll", + 2: "right_hip_pitch", + 3: "right_knee", + 4: "right_ankle", + 5: "left_hip_yaw", + 6: "left_hip_roll", + 7: "left_hip_pitch", + 8: "left_knee", + 9: "left_ankle", + 10: "neck_pitch", + 11: "head_pitch", + 12: "head_yaw", + 13: "left_antenna", + 14: "right_antenna", +} +# command_value = np.array(command_value) +fig, axs = plt.subplots(4, 4) +dof_id = 0 +for i in range(4): + for j in range(4): + if i == 3 and j == 3: + break + print(4 * i + j) + command = [] + value = [] + for k in range(len(command_value)): + command.append(command_value[k][0][4 * i + j]) + value.append(command_value[k][1][4 * i + j]) + axs[i, j].plot(command, label="command") + axs[i, j].plot(value, label="value") + axs[i, j].legend() + axs[i, j].set_title(f"{dofs[dof_id]}") + dof_id += 1 + + +name = args.data.split("/")[-1].split(".")[0] +fig.suptitle(title) +plt.show() diff --git a/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py b/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py new file mode 100644 index 0000000..96692a9 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py @@ -0,0 +1,204 @@ +import argparse +import pickle + +import matplotlib.pyplot as plt +import numpy as np +from scipy.spatial.transform import Rotation as R + +isaac_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + +parser = argparse.ArgumentParser() +parser.add_argument("--robot_obs", type=str, required=True) +parser.add_argument("--hardware", action="store_true") +args = parser.parse_args() + +robot_obs = pickle.load(open(args.robot_obs, "rb")) + +robot_channels = [] + + +# convert quat to euler for easier reading by a simple human +if not args.hardware: + for i in range(len(robot_obs)): + robot_quat = robot_obs[i][:4] + robot_euler = R.from_quat(robot_quat).as_euler("xyz") + + robot_obs[i] = robot_obs[i][1:] + + robot_obs[i][:3] = robot_euler + + +if not args.hardware: + channels = [ + "base_roll", + "base_pitch", + "base_yaw", + "base_ang_vel[0]", + "base_ang_vel[1]", + "base_ang_vel[2]", + "dof_pos[0]", + "dof_pos[1]", + "dof_pos[2]", + "dof_pos[3]", + "dof_pos[4]", + "dof_pos[5]", + "dof_pos[6]", + "dof_pos[7]", + "dof_pos[8]", + "dof_pos[9]", + "dof_pos[10]", + "dof_pos[11]", + "dof_pos[12]", + "dof_pos[13]", + "dof_pos[14]", + "dof_vel[0]", + "dof_vel[1]", + "dof_vel[2]", + "dof_vel[3]", + "dof_vel[4]", + "dof_vel[5]", + "dof_vel[6]", + "dof_vel[7]", + "dof_vel[8]", + "dof_vel[9]", + "dof_vel[10]", + "dof_vel[11]", + "dof_vel[12]", + "dof_vel[13]", + "dof_vel[14]", + "prev_action[0]", + "prev_action[1]", + "prev_action[2]", + "prev_action[3]", + "prev_action[4]", + "prev_action[5]", + "prev_action[6]", + "prev_action[7]", + "prev_action[8]", + "prev_action[9]", + "prev_action[10]", + "prev_action[11]", + "prev_action[12]", + "prev_action[13]", + "prev_action[14]", + "commands[0]", + "commands[1]", + "commands[2]", + ] +else: + channels = [ + "base_lin_vel[0]", + "base_lin_vel[1]", + "base_lin_vel[2]", + "base_ang_vel[0]", + "base_ang_vel[1]", + "base_ang_vel[2]", + "projected_gravity[0]", + "projected_gravity[1]", + "projected_gravity[2]", + "commands[0]", + "commands[1]", + "commands[2]", + "dof_pos[0]", + "dof_pos[1]", + "dof_pos[2]", + "dof_pos[3]", + "dof_pos[4]", + "dof_pos[5]", + "dof_pos[6]", + "dof_pos[7]", + "dof_pos[8]", + "dof_pos[9]", + "dof_pos[10]", + "dof_pos[11]", + "dof_pos[12]", + "dof_pos[13]", + "dof_pos[14]", + "dof_vel[0]", + "dof_vel[1]", + "dof_vel[2]", + "dof_vel[3]", + "dof_vel[4]", + "dof_vel[5]", + "dof_vel[6]", + "dof_vel[7]", + "dof_vel[8]", + "dof_vel[9]", + "dof_vel[10]", + "dof_vel[11]", + "dof_vel[12]", + "dof_vel[13]", + "dof_vel[14]", + "actions[0]", + "actions[1]", + "actions[2]", + "actions[3]", + "actions[4]", + "actions[5]", + "actions[6]", + "actions[7]", + "actions[8]", + "actions[9]", + "actions[10]", + "actions[11]", + "actions[12]", + "actions[13]", + "actions[14]", + ] + +# base_lin_vel * self.obs_scales.lin_vel, +# base_ang_vel * self.obs_scales.ang_vel, +# self.projected_gravity, +# self.commands[:, :3] * self.commands_scale, +# (self.dof_pos - self.default_dof_pos) * self.obs_scales.dof_pos, +# self.dof_vel * self.obs_scales.dof_vel, +# self.actions, + +nb_channels = len(robot_obs[0]) +dof_poses = [] +prev_actions = [] + +# select dof_pos and prev_action +for i in range(nb_channels): + if "dof_pos" in channels[i]: + dof_poses.append([obs[i] for obs in robot_obs]) + elif "action" in channels[i]: + prev_actions.append([obs[i] for obs in robot_obs]) + +# print(len(dof_poses)) +# print(len(prev_actions)) +# exit() + +# plot prev action vs dof pos + +nb_dofs = len(dof_poses) +nb_rows = int(np.sqrt(nb_dofs)) +nb_cols = int(np.ceil(nb_dofs / nb_rows)) + +fig, axs = plt.subplots(nb_rows, nb_cols, sharex=True, sharey=True) +for i in range(nb_rows): + for j in range(nb_cols): + if i * nb_cols + j >= nb_dofs: + break + axs[i, j].plot(prev_actions[i * nb_cols + j], label="command") + axs[i, j].plot(dof_poses[i * nb_cols + j], label="value") + axs[i, j].legend() + axs[i, j].set_title(f"{isaac_joints_order[i * nb_cols + j]}") + +plt.show() diff --git a/Open_Duck_Mini-2/experiments/identification/plot_obs.py b/Open_Duck_Mini-2/experiments/identification/plot_obs.py new file mode 100644 index 0000000..f9f12a1 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/identification/plot_obs.py @@ -0,0 +1,121 @@ +import argparse +import pickle + +import matplotlib.pyplot as plt +import numpy as np +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("--mujoco_obs", type=str, required=True) +parser.add_argument("--robot_obs", type=str, required=True) +args = parser.parse_args() + +mujoco_obs = pickle.load(open(args.mujoco_obs, "rb")) +robot_obs = pickle.load(open(args.robot_obs, "rb")) + + +mujoco_channels = [] +robot_channels = [] + +# # convert quat to euler for easier reading by a simple human +# for i in range(min(len(mujoco_obs), len(robot_obs))): +# mujoco_quat = mujoco_obs[i][:4] +# mujoco_euler = R.from_quat(mujoco_quat).as_euler("xyz") + +# robot_quat = robot_obs[i][:4] +# robot_euler = R.from_quat(robot_quat).as_euler("xyz") + +# mujoco_obs[i] = mujoco_obs[i][1:] +# robot_obs[i] = robot_obs[i][1:] + +# mujoco_obs[i][:3] = mujoco_euler +# robot_obs[i][:3] = robot_euler + +nb_channels = len(mujoco_obs[0]) + +for i in range(nb_channels): + mujoco_channels.append([obs[i] for obs in mujoco_obs]) + robot_channels.append([obs[i] for obs in robot_obs]) + +channels = [ + "base_roll", + "base_pitch", + "base_yaw", + "base_quat[0]", + "base_quat[1]", + "base_quat[2]", + "base_quat[3]", + "base_ang_vel[0]", + "base_ang_vel[1]", + "base_ang_vel[2]", + "dof_pos[0]", + "dof_pos[1]", + "dof_pos[2]", + "dof_pos[3]", + "dof_pos[4]", + "dof_pos[5]", + "dof_pos[6]", + "dof_pos[7]", + "dof_pos[8]", + "dof_pos[9]", + "dof_pos[10]", + "dof_pos[11]", + "dof_pos[12]", + "dof_pos[13]", + "dof_pos[14]", + "dof_vel[0]", + "dof_vel[1]", + "dof_vel[2]", + "dof_vel[3]", + "dof_vel[4]", + "dof_vel[5]", + "dof_vel[6]", + "dof_vel[7]", + "dof_vel[8]", + "dof_vel[9]", + "dof_vel[10]", + "dof_vel[11]", + "dof_vel[12]", + "dof_vel[13]", + "dof_vel[14]", + "prev_action[0]", + "prev_action[1]", + "prev_action[2]", + "prev_action[3]", + "prev_action[4]", + "prev_action[5]", + "prev_action[6]", + "prev_action[7]", + "prev_action[8]", + "prev_action[9]", + "prev_action[10]", + "prev_action[11]", + "prev_action[12]", + "prev_action[13]", + "prev_action[14]", + "commands[0]", + "commands[1]", + "commands[2]", +] + +# one sub plot per channel, robot vs mujoco +# arrange as an array of sqrt(nb_channels) x sqrt(nb_channels) + +nb_rows = int(np.sqrt(nb_channels)) +nb_cols = int(np.ceil(nb_channels / nb_rows)) + +fig, axs = plt.subplots(nb_rows, nb_cols, sharex=True, sharey=True) +for i in range(nb_rows): + for j in range(nb_cols): + if i * nb_cols + j >= nb_channels: + break + axs[i, j].plot(mujoco_channels[i * nb_cols + j], label="mujoco") + axs[i, j].plot(robot_channels[i * nb_cols + j], label="robot") + axs[i, j].legend() + axs[i, j].set_title(f"{channels[i * nb_cols + j]}") + + +fig.suptitle("Mujoco vs Robot") +# tight layout +# plt.tight_layout() +plt.show() diff --git a/Open_Duck_Mini-2/experiments/identification/plot_speeds.py b/Open_Duck_Mini-2/experiments/identification/plot_speeds.py new file mode 100644 index 0000000..a5c48cc --- /dev/null +++ b/Open_Duck_Mini-2/experiments/identification/plot_speeds.py @@ -0,0 +1,22 @@ +import pickle + +# data is +# recording = {} +# recording["mujoco_vel"] = [] +# recording["robot_vel"] = [] +data = pickle.load(open("speeds.pkl", "rb")) + + +import matplotlib.pyplot as plt + +mujoco_vel = data.get("mujoco_vel", []) +robot_vel = data.get("robot_vel", []) + +plt.figure() +plt.plot(mujoco_vel, label="Mujoco Velocity") +plt.plot(robot_vel, label="Robot Velocity") +plt.xlabel("Time Step") +plt.ylabel("Velocity") +plt.title("Mujoco Velocity vs Robot Velocity") +plt.legend() +plt.show() diff --git a/Open_Duck_Mini-2/experiments/identification/utils.py b/Open_Duck_Mini-2/experiments/identification/utils.py new file mode 100644 index 0000000..2919a7b --- /dev/null +++ b/Open_Duck_Mini-2/experiments/identification/utils.py @@ -0,0 +1,42 @@ +import numpy as np + +mujoco_init_pos = np.array( + [ + # right_leg + 0.013627156377842975, + 0.07738878096596595, + 0.5933527914082196, + -1.630548419252953, + 0.8621333440557593, + # left leg + -0.013946457213457239, + 0.07918837709879874, + 0.5325073962634973, + -1.6225192902713386, + 0.9149246381274986, + # head + -0.17453292519943295, + -0.17453292519943295, + 8.65556854322817e-27, + 0, + 0, + ] +) +id_to_dof = { + 0: "right_hip_yaw", + 1: "right_hip_roll", + 2: "right_hip_pitch", + 3: "right_knee", + 4: "right_ankle", + 5: "left_hip_yaw", + 6: "left_hip_roll", + 7: "left_hip_pitch", + 8: "left_knee", + 9: "left_ankle", + 10: "neck_pitch", + 11: "head_pitch", + 12: "head_yaw", + 13: "left_antenna", + 14: "right_antenna", +} +dof_to_id = {v: k for k, v in id_to_dof.items()} diff --git a/Open_Duck_Mini-2/experiments/mujoco/.gitignore b/Open_Duck_Mini-2/experiments/mujoco/.gitignore new file mode 100644 index 0000000..fac783a --- /dev/null +++ b/Open_Duck_Mini-2/experiments/mujoco/.gitignore @@ -0,0 +1 @@ +*.onnx \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py b/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py new file mode 100644 index 0000000..9d3d7b2 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py @@ -0,0 +1,137 @@ +import argparse +import json +import os +import time +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco +import mujoco_viewer +import pygame +import numpy as np +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.rl_utils import action_to_pd_targets, mujoco_to_isaac +from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.utils.rl_utils import ( + isaac_to_mujoco, +) + +parser = argparse.ArgumentParser() +parser.add_argument("-x", "--xbox_controller", action="store_true") +parser.add_argument("-k", "--keyboard", action="store_true") +args = parser.parse_args() + +if args.xbox_controller: + xbox = XboxController() + + +if args.keyboard: + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf") + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.0001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +# lower com 0.16 +isaac_init_pos = np.array( + [ + -0.03455234018541292, + 0.055730747490168285, + 0.5397158397618105, + -1.3152788306721914, + 0.6888361815639528, + -0.1745314896173976, + -0.17453429522668937, + 0, + 0, + 0, + -0.03646051060835733, + -0.03358034284950263, + 0.5216150220237578, + -1.326235199315616, + 0.7179857110436013, + ] +) + +mujoco_init_pos = np.array(isaac_to_mujoco(isaac_init_pos)) + + +def xbox_input(): + inputs = xbox.read() + step_size_x = -inputs["l_y"] * 0.02 + step_size_y = -inputs["l_x"] * 0.02 + yaw = -inputs["r_x"] * 0.2 + 0.001 + + return step_size_x, step_size_y, yaw + + +def keyboard_input(): + keys = pygame.key.get_pressed() + step_size_x = 0 + step_size_y = 0 + yaw = 0 + if keys[pygame.K_z]: + step_size_x = 0.03 + if keys[pygame.K_s]: + step_size_x = -0.03 + if keys[pygame.K_q]: + yaw = 0.2 + if keys[pygame.K_d]: + yaw = -0.2 + pygame.event.pump() # process event queue + + return step_size_x, step_size_y, yaw + + +data.qpos[3 : 3 + 4] = [1, 0, 0.05, 0] +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos +prev = time.time() +while True: + t = time.time() + + if args.xbox_controller: + step_size_x, step_size_y, yaw = xbox_input() + elif args.keyboard: + step_size_x, step_size_y, yaw = keyboard_input() + else: + step_size_x, step_size_y, yaw = 0.02, 0, 0.001 + + pwe.set_traj(step_size_x, step_size_y, yaw) + + right_contact, left_contact = get_feet_contact() + + pwe.tick(t - prev, left_contact, right_contact) + # if pwe.t < 0: # for stand + angles = pwe.get_angles() + action = isaac_to_mujoco(list(angles.values())) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 50) # 4 seems good + + # if pwe.t <= 0: + # print("waiting ...") + # viewer.render() + # continue + prev = t + viewer.render() diff --git a/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py b/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py new file mode 100644 index 0000000..04c6cbf --- /dev/null +++ b/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py @@ -0,0 +1,159 @@ +import argparse +import json +import os +import time +from glob import glob + +import cv2 +import FramesViewer.utils as fv_utils +import mujoco +import mujoco_viewer +import numpy as np +from imitation.data.types import Trajectory +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.rl_utils import mujoco_to_isaac + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf") + +EPISODE_LENGTH = 10 +NB_EPISODES_TO_RECORD = 1 +FPS = 60 + +# For IsaacGymEnvs +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + +parser = argparse.ArgumentParser() +parser.add_argument( + "--hardware", action="store_true", help="use AMP_for_hardware format" +) +args = parser.parse_args() + +episodes = [] + +current_episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], +} + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +mujoco_init_pos = np.array( + [ + # right_leg + -0.014, + 0.08, + 0.53, + -1.62, + 0.91, + # left leg + 0.013, + 0.077, + 0.59, + -1.63, + 0.86, + # head + -0.17, + -0.17, + 0.0, + 0.0, + 0.0, + ] +) + +data.qpos[3 : 3 + 4] = [1, 0, 0.05, 0] +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos +x_qvels = [] +while True: + if len(episodes) >= NB_EPISODES_TO_RECORD: + print("DONE, RECORDED", NB_EPISODES_TO_RECORD, "EPISODES") + break + print("Starting episode") + done = False + prev = time.time() + start = time.time() + last_record = time.time() + pwe.set_traj(0.02, 0.0, 0.001) + while not done: + t = time.time() + dt = t - prev + x_qvels.append(data.qvel[0].copy()) + print(np.around(np.mean(x_qvels[-30:]), 3)) + # qpos = env.data.qpos[:3].copy() + # qpos[2] = 0.15 + # env.data.qpos[:3] = qpos + right_contact, left_contact = get_feet_contact() + + pwe.tick(dt, left_contact, right_contact) + # if pwe.t < 0: # for stand + angles = pwe.get_angles() + action = list(angles.values()) + action = np.array(action) + data.ctrl[:] = action + mujoco.mj_step(model, data, 7) # 4 seems good + + if pwe.t <= 0: + start = time.time() + print("waiting ...") + prev = t + viewer.render() + continue + + if t - last_record >= 1 / FPS: + root_position = list(np.around(data.body("base").xpos, 3)) + root_orientation = list(np.around(data.body("base").xquat, 3)) # w, x, y, z + + # convert to x, y, z, w + root_orientation = [ + root_orientation[1], + root_orientation[2], + root_orientation[3], + root_orientation[0], + ] + + joints_positions = list(np.around(data.qpos[7 : 7 + 15], 3)) + + joints_positions = mujoco_to_isaac(joints_positions) + + current_episode["Frames"].append( + root_position + root_orientation + joints_positions + ) + last_record = time.time() + + if time.time() - start > EPISODE_LENGTH * 2: + print("Episode done") + print(len(current_episode["Frames"])) + episodes.append(current_episode) + + # save json as bdx_walk.txt + with open("bdx_walk.txt", "w") as f: + json.dump(current_episode, f) + + current_episode = { + "LoopMode": "Wrap", + "FrameDuration": 0.01667, + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Frames": [], + } + done = True + viewer.render() + prev = t diff --git a/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py b/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py new file mode 100644 index 0000000..35e7e27 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py @@ -0,0 +1,189 @@ +import argparse +import time + +import mujoco +import mujoco.viewer +import numpy as np +import placo +from scipy.spatial.transform import Rotation as R + +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser() +parser.add_argument("-x", "--xbox_controller", action="store_true") +args = parser.parse_args() + +if args.xbox_controller: + xbox = XboxController() + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) + + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +time_since_last_left_contact = 0 +time_since_last_right_contact = 0 +walking = False + +start_button_timeout = time.time() + + +def xbox_input(): + global target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.5: + target_head_pitch = inputs["r_y"] * np.deg2rad(45) + target_head_yaw = -inputs["r_x"] * np.deg2rad(120) + target_head_z_offset = inputs["r_trigger"] * 0.08 + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + +# Tune the walk +def key_callback(keycode): + global target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, max_target_step_size_x, max_target_step_size_y, max_target_yaw + if keycode == 265: # up + max_target_step_size_x += 0.005 + if keycode == 264: # down + max_target_step_size_x -= 0.005 + if keycode == 263: # left + max_target_step_size_y -= 0.005 + if keycode == 262: # right + max_target_step_size_y += 0.005 + if keycode == 81: # a + max_target_yaw += np.deg2rad(1) + if keycode == 69: # e + max_target_yaw -= np.deg2rad(1) + if keycode == 257: # enter + walking = not walking + if keycode == 86: # v + walk_engine.trunk_pitch_roll_compensation = ( + not walk_engine.trunk_pitch_roll_compensation + ) + if keycode == 79: # o + walk_engine.swing_gain -= 0.005 + if keycode == 80: # p + walk_engine.swing_gain += 0.005 + if keycode == 76: # l + walk_engine.tune_trunk_x_offset += 0.001 + if keycode == 59: # m + walk_engine.tune_trunk_x_offset -= 0.001 + if keycode == 266: # page up + walk_engine.frequency += 0.1 + if keycode == 267: # page down + walk_engine.frequency -= 0.1 + if keycode == 268: # begin line + walk_engine.default_trunk_z_offset -= 0.001 + if keycode == 269: # end line + walk_engine.default_trunk_z_offset += 0.001 + if keycode == 32: # space + target_step_size_x = 0 + target_step_size_y = 0 + target_yaw = 0 + if keycode == 261: # delete + walking = False + target_step_size_x = 0 + target_step_size_y = 0 + target_yaw = 0 + walk_engine.reset() + data.qpos[:7] = 0 + data.qpos[2] = 0.19 + data.ctrl[:] = 0 + t = 0 + print(keycode) + print("----------------") + print("walking" if walking else "not walking") + print( + "trunk pitch roll compensation (v)", walk_engine.trunk_pitch_roll_compensation + ) + print("MAX_TARGET_STEP_SIZE_X (up, down)", max_target_step_size_x) + print("MAX_TARGET_STEP_SIZE_Y (left, right)", max_target_step_size_y) + print("MAX_TARGET_YAW (a, e)", np.rad2deg(max_target_yaw)) + print("swing gain (o, p)", walk_engine.swing_gain) + print("trunk x offset (l, m)", walk_engine.trunk_x_offset) + print("default trunk z offset (begin, end)", walk_engine.default_trunk_z_offset) + print("frequency (pageup, pagedown)", walk_engine.frequency) + print("----------------") + + +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine( + robot, default_trunk_x_offset=-0.03, frequency=3 +) # , max_rise_gain=0.1) + + +def get_imu(data): + + rot_mat = np.array(data.body("base").xmat).reshape(3, 3) + gyro = R.from_matrix(rot_mat).as_euler("xyz") + + accelerometer = np.array(data.body("base").cvel)[3:] + + return gyro, accelerometer + + +def get_feet_contact(data): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +prev = data.time +try: + while True: + dt = data.time - prev + + if args.xbox_controller: + xbox_input() + + # Get sensor data + right_contact, left_contact = get_feet_contact(data) + gyro, accelerometer = get_imu(data) + + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ) + + angles = walk_engine.get_angles() + # apply the angles to the robot + data.ctrl[:] = list(angles.values()) + [0, 0] + + prev = data.time + mujoco.mj_step(model, data, 4) + viewer.sync() + # time.sleep(model.opt.timestep / 2.5) + +except KeyboardInterrupt: + viewer.close() diff --git a/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py b/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py new file mode 100644 index 0000000..ca9dc59 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py @@ -0,0 +1,297 @@ +import argparse +import pickle +import time + +import mujoco +import mujoco_viewer +import numpy as np +import pygame +from scipy.spatial.transform import Rotation as R + +from mini_bdx.onnx_infer import OnnxInfer +from mini_bdx.utils.rl_utils import ( + action_to_pd_targets, + isaac_to_mujoco, + mujoco_to_isaac, +) +from mini_bdx_runtime.rl_utils import LowPassActionFilter + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("-k", action="store_true", default=False) +parser.add_argument("--rma", action="store_true", default=False) +parser.add_argument("--awd", action="store_true", default=False) +parser.add_argument("--adaptation_module_path", type=str, required=False) +parser.add_argument("--replay_obs", type=str, required=False, default=None) +args = parser.parse_args() + +if args.k: + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + + +if args.replay_obs is not None: + path = args.replay_obs + path = path[: -len("obs.pkl")] + actions_path = path + "actions.pkl" + replay_obs = pickle.load(open(args.replay_obs, "rb")) + replay_actions = pickle.load(open(actions_path, "rb")) +replay_index = 0 + +# Params +dt = 0.0001 +linearVelocityScale = 2.0 if not args.awd else 0.5 +angularVelocityScale = 0.25 +dof_pos_scale = 1.0 +dof_vel_scale = 0.05 +action_clip = (-1, 1) +obs_clip = (-5, 5) +action_scale = 0.25 if not args.awd else 1.0 + + +isaac_init_pos = np.array( + [ + -0.0285397830292128, + 0.01626303761810685, + 1.0105624704499077, + -1.4865015965817336, + 0.6504953719748071, + -0.17453292519943295, + -0.17453292519943295, + 0, + 0, + 0, + 0.001171696610228082, + 0.006726989242258406, + 1.0129772861831692, + -1.4829304760981399, + 0.6444901047812701, + ] +) + + +mujoco_init_pos = np.array(isaac_to_mujoco(isaac_init_pos)) + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) +# model.opt.gravity[:] = [0, 0, 0] # no gravity + +NUM_OBS = 51 + +policy = OnnxInfer(args.onnx_model_path, awd=args.awd) +if args.rma: + adaptation_module = OnnxInfer(args.adaptation_module_path, "obs_history") + obs_history_size = 15 + obs_history = np.zeros((obs_history_size, NUM_OBS)).tolist() + + +class ObsDelaySimulator: + def __init__(self, delay_ms): + self.delay_ms = delay_ms + self.obs = [] + self.t0 = None + + def push(self, obs, t): + self.obs.append(obs) + if self.t0 is None: + self.t0 = t + + def get(self, t): + if t - self.t0 < self.delay_ms / 1000: + return np.zeros(NUM_OBS) + print(len(self.obs)) + return self.obs.pop(0) + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +def get_obs(data, prev_isaac_action, commands): + global replay_index + if args.replay_obs is not None: + obs = replay_obs[replay_index] + return obs + + base_lin_vel = ( + data.sensor("linear-velocity").data.astype(np.double) * linearVelocityScale + ) + + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + # # Remove yaw component + # rot_euler = R.from_quat(base_quat).as_euler("xyz", degrees=False) + # rot_euler[1] += np.deg2rad(-15) + # base_quat = R.from_euler("xyz", rot_euler, degrees=False).as_quat() + + base_ang_vel = ( + data.sensor("angular-velocity").data.astype(np.double) * angularVelocityScale + ) + + mujoco_dof_pos = data.qpos[7 : 7 + 15].copy() + isaac_dof_pos = mujoco_to_isaac(mujoco_dof_pos) + + isaac_dof_pos_scaled = (isaac_dof_pos - isaac_init_pos) * dof_pos_scale + + mujoco_dof_vel = data.qvel[6 : 6 + 15].copy() + isaac_dof_vel = mujoco_to_isaac(mujoco_dof_vel) + isaac_dof_vel_scaled = list(np.array(isaac_dof_vel) * dof_vel_scale) + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + if not args.awd: + obs = np.concatenate( + [ + projected_gravity, + commands, + isaac_dof_pos_scaled, + isaac_dof_vel_scaled, + prev_isaac_action, + ] + ) + else: + obs = np.concatenate( + [ + projected_gravity, + isaac_dof_pos, + isaac_dof_vel, + prev_isaac_action, + commands, + ] + ) + return obs + + +prev_isaac_action = np.zeros(15) +commands = [0.14 * 2, 0.0, 0.0] +prev = data.time +last_control = data.time +control_freq = 30 # hz +hist_freq = 30 # hz +adaptation_module_freq = 5 # hz +last_adaptation = data.time +last_hist = data.time +i = 0 +data.qpos[3 : 3 + 4] = [1, 0, 0.0, 0] +cutoff_frequency = 40 + + +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos + +action_filter = LowPassActionFilter( + control_freq=control_freq, cutoff_frequency=cutoff_frequency +) + +mujoco_saved_obs = [] +# obs_delay_simulator = ObsDelaySimulator(0) +start = time.time() +saved_latent = [] +try: + start = time.time() + while True: + # t = time.time() + t = data.time + if time.time() - start < 1: + last_control = t + if t - last_control >= 1 / control_freq: + isaac_obs = get_obs(data, prev_isaac_action, commands) + # obs_delay_simulator.push(isaac_obs, t) + # isaac_obs = obs_delay_simulator.get(t) + if args.rma: + if t - last_hist >= 1 / hist_freq: + obs_history.append(isaac_obs) + obs_history = obs_history[-obs_history_size:] + last_hist = t + + mujoco_saved_obs.append(isaac_obs) + + if args.rma: + if t - last_adaptation >= 1 / adaptation_module_freq: + latent = adaptation_module.infer(np.array(obs_history).flatten()) + last_adaptation = t + saved_latent.append(latent) + policy_input = np.concatenate([isaac_obs, latent]) + isaac_action = policy.infer(policy_input) + else: + isaac_action = policy.infer(isaac_obs) + + if args.replay_obs: + replayed_action = replay_actions[replay_index] + print("infered action", np.around(isaac_action, 3)) + print("replayed action", np.around(replayed_action, 3)) + print("diff", np.around(isaac_action - replayed_action, 3)) + print("==") + # action_filter.push(isaac_action) + # isaac_action = action_filter.get_filtered_action() + + prev_isaac_action = isaac_action.copy() # * action_scale + + isaac_action = isaac_action * action_scale + isaac_init_pos + + mujoco_action = isaac_to_mujoco(isaac_action) + + data.ctrl[:] = mujoco_action.copy() + + last_control = t + i += 1 + + if args.k: + keys = pygame.key.get_pressed() + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keys[pygame.K_z]: + lin_vel_x = 0.14 + if keys[pygame.K_s]: + lin_vel_x = -0.14 + if keys[pygame.K_q]: + lin_vel_y = 0.1 + if keys[pygame.K_d]: + lin_vel_y = -0.1 + if keys[pygame.K_a]: + ang_vel = 0.3 + if keys[pygame.K_e]: + ang_vel = -0.3 + + commands[0] = lin_vel_x + commands[1] = lin_vel_y + commands[2] = ang_vel + + commands = list( + np.array(commands) + * np.array( + [ + linearVelocityScale, + linearVelocityScale, + angularVelocityScale, + ] + ) + ) + pygame.event.pump() # process event queue + replay_index += 1 + print(commands) + mujoco.mj_step(model, data, 50) + + viewer.render() + prev = t +except KeyboardInterrupt: + pickle.dump(mujoco_saved_obs, open("mujoco_saved_obs.pkl", "wb")) + pickle.dump(saved_latent, open("mujoco_saved_latent.pkl", "wb")) diff --git a/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py b/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py new file mode 100644 index 0000000..59f0603 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py @@ -0,0 +1,278 @@ +import argparse +import pickle +import time + +import mujoco +import mujoco_viewer +import numpy as np +from scipy.spatial.transform import Rotation as R + +from mini_bdx.onnx_infer import OnnxInfer +from mini_bdx.utils.rl_utils import ( + action_to_pd_targets, + isaac_to_mujoco, + mujoco_to_isaac, +) + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("--saved_obs", type=str, required=False) +parser.add_argument("--saved_actions", type=str, required=False) +args = parser.parse_args() + +if args.saved_obs is not None: + saved_obs = pickle.loads(open("saved_obs.pkl", "rb").read()) +if args.saved_actions is not None: + saved_actions = pickle.loads(open("saved_actions.pkl", "rb").read()) + + +# Params +dt = 0.0001 +linearVelocityScale = 2.0 +angularVelocityScale = 0.25 +dof_pos_scale = 1.0 +dof_vel_scale = 0.05 +action_clip = (-1, 1) +obs_clip = (-5, 5) +action_scale = 1.0 + + +# mujoco_init_pos = np.array( +# [ +# # right_leg +# -0.014, +# 0.08, +# 0.53, +# -1.32, +# # -1.52, +# 0.91, +# # left leg +# 0.013, +# 0.077, +# 0.59, +# -1.33, +# # -1.53, +# 0.86, +# # head +# -0.17, +# -0.17, +# 0.0, +# 0.0, +# 0.0, +# ] +# ) + +# Higher +mujoco_init_pos = np.array( + [ + -0.03676731090962078, + -0.030315211140564333, + 0.4065815100399598, + -1.0864064934571644, + 0.5932324840794684, + -0.03485756878823724, + 0.052286054888550475, + 0.36623601032755765, + -0.964204465274923, + 0.5112970996901808, + -0.17453292519943295, + -0.17453292519943295, + 0, + 0.0, + 0.0, + ] +) + +pd_action_offset = [ + 0.0000, + -0.5672, + 0.5236, + 0.0000, + 0.0000, + -0.5672, + 0.0000, + 0.0000, + 0.4800, + -0.4800, + 0.0000, + -0.5672, + 0.5236, + 0.0000, + 0.0000, +] + +pd_action_scale = [ + 0.9774, + 1.4050, + 1.4661, + 2.9322, + 2.1991, + 1.0385, + 0.9774, + 2.9322, + 2.2602, + 2.2602, + 0.9774, + 1.4050, + 1.4661, + 2.9322, + 2.1991, +] + + +isaac_init_pos = np.array(mujoco_to_isaac(mujoco_init_pos)) + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +model.opt.timestep = dt +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) +# model.opt.gravity[:] = [0, 0, 0] # no gravity + +policy = OnnxInfer(args.onnx_model_path) + + +class ImuDelaySimulator: + def __init__(self, delay_ms): + self.delay_ms = delay_ms + self.rot = [] + self.ang_rot = [] + self.t0 = None + + def push(self, rot, ang_rot, t): + self.rot.append(rot) + self.ang_rot.append(ang_rot) + if self.t0 is None: + self.t0 = t + + def get(self): + if time.time() - self.t0 < self.delay_ms / 1000: + return [0, 0, 0, 0], [0, 0, 0] + rot = self.rot.pop(0) + ang_rot = self.ang_rot.pop(0) + + return rot, ang_rot + + +def get_obs(data, isaac_action, commands, imu_delay_simulator: ImuDelaySimulator): + base_lin_vel = ( + data.sensor("linear-velocity").data.astype(np.double) * linearVelocityScale + ) + + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + # Remove yaw component + rot_euler = R.from_quat(base_quat).as_euler("xyz", degrees=False) + rot_euler[2] = 0 + base_quat = R.from_euler("xyz", rot_euler, degrees=False).as_quat() + + base_ang_vel = ( + data.sensor("angular-velocity").data.astype(np.double) * angularVelocityScale + ) + + mujoco_dof_pos = data.qpos[7 : 7 + 15].copy() + isaac_dof_pos = mujoco_to_isaac(mujoco_dof_pos) + + isaac_dof_pos_scaled = (isaac_dof_pos - isaac_init_pos) * dof_pos_scale + + mujoco_dof_vel = data.qvel[6 : 6 + 15].copy() + isaac_dof_vel = mujoco_to_isaac(mujoco_dof_vel) + isaac_dof_vel_scaled = list(np.array(isaac_dof_vel) * dof_vel_scale) + + imu_delay_simulator.push(base_quat, base_ang_vel, time.time()) + base_quat, base_ang_vel = imu_delay_simulator.get() + + obs = np.concatenate( + [ + base_quat, + # base_lin_vel, + base_ang_vel, + isaac_dof_pos_scaled, + isaac_dof_vel_scaled, + isaac_action, + commands, + ] + ) + + return obs + + +prev_isaac_action = np.zeros(15) +commands = [0.15, 0.0, 0.0] +# commands = [0.0, 0.0, 0.0] +# prev = time.time() +# last_control = time.time() +prev = data.time +last_control = data.time +control_freq = 60 # hz +i = 0 +# data.qpos[3 : 3 + 4] = [1, 0, 0.08, 0] + +# init_rot = [0, -0.1, 0] +# init_rot = [0, 0, 0] +# init_quat = R.from_euler("xyz", init_rot, degrees=False).as_quat() +# data.qpos[3 : 3 + 4] = init_quat +# data.qpos[3 : 3 + 4] = [init_quat[3], init_quat[1], init_quat[2], init_quat[0]] +# data.qpos[3 : 3 + 4] = [1, 0, 0.13, 0] + + +data.qpos[7 : 7 + 15] = mujoco_init_pos +data.ctrl[:] = mujoco_init_pos + +mujoco_saved_obs = [] +mujoco_saved_actions = [] +command_value = [] +imu_delay_simulator = ImuDelaySimulator(1) +try: + start = time.time() + while True: + # t = time.time() + t = data.time + if t - last_control >= 1 / control_freq: + isaac_obs = get_obs(data, prev_isaac_action, commands, imu_delay_simulator) + mujoco_saved_obs.append(isaac_obs) + + if args.saved_obs is not None: + isaac_obs = saved_obs[i] # works with saved obs + + isaac_obs = np.clip(isaac_obs, obs_clip[0], obs_clip[1]) + + isaac_action = policy.infer(isaac_obs) + if args.saved_actions is not None: + isaac_action = saved_actions[i][0] + isaac_action = np.clip(isaac_action, action_clip[0], action_clip[1]) + prev_isaac_action = isaac_action.copy() + + # isaac_action = action_to_pd_targets( + # isaac_action, pd_action_offset, pd_action_scale + # ) + + isaac_action = isaac_init_pos + isaac_action + + mujoco_action = isaac_to_mujoco(isaac_action) + + last_control = t + i += 1 + + data.ctrl[:] = mujoco_action.copy() + # data.ctrl[:] = mujoco_init_pos + # euler_rot = [np.sin(2 * np.pi * 0.5 * t), 0, 0] + # quat = R.from_euler("xyz", euler_rot, degrees=False).as_quat() + # data.qpos[3 : 3 + 4] = quat + mujoco_saved_actions.append(mujoco_action) + + command_value.append([data.ctrl.copy(), data.qpos[7:].copy()]) + mujoco.mj_step(model, data, 50) + + viewer.render() + prev = t +except KeyboardInterrupt: + data = { + "config": {}, + "mujoco": command_value, + } + pickle.dump(data, open("mujoco_command_value.pkl", "wb")) + pickle.dump(mujoco_saved_obs, open("mujoco_saved_obs.pkl", "wb")) + pickle.dump(mujoco_saved_actions, open("mujoco_saved_actions.pkl", "wb")) diff --git a/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py b/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py new file mode 100644 index 0000000..fe445b7 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py @@ -0,0 +1,15 @@ +import pickle +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument( + "--latent", type=str, required=False, default="mujoco_saved_latent.pkl" +) +args = parser.parse_args() + +latent = pickle.load(open(args.latent, "rb")) + +import matplotlib.pyplot as plt + +plt.plot(latent) +plt.show() diff --git a/Open_Duck_Mini-2/experiments/placo/.gitignore b/Open_Duck_Mini-2/experiments/placo/.gitignore new file mode 100644 index 0000000..8587f59 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/.gitignore @@ -0,0 +1,2 @@ +test/ +placo_walk_examples/ \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py b/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py new file mode 100644 index 0000000..da2b1fb --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py @@ -0,0 +1,45 @@ +import os +import argparse +from placo_record_amp import record +from mini_bdx.placo_walk_engine import PlacoWalkEngine +import numpy as np + +import argparse + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--output_dir", type=str, default="recordings") +parser.add_argument( + "-n", "--num_samples", type=int, default=10, help="Number of samples" +) +args = parser.parse_args() + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True) + +dx_range = [-0.04, 0.04] +dy_range = [-0.05, 0.05] +dtheta_range = [-0.15, 0.15] +length = 8 +num_samples = args.num_samples + +args_dict = {} +args_dict["name"] = "test" +args_dict["dx"] = 0 +args_dict["dy"] = 0 +args_dict["dtheta"] = 0 +args_dict["length"] = length +args_dict["meshcat_viz"] = False +args_dict["skip_warmup"] = False +args_dict["stand"] = False +args_dict["hardware"] = True +args_dict["output_dir"] = args.output_dir + +for i in range(num_samples): + args_dict["dx"] = round(np.random.uniform(dx_range[0], dx_range[1]), 2) + args_dict["dy"] = round(np.random.uniform(dy_range[0], dy_range[1]), 2) + args_dict["dtheta"] = round(np.random.uniform(dtheta_range[0], dtheta_range[1]), 2) + args_dict["name"] = str(i) + print("RECORDING ", args_dict["name"]) + print("dx", args_dict["dx"], "dy", args_dict["dy"], "dtheta", args_dict["dtheta"]) + print("==") + pwe.reset() + record(pwe, args_dict) diff --git a/Open_Duck_Mini-2/experiments/placo/bdx_walk.py b/Open_Duck_Mini-2/experiments/placo/bdx_walk.py new file mode 100644 index 0000000..5391892 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/bdx_walk.py @@ -0,0 +1,322 @@ +import argparse +import time +import warnings + +import numpy as np +import placo +from placo_utils.visualization import ( + footsteps_viz, + frame_viz, + line_viz, + robot_frame_viz, + robot_viz, +) + +from mini_bdx.bdx_mujoco_server import BDXMujocoServer +from mini_bdx.hwi import HWI + +warnings.filterwarnings("ignore") + +parser = argparse.ArgumentParser(description="Process some integers.") +parser.add_argument( + "-p", "--pybullet", action="store_true", help="PyBullet visualization" +) +parser.add_argument( + "-m", "--meshcat", action="store_true", help="MeshCat visualization" +) +parser.add_argument("-r", "--robot", action="store_true", help="run on real robot") +parser.add_argument("--mujoco", action="store_true", help="Mujoco visualization") +args = parser.parse_args() + +DT = 0.01 +REFINE = 10 +model_filename = "../../mini_bdx/robots/bdx/robot.urdf" + +# Loading the robot +robot = placo.HumanoidRobot(model_filename) +robot.set_joint_limits("left_knee", -2, -0.01) +robot.set_joint_limits("right_knee", -2, -0.01) + +# Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency +parameters = placo.HumanoidParameters() + +parameters.double_support_ratio = 0.2 # Ratio of double support (0.0 to 1.0) +parameters.startend_double_support_ratio = ( + 1.5 # Ratio duration of supports for starting and stopping walk +) +parameters.planned_timesteps = 48 # Number of timesteps planned ahead +parameters.replan_timesteps = 10 # Replanning each n timesteps +# parameters.zmp_reference_weight = 1e-6 + +# Posture parameters +parameters.walk_com_height = 0.15 # Constant height for the CoM [m] +parameters.walk_foot_height = 0.006 # Height of foot rising while walking [m] +parameters.walk_trunk_pitch = np.deg2rad(10) # Trunk pitch angle [rad] +parameters.walk_foot_rise_ratio = ( + 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0) +) + +# Timing parameters +# parameters.single_support_duration = 1 / ( +# 0.5 * np.sqrt(9.81 / parameters.walk_com_height) +# ) # Constant height for the CoM [m] # Duration of single support phase [s] +parameters.single_support_duration = 0.2 # Duration of single support phase [s] +parameters.single_support_timesteps = ( + 10 # Number of planning timesteps per single support phase +) + +# Feet parameters +parameters.foot_length = 0.06 # Foot length [m] +parameters.foot_width = 0.006 # Foot width [m] +parameters.feet_spacing = 0.12 # Lateral feet spacing [m] +parameters.zmp_margin = 0.0 # ZMP margin [m] +parameters.foot_zmp_target_x = 0.0 # Reference target ZMP position in the foot [m] +parameters.foot_zmp_target_y = 0.0 # Reference target ZMP position in the foot [m] + +# Limit parameters +parameters.walk_max_dtheta = 1 # Maximum dtheta per step [rad] +parameters.walk_max_dy = 0.1 # Maximum dy per step [m] +parameters.walk_max_dx_forward = 0.08 # Maximum dx per step forward [m] +parameters.walk_max_dx_backward = 0.03 # Maximum dx per step backward [m] + +# Creating the kinematics solver +solver = placo.KinematicsSolver(robot) +solver.enable_velocity_limits(True) +# solver.enable_joint_limits(False) +robot.set_velocity_limits(12.0) +solver.dt = DT / REFINE + +# Creating the walk QP tasks +tasks = placo.WalkTasks() +# tasks.trunk_mode = True +# tasks.com_x = 0.04 +tasks.initialize_tasks(solver, robot) +tasks.left_foot_task.orientation().mask.set_axises("yz", "local") +tasks.right_foot_task.orientation().mask.set_axises("yz", "local") +# tasks.trunk_orientation_task.configure("trunk_orientation", "soft", 1e-4) +# tasks.left_foot_task.orientation().configure("left_foot_orientation", "soft", 1e-6) +# tasks.right_foot_task.orientation().configure("right_foot_orientation", "soft", 1e-6) + +# Creating a joint task to assign DoF values for upper body +elbow = -50 * np.pi / 180 +shoulder_roll = 0 * np.pi / 180 +shoulder_pitch = 20 * np.pi / 180 +joints_task = solver.add_joints_task() +joints_task.set_joints( + { + # "left_shoulder_roll": shoulder_roll, + # "left_shoulder_pitch": shoulder_pitch, + # "left_elbow": elbow, + # "right_shoulder_roll": -shoulder_roll, + # "right_shoulder_pitch": shoulder_pitch, + # "right_elbow": elbow, + "head_pitch": 0.0, + "head_yaw": 0.0, + "neck_pitch": 0.0, + "left_antenna": 0.0, + "right_antenna": 0.0, + # "left_ankle_roll": 0.0, + # "right_ankle_roll": 0.0 + } +) +joints_task.configure("joints", "soft", 1.0) + +# cam = solver.add_centroidal_momentum_task(np.array([0., 0., 0.])) +# cam.mask.set_axises("x", "custom") +# cam.mask.R_custom_world = robot.get_T_world_frame("trunk")[:3, :3].T +# cam.configure("cam", "soft", 1e-3) + +# Placing the robot in the initial position +print("Placing the robot in the initial position...") +tasks.reach_initial_pose( + np.eye(4), + parameters.feet_spacing, + parameters.walk_com_height, + parameters.walk_trunk_pitch, +) +print("Initial position reached") + + +# Creating the FootstepsPlanner +repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive(parameters) +d_x = 0.1 +d_y = 0.0 +d_theta = 0.0 +nb_steps = 5 +repetitive_footsteps_planner.configure(d_x, d_y, d_theta, nb_steps) + +# Planning footsteps +T_world_left = placo.flatten_on_floor(robot.get_T_world_left()) +T_world_right = placo.flatten_on_floor(robot.get_T_world_right()) +footsteps = repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, T_world_left, T_world_right +) + +supports = placo.FootstepsPlanner.make_supports( + footsteps, True, parameters.has_double_support(), True +) + +# Creating the pattern generator and making an initial plan +walk = placo.WalkPatternGenerator(robot, parameters) +trajectory = walk.plan(supports, robot.com_world(), 0.0) + +if args.pybullet: + # Loading the PyBullet simulation + import pybullet as p + from onshape_to_robot.simulation import Simulation + + sim = Simulation(model_filename, realTime=True, dt=DT, ignore_self_collisions=True) +elif args.meshcat: + # Starting Meshcat viewer + viz = robot_viz(robot) + footsteps_viz(trajectory.get_supports()) +elif args.robot: + hwi = HWI() + hwi.turn_on() + time.sleep(2) +elif args.mujoco: + time_since_last_right_contact = 0.0 + time_since_last_left_contact = 0.0 + bdx_mujoco_server = BDXMujocoServer( + model_path="../../mini_bdx/robots/bdx", gravity_on=True + ) + bdx_mujoco_server.start() +else: + print("No visualization selected, use either -p or -m") + exit() + +# Timestamps +start_t = time.time() +initial_delay = -1.0 +t = initial_delay +last_display = time.time() +last_replan = 0 +petage_de_gueule = False +got_input = False +while True: + if got_input: + repetitive_footsteps_planner.configure(d_x, d_y, d_theta, nb_steps) + got_input = False + + # Invoking the IK QP solver + for k in range(REFINE): + # Updating the QP tasks from planned trajectory + if not petage_de_gueule: + tasks.update_tasks_from_trajectory(trajectory, t - DT + k * DT / REFINE) + + robot.update_kinematics() + qd_sol = solver.solve(True) + # solver.dump_status() + + # Ensuring the robot is kinematically placed on the floor on the proper foot to avoid integration drifts + # if not trajectory.support_is_both(t): + # robot.update_support_side(str(trajectory.support_side(t))) + # robot.ensure_on_floor() + + # If enough time elapsed and we can replan, do the replanning + if ( + t - last_replan > parameters.replan_timesteps * parameters.dt() + and walk.can_replan_supports(trajectory, t) + ): + last_replan = t + + # Replanning footsteps from current trajectory + supports = walk.replan_supports(repetitive_footsteps_planner, trajectory, t) + + # Replanning CoM trajectory, yielding a new trajectory we can switch to + trajectory = walk.replan(supports, trajectory, t) + + if args.meshcat: + # Drawing footsteps + footsteps_viz(supports) + + # Drawing planned CoM trajectory on the ground + coms = [ + [*trajectory.get_p_world_CoM(t)[:2], 0.0] + for t in np.linspace(trajectory.t_start, trajectory.t_end, 100) + ] + line_viz("CoM_trajectory", np.array(coms), 0xFFAA00) + + # During the warmup phase, the robot is enforced to stay in the initial position + if args.pybullet: + if t < -2: + T_left_origin = sim.transformation("origin", "left_foot_frame") + T_world_left = sim.poseToMatrix(([0.0, 0.0, 0.05], [0.0, 0.0, 0.0, 1.0])) + T_world_origin = T_world_left @ T_left_origin + + sim.setRobotPose(*sim.matrixToPose(T_world_origin)) + + joints = {joint: robot.get_joint(joint) for joint in sim.getJoints()} + applied = sim.setJoints(joints) + sim.tick() + + # Updating meshcat display periodically + elif args.meshcat: + if time.time() - last_display > 0.01: + last_display = time.time() + viz.display(robot.state.q) + + # frame_viz("left_foot_target", trajectory.get_T_world_left(t)) + # frame_viz("right_foot_target", trajectory.get_T_world_right(t)) + # robot_frame_viz(robot, "left_foot") + # robot_frame_viz(robot, "right_foot") + + T_world_trunk = np.eye(4) + T_world_trunk[:3, :3] = trajectory.get_R_world_trunk(t) + T_world_trunk[:3, 3] = trajectory.get_p_world_CoM(t) + frame_viz("trunk_target", T_world_trunk) + # footsteps_viz(trajectory.get_supports()) + + if args.robot or args.mujoco: + angles = { + "right_hip_yaw": robot.get_joint("right_hip_yaw"), + "right_hip_roll": robot.get_joint("right_hip_roll"), + "right_hip_pitch": robot.get_joint("right_hip_pitch"), + "right_knee": robot.get_joint("right_knee"), + "right_ankle": robot.get_joint("right_ankle"), + "left_hip_yaw": robot.get_joint("left_hip_yaw"), + "left_hip_roll": robot.get_joint("left_hip_roll"), + "left_hip_pitch": robot.get_joint("left_hip_pitch"), + "left_knee": robot.get_joint("left_knee"), + "left_ankle": robot.get_joint("left_ankle"), + "neck_pitch": robot.get_joint("neck_pitch"), + "head_pitch": robot.get_joint("head_pitch"), + "head_yaw": robot.get_joint("head_yaw"), + } + if args.robot: + hwi.set_position_all(angles) + elif args.mujoco: + right_contact, left_contact = bdx_mujoco_server.get_feet_contact() + if left_contact: + time_since_last_left_contact = 0.0 + if right_contact: + time_since_last_right_contact = 0.0 + # print("time since last left contact :", time_since_last_left_contact) + # print("time since last right contact :", time_since_last_right_contact) + bdx_mujoco_server.send_action(list(angles.values())) + + if ( + time_since_last_left_contact > parameters.single_support_duration + or time_since_last_right_contact > parameters.single_support_duration + ): + petage_de_gueule = True + # print("pétage de gueule") + else: + petage_de_gueule = False + + time_since_last_left_contact += DT + time_since_last_right_contact += DT + + if bdx_mujoco_server.key_pressed is not None: + got_input = True + d_x = 0.05 + else: + got_input = True + d_x = 0 + d_y = 0 + d_theta = 0 + + t += DT + # print(t) + while time.time() < start_t + t: + time.sleep(1e-5) diff --git a/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py b/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py new file mode 100644 index 0000000..eea7eeb --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py @@ -0,0 +1,56 @@ +import argparse +import json +import os + +import FramesViewer.utils as fv_utils +import numpy as np +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-f", "--file", type=str, required=True) +parser.add_argument("-o", "--output_dir", type=str, required=True) +args = parser.parse_args() + +os.makedirs(args.output_dir, exist_ok=True) + +episode = json.load(open(args.file)) + +frame_duration = episode["FrameDuration"] + +frames = episode["Frames"] + +step = 5 +yaw_orientations = np.arange(360, step=step) - (180 - step) +# print(yaw_orientations) +# yaw_orientations = [180] + +for yaw_orientation in yaw_orientations: + new_frames = [] + for i, frame in enumerate(frames): + root_position = frame[:3] # x, y, z + root_orientation_quat = frame[3:7] # quat + root_orientation_mat = R.from_quat(root_orientation_quat).as_matrix() + # rotate around z axis + root_orientation_mat = ( + R.from_euler("z", yaw_orientation, degrees=True).as_matrix() + @ root_orientation_mat + ) + root_orientation_quat = R.from_matrix(root_orientation_mat).as_quat() + + # rotate root position too around z at origin + root_position = ( + R.from_euler("z", yaw_orientation, degrees=True).as_matrix() @ root_position + ) + + new_frames.append(list(root_position) + list(root_orientation_quat) + frame[7:]) + + new_episode = episode.copy() + new_episode["Frames"] = new_frames + + output_file = os.path.join( + args.output_dir, + os.path.basename(args.file).replace(".txt", f"_{yaw_orientation}.txt"), + ) + + with open(output_file, "w") as f: + json.dump(new_episode, f) diff --git a/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py b/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py new file mode 100644 index 0000000..62bcabe --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py @@ -0,0 +1,223 @@ +import argparse +import json +import os +import time +from os.path import join +from threading import current_thread + +import numpy as np +from numpy.ma.extras import average +import placo +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz +from scipy.spatial.transform import Rotation as R + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.rl_utils import mujoco_to_isaac, test + +FPS = 60 +MESHCAT_FPS = 20 + +# For IsaacGymEnvs +# [root position, root orientation, joint poses (e.g. rotations)] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15] + + +# For amp for hardware +# [root position, root orientation, joint poses (e.g. rotations), target toe positions, linear velocity, angular velocity, joint velocities, target toe velocities] +# [x, y, z, qw, qx, qy, qz, j1, j2, j3, j4, j5, j6, j7, j8, j9, j10, j11, j12, j13, j14, j15, l_toe_x, l_toe_y, l_toe_z, r_toe_x, r_toe_y, r_toe_z, lin_vel_x, lin_vel_y, lin_vel_z, ang_vel_x, ang_vel_y, ang_vel_z, j1_vel, j2_vel, j3_vel, j4_vel, j5_vel, j6_vel, j7_vel, j8_vel, j9_vel, j10_vel, j11_vel, j12_vel, j13_vel, j14_vel, j15_vel, l_toe_vel_x, l_toe_vel_y, l_toe_vel_z, r_toe_vel_x, r_toe_vel_y, r_toe_vel_z] +def record(pwe, args_dict): + episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Debug_info": [], + "Frames": [], + "MotionWeight": 1, + } + + first_joints_positions = list(pwe.get_angles().values()) + first_T_world_fbase = pwe.robot.get_T_world_fbase() + first_T_world_leftFoot = pwe.robot.get_T_world_left() + first_T_world_rightFoot = pwe.robot.get_T_world_right() + + # pwe.parameters.single_support_duration = 0.25 # slow + # pwe.parameters.single_support_duration = 0.20 # normal + pwe.parameters.single_support_duration = 0.2 # Fast ? + + pwe.set_traj(args_dict["dx"], args_dict["dy"], args_dict["dtheta"] + 0.001) + if args_dict["meshcat_viz"]: + viz = robot_viz(pwe.robot) + DT = 0.001 + start = time.time() + + last_record = 0 + last_meshcat_display = 0 + prev_root_position = [0, 0, 0] + prev_root_orientation_euler = [0, 0, 0] + prev_left_toe_pos = [0, 0, 0] + prev_right_toe_pos = [0, 0, 0] + prev_joints_positions = [0] * 15 + i = 0 + prev_initialized = False + avg_x_lin_vel = [] + avg_yaw_vel = [] + while True: + # print("t", pwe.t) + pwe.tick(DT) + if pwe.t <= 0 + args_dict["skip_warmup"] * 1: + start = pwe.t + last_record = pwe.t + 1 / FPS + last_meshcat_display = pwe.t + 1 / MESHCAT_FPS + continue + + # print(np.around(pwe.robot.get_T_world_fbase()[:3, 3], 3)) + + if pwe.t - last_record >= 1 / FPS: + if args_dict["stand"]: + T_world_fbase = first_T_world_fbase + else: + T_world_fbase = pwe.robot.get_T_world_fbase() + root_position = list(T_world_fbase[:3, 3]) + root_orientation_quat = list(R.from_matrix(T_world_fbase[:3, :3]).as_quat()) + + if args_dict["stand"]: + joints_positions = first_joints_positions + else: + joints_positions = list(pwe.get_angles().values()) + + if args_dict["stand"]: + T_world_leftFoot = first_T_world_leftFoot + T_world_rightFoot = first_T_world_rightFoot + else: + T_world_leftFoot = pwe.robot.get_T_world_left() + T_world_rightFoot = pwe.robot.get_T_world_right() + + T_body_leftFoot = np.linalg.inv(T_world_fbase) @ T_world_leftFoot + T_body_rightFoot = np.linalg.inv(T_world_fbase) @ T_world_rightFoot + + left_toe_pos = list(T_body_leftFoot[:3, 3]) + right_toe_pos = list(T_body_rightFoot[:3, 3]) + + world_linear_vel = list( + (np.array(root_position) - np.array(prev_root_position)) / (1 / FPS) + ) + avg_x_lin_vel.append(world_linear_vel[0]) + body_rot_mat = T_world_fbase[:3, :3] + body_linear_vel = list(body_rot_mat.T @ world_linear_vel) + + world_angular_vel = list( + ( + R.from_quat(root_orientation_quat).as_euler("xyz") + - prev_root_orientation_euler + ) + / (1 / FPS) + ) + avg_yaw_vel.append(world_angular_vel[2]) + body_angular_vel = list(body_rot_mat.T @ world_angular_vel) + # print("world angular vel", world_angular_vel) + # print("body angular vel", body_angular_vel) + + joints_vel = list( + (np.array(joints_positions) - np.array(prev_joints_positions)) + / (1 / FPS) + ) + left_toe_vel = list( + (np.array(left_toe_pos) - np.array(prev_left_toe_pos)) / (1 / FPS) + ) + right_toe_vel = list( + (np.array(right_toe_pos) - np.array(prev_right_toe_pos)) / (1 / FPS) + ) + + if prev_initialized: + if args_dict["hardware"]: + episode["Frames"].append( + root_position + + root_orientation_quat + + joints_positions + + left_toe_pos + + right_toe_pos + + world_linear_vel + + world_angular_vel + + joints_vel + + left_toe_vel + + right_toe_vel + ) + else: + episode["Frames"].append( + root_position + root_orientation_quat + joints_positions + ) + episode["Debug_info"].append( + { + "left_foot_pose": list(T_world_leftFoot.flatten()), + "right_foot_pose": list(T_world_rightFoot.flatten()), + } + ) + + prev_root_position = root_position.copy() + prev_root_orientation_euler = ( + R.from_quat(root_orientation_quat).as_euler("xyz").copy() + ) + prev_left_toe_pos = left_toe_pos.copy() + prev_right_toe_pos = right_toe_pos.copy() + prev_joints_positions = joints_positions.copy() + prev_initialized = True + + # print("avg x vel", np.mean(avg_x_lin_vel[-50:])) + # print("avg yaw vel", np.mean(avg_yaw_vel[-50:])) + # print("=") + + last_record = pwe.t + # print("saved frame") + + if args_dict["meshcat_viz"] and pwe.t - last_meshcat_display >= 1 / MESHCAT_FPS: + last_meshcat_display = pwe.t + viz.display(pwe.robot.state.q) + footsteps_viz(pwe.trajectory.get_supports()) + + if pwe.t - start > args_dict["length"]: + break + + i += 1 + + print("recorded", len(episode["Frames"]), "frames") + file_name = args_dict["name"] + str(".txt") + file_path = os.path.join(args_dict["output_dir"], file_name) + os.makedirs(args_dict["output_dir"], exist_ok=True) + print("DONE, saving", file_name) + with open(file_path, "w") as f: + json.dump(episode, f) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("-n", "--name", type=str, required=True) + parser.add_argument("-o", "--output_dir", type=str, default="recordings") + parser.add_argument("--dx", type=float, required=True) + parser.add_argument("--dy", type=float, required=True) + parser.add_argument("--dtheta", type=float, required=True) + parser.add_argument("-l", "--length", type=int, default=10) + parser.add_argument("-m", "--meshcat_viz", action="store_true", default=False) + parser.add_argument( + "-s", + "--skip_warmup", + action="store_true", + default=False, + help="don't record warmup motion", + ) + parser.add_argument( + "--stand", + action="store_true", + default=False, + help="hack to record a standing pose", + ) + parser.add_argument( + "--hardware", + action="store_true", + help="use AMP_for_hardware format. If false, use IsaacGymEnvs format", + ) + args = parser.parse_args() + pwe = PlacoWalkEngine( + "../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True + ) + record(pwe, vars(args)) diff --git a/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py b/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py new file mode 100644 index 0000000..56cfb7a --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py @@ -0,0 +1,178 @@ +import time +import json +import os +import warnings +from placo_utils.visualization import robot_viz +import numpy as np +import placo +import FramesViewer.utils as fv_utils +from scipy.spatial.transform import Rotation as R + +FPS = 60 + +episode = { + "LoopMode": "Wrap", + "FrameDuration": np.around(1 / FPS, 4), + "EnableCycleOffsetPosition": True, + "EnableCycleOffsetRotation": False, + "Debug_info": [], + "Frames": [], + "MotionWeight": 1, +} + +DT = 0.01 +REFINE = 10 +MESHCAT_FPS = 20 +robot = placo.HumanoidRobot("../../mini_bdx/robots/bdx/robot.urdf") +solver = placo.KinematicsSolver(robot) +# viz = robot_viz(robot) + +robot.set_joint_limits("left_knee", -2, -0.01) +robot.set_joint_limits("right_knee", -2, -0.01) + +T_world_trunk = np.eye(4) +T_world_trunk[:3, 3] = [0, 0, 0.175] +T_world_trunk = fv_utils.rotateInSelf(T_world_trunk, [0, 2, 0]) +trunk_task = solver.add_frame_task("trunk", T_world_trunk) +trunk_task.configure("trunk", "hard") + +T_trunk_leftFoot = np.eye(4) +T_trunk_leftFoot[:3, 3] = [-0.03, 0.06, -0.17] +T_world_leftFoot = T_world_trunk @ T_trunk_leftFoot +T_world_leftFoot = placo.flatten_on_floor(T_world_leftFoot) +left_foot_task = solver.add_frame_task("left_foot_frame", T_world_leftFoot) +left_foot_task.configure("left_foot_frame", "soft", 1.0) + +T_trunk_rightFoot = np.eye(4) +T_trunk_rightFoot[:3, 3] = [-0.03, -0.06, -0.17] +T_world_rightFoot = T_world_trunk @ T_trunk_rightFoot +T_world_rightFoot = placo.flatten_on_floor(T_world_rightFoot) +right_foot_task = solver.add_frame_task("right_foot_frame", T_world_rightFoot) +right_foot_task.configure("right_foot_frame", "soft", 1.0) + + +left_foot_task.orientation().mask.set_axises("yz", "local") +right_foot_task.orientation().mask.set_axises("yz", "local") + + +def get_angles(): + angles = { + "left_hip_yaw": robot.get_joint("left_hip_yaw"), + "left_hip_roll": robot.get_joint("left_hip_roll"), + "left_hip_pitch": robot.get_joint("left_hip_pitch"), + "left_knee": robot.get_joint("left_knee"), + "left_ankle": robot.get_joint("left_ankle"), + "neck_pitch": robot.get_joint("neck_pitch"), + "head_pitch": robot.get_joint("head_pitch"), + "head_yaw": robot.get_joint("head_yaw"), + "left_antenna": robot.get_joint("left_antenna"), + "right_antenna": robot.get_joint("right_antenna"), + "right_hip_yaw": robot.get_joint("right_hip_yaw"), + "right_hip_roll": robot.get_joint("right_hip_roll"), + "right_hip_pitch": robot.get_joint("right_hip_pitch"), + "right_knee": robot.get_joint("right_knee"), + "right_ankle": robot.get_joint("right_ankle"), + } + + return angles + + +last_meshcat_display = 0 +last_record = 0 +prev_root_position = [0, 0, 0] +prev_root_orientation_euler = [0, 0, 0] +prev_left_toe_pos = [0, 0, 0] +prev_right_toe_pos = [0, 0, 0] +prev_joints_positions = [0] * 15 +prev_initialized = False + +t = 0 +start = 0 +while True: + # if t - last_meshcat_display > 1 / MESHCAT_FPS: + # last_meshcat_display = t + # viz.display(robot.state.q) + + trunk_task.T_world_frame = fv_utils.translateInSelf( + T_world_trunk, [0, 0.01 * np.sin(2 * t), 0.01 * np.sin(3 * t)] + ) + + if t - last_record >= 1 / FPS: + T_world_fbase = robot.get_T_world_fbase() + root_position = list(T_world_fbase[:3, 3]) + root_orientation_quat = list(R.from_matrix(T_world_fbase[:3, :3]).as_quat()) + joints_positions = list(get_angles().values()) + + T_world_leftFoot = robot.get_T_world_left() + T_world_rightFoot = robot.get_T_world_right() + + T_body_leftFoot = np.linalg.inv(T_world_fbase) @ T_world_leftFoot + T_body_rightFoot = np.linalg.inv(T_world_fbase) @ T_world_rightFoot + + left_toe_pos = list(T_body_leftFoot[:3, 3]) + right_toe_pos = list(T_body_rightFoot[:3, 3]) + + world_linear_vel = list( + (np.array(root_position) - np.array(prev_root_position)) / (1 / FPS) + ) + + world_angular_vel = list( + ( + R.from_quat(root_orientation_quat).as_euler("xyz") + - prev_root_orientation_euler + ) + / (1 / FPS) + ) + + joints_vel = list( + (np.array(joints_positions) - np.array(prev_joints_positions)) / (1 / FPS) + ) + left_toe_vel = list( + (np.array(left_toe_pos) - np.array(prev_left_toe_pos)) / (1 / FPS) + ) + right_toe_vel = list( + (np.array(right_toe_pos) - np.array(prev_right_toe_pos)) / (1 / FPS) + ) + + if prev_initialized: + episode["Frames"].append( + root_position + + root_orientation_quat + + joints_positions + + left_toe_pos + + right_toe_pos + + world_linear_vel + + world_angular_vel + + joints_vel + + left_toe_vel + + right_toe_vel + ) + + episode["Debug_info"].append( + { + "left_foot_pose": list(T_world_leftFoot.flatten()), + "right_foot_pose": list(T_world_rightFoot.flatten()), + } + ) + + prev_root_position = root_position.copy() + prev_root_orientation_euler = ( + R.from_quat(root_orientation_quat).as_euler("xyz").copy() + ) + prev_left_toe_pos = left_toe_pos.copy() + prev_right_toe_pos = right_toe_pos.copy() + prev_joints_positions = joints_positions.copy() + prev_initialized = True + last_record = t + if t - start >= 10: + break + + robot.update_kinematics() + _ = solver.solve(True) + t += DT + +print("recorded", len(episode["Frames"]), "frames") +file_path = "wiggle" + str(".txt") +print("DONE, saving", file_path) +with open(file_path, "w") as f: + json.dump(episode, f) diff --git a/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py b/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py new file mode 100644 index 0000000..6a8aee3 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py @@ -0,0 +1,114 @@ +import argparse + +import mujoco +import mujoco.viewer +import numpy as np + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.mujoco_utils import check_contact +from mini_bdx.utils.xbox_controller import XboxController + +parser = argparse.ArgumentParser() +parser.add_argument("-x", action="store_true", default=False) +args = parser.parse_args() + +if args.x: + xbox = XboxController() + +d_x = 0 +d_y = 0 +d_theta = 0 + + +# TODO placo mistakes the antennas for leg joints ? +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf") + + +def xbox_input(): + global d_x, d_y, d_theta + inputs = xbox.read() + + d_x = -inputs["l_y"] * pwe.parameters.walk_max_dx_forward / 2 + d_y = -inputs["l_x"] * pwe.parameters.walk_max_dy / 3 + d_theta = -inputs["r_x"] * pwe.parameters.walk_max_dtheta / 3 + + print(d_x, d_y, d_theta) + + +def key_callback(keycode): + global d_x, d_y, d_theta + if keycode == 265: # up + d_x = 0.05 + # max_target_step_size_x += 0.005 + # if keycode == 264: # down + # max_target_step_size_x -= 0.005 + # if keycode == 263: # left + # max_target_step_size_y -= 0.005 + # if keycode == 262: # right + # max_target_step_size_y += 0.005 + # if keycode == 81: # a + # max_target_yaw += np.deg2rad(1) + # if keycode == 69: # e + + +model = mujoco.MjModel.from_xml_path("../../mini_bdx/robots/bdx/scene.xml") +data = mujoco.MjData(model) +viewer = mujoco.viewer.launch_passive(model, data, key_callback=key_callback) + + +def get_feet_contact(): + right_contact = check_contact(data, model, "foot_module", "floor") + left_contact = check_contact(data, model, "foot_module_2", "floor") + return right_contact, left_contact + + +# joints_velocities_min = 1000 +# joints_velocities_max = -1000 +# angular_velocity_min = 1000 +# angular_velocity_max = -1000 +# linear_velocity_min = 1000 +# linear_velocity_max = -1000 +speed = 4 # 1 is slowest, 3 looks real time on my machine +prev = data.time +while True: + t = data.time + dt = t - prev + + if args.x: + xbox_input() + + pwe.d_x = d_x + pwe.d_y = d_y + pwe.d_theta = d_theta + right_contact, left_contact = get_feet_contact() + pwe.tick(dt, left_contact, right_contact) + + angles = pwe.get_angles() + data.ctrl[:] = list(angles.values()) + + # joints_velocities = data.qvel[6 : 6 + 15] + # angular_velocity = data.body("base").cvel[:3] + # linear_velocity = data.body("base").cvel[3:] + # # print(np.around(joints_velocities, 3)) + # if np.min(joints_velocities) < joints_velocities_min: + # joints_velocities_min = np.min(joints_velocities) + # if np.max(joints_velocities) > joints_velocities_max: + # joints_velocities_max = np.max(joints_velocities) + + # if np.min(angular_velocity) < angular_velocity_min: + # angular_velocity_min = np.min(angular_velocity) + # if np.max(angular_velocity) > angular_velocity_max: + # angular_velocity_max = np.max(angular_velocity) + + # if np.min(linear_velocity) < linear_velocity_min: + # linear_velocity_min = np.min(linear_velocity) + # if np.max(linear_velocity) > linear_velocity_max: + # linear_velocity_max = np.max(linear_velocity) + + # print("joints velocities amplitude", joints_velocities_min, joints_velocities_max) + # print("angular velocity amplitude", angular_velocity_min, angular_velocity_max) + # print("linear velocity amplitude", linear_velocity_min, linear_velocity_max) + + mujoco.mj_step(model, data, speed) # 4 seems good + viewer.sync() + prev = t diff --git a/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py b/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py new file mode 100644 index 0000000..5c12b53 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py @@ -0,0 +1,51 @@ +import argparse +import time + +import numpy as np +import placo +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz + +from mini_bdx.placo_walk_engine import PlacoWalkEngine +from mini_bdx.utils.xbox_controller import XboxController + +parser = argparse.ArgumentParser() +parser.add_argument("-x", action="store_true", default=False) +args = parser.parse_args() + +if args.x: + xbox = XboxController() + +d_x = 0.05 +d_y = 0 +d_theta = 0.2 + + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True) + +pwe.set_traj(d_x, d_y, d_theta) +viz = robot_viz(pwe.robot) + + +def xbox_input(): + global d_x, d_y, d_theta + inputs = xbox.read() + + d_x = -inputs["l_y"] * pwe.parameters.walk_max_dx_forward / 2 + d_y = -inputs["l_x"] * pwe.parameters.walk_max_dy / 3 + d_theta = -inputs["r_x"] * pwe.parameters.walk_max_dtheta / 3 + + print(d_x, d_y, d_theta) + + +prev = time.time() +start = time.time() +while True: + t = time.time() + dt = t - prev + if args.x: + xbox_input() + + viz.display(pwe.robot.state.q) + + pwe.tick(dt) + prev = t diff --git a/Open_Duck_Mini-2/experiments/placo/replay_amp.py b/Open_Duck_Mini-2/experiments/placo/replay_amp.py new file mode 100644 index 0000000..150e48c --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/replay_amp.py @@ -0,0 +1,85 @@ +import argparse +import json +import time + +import FramesViewer.utils as fv_utils +import numpy as np +from FramesViewer.viewer import Viewer +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-f", "--file", type=str, required=True) +parser.add_argument( + "--hardware", + action="store_true", + help="use AMP_for_hardware format. If false, use IsaacGymEnvs format", +) +args = parser.parse_args() + +fv = Viewer() +fv.start() + +episode = json.load(open(args.file)) + +frame_duration = episode["FrameDuration"] + +frames = episode["Frames"] +if "Debug_info" in episode: + debug = episode["Debug_info"] +else: + debug = None +pose = np.eye(4) +if args.hardware: + vels = {} + vels["linear_vel"] = [] + vels["angular_vel"] = [] + vels["joint_vels"] = [] +for i, frame in enumerate(frames): + root_position = frame[:3] + root_orientation_quat = frame[3:7] + root_orientation_mat = R.from_quat(root_orientation_quat).as_matrix() + + pose[:3, 3] = root_position + pose[:3, :3] = root_orientation_mat + + fv.pushFrame(pose, "aze") + + if debug is not None: + left_foot_pose = np.array(debug[i]["left_foot_pose"]).reshape(4, 4) + right_foot_pose = np.array(debug[i]["right_foot_pose"]).reshape(4, 4) + fv.pushFrame(left_foot_pose, "left") + fv.pushFrame(right_foot_pose, "right") + + if args.hardware: + vels["linear_vel"].append(frame[28:31]) + vels["angular_vel"].append(frame[31:34]) + vels["joint_vels"].append(frame[34:49]) + + left_toe_pos = frame[22:25] + right_toe_pos = frame[25:28] + fv.pushFrame(fv_utils.make_pose(left_toe_pos, [0, 0, 0]), "left_toe") + fv.pushFrame(fv_utils.make_pose(right_toe_pos, [0, 0, 0]), "right_toe") + + time.sleep(frame_duration) + + +if args.hardware: + # plot vels + from matplotlib import pyplot as plt + + # TODO + x_lin_vel = [vels["linear_vel"][i][0] for i in range(len(frames))] + y_lin_vel = [vels["linear_vel"][i][1] for i in range(len(frames))] + z_lin_vel = [vels["linear_vel"][i][2] for i in range(len(frames))] + + joints_vel = [vels["joint_vels"][i] for i in range(len(frames))] + angular_vel_x = [vels["angular_vel"][i][0] for i in range(len(frames))] + angular_vel_y = [vels["angular_vel"][i][1] for i in range(len(frames))] + angular_vel_z = [vels["angular_vel"][i][2] for i in range(len(frames))] + + plt.plot(angular_vel_x, label="angular_vel_x") + plt.plot(angular_vel_y, label="angular_vel_y") + plt.plot(angular_vel_z, label="angular_vel_z") + + plt.legend() + plt.show() diff --git a/Open_Duck_Mini-2/experiments/placo/test_bdx.py b/Open_Duck_Mini-2/experiments/placo/test_bdx.py new file mode 100644 index 0000000..3479ba3 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/test_bdx.py @@ -0,0 +1,88 @@ +import argparse +import time +import warnings + +import numpy as np +import placo +from placo_utils.visualization import robot_viz +from placo_utils.tf import tf +import time +import pickle +import FramesViewer.utils as fv_utils + +warnings.filterwarnings("ignore") +model_filename = "../../mini_bdx/robots/bdx/robot.urdf" +robot = placo.RobotWrapper(model_filename) +robot.set_joint_limits("left_knee", -2, -0.01) +robot.set_joint_limits("right_knee", -2, -0.01) +solver = placo.KinematicsSolver(robot) +# solver.mask_fbase(True) + +left_foot_task = solver.add_frame_task("left_foot_frame", np.eye(4)) +left_foot_task.configure("left_foot_frame", "soft", 1.0) +right_foot_task = solver.add_frame_task("right_foot_frame", np.eye(4)) +right_foot_task.configure("right_foot_frame", "soft", 1.0) + +T_world_trunk = np.eye(4) +T_world_trunk = fv_utils.rotateInSelf(T_world_trunk, [0, -2, 0]) +trunk_task = solver.add_frame_task("trunk", T_world_trunk) +trunk_task.configure("trunk", "hard") + +T_world_head = np.eye(4) +T_world_head[:3, 3][2] = 0.1 +head_task = solver.add_frame_task( + "head", + T_world_head, +) +head_task.configure("head", "soft") + +robot.update_kinematics() + +move = [] +viz = robot_viz(robot) +while True: # some main loop + # Update tasks data here + + # left_foot_task.T_world_frame = tf.translation_matrix( + # [0, -0.12 / 2, np.sin(time.time())] + # ) + z_offset = 0.015 * np.sin(2 * time.time()) + left_foot_task.T_world_frame = tf.translation_matrix( + [-0.005, 0.12 / 2, -0.17 + z_offset] + ) + right_foot_task.T_world_frame = tf.translation_matrix( + [-0.005, -0.12 / 2, -0.17 + z_offset] + ) + + head_task.T_world_frame = tf.translation_matrix( + [0.05 * np.sin(2 * np.pi * 2 * time.time()), 0, 0.1] + ) + # Solve the IK + solver.solve(True) + + # Update frames and jacobians + robot.update_kinematics() + + angles = { + "right_hip_yaw": robot.get_joint("right_hip_yaw"), + "right_hip_roll": robot.get_joint("right_hip_roll"), + "right_hip_pitch": robot.get_joint("right_hip_pitch"), + "right_knee": robot.get_joint("right_knee"), + "right_ankle": robot.get_joint("right_ankle"), + "left_hip_yaw": robot.get_joint("left_hip_yaw"), + "left_hip_roll": robot.get_joint("left_hip_roll"), + "left_hip_pitch": robot.get_joint("left_hip_pitch"), + "left_knee": robot.get_joint("left_knee"), + "left_ankle": robot.get_joint("left_ankle"), + "neck_pitch": robot.get_joint("neck_pitch"), + "head_pitch": robot.get_joint("head_pitch"), + "head_yaw": robot.get_joint("head_yaw"), + "left_antenna": robot.get_joint("left_antenna"), + "right_antenna": robot.get_joint("right_antenna"), + } + move.append(angles) + time.sleep(1 / 60) + pickle.dump(move, open("move.pkl", "wb")) + # Optionally: dump the solver status + solver.dump_status() + viz.display(robot.state.q) diff --git a/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py b/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py new file mode 100644 index 0000000..a9eb1f4 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py @@ -0,0 +1,18 @@ +import time + +import numpy as np +from mini_bdx_runtime.hwi import HWI + +hwi = HWI(usb_port="/dev/ttyUSB0") + +hwi.set_pid_all([1000, 0, 500]) +hwi.turn_on() +time.sleep(1) + +while True: + + ankle_pos = 0.3 * np.sin(2 * np.pi * 0.5 * time.time()) + hwi.set_position("right_ankle", ankle_pos) + hwi.set_position("left_ankle", ankle_pos) + + time.sleep(1 / 60) diff --git a/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py b/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py new file mode 100644 index 0000000..5bb5f64 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py @@ -0,0 +1,78 @@ +import argparse +import pickle +import time + +import FramesViewer.utils as fv_utils +import numpy as np +from FramesViewer.viewer import Viewer +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--data", type=str, required=True) +args = parser.parse_args() +# from utils import ImuFilter + +FPS = 30 +gyro_data = pickle.load(open(args.data, "rb")) +fv = Viewer() +fv.start() + +pose_euler = fv_utils.make_pose([0.1, 0.1, 0.1], [0, 0, 0]) + +quat = gyro_data[0][0] +quat = [quat[3], quat[0], quat[1], quat[2]] +rot = R.from_quat(quat).as_matrix() +rot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) @ rot +pose_euler[:3, :3] = rot +pose_euler = fv_utils.rotateInSelf(pose_euler, [0, 0, 90]) +initial_pose = pose_euler.copy() +initial_pose = fv_utils.translateAbsolute(initial_pose, [0.1, 0.1, 0]) + +pose_ang_vel = initial_pose.copy() +i = 1 +while True: + quat = gyro_data[i][0] + ang_vel = gyro_data[i][1] + ang_vel = [-ang_vel[1], ang_vel[0], ang_vel[2]] + + quat = [quat[3], quat[0], quat[1], quat[2]] + rot = R.from_quat(quat).as_matrix() + + rot = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) @ rot + + pose_euler[:3, :3] = rot + + pose_euler = fv_utils.rotateInSelf(pose_euler, [0, 0, 90]) + + rot_euler = R.from_matrix(pose_ang_vel[:3, :3]).as_euler("xyz", degrees=False) + new_rot_euler = np.array(rot_euler) + (np.array(ang_vel) * (1 / FPS)) + rot = R.from_euler("xyz", new_rot_euler, degrees=False).as_matrix() + pose_ang_vel[:3, :3] = rot + + fv.pushFrame(pose_euler, "euler", color=(255, 0, 0)) + fv.pushFrame(pose_ang_vel, "ang_vel") + time.sleep(1 / FPS) + i += 1 + if i >= len(gyro_data) - 1: + print("end, looping ") + time.sleep(2) + pose_ang_vel = initial_pose.copy() + i = 0 + + +# imu_filter = ImuFilter(window_size=100) + +linear_velocity = np.array([0.0, 0.0, 0.0]) +position = np.array([0.0, 0.0, 0.0]) +for linear_acceleration in linear_accelerations: + linear_acceleration = np.array(linear_acceleration) + imu_filter.push_data(linear_acceleration) + linear_acceleration = imu_filter.get_filtered_data() + # print(linear_acceleration) + linear_velocity += linear_acceleration * (1 / FPS) + position += linear_velocity * (1 / FPS) + # pose[:3, 3] = linear_acceleration * 0.1 + pose[:3, 3] = position + print(position) + fv.pushFrame(pose, "imu") + time.sleep(1 / FPS) diff --git a/Open_Duck_Mini-2/experiments/real_robot/move_test.py b/Open_Duck_Mini-2/experiments/real_robot/move_test.py new file mode 100644 index 0000000..a097192 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/move_test.py @@ -0,0 +1,45 @@ +from mini_bdx.hwi import HWI +import pickle +import time +import numpy as np + +move = pickle.load(open("move.pkl", "rb")) +hwi = HWI(usb_port="/dev/ttyUSB0") + +move_without_antennas = [] +# remove antennas keys +for m in move: + m = {k: v for k, v in m.items() if "antenna" not in k} + move_without_antennas.append(m) + + +command_value = [] + +hwi.turn_on() +time.sleep(1) +ctrl_freq = 60 # hz +start = time.time() +i = 0 +while True: + # pos = hwi.init_pos.copy() + # pos["left_hip_pitch"] += np.sin(5 * time.time()) / 3 + # pos["left_knee"] -= np.sin(5 * time.time()) / 3 + # pos["left_ankle"] -= np.sin(5 * time.time()) / 3 + + # pos["right_hip_pitch"] += np.sin(5 * time.time()) / 3 + # pos["right_knee"] -= np.sin(5 * time.time()) / 3 + # pos["right_ankle"] -= np.sin(5 * time.time()) / 3 + # print(pos["right_ankle"]) + + pos = move_without_antennas[i] + hwi.set_position_all(pos) + + command_value.append((list(pos.values()), hwi.get_present_positions())) + + time.sleep(1 / ctrl_freq) + + i += 1 + if i >= len(move) - 1: + i = 0 + +pickle.dump(command_value, open("command_value.pkl", "wb")) diff --git a/Open_Duck_Mini-2/experiments/real_robot/new_run.py b/Open_Duck_Mini-2/experiments/real_robot/new_run.py new file mode 100644 index 0000000..32f826c --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/new_run.py @@ -0,0 +1,38 @@ +from mini_bdx_runtime.hwi import HWI +import time +import numpy as np +from mini_bdx.placo_walk_engine import PlacoWalkEngine + +PLACO_DT = 0.001 + +pwe = PlacoWalkEngine("../../mini_bdx/robots/bdx/robot.urdf", ignore_feet_contact=True) +pwe.set_traj(0.00, 0, 0 + 0.001) + +hwi = HWI("/dev/ttyUSB0") + +pid = (1000, 0, 100) + +hwi.turn_on() +hwi.set_pid_all(pid) + +# exit() + +control_freq = 60 +prev = time.time() +while True: + t = time.time() + dt = t - prev + pwe.tick(dt) + print(pwe.t) + + joints_positions = pwe.get_angles() + del joints_positions["left_antenna"] + del joints_positions["right_antenna"] + print(joints_positions) + if pwe.t >= 0: + exit() + hwi.set_position_all(joints_positions) + + took = time.time() - t + time.sleep(max(0, (1 / control_freq) - took)) + prev = t diff --git a/Open_Duck_Mini-2/experiments/real_robot/plot.py b/Open_Duck_Mini-2/experiments/real_robot/plot.py new file mode 100644 index 0000000..d466892 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/plot.py @@ -0,0 +1,45 @@ +import pickle +import matplotlib.pyplot as plt + +import numpy as np + +command_value = pickle.load(open("command_value.pkl", "rb")) + +dofs = { + 0: "right_hip_yaw", + 1: "right_hip_roll", + 2: "right_hip_pitch", + 3: "right_knee", + 4: "right_ankle", + 5: "left_hip_yaw", + 6: "left_hip_roll", + 7: "left_hip_pitch", + 8: "left_knee", + 9: "left_ankle", + 10: "neck_pitch", + 11: "head_pitch", + 12: "head_yaw", + # 13: "left_antenna", + # 14: "right_antenna", +} +# command_value = np.array(command_value) +fig, axs = plt.subplots(4, 4) +dof_id = 0 +for i in range(4): + for j in range(4): + if 4 * i + j >= 13: + continue + print(4 * i + j) + command = [] + value = [] + for k in range(len(command_value)): + command.append(command_value[k][0][4 * i + j]) + value.append(command_value[k][1][4 * i + j]) + axs[i, j].plot(command, label="command") + axs[i, j].plot(value, label="value") + axs[i, j].legend() + axs[i, j].set_title(f"{dofs[dof_id]}") + dof_id += 1 + + +plt.show() diff --git a/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py b/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py new file mode 100644 index 0000000..a667469 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py @@ -0,0 +1,33 @@ +import pickle +import numpy as np +import time +import matplotlib.pyplot as plt +from utils import ImuFilter + +FPS = 60 +linear_accelerations = pickle.load(open("gyro_data.pkl", "rb")) + +# plot +filtered_linear_accelerations = [] +imu_filter = ImuFilter(window_size=100) +for linear_acceleration in linear_accelerations: + imu_filter.push_data(linear_acceleration) + filtered_linear_accelerations.append(imu_filter.get_filtered_data()) + +linear_accelerations = filtered_linear_accelerations + + +axes = ["x", "y", "z"] +fig, axs = plt.subplots(3, 1) +for i in range(3): + linear_acceleration = [] + for k in range(len(linear_accelerations)): + linear_acceleration.append(linear_accelerations[k][i]) + axs[i].plot(linear_acceleration, label="linear_acceleration") + axs[i].legend() + axs[i].set_title(f"linear_acceleration {axes[i]}") + # same range for all plots + + axs[i].set_ylim([-10, 10]) + +plt.show() diff --git a/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py b/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py new file mode 100644 index 0000000..3add6cd --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py @@ -0,0 +1,15 @@ +import time + +from mini_bdx.hwi import HWI + +hwi = HWI(usb_port="/dev/ttyUSB0") + +while True: + present_current_right_ankle = hwi.get_present_current("right_ankle") + present_current_left_ankle = hwi.get_present_current("left_ankle") + + goal_current_right_ankle = hwi.get_goal_current("right_ankle") + goal_current_left_ankle = hwi.get_goal_current("left_ankle") + + print("present", present_current_right_ankle, "goal", goal_current_right_ankle) + time.sleep(0.1) diff --git a/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py b/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py new file mode 100644 index 0000000..1329134 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py @@ -0,0 +1,50 @@ +import argparse +import pickle +import time + +import FramesViewer.utils as fv_utils +import numpy as np +from FramesViewer.viewer import Viewer +from scipy.spatial.transform import Rotation as R + +parser = argparse.ArgumentParser() +parser.add_argument("-d", "--data", type=str, required=True) +args = parser.parse_args() + +FPS = 30 +gyro_data = pickle.load(open(args.data, "rb")) +fv = Viewer() +fv.start() + +pose_quat = fv_utils.make_pose([0.1, 0.1, 0.1], [0, 0, 0]) + +quat = gyro_data[0][0] +# quat = [quat[3], quat[0], quat[1], quat[2]] +rot = R.from_quat(quat).as_matrix() +pose_quat[:3, :3] = rot +initial_pose = pose_quat.copy() +initial_pose = fv_utils.translateAbsolute(initial_pose, [0.1, 0.1, 0]) + +pose_ang_vel = initial_pose.copy() +i = 1 +while True: + print(i) + quat = gyro_data[i][0] + ang_vel = gyro_data[i][1] + rot = R.from_quat(quat).as_matrix() + pose_quat[:3, :3] = rot + + rot_ang_vel = R.from_matrix(pose_ang_vel[:3, :3]).as_euler("xyz", degrees=False) + new_rot_ang_vel = np.array(rot_ang_vel) + (np.array(ang_vel) * (1 / FPS)) + rot_ang_vel = R.from_euler("xyz", new_rot_ang_vel, degrees=False).as_matrix() + pose_ang_vel[:3, :3] = rot + + fv.pushFrame(pose_quat, "quat", color=(255, 0, 0)) + fv.pushFrame(pose_ang_vel, "ang_vel") + time.sleep(1 / FPS) + i += 1 + if i >= len(gyro_data) - 1: + print("end, looping ") + time.sleep(2) + pose_ang_vel = initial_pose.copy() + i = 0 diff --git a/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py b/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py new file mode 100644 index 0000000..5b3f99e --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py @@ -0,0 +1,44 @@ +""" +Replays imu data from a .pkl which is a list of quaternions +Shows the orientation in a 3D viewer +""" + +from FramesViewer.viewer import Viewer +import pickle +from scipy.spatial.transform import Rotation as R +import numpy as np +import time + +data = pickle.load(open("imu_data.pkl", "rb")) + +fv = Viewer() +fv.start() + + +def reorient(quat): + """ + Reorients because the IMU is mounted upside down + """ + + euler = R.from_quat(quat).as_euler("xyz") + # euler = [np.pi-euler[1], euler[2], -euler[0]] + reoriented_quat = R.from_euler("xyz", euler).as_quat() + return reoriented_quat + + +imu = np.eye(4) +imu[:3, 3] = [0.1, 0.1, 0.1] # just easier to see + +for raw_quat in data: + try: + quat = reorient(raw_quat) + except: + continue + + rot_mat = R.from_quat(quat).as_matrix() + imu[:3, :3] = rot_mat + fv.pushFrame(imu, "imu") + time.sleep(1/30) + + + diff --git a/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py b/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py new file mode 100644 index 0000000..a9e0de6 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py @@ -0,0 +1,103 @@ +import pickle +import time + +import numpy as np + +from mini_bdx.hwi import HWI +from mini_bdx.onnx_infer import OnnxInfer +from mini_bdx.utils.rl_utils import ( + action_to_pd_targets, + isaac_joints_order, + isaac_to_mujoco, +) + +pd_action_offset = [ + 0.0, + -0.57, + 0.52, + 0.0, + 0.0, + -0.57, + 0.0, + 0.0, + 0.48, + -0.48, + 0.0, + -0.57, + 0.52, + 0.0, + 0.0, +] +pd_action_scale = [ + 0.98, + 1.4, + 1.47, + 2.93, + 2.2, + 1.04, + 0.98, + 2.93, + 2.26, + 2.26, + 0.98, + 1.4, + 1.47, + 2.93, + 2.2, +] + + +def make_action_dict(action): + action_dict = {} + for i, a in enumerate(action): + if "antenna" not in isaac_joints_order[i]: + action_dict[isaac_joints_order[i]] = a + + return action_dict + + +# TODO +def get_obs(hwi, imu): + # Don't forget to re invert the angles from the hwi + pass + + +policy = OnnxInfer("/home/antoine/MISC/IsaacGymEnvs/isaacgymenvs/ONNX_NO_PUSH.onnx") + +fake_obs = pickle.loads(open("saved_obs.pkl", "rb").read()) +fake_actions = pickle.loads(open("saved_actions.pkl", "rb").read()) + +hwi = HWI(usb_port="/dev/ttyUSB0") + +command_value = [] + +hwi.turn_on() +time.sleep(1) +skip = 10 +i = 0 +ctrl_freq = 30 # hz +while True: + if skip > 0: + skip -= 1 + start = time.time() + obs = fake_obs[i] # for now + # obs = get_obs(hwi, imu) + + obs = np.clip(obs, -5, 5) + action = policy.infer(obs) + action = fake_actions[i][0] + action = np.clip(action, -1, 1) + action = action_to_pd_targets(action, pd_action_offset, pd_action_scale) + action_dict = make_action_dict(action) + # print(action_dict) + hwi.set_position_all(action_dict) + took = time.time() - start + # time.sleep((max(0, (1 / ctrl_freq) - took))) + time.sleep(1 / ctrl_freq) + i += 1 + if i >= len(fake_obs) - 1: + break + + command_value.append((list(action_dict.values()), hwi.get_present_positions())) + # print(len(command_value[0][0]), len(command_value[0][1])) + pickle.dump(command_value, open("command_value.pkl", "wb")) diff --git a/Open_Duck_Mini-2/experiments/real_robot/run.py b/Open_Duck_Mini-2/experiments/real_robot/run.py new file mode 100644 index 0000000..ead6d90 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/run.py @@ -0,0 +1,166 @@ +import argparse +import time + +import cv2 +import numpy as np +import placo +from mini_bdx_runtime.hwi import HWI + +# from mini_bdx.hwi import HWI +from mini_bdx.utils.xbox_controller import XboxController +from mini_bdx.walk_engine import WalkEngine + +parser = argparse.ArgumentParser() +parser.add_argument("-x", action="store_true", default=False) +args = parser.parse_args() + +if args.x: + xbox = XboxController() + + +hwi = HWI(usb_port="/dev/ttyUSB0") + +max_target_step_size_x = 0.03 +max_target_step_size_y = 0.03 +max_target_yaw = np.deg2rad(15) +target_step_size_x = 0 +target_step_size_y = 0 +target_yaw = 0 +target_head_pitch = 0 +target_head_yaw = 0 +target_head_z_offset = 0 +time_since_last_left_contact = 0 +time_since_last_right_contact = 0 +walking = False +start_button_timeout = time.time() + +robot = placo.RobotWrapper( + "../../mini_bdx/robots/bdx/robot.urdf", placo.Flags.ignore_collisions +) + +walk_engine = WalkEngine( + robot, + frequency=1.5, + swing_gain=0.0, + default_trunk_x_offset=-0.013, + default_trunk_z_offset=-0.023, + target_trunk_pitch=-11.0, + max_rise_gain=0.01, +) + + +def xbox_input(): + global target_step_size_x, target_step_size_y, target_yaw, walking, t, walk_engine, target_head_pitch, target_head_yaw, target_head_z_offset, start_button_timeout, max_target_step_size_x, max_target_step_size_y, max_target_yaw + inputs = xbox.read() + # print(inputs) + target_step_size_x = -inputs["l_y"] * max_target_step_size_x + target_step_size_y = inputs["l_x"] * max_target_step_size_y + if inputs["l_trigger"] > 0.2: + target_head_pitch = inputs["r_y"] / 2 * np.deg2rad(70) + print("=== target head pitch", target_head_pitch) + target_head_yaw = -inputs["r_x"] / 2 * np.deg2rad(150) + target_head_z_offset = inputs["r_trigger"] * 4 * 0.2 + print(target_head_z_offset) + # print("======", target_head_z_offset) + else: + target_yaw = -inputs["r_x"] * max_target_yaw + + if inputs["start"] and time.time() - start_button_timeout > 0.5: + walking = not walking + start_button_timeout = time.time() + + +im = np.zeros((80, 80, 3), dtype=np.uint8) + + +# TODO +def get_imu(): + return [0, 0, 0], [0, 0, 0] + + +# hwi.turn_off() +# exit() +hwi.turn_on() +time.sleep(1) +# hwi.goto_init() +# exit() +gyro = [0, 0.0, 0] +accelerometer = [0, 0, 0] + +skip = 10 +prev = time.time() +while True: + dt = time.time() - prev + t = time.time() + if args.x: + xbox_input() + + # Get sensor data + # gyro, accelerometer = get_imu() + right_contact = abs(hwi.get_present_current("right_ankle")) > 1 + left_contact = abs(hwi.get_present_current("left_ankle")) > 1 + # print("left_contact", left_contact, "right_contact", right_contact) + walk_engine.update( + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_size_x, + target_step_size_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ignore_feet_contact=True, + ) + angles = walk_engine.get_angles() + + if skip > 0: + skip -= 1 + continue + hwi.set_position_all(angles) + + # print("-") + cv2.imshow("image", im) + key = cv2.waitKey(1) + if key == ord("p"): + # gyro[1] += 0.001 + walk_engine.target_trunk_pitch += 0.1 + if key == ord("o"): + walk_engine.target_trunk_pitch -= 0.1 + # gyro[1] -= 0.001 + if key == ord("m"): + walk_engine.max_rise_gain += 0.001 + if key == ord("l"): + walk_engine.max_rise_gain -= 0.001 + if key == ord("b"): + walk_engine.default_trunk_x_offset += 0.001 + if key == ord("n"): + walk_engine.default_trunk_x_offset -= 0.001 + if key == ord("i"): + walk_engine.default_trunk_z_offset += 0.001 + if key == ord("u"): + walk_engine.default_trunk_z_offset -= 0.001 + if key == ord("f"): + walk_engine.frequency -= 0.1 + if key == ord("g"): + walk_engine.frequency += 0.1 + if key == ord("c"): + walk_engine.swing_gain -= 0.001 + if key == ord("v"): + walk_engine.swing_gain += 0.001 + if key == ord("w"): + walking = not walking + + # print("gyro : ", gyro) + # print("target_trunk pitch", walk_engine.trunk_pitch) + # print("trunk x offset", walk_engine.default_trunk_x_offset) + # print("trunk z offset", walk_engine.default_trunk_z_offset) + # print("max rise gain", walk_engine.max_rise_gain) + # print("frequency", walk_engine.frequency) + # print("swing gain", walk_engine.swing_gain) + # print("===") + + prev = t diff --git a/Open_Duck_Mini-2/experiments/real_robot/utils.py b/Open_Duck_Mini-2/experiments/real_robot/utils.py new file mode 100644 index 0000000..f3ecf4c --- /dev/null +++ b/Open_Duck_Mini-2/experiments/real_robot/utils.py @@ -0,0 +1,15 @@ +import numpy as np + + +class ImuFilter: + def __init__(self, window_size=10): + self.window_size = window_size + self.data = [] + self.filtered_data = [] + + def push_data(self, data): + self.data.append(data) + + def get_filtered_data(self): + data = self.data[-self.window_size :] + return np.mean(data, axis=0) diff --git a/Open_Duck_Mini-2/experiments/v2/bench_com_time.py b/Open_Duck_Mini-2/experiments/v2/bench_com_time.py new file mode 100644 index 0000000..7da04e8 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/bench_com_time.py @@ -0,0 +1,25 @@ +from pypot.feetech import FeetechSTS3215IO +import time +import numpy as np + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) + +# id = 24 +ids = [10, 11, 12, 13, 14, 20, 21, 22, 23, 24, 30, 31, 32, 33] + +io.enable_torque(ids) +io.set_mode({id: 0 for id in ids}) +times = [] +for i in range(1000): + s = time.time() + io.get_present_position(ids) + io.set_goal_position({id: 0 for id in ids}) + times.append(time.time() - s) + + time.sleep(1 / 100) + +print("avg :", np.mean(times)) \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/v2/configure_motors.py b/Open_Duck_Mini-2/experiments/v2/configure_motors.py new file mode 100644 index 0000000..141b67a --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/configure_motors.py @@ -0,0 +1,123 @@ +from pypot.feetech import FeetechSTS3215IO +import argparse +import time + +parser = argparse.ArgumentParser() +parser.add_argument("--usb_port", type=str, default="/dev/ttyACM0") +args = parser.parse_args() + + +joints = { + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, +} + +# TODO scan baudrates +io = FeetechSTS3215IO( + args.usb_port, + baudrate=1000000, + use_sync_read=True, +) + + +id = 200 +SKIP_SCAN = False +if not SKIP_SCAN: + try: + io.get_present_position([id]) + except Exception: + print( + "Didn't find motor with id 1, motor has probably been configured before. scanning for other motors" + ) + print("Scanning... ") + found_ids = io.scan() + print("Found ids: ", found_ids) + if len(found_ids) > 1: + print("More than one motor found, please connect only one motor") + exit() + elif len(found_ids) == 0: + print("No motor found") + exit() + + id = found_ids[0] + +exit() +print("Select the dof you want to configure : ") +for i, key in enumerate(joints.keys()): + print(f"{i}: {key}") + +dof_index = int(input("> ")) +if dof_index not in range(len(joints)): + print("Invalid choice") + exit() + +dof_name = list(joints.keys())[dof_index] +dof_id = joints[dof_name] + +print("") +print("===") +print("Configuring motor ", dof_name, " with id ", dof_id) +print("===") + +io.set_lock({id: 1}) + +io.set_offset({id: 0}) +print("- setting new id ") +io.change_id({id: dof_id}) +id = dof_id +print("- setting new baudrate") +io.change_baudrate({id: 0}) # 1 000 000 + +exit() +# WARNING offset management is not understood yet. + +print("") +print("The motor will now move to the zero position.") +print("Press enter to continue") +input() + +io.enable_torque([id]) +io.set_goal_position({id: 0}) +time.sleep(1) +zero_pos = io.get_present_position([id])[0] + +print("") +print( + "Now, place the contraption (???) on the motor, and adjust the position to fit (???) (TODO: clarify)" +) +print("Press enter to continue") + +io.disable_torque([id]) +input() +new_pos = io.get_present_position([id])[0] +offset = zero_pos - new_pos +print("Offset: ", offset) +io.set_offset({id: offset}) +time.sleep(1) + + +io.set_lock({id: 0}) + +print("") +print( + "To confirm the offset, please move the motor to a random position, then press enter to go back to zero." +) +input() +io.enable_torque([id]) +io.set_goal_position({id: 0}) +time.sleep(1) +print("position: ", io.get_present_position([id])[0]) + +io.disable_torque([id]) diff --git a/Open_Duck_Mini-2/experiments/v2/identification.py b/Open_Duck_Mini-2/experiments/v2/identification.py new file mode 100644 index 0000000..f19f605 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/identification.py @@ -0,0 +1,80 @@ +from pypot.feetech import FeetechSTS3215IO +import time +import pickle + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) + +id = 24 +max_acceleration = 254 +io.set_D_coefficient({id: 0}) +io.set_acceleration({id: max_acceleration}) + +io.set_goal_position({id: 0}) + +# present position +# goal position +# present load +# present current +# present speed +# 0 deg pendant 2s, 90° pendant 2s etc + + +present_positions = [] +goal_positions = [] +present_loads = [] +present_currents = [] +present_speeds = [] +times = [] + +input("press enter to start") + +io.set_goal_position({id: 90}) +s = time.time() +while time.time() - s < 2: + present_positions.append(io.get_present_position([id])[0]) + goal_positions.append(io.get_goal_position([id])[0]) + present_loads.append(io.get_present_load([id])[0]) + present_currents.append(io.get_present_current([id])[0]) + present_speeds.append(io.get_present_speed([id])[0]) + times.append(time.time()) + + +io.set_goal_position({id: 0}) +s = time.time() +while time.time() - s < 2: + present_positions.append(io.get_present_position([id])[0]) + goal_positions.append(io.get_goal_position([id])[0]) + present_loads.append(io.get_present_load([id])[0]) + present_currents.append(io.get_present_current([id])[0]) + present_speeds.append(io.get_present_speed([id])[0]) + times.append(time.time()) + +io.set_goal_position({id: 90}) +s = time.time() +while time.time() - s < 2: + present_positions.append(io.get_present_position([id])[0]) + goal_positions.append(io.get_goal_position([id])[0]) + present_loads.append(io.get_present_load([id])[0]) + present_currents.append(io.get_present_current([id])[0]) + present_speeds.append(io.get_present_speed([id])[0]) + times.append(time.time()) + + +data = { + "present_positions": present_positions, + "goal_positions": goal_positions, + "present_loads": present_loads, + "present_currents": present_currents, + "present_speeds": present_speeds, + "times": times, +} + +pickle.dump(data, open(f"data_max_acc_{max_acceleration}.pkl", "wb")) + + + + diff --git a/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py b/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py new file mode 100644 index 0000000..9bb8de3 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py @@ -0,0 +1,86 @@ +from mini_bdx.placo_walk_engine.placo_walk_engine import PlacoWalkEngine +import time +import json +import mujoco +import mujoco.viewer +import pickle +from mini_bdx.utils.mujoco_utils import check_contact +import numpy as np + +# DT = 0.01 +DT = 0.002 +decimation = 10 +pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2", + model_filename="robot.urdf", + init_params=json.load(open("placo_defaults.json")), + ignore_feet_contact=True, +) +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/openduckminiv2_playground/env/locomotion/open_duck_mini_v2/xmls/scene_mjx_flat_terrain.xml" +) +model.opt.timestep = DT +data = mujoco.MjData(model) + +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + # 0.0, + # 0, + # 0, + # 0, + # 0, + # 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +# angles = pickle.load(open("init_angles.pkl", "rb")) + +data.ctrl[:] = init_pos +data.qpos[3 + 4 :] = init_pos +data.qpos[3 : 3 + 4] = [1, 0, 0.06, 0] + + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return right_contact, left_contact + + +pwe.set_traj(0.05, 0, 0.0) +counter = 0 +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + while True: + # right_contact, left_contact = get_feet_contact() + if counter % decimation == 0: + pwe.tick(DT * decimation) + angles = list( + pwe.get_angles( + ignore=[ + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + ] + ).values() + ) + data.ctrl[:] = angles + + counter += 1 + + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(DT) diff --git a/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py b/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py new file mode 100644 index 0000000..b47a717 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py @@ -0,0 +1,243 @@ +import mujoco +import numpy as np + +import mujoco.viewer +import time +import pygame +import argparse +from mini_bdx.utils.mujoco_utils import check_contact + +from mini_bdx_runtime.onnx_infer import OnnxInfer +import pickle +from bam.model import load_model +from bam.mujoco import MujocoController +from mini_bdx_runtime.rl_utils import mujoco_joints_order + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("-k", action="store_true", default=False) +parser.add_argument("--bam", action="store_true", default=False) +# parser.add_argument("--rma", action="store_true", default=False) +# parser.add_argument("--awd", action="store_true", default=False) +# parser.add_argument("--adaptation_module_path", type=str, required=False) +parser.add_argument("--replay_obs", type=str, required=False, default=None) +args = parser.parse_args() + +if args.k: + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + +if args.replay_obs is not None: + with open(args.replay_obs, "rb") as f: + replay_obs = pickle.load(f) + replay_obs = np.array(replay_obs) + +# Params +linearVelocityScale = 1.0 +angularVelocityScale = 1.0 +dof_pos_scale = 1.0 +dof_vel_scale = 1.0 +action_scale = 0.25 + + +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +# model = mujoco.MjModel.from_xml_path( +# "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene_position.xml" +# ) +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mujoco_menagerie/open_duck_mini_v2/scene.xml" +) +model.opt.timestep = 0.005 +# model.opt.timestep = 1 / 240 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) + +if args.bam: + sts3215_model = load_model("params_m6.json") + mujoco_controllers = {} + for joint_name in mujoco_joints_order: + mujoco_controllers[joint_name] = MujocoController( + sts3215_model, joint_name, model, data + ) + + +NUM_OBS = 56 + +policy = OnnxInfer(args.onnx_model_path, awd=True) + +COMMANDS_RANGE_X = [-0.2, 0.3] +COMMANDS_RANGE_Y = [-0.2, 0.2] +COMMANDS_RANGE_THETA = [-0.3, 0.3] + +prev_action = np.zeros(16) +commands = [0.3, 0.0, 0.0] +decimation = 4 +data.qpos[3 : 3 + 4] = [1, 0, 0.0, 0] + +data.qpos[7 : 7 + 16] = init_pos +data.ctrl[:16] = init_pos + +replay_index = 0 +saved_obs = [] + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos[7 : 7 + 16].copy() + + dof_vel = data.qvel[6 : 6 + 16].copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + feet_contacts = get_feet_contact() + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + + +def handle_keyboard(): + global commands + keys = pygame.key.get_pressed() + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keys[pygame.K_z]: + lin_vel_x = COMMANDS_RANGE_X[1] + if keys[pygame.K_s]: + lin_vel_x = COMMANDS_RANGE_X[0] + if keys[pygame.K_q]: + lin_vel_y = COMMANDS_RANGE_Y[1] + if keys[pygame.K_d]: + lin_vel_y = COMMANDS_RANGE_Y[0] + if keys[pygame.K_a]: + ang_vel = COMMANDS_RANGE_THETA[1] + if keys[pygame.K_e]: + ang_vel = COMMANDS_RANGE_THETA[0] + + commands[0] = lin_vel_x + commands[1] = lin_vel_y + commands[2] = ang_vel + + commands = list( + np.array(commands) + * np.array( + [ + linearVelocityScale, + linearVelocityScale, + angularVelocityScale, + ] + ) + ) + print(commands) + pygame.event.pump() # process event queue + + +try: + with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False + ) as viewer: + counter = 0 + while True: + + step_start = time.time() # Was + # step_start = data.time + + mujoco.mj_step(model, data) + + counter += 1 + if counter % decimation == 0: + + if args.replay_obs is not None: + obs = replay_obs[replay_index] + else: + obs = get_obs(data, prev_action, commands) + saved_obs.append(obs) + + obs = list(obs) + list(np.zeros(18)) + action = policy.infer(obs) + + prev_action = action.copy() + + action = action * action_scale + init_pos + + # if args.bam: + # for i, joint_name in enumerate(mujoco_joints_order): + # mujoco_controllers[joint_name].update(action[i]) + # else: + # data.ctrl = action.copy() + + if args.k: + handle_keyboard() + # print(commands) + + replay_index += 1 + if args.replay_obs is not None and replay_index >= len(replay_obs): + replay_index = 0 + + viewer.sync() + + # Rudimentary time keeping, will drift relative to wall clock. + # time_until_next_step = model.opt.timestep - (data.time - step_start) + # if time_until_next_step > 0: + # time.sleep(time_until_next_step) + + # Was + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) + +except KeyboardInterrupt: + pickle.dump(saved_obs, open("mujoco_saved_obs.pkl", "wb")) diff --git a/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py b/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py new file mode 100644 index 0000000..0a45410 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py @@ -0,0 +1,262 @@ +import mujoco.viewer + +import time +import mujoco +import argparse +import pickle +import numpy as np + +from mini_bdx.utils.mujoco_utils import check_contact + +from mini_bdx_runtime.onnx_infer import OnnxInfer + + +parser = argparse.ArgumentParser() +parser.add_argument("-o", "--onnx_model_path", type=str, required=True) +parser.add_argument("-k", action="store_true", default=False) +parser.add_argument("--replay_obs", type=str, required=False, default=None) +parser.add_argument("--rma", action="store_true", default=False) +parser.add_argument("--adaptation_module_path", type=str, required=False) +parser.add_argument("--zero_head", action="store_true", default=False) +args = parser.parse_args() + +if args.k: + import pygame + + pygame.init() + # open a blank pygame window + screen = pygame.display.set_mode((100, 100)) + pygame.display.set_caption("Press arrow keys to move robot") + +if args.replay_obs is not None: + with open(args.replay_obs, "rb") as f: + replay_obs = pickle.load(f) + replay_obs = np.array(replay_obs) + + +def pd_control(target_q, q, kp, target_dq, dq, kd, clip_val, init_pos, action_scale): + """Calculates torques from position commands""" + tau = (target_q * action_scale + init_pos - q) * kp# - (dq * kd) + tau = np.clip(tau, -clip_val, clip_val) + tau -= dq * kd + # tau += (target_dq - dq) * kd + return tau + # return (target_q - q) * kp + (target_dq - dq) * kd + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos[7 : 7 + 16].copy() + + dof_vel = data.qvel[6 : 6 + 16].copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + feet_contacts = get_feet_contact() + # feet_contacts = [0, 0] + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + +def set_zero_head(pos): + pos[5] = np.deg2rad(10) + pos[6] = np.deg2rad(-10) + # pos[5] = 0 + # pos[6] = 0 + pos[7] = 0 + pos[8] = 0 + return pos + +def handle_keyboard(): + global commands + keys = pygame.key.get_pressed() + lin_vel_x = 0 + lin_vel_y = 0 + ang_vel = 0 + if keys[pygame.K_z]: + lin_vel_x = 0.3 + if keys[pygame.K_s]: + lin_vel_x = -0.2 + if keys[pygame.K_q]: + lin_vel_y = 0.2 + if keys[pygame.K_d]: + lin_vel_y = -0.2 + if keys[pygame.K_a]: + ang_vel = 0.2 + if keys[pygame.K_e]: + ang_vel = -0.2 + + commands[0] = lin_vel_x + commands[1] = lin_vel_y + commands[2] = ang_vel + + # commands = list( + # np.array(commands) + # * np.array( + # [ + # linearVelocityScale, + # linearVelocityScale, + # angularVelocityScale, + # ] + # ) + # ) + pygame.event.pump() # process event queue + + +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene.xml" +) +# model = mujoco.MjModel.from_xml_path( +# "/home/antoine/MISC/mujoco_menagerie/open_duck_mini_v2/scene.xml" +# ) +model.opt.timestep = 0.005 +# model.opt.timestep = 1 / 120 +# model.opt.timestep = 1 / 60 # /2 substeps ? +# model.opt.integrator = mujoco.mjtIntegrator.mjINT_IMPLICITFAST +data = mujoco.MjData(model) +# mujoco.mj_step(model, data) +control_decimation = 4 + +data.qpos[3 : 3 + 4] = [1, 0, 0.0, 0] +data.qpos[7 : 7 + 16] = init_pos +data.ctrl[:16] = init_pos + +NUM_OBS = 56 + +policy = OnnxInfer(args.onnx_model_path, awd=True) +if args.rma: + adaptation_module = OnnxInfer(args.adaptation_module_path, "rma_history", awd=True) + obs_history_size = 20 + obs_history = np.zeros((obs_history_size, NUM_OBS)).tolist() + rma_decimation = 5 # 10 hz if control_decimation = 4 + rma_counter = 0 + +commands = [0.2, 0.0, 0.0] + +# define context variables +prev_action = np.zeros(16) +target_dof_pos = init_pos.copy() +action_scale = 0.5 + +kps = np.array([6.55] * 16) +kds = np.array([0.65] * 16) + +counter = 0 +replay_counter = 0 +latent = None +start = time.time() +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + adaptation_module_latents = [] + while True: + step_start = time.time() + + tau = pd_control( + target_dof_pos, + data.qpos[7:].copy(), + kps, + np.zeros_like(kds), + data.qvel[6:].copy(), + kds, + 3.57, + init_pos, + action_scale, + ) + data.ctrl[:] = tau + + mujoco.mj_step(model, data) + counter += 1 + + if counter % control_decimation == 0 and time.time() - start > 0: + if args.replay_obs is not None: + obs = replay_obs[replay_counter] + replay_counter += 1 + else: + obs = get_obs(data, prev_action, commands) + if args.rma: + rma_counter += 1 + + # Shift to right and set new obs at index 0 + obs_history = np.roll(obs_history, 1, axis=0) + obs_history[0] = obs + + # obs_history.append(obs) + # obs_history = obs_history[-obs_history_size:] + + if rma_counter % rma_decimation == 0 or latent is None: + latent = adaptation_module.infer(np.array(obs_history).flatten()) + adaptation_module_latents.append(latent) + pickle.dump(adaptation_module_latents, open("adaptation_module_latents.pkl", "wb")) + obs = np.concatenate([obs, latent]) + + obs = np.clip(obs, -100, 100) + action = policy.infer(obs) + action = np.clip(action, -5, 5) + if args.zero_head: + action = set_zero_head(action) + prev_action = action.copy() + + + target_dof_pos = np.array(action) # * action_scale + init_pos + + viewer.sync() + + if args.k: + handle_keyboard() + time_until_next_step = model.opt.timestep - (time.time() - step_start) + if time_until_next_step > 0: + time.sleep(time_until_next_step) diff --git a/Open_Duck_Mini-2/experiments/v2/params_m6.json b/Open_Duck_Mini-2/experiments/v2/params_m6.json new file mode 100644 index 0000000..684ca71 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/params_m6.json @@ -0,0 +1 @@ +{"kt": 1.4303213704900366, "R": 2.0430791581753445, "armature": 0.009877609369241102, "friction_base": 0.010907201185572422, "friction_stribeck": 0.024827228916629952, "load_friction_motor": 0.190200316386506, "load_friction_external": 0.14527733955866906, "load_friction_motor_stribeck": 0.04566008837303702, "load_friction_external_stribeck": 0.7340661702898804, "load_friction_motor_quad": 0.006871499097705957, "load_friction_external_quad": 0.006948188153376718, "dtheta_stribeck": 0.09985941135196887, "alpha": 2.653878573867841, "friction_viscous": 0.023286828043639525, "model": "m6", "actuator": "sts3215"} \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/v2/placo_defaults.json b/Open_Duck_Mini-2/experiments/v2/placo_defaults.json new file mode 100644 index 0000000..cba3d00 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/placo_defaults.json @@ -0,0 +1,52 @@ +{ + "dx": 0.0, + "dy": 0.0, + "dtheta": 0.0, + "duration": 8, + "hardware": true, + "double_support_ratio": 0.2, + "startend_double_support_ratio": 1.5, + "planned_timesteps": 48, + "replan_timesteps": 10, + "walk_com_height": 0.2, + "walk_foot_height": 0.02, + "walk_trunk_pitch": 0, + "walk_foot_rise_ratio": 0.3, + "single_support_duration": 0.17, + "single_support_timesteps": 10, + "foot_length": 0.06, + "feet_spacing": 0.16, + "zmp_margin": 0.0, + "foot_zmp_target_x": 0.0, + "foot_zmp_target_y": 0.0, + "walk_max_dtheta": 0.3, + "walk_max_dy": 0.1, + "walk_max_dx_forward": 0.08, + "walk_max_dx_backward": 0.03, + "joints": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna" + ], + "joint_angles": { + "neck_pitch": 0, + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + "left_antenna": 0, + "right_antenna": 0 + } +} diff --git a/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py b/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py new file mode 100644 index 0000000..0dd057f --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py @@ -0,0 +1,167 @@ +import time +import placo +import numpy as np +from ischedule import schedule, run_loop +from placo_utils.visualization import robot_viz, robot_frame_viz, frame_viz +from placo_utils.tf import tf + +from mini_bdx_runtime.hwi_feetech_pypot import HWI + +MESHCAT_VIZ = False + +joints = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", + # "neck_pitch", + # "head_pitch", + # "head_yaw", +] + +if not MESHCAT_VIZ: + hwi = HWI() + hwi.turn_on() + +time.sleep(1) +exit() + + +DT = 0.01 +# robot = placo.HumanoidRobot("/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/robot.urdf") +robot = placo.RobotWrapper( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/robot.urdf", + placo.Flags.ignore_collisions, +) + +# Placing the left foot in world origin +robot.set_joint("left_knee", 0.1) +robot.set_joint("right_knee", 0.1) +robot.update_kinematics() +robot.set_T_world_frame("left_foot", np.eye(4)) +robot.update_kinematics() + +solver = placo.KinematicsSolver(robot) + +# Retrieving initial position of the feet, com and trunk orientation +T_world_left = robot.get_T_world_frame("left_foot") +T_world_right = robot.get_T_world_frame("right_foot") + +if MESHCAT_VIZ: + viz = robot_viz(robot) + +T_world_trunk = robot.get_T_world_frame("trunk") +T_world_trunk[2, 3] = 0.25 +trunk_task = solver.add_frame_task("trunk", T_world_trunk) +trunk_task.configure("trunk_task", "soft", 1e3, 1e3) + +# Keep left and right foot on the floor +left_foot_task = solver.add_frame_task("left_foot", T_world_left) +left_foot_task.configure("left_foot", "soft", 1.0, 1.0) + +right_foot_task = solver.add_frame_task("right_foot", T_world_right) +right_foot_task.configure("right_foot", "soft", 1e3, 1e3) + +# Regularization task +posture_regularization_task = solver.add_joints_task() +posture_regularization_task.set_joints({dof: 0.0 for dof in robot.joint_names()}) +posture_regularization_task.configure("reg", "soft", 1e-5) + + +# Initializing robot position before enabling constraints +for _ in range(32): + solver.solve(True) + robot.update_kinematics() + +# Enabling joint and velocity limits +solver.enable_joint_limits(True) +solver.enable_velocity_limits(True) + +t = 0 +dt = 0.01 +last = 0 +solver.dt = dt +start_t = time.time() +robot.update_kinematics() + + +def get_angles(): + angles = {joint: robot.get_joint(joint) for joint in joints} + return angles + + +# original_T_world_frame = T_world_trunk.copy() +while True: + + # T_world_frame = original_T_world_frame.copy() + + trunk_task.T_world_frame = tf.translation_matrix([0, 0, 0.25]) @ tf.rotation_matrix( + 0.25 * np.sin(2 * np.pi * 0.5 * t), [0, 0, 1] + ) + + # y_targ = 0.05 * np.sin(2 * np.pi * 0.5 * t) + # x_targ = 0.05 * np.cos(2 * np.pi * 0.5 * t) + + # T_world_frame[:3, 3] += [0, y_targ, 0] + + # trunk_task.T_world_frame = T_world_frame.copy() + + solver.solve(True) + robot.update_kinematics() + + if not MESHCAT_VIZ: + all_angles = list(get_angles().values()) + + angles = {} + for i, motor_name in enumerate(hwi.joints.keys()): + angles[motor_name] = all_angles[i] + + hwi.set_position_all(angles) + + if MESHCAT_VIZ: + viz.display(robot.state.q) + robot_frame_viz(robot, "left_foot") + frame_viz("left_foot_target", left_foot_task.T_world_frame, opacity=0.25) + + time.sleep(DT) + t += DT + print(t) + +# @schedule(interval=dt) +# def loop(): +# global t + +# # # Updating left foot target +# # left_foot_task.T_world_frame = tf.translation_matrix( +# # [np.sin(t * 2.5) * 0.05, np.sin(t * 3) * 0.1, 0.04] +# # ) @ tf.rotation_matrix(np.sin(t) * 0.25, [1, 0, 0]) + +# trunk_task.T_world_frame = tf.translation_matrix( +# [0, 0, 0.25] +# ) @ tf.rotation_matrix(np.sin(t) * 0.25, [1, 0, 0]) + +# solver.solve(True) +# robot.update_kinematics() + + +# if MESHCAT_VIZ: +# viz.display(robot.state.q) +# robot_frame_viz(robot, "left_foot") +# frame_viz("left_foot_target", left_foot_task.T_world_frame, opacity=0.25) + +# t += dt + + +# run_loop() +# # task = + +# # while True: + +# # robot.update_kinematics() +# # time.sleep(DT) diff --git a/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py b/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py new file mode 100644 index 0000000..b3ec714 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py @@ -0,0 +1,114 @@ +from mini_bdx_runtime.hwi_feetech_pypot import HWI +from mini_bdx.placo_walk_engine.placo_walk_engine import PlacoWalkEngine +from placo_utils.visualization import robot_viz +import json +import time +import argparse +from queue import Queue + +parser = argparse.ArgumentParser() +parser.add_argument("--xbox", action="store_true") +parser.add_argument("--viz", action="store_true") +args = parser.parse_args() + + +MAX_X = 0.02 +MAX_Y = 0.02 +MAX_THETA = 0.1 + + +if args.xbox: + from threading import Thread + import pygame + + pygame.init() + _p1 = pygame.joystick.Joystick(0) + _p1.init() + print(f"Loaded joystick with {_p1.get_numaxes()} axes.") + + cmd_queue = Queue(maxsize=1) + + def commands_worker(): + global cmd_queue + + while True: + for event in pygame.event.get(): + l_x = round(_p1.get_axis(0), 3) + l_y = round(_p1.get_axis(1), 3) + r_x = round(_p1.get_axis(3), 3) + r_y = round(_p1.get_axis(4), 3) + l_x = 0.0 if abs(l_x) < 0.1 else l_x + l_y = 0.0 if abs(l_y) < 0.1 else l_y + r_x = 0.0 if abs(r_x) < 0.1 else r_x + r_y = 0.0 if abs(r_y) < 0.1 else r_y + + pygame.event.pump() # process event queue + cmd = { + "l_x": l_x, + "l_y": l_y, + "r_x": r_x, + "r_y": r_y, + } + + cmd_queue.put(cmd) + time.sleep(1 / 30) + + Thread(target=commands_worker, daemon=True).start() + last_commands = { + "l_x": 0.0, + "l_y": 0.0, + "r_x": 0.0, + "r_y": 0.0, + } + + def get_last_command(): + global cmd_queue, last_commands + try: + last_commands = cmd_queue.get(False) # non blocking + except Exception: + pass + + return last_commands + + +if not args.viz: + hwi = HWI() + hwi.turn_on() + + +DT = 0.01 +pwe = PlacoWalkEngine( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2", + model_filename="robot.urdf", + init_params=json.load(open("placo_defaults.json")), + ignore_feet_contact=True, +) +if args.viz: + viz = robot_viz(pwe.robot) + +pwe.set_traj(0.0, 0, 0.0) +while True: + pwe.tick(DT) + + if args.xbox: + commands = get_last_command() + placo_commands = [ + -commands["l_y"] * MAX_X, + -commands["l_x"] * MAX_Y, + -commands["r_x"] * MAX_THETA, + ] + print(placo_commands) + print("====") + if commands is not None: + pwe.set_traj(*placo_commands) + + if not args.viz: + all_angles = list(pwe.get_angles().values()) + angles = {} + for i, motor_name in enumerate(hwi.joints.keys()): + angles[motor_name] = all_angles[i] + hwi.set_position_all(angles) + else: + viz.display(pwe.robot.state.q) + + time.sleep(DT) diff --git a/Open_Duck_Mini-2/experiments/v2/plot.py b/Open_Duck_Mini-2/experiments/v2/plot.py new file mode 100644 index 0000000..ea07bc0 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/plot.py @@ -0,0 +1,40 @@ +import pickle + +data = pickle.load(open("data_pwm_control.pkl", "rb")) + + +# data looks like: +# data = { +# "present_positions": present_positions, +# "goal_positions": goal_positions, +# "present_loads": present_loads, +# "present_currents": present_currents, +# "present_speeds": present_speeds, +# "times": times, +# } + +# present_positions, goal_positions etc are lists. All of the same size. + +# plot all on the same plot against time with shared x +# Label everything + + +import matplotlib.pyplot as plt + +fig, axs = plt.subplots(4, 1, sharex=True) + +axs[0].plot(data["times"], data["present_positions"], label="Present positions") +axs[0].plot(data["times"], data["goal_positions"], label="Goal positions") +axs[0].set_ylabel("Positions") +axs[0].legend() + +axs[1].plot(data["times"], data["present_loads"], label="Present loads") +axs[1].set_ylabel("Loads") + +axs[2].plot(data["times"], data["present_currents"], label="Present currents") +axs[2].set_ylabel("Currents") + +axs[3].plot(data["times"], data["present_speeds"], label="Present speeds") +axs[3].set_ylabel("Speeds") + +plt.show() diff --git a/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py b/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py new file mode 100644 index 0000000..14035a9 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py @@ -0,0 +1,11 @@ +import pickle + +# [[x_t0, y_t0, z_t0...], [x_t1, y_t1, z_t1...], ...] +# adaptation_module_latent = pickle.load(open("adaptation_module_latents.pkl", "rb")) +adaptation_module_latent = pickle.load(open("robot_latents.pkl", "rb")) + +from matplotlib import pyplot as plt + +plt.plot(adaptation_module_latent) +plt.show() + diff --git a/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ b/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ new file mode 100644 index 0000000..e67326d --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ @@ -0,0 +1,11 @@ +import pickle + +# [[x_t0, y_t0, z_t0...], [x_t1, y_t1, z_t1...], ...] +adaptation_module_latent = pickle.load(open("adaptation_module_latents.pkl", "rb")) +# adaptation_module_latent = pickle.load(open("robot_latents.pkl", "rb")) + +from matplotlib import pyplot as plt + +plt.plot(adaptation_module_latent) +plt.show() + diff --git a/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py b/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py new file mode 100644 index 0000000..b9ee743 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py @@ -0,0 +1,142 @@ +from pypot.feetech import FeetechSTS3215IO +import time +import numpy as np +from threading import Thread +import pickle + + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) +# io.disable_torque([1]) +# input() +# io.enable_torque([1]) +# input() +# io.disable_torque([1]) +# exit() + +class FeetechPWMControl: + def __init__(self): + self.io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, + ) + self.id = 1 + + # TODO zero first + self.io.enable_torque([self.id]) + self.io.set_mode({self.id: 0}) + self.io.set_goal_position({self.id: 0}) + time.sleep(1) + # exit() + + self.io.set_mode({self.id: 2}) + self.kp = self.io.get_P_coefficient([self.id])[0] + + self.control_freq = 100 # Hz + self.goal_position = 0 + self.present_position = 0 + Thread(target=self.update, daemon=True).start() + + def disable_torque(self): + self.io.set_mode({self.id: 0}) + self.io.disable_torque([self.id]) + + def enable_torque(self): + self.io.enable_torque([self.id]) + self.io.set_mode({self.id: 2}) + + + def update(self): + while True: + self.present_position = self.io.get_present_position([self.id])[0] + error = self.goal_position - self.present_position + + pwm = self.kp * error + # pwm *= 10 + pwm = np.int16(pwm) + + pwm_magnitude = abs(pwm) + if pwm_magnitude >= 2**10: + pwm_magnitude = (2**10) - 1 + + direction_bit = 1 if pwm >= 0 else 0 + + goal_time = (direction_bit << 10) | pwm_magnitude + + self.io.set_goal_time({self.id: goal_time}) + + time.sleep(1 / self.control_freq) + + +motor = FeetechPWMControl() + +s = time.time() +while True: + + target = 20 * np.sin(2 * np.pi * 0.5 * time.time()) + motor.goal_position = target + + if time.time() - s > 3 and time.time() - s < 6: + motor.disable_torque() + print("disbale") + + if time.time() - s > 6: + motor.enable_torque() + print("enable") + time.sleep(1 / 60) + + +present_positions = [] +goal_positions = [] +present_loads = [] +present_currents = [] +present_speeds = [] +times = [] + +motor.goal_position = 90 + +log_start = time.time() +s = time.time() +while time.time() - s < 2: + present_positions.append(motor.io.get_present_position([motor.id])[0]) + goal_positions.append(motor.goal_position) + present_loads.append(motor.io.get_present_load([motor.id])[0]) + present_currents.append(motor.io.get_present_current([motor.id])[0]) + present_speeds.append(motor.io.get_present_speed([motor.id])[0]) + times.append(time.time() - log_start) + +motor.goal_position = -90 +s = time.time() +while time.time() - s < 2: + present_positions.append(motor.io.get_present_position([motor.id])[0]) + goal_positions.append(motor.goal_position) + present_loads.append(motor.io.get_present_load([motor.id])[0]) + present_currents.append(motor.io.get_present_current([motor.id])[0]) + present_speeds.append(motor.io.get_present_speed([motor.id])[0]) + times.append(time.time() - log_start) +motor.goal_position = 90 + +s = time.time() +while time.time() - s < 2: + present_positions.append(motor.io.get_present_position([motor.id])[0]) + goal_positions.append(motor.goal_position) + present_loads.append(motor.io.get_present_load([motor.id])[0]) + present_currents.append(motor.io.get_present_current([motor.id])[0]) + present_speeds.append(motor.io.get_present_speed([motor.id])[0]) + times.append(time.time() - log_start) + + +data = { + "present_positions": present_positions, + "goal_positions": goal_positions, + "present_loads": present_loads, + "present_currents": present_currents, + "present_speeds": present_speeds, + "times": times, +} + +pickle.dump(data, open("data_pwm_control.pkl", "wb")) diff --git a/Open_Duck_Mini-2/experiments/v2/test.py b/Open_Duck_Mini-2/experiments/v2/test.py new file mode 100644 index 0000000..6198845 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/test.py @@ -0,0 +1,39 @@ +import mujoco +import numpy as np +import mujoco_viewer +from mini_bdx_runtime.rl_utils import mujoco_joints_order + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene.xml" +) +model.opt.timestep = 0.001 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +viewer = mujoco_viewer.MujocoViewer(model, data) + +target = [0] * 16 +# data.ctrl[:] = np.zeros((16)) +id = 4 +max_vel = 0 +min_vel = 10000 +while True: + target[id] = 0.2*np.sin(2*np.pi*0.5*data.time) + # target[id] = 0.2 + tau = 6.16*(np.array(target) - data.qpos) - 0.1*data.qvel + data.ctrl[:] = tau + for i, joint_name in enumerate(mujoco_joints_order): + if i == id: + pos = np.around(data.qpos[i], 2) + vel = np.around(data.qvel[i], 2) + # print(f"{joint_name}: pos : {pos}, vel : {vel}") + if vel > max_vel: + max_vel = vel + + if vel < min_vel: + min_vel = vel + + print(f"max vel : {max_vel}, min vel : {min_vel}") + + print("==") + mujoco.mj_step(model, data, 15) + viewer.render() diff --git a/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer.py b/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer.py new file mode 100644 index 0000000..b3303b8 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer.py @@ -0,0 +1,117 @@ +import mujoco +import pickle +from mini_bdx.utils.mujoco_utils import check_contact +import mujoco.viewer +import time +import numpy as np + + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene_position.xml" +) +model.opt.timestep = 0.005 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +decimation = 4 +ctrl_dt = model.opt.timestep * decimation +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos.copy() + + dof_vel = data.qvel.copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + feet_contacts = get_feet_contact() + # feet_contacts = [0, 0] + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + +target = [0] * 16# + init_pos +data.qpos = target +data.ctrl = target + +A = 0.3 +F = 2.0 +joint_id = 2 +counter = 0 +saved_obs = [] +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + t = 0 + while True: + t += model.opt.timestep + # step_start = data.time + mujoco.mj_step(model, data) + counter += 1 + if counter % decimation == 0: + target[joint_id] = A * np.sin(2 * np.pi * F * t)# - init_pos[joint_id] + data.ctrl[joint_id] = target[joint_id] + obs = get_obs(data, data.ctrl, [0, 0, 0]) + saved_obs.append(obs) + + if len(saved_obs) > (1/ctrl_dt) * 10 : # 10 seconds + break + + + viewer.sync() + + # time_until_next_step = model.opt.timestep - (data.time - step_start) + # if time_until_next_step > 0: + # time.sleep(time_until_next_step) + + +pickle.dump(saved_obs, open("mujoco_saved_obs.pkl", "wb")) \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer_motor_control.py b/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer_motor_control.py new file mode 100644 index 0000000..a83052e --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer_motor_control.py @@ -0,0 +1,140 @@ +import mujoco +import pickle +from mini_bdx.utils.mujoco_utils import check_contact +import mujoco.viewer +import time +import numpy as np + + +model = mujoco.MjModel.from_xml_path( + "/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2/scene.xml" +) +model.opt.timestep = 0.005 +data = mujoco.MjData(model) +mujoco.mj_step(model, data) +decimation = 4 +ctrl_dt = model.opt.timestep * decimation +init_pos = np.array( + [ + 0.002, + 0.053, + -0.63, + 1.368, + -0.784, + 0.0, + 0, + 0, + 0, + 0, + 0, + -0.003, + -0.065, + 0.635, + 1.379, + -0.796, + ] +) + +def pd_control(target_q, q, kp, target_dq, dq, kd, clip_val, init_pos, action_scale): + """Calculates torques from position commands""" + tau = (target_q * action_scale - q) * kp - (dq * kd) + tau = np.clip(tau, -clip_val, clip_val) + return tau + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + +def get_feet_contact(): + left_contact = check_contact(data, model, "foot_assembly", "floor") + right_contact = check_contact(data, model, "foot_assembly_2", "floor") + return [left_contact, right_contact] + +def get_obs(data, prev_action, commands): + base_quat = data.qpos[3 : 3 + 4].copy() + base_quat = [base_quat[1], base_quat[2], base_quat[3], base_quat[0]] + + dof_pos = data.qpos.copy() + + dof_vel = data.qvel.copy() + + projected_gravity = quat_rotate_inverse(base_quat, [0, 0, -1]) + + feet_contacts = get_feet_contact() + # feet_contacts = [0, 0] + + obs = np.concatenate( + [ + projected_gravity, + dof_pos, + dof_vel, + feet_contacts, + prev_action, + commands, + ] + ) + + return obs + +target = [0] * 16# + init_pos +data.qpos = target +data.ctrl = target + +kps = np.array([9.5] * 16) +kds = np.array([1.0] * 16) + +A = 0.3 +F = 2.0 +joint_id = 2 +counter = 0 +saved_obs = [] +with mujoco.viewer.launch_passive( + model, data, show_left_ui=False, show_right_ui=False +) as viewer: + t = 0 + while True: + t += model.opt.timestep + # step_start = data.time + + tau = pd_control( + target, + data.qpos.copy(), + kps, + np.zeros_like(kds), + data.qvel.copy(), + kds, + 5.2, + init_pos, + 1, + ) + data.ctrl = tau + + mujoco.mj_step(model, data) + counter += 1 + if counter % decimation == 0: + target[joint_id] = A * np.sin(2 * np.pi * F * t)# - init_pos[joint_id] + # data.ctrl[joint_id] = target[joint_id] + obs = get_obs(data, target, [0, 0, 0]) + saved_obs.append(obs) + + if len(saved_obs) > (1/ctrl_dt) * 10 : # 10 seconds + break + + + viewer.sync() + + # time_until_next_step = model.opt.timestep - (data.time - step_start) + # if time_until_next_step > 0: + # time.sleep(time_until_next_step) + + +pickle.dump(saved_obs, open("mujoco_saved_obs.pkl", "wb")) \ No newline at end of file diff --git a/Open_Duck_Mini-2/experiments/v2/walk_test.py b/Open_Duck_Mini-2/experiments/v2/walk_test.py new file mode 100644 index 0000000..4ab6f54 --- /dev/null +++ b/Open_Duck_Mini-2/experiments/v2/walk_test.py @@ -0,0 +1,16 @@ +from mini_bdx.placo_walk_engine.placo_walk_engine import PlacoWalkEngine +import time +from placo_utils.visualization import footsteps_viz, robot_frame_viz, robot_viz +import json + + + +DT = 0.01 +pwe = PlacoWalkEngine("/home/antoine/MISC/mini_BDX/mini_bdx/robots/open_duck_mini_v2", model_filename="robot.urdf", init_params=json.load(open("placo_defaults.json"))) +viz = robot_viz(pwe.robot) + +pwe.set_traj(0.0, 0, 0.0) +while True: + pwe.tick(DT) + viz.display(pwe.robot.state.q) + time.sleep(DT) diff --git a/Open_Duck_Mini-2/mini_bdx/__init__.py b/Open_Duck_Mini-2/mini_bdx/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/__init__.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/__init__.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/__init__.py new file mode 100644 index 0000000..eb1ba4b --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/__init__.py @@ -0,0 +1 @@ +from .walk_engine import WalkEngine diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/walk_engine.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/walk_engine.py new file mode 100644 index 0000000..47482c9 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/walk_engine.py @@ -0,0 +1,477 @@ +# Based on https://github.com/Rhoban/walk_engine + +import FramesViewer.utils as fv_utils +import numpy as np +import placo + +from mini_bdx.utils import PolySpline + + +class FootPose: + def __init__(self): + self.x = 0 + self.y = 0 + self.z = 0 + self.yaw = 0 + + def __eq__(self, other): + return ( + self.x == other.x + and self.y == other.y + and self.z == other.z + and self.yaw == other.yaw + ) + + @property + def foot_to_trunk(self): + # Returns a frame from foot to trunk frame (this is actually a 2d matrix) + # TODO + pass + + +class Foot: + def __init__(self): + self.x_spline = PolySpline() + self.y_spline = PolySpline() + self.z_spline = PolySpline() + self.yaw_spline = PolySpline() + self.trunk_y_offset = 0 + + def get_position(self, t): + # Get the foot position, t is from 0 to 1, playing the footstep + foot_pose = FootPose() + foot_pose.x = self.x_spline.get(t) + foot_pose.y = self.y_spline.get(t) + foot_pose.z = self.z_spline.get(t) + foot_pose.yaw = self.yaw_spline.get(t) + return foot_pose + + def clear_splines(self): + self.x_spline.clear() + self.y_spline.clear() + self.z_spline.clear() + self.yaw_spline.clear() + + def copy(self): + foot = Foot() + foot.x_spline = self.x_spline.copy() + foot.y_spline = self.y_spline.copy() + foot.z_spline = self.z_spline.copy() + foot.yaw_spline = self.yaw_spline.copy() + foot.trunk_y_offset = self.trunk_y_offset + return foot + + +class WalkEngine: + def __init__( + self, + robot: placo.RobotWrapper, + default_trunk_x_offset: float = 0.007, # 0.007 + default_trunk_z_offset: float = -0.003, + foot_y_offset: float = 0.0, + max_rise_gain: float = 0.015, + rise_duration: float = 0.2, + frequency: float = 2.0, + swing_gain: float = -0.001, + swing_phase: float = 0.0, + foot_y_offset_per_step_size_y: float = 0.02, + target_trunk_pitch: float = 0, + target_trunk_roll: float = 0, + step_size_x: float = 0, + step_size_y: float = 0, + step_size_yaw: float = 0, + ): + kinematics_solver = placo.KinematicsSolver(robot) + self.left = Foot() + self.right = Foot() + self.swing_spline = PolySpline() + + self.trunk_pitch = 0 + self.trunk_roll = 0 + + self.trunk_pitch_timeout = 1.0 # s + + self.target_trunk_pitch = target_trunk_pitch + self.target_trunk_roll = target_trunk_roll + + self.trunk_pitch_roll_compensation = False + + self.robot = robot + self.kinematics_solver = kinematics_solver + + self.T_world_trunk = np.eye(4) + self.T_world_trunk = fv_utils.rotateInSelf( + self.T_world_trunk, [0, self.trunk_pitch, 0], degrees=True + ) + + self.T_world_head = robot.get_T_world_frame("head") + self.T_world_head = fv_utils.translateInSelf( + self.T_world_head, [-0.05, 0, -0.05] + ) + self.head_task = self.kinematics_solver.add_frame_task( + "head", self.T_world_head + ) + self.head_task.configure("head", "soft") + + self.trunk_task = self.kinematics_solver.add_frame_task( + "trunk", self.T_world_trunk + ) + self.trunk_task.configure("trunk", "hard") + + self.right_foot_task = self.kinematics_solver.add_frame_task( + "right_foot", robot.get_T_world_frame("right_foot") + ) + self.right_foot_task.configure("right_foot", "soft", 5.0, 0.1) + + self.left_foot_task = self.kinematics_solver.add_frame_task( + "left_foot", robot.get_T_world_frame("left_foot") + ) + self.left_foot_task.configure("left_foot", "soft", 5.0, 0.1) + + self.is_left_support = False + + self.trunk_height = -robot.get_T_world_frame("left_foot")[:3, 3][2] / 1.2 + self.foot_distance = np.abs(robot.get_T_world_frame("left_foot")[:3, 3][1]) + + self.default_trunk_x_offset = default_trunk_x_offset + self.forward_trunk_x_offset = self.default_trunk_x_offset - 0.002 + self.backward_trunk_x_offset = self.default_trunk_x_offset + self.tune_trunk_x_offset = 0 + + self.default_trunk_z_offset = default_trunk_z_offset + self.foot_y_offset = foot_y_offset + self.max_rise_gain = max_rise_gain + self.rise_gain = 0 + self.rise_duration = rise_duration + self.frequency = frequency + self.swing_gain = swing_gain + self.swing_phase = swing_phase + self.foot_y_offset_per_step_size_y = foot_y_offset_per_step_size_y + self.step_size_x = step_size_x + self.step_size_y = step_size_y + self.step_size_yaw = step_size_yaw + + self.time_since_last_step = 0 + self.time_since_last_left_contact = 0 + self.time_since_last_right_contact = 0 + + self.step_duration = 0 + self._swing_gain = 0 + + self.reset() + + def get_left_foot_pose(self, t): + left_position = self.left.get_position(t) + + T_world_left_foot = np.eye(4) + T_world_left_foot[:3, 3] = [ + left_position.x, + left_position.y, + left_position.z, + ] + T_world_left_foot = fv_utils.rotateInSelf( + T_world_left_foot, [0, 0, left_position.yaw], degrees=False + ) + + return T_world_left_foot + + def get_right_foot_pose(self, time_since_last_step): + right_position = self.right.get_position(time_since_last_step) + T_world_right_foot = np.eye(4) + T_world_right_foot[:3, 3] = [ + right_position.x, + right_position.y, + right_position.z, + ] + T_world_right_foot = fv_utils.rotateInSelf( + T_world_right_foot, [0, 0, right_position.yaw], degrees=False + ) + return T_world_right_foot + + # gyro is angular position of the trunk [roll, pitch, yaw] + # accelerometer is the acceleration of the trunk [x, y, z] + def update( + self, + walking, + gyro, + accelerometer, + left_contact, + right_contact, + target_step_x, + target_step_y, + target_yaw, + target_head_pitch, + target_head_yaw, + target_head_z_offset, + dt, + ignore_feet_contact=False, + ): + if self.trunk_pitch_timeout > 0: + self.trunk_pitch_timeout -= dt + if left_contact: + self.time_since_last_left_contact = 0 + if right_contact: + self.time_since_last_right_contact = 0 + + if ignore_feet_contact or ( + not self.time_since_last_left_contact > self.rise_duration + and not self.time_since_last_right_contact > self.rise_duration + ): + self.time_since_last_step += dt + + self.time_since_last_left_contact += dt + self.time_since_last_right_contact += dt + + if self.time_since_last_step > self.step_duration: + self.time_since_last_step = 0 + self.new_step() + + target_rise_gain = self.max_rise_gain if walking else 0 + + # slowly increase self.step_size_x and self.step_size_y to target_step_x and target_step_y + # target can be negative or positive or 0 + delta_x = target_step_x - self.step_size_x + delta_y = target_step_y - self.step_size_y + delta_yaw = target_yaw - self.step_size_yaw + delta_rise_gain = target_rise_gain - self.rise_gain + + self.step_size_x = self.step_size_x + (delta_x / 100) + self.step_size_y = self.step_size_y + (delta_y / 100) + self.step_size_yaw = self.step_size_yaw + (delta_yaw / 100) + self.rise_gain = self.rise_gain + (delta_rise_gain / 100) + + swing = 0 + if walking: + self.left_foot_task.T_world_frame = self.get_left_foot_pose( + self.time_since_last_step + ) + self.right_foot_task.T_world_frame = self.get_right_foot_pose( + self.time_since_last_step + ) + + swing_P = 0 if self.is_left_support else np.pi + swing_P += np.pi * 2 * self.swing_phase + swing = self._swing_gain * np.sin( + np.pi * self.time_since_last_step / self.step_duration + swing_P + ) + else: + self.step_size_x = 0 + self.step_size_y = 0 + self.step_size_yaw = 0 + self.left_foot_task.T_world_frame = self.get_left_foot_pose(0) + self.right_foot_task.T_world_frame = self.get_right_foot_pose(0) + + # Trunk pitch and roll + if self.trunk_pitch_roll_compensation: + self.trunk_pitch = max(-30, min(30, -np.rad2deg(gyro[1]) * 5)) + self.trunk_roll = max(-10, min(10, np.rad2deg(gyro[0]))) + # else: + # self.trunk_pitch = 0 + # self.trunk_roll = 0 + + if self.trunk_pitch_timeout <= 0: + delta_trunk_pitch = self.target_trunk_pitch - self.trunk_pitch + self.trunk_pitch = self.trunk_pitch + (delta_trunk_pitch / 100) + + self.T_world_trunk = np.eye(4) + self.T_world_trunk = fv_utils.rotateInSelf( + self.T_world_trunk, [self.trunk_roll, self.trunk_pitch, 0], degrees=True + ) + self.T_world_trunk[:3, 3] = [0, swing, 0] + + fr = self.T_world_trunk + fr[:3, 3] = [0, swing, 0] + self.trunk_task.T_world_frame = fr + + # Head + tmp = self.T_world_head.copy() + tmp = fv_utils.translateInSelf(tmp, [0, 0, -target_head_z_offset]) + tmp = fv_utils.rotateInSelf( + tmp, [0, target_head_pitch, target_head_yaw], degrees=False + ) + self.head_task.T_world_frame = tmp + + self.robot.update_kinematics() + self.kinematics_solver.solve(True) + + def get_angles(self): + angles = { + "right_hip_yaw": self.robot.get_joint("right_hip_yaw"), + "right_hip_roll": self.robot.get_joint("right_hip_roll"), + "right_hip_pitch": self.robot.get_joint("right_hip_pitch"), + "right_knee": self.robot.get_joint("right_knee"), + "right_ankle": self.robot.get_joint("right_ankle"), + "left_hip_yaw": self.robot.get_joint("left_hip_yaw"), + "left_hip_roll": self.robot.get_joint("left_hip_roll"), + "left_hip_pitch": self.robot.get_joint("left_hip_pitch"), + "left_knee": self.robot.get_joint("left_knee"), + "left_ankle": self.robot.get_joint("left_ankle"), + "neck_pitch": self.robot.get_joint("neck_pitch"), + "head_pitch": self.robot.get_joint("head_pitch"), + "head_yaw": self.robot.get_joint("head_yaw"), + } + return angles + + def reset(self): + self.left.trunk_y_offset = self.foot_distance + self.foot_y_offset + self.right.trunk_y_offset = -(self.foot_distance + self.foot_y_offset) + + self.is_left_support = False + + self.step_duration = 1.0 / (2 * self.frequency) + self.left.clear_splines() + self.right.clear_splines() + self.left.x_spline.add_point(self.step_duration, self.trunk_x_offset, 0) + self.left.y_spline.add_point(self.step_duration, self.left.trunk_y_offset, 0) + self.left.yaw_spline.add_point(self.step_duration, 0, 0) + self.right.x_spline.add_point(self.step_duration, self.trunk_x_offset, 0) + self.right.y_spline.add_point(self.step_duration, self.right.trunk_y_offset, 0) + self.right.yaw_spline.add_point(self.step_duration, 0, 0) + + self.trunk_pitch = 0 + self.trunk_roll = 0 + + self.new_step() + + def new_step(self): + self._swing_gain = self.swing_gain + previous_step_duration = self.step_duration + self.step_duration = 1.0 / (2 * self.frequency) + + old_left = self.left.copy() + old_right = self.right.copy() + self.left.clear_splines() + self.right.clear_splines() + + self.left.trunk_y_offset = ( + self.foot_distance + + self.foot_y_offset + + self.foot_y_offset_per_step_size_y * np.abs(self.step_size_y) + ) + self.right.trunk_y_offset = -( + self.foot_distance + + self.foot_y_offset + + self.foot_y_offset_per_step_size_y * np.abs(self.step_size_y) + ) + + self.left.x_spline.add_point( + 0, + old_left.x_spline.get(previous_step_duration), + old_left.x_spline.get_vel(previous_step_duration), + ) + self.left.y_spline.add_point( + 0, + old_left.y_spline.get(previous_step_duration), + old_left.y_spline.get_vel(previous_step_duration), + ) + self.left.yaw_spline.add_point( + 0, + old_left.yaw_spline.get(previous_step_duration), + old_left.yaw_spline.get_vel(previous_step_duration), + ) + + self.right.x_spline.add_point( + 0, + old_right.x_spline.get(previous_step_duration), + old_right.x_spline.get_vel(previous_step_duration), + ) + self.right.y_spline.add_point( + 0, + old_right.y_spline.get(previous_step_duration), + old_right.y_spline.get_vel(previous_step_duration), + ) + self.right.yaw_spline.add_point( + 0, + old_right.yaw_spline.get(previous_step_duration), + old_right.yaw_spline.get_vel(previous_step_duration), + ) + + self.is_left_support = not self.is_left_support + + step_low = -self.trunk_height + self.default_trunk_z_offset + step_high = -self.trunk_height + self.default_trunk_z_offset + self.rise_gain + self.support_foot.z_spline.add_point(0, step_low, 0) + self.support_foot.z_spline.add_point(self.step_duration, step_low, 0) + + self.flying_foot.z_spline.add_point(0, step_low, 0) + if self.rise_duration > 0: + self.flying_foot.z_spline.add_point( + self.step_duration * (0.5 - self.rise_duration / 2.0), + step_high, + 0, + ) + self.flying_foot.z_spline.add_point( + self.step_duration * (0.5 + self.rise_duration / 2.0), + step_high, + 0, + ) + else: + self.flying_foot.z_spline.add_point(self.step_duration * 0.5, step_high, 0) + self.flying_foot.z_spline.add_point(self.step_duration, step_low, 0) + + self.plan_step_end() + + def plan_step_end(self): + self.support_foot.x_spline.add_point( + self.step_duration, self.trunk_x_offset - self.step_size_x / 2.0, 0 + ) + self.support_foot.y_spline.add_point( + self.step_duration, + self.support_foot.trunk_y_offset - self.step_size_y / 2.0, + 0, + ) + + self.flying_foot.x_spline.add_point( + self.step_duration, self.trunk_x_offset + self.step_size_x / 2.0, 0 + ) + self.flying_foot.y_spline.add_point( + self.step_duration, + self.flying_foot.trunk_y_offset + self.step_size_y / 2.0, + 0, + ) + + self.support_foot.yaw_spline.add_point( + self.step_duration, -self.step_size_yaw / 2, 0 + ) + self.flying_foot.yaw_spline.add_point( + self.step_duration, self.step_size_yaw / 2, 0 + ) + + def replan(self): + splines = [ + self.support_foot.x_spline, + self.flying_foot.x_spline, + self.support_foot.y_spline, + self.flying_foot.y_spline, + self.support_foot.yaw_spline, + self.flying_foot.yaw_spline, + ] + + for spline in splines: + old_spline = spline.copy() + spline.clear() + spline.add_point(0, old_spline.get(0), old_spline.get_vel(0)) + spline.add_point( + self.time_since_last_step, + old_spline.get(self.time_since_last_step), + old_spline.get_vel(self.time_since_last_step), + ) + + self.plan_step_end() + + @property + def support_foot(self): + return self.left if self.is_left_support else self.right + + @property + def flying_foot(self): + return self.right if self.is_left_support else self.left + + @property + def trunk_x_offset(self): + if self.step_size_x > 0: + return self.forward_trunk_x_offset + self.tune_trunk_x_offset + elif self.step_size_x == 0: + return self.default_trunk_x_offset + self.tune_trunk_x_offset + else: + return self.backward_trunk_x_offset + self.tune_trunk_x_offset diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/__init__.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/__init__.py new file mode 100644 index 0000000..fca1e25 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/__init__.py @@ -0,0 +1 @@ +from .placo_walk_engine import PlacoWalkEngine diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py new file mode 100644 index 0000000..f2aa415 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py @@ -0,0 +1,301 @@ +import time +import warnings +import json + +import numpy as np +import placo +import os + +warnings.filterwarnings("ignore") + +DT = 0.01 +REFINE = 10 + + +class PlacoWalkEngine: + def __init__( + self, + asset_path: str = "", + model_filename: str = "go_bdx.urdf", + init_params: dict = {}, + ignore_feet_contact: bool = False, + ) -> None: + model_filename = os.path.join(asset_path, model_filename) + self.asset_path = asset_path + self.model_filename = model_filename + self.ignore_feet_contact = ignore_feet_contact + + # Loading the robot + self.robot = placo.HumanoidRobot(model_filename) + + self.parameters = placo.HumanoidParameters() + if init_params is not None: + self.load_parameters(init_params) + else: + defaults_filename = os.path.join(asset_path, "placo_defaults.json") + self.load_defaults(defaults_filename) + + # Creating the kinematics solver + self.solver = placo.KinematicsSolver(self.robot) + self.solver.enable_velocity_limits(True) + self.robot.set_velocity_limits(12.0) + self.solver.enable_joint_limits(False) + self.solver.dt = DT / REFINE + + self.robot.set_joint_limits("left_knee", 2, 0.01) + self.robot.set_joint_limits("right_knee", 2, 0.01) + + # Creating the walk QP tasks + self.tasks = placo.WalkTasks() + if hasattr(self.parameters, "trunk_mode"): + self.tasks.trunk_mode = self.parameters.trunk_mode + self.tasks.com_x = 0.0 + self.tasks.initialize_tasks(self.solver, self.robot) + self.tasks.left_foot_task.orientation().mask.set_axises("yz", "local") + self.tasks.right_foot_task.orientation().mask.set_axises("yz", "local") + # tasks.trunk_orientation_task.configure("trunk_orientation", "soft", 1e-4) + # tasks.left_foot_task.orientation().configure("left_foot_orientation", "soft", 1e-6) + # tasks.right_foot_task.orientation().configure("right_foot_orientation", "soft", 1e-6) + + # # Creating a joint task to assign DoF values for upper body + self.joints = self.parameters.joints + joint_degrees = self.parameters.joint_angles + joint_radians = { + joint: np.deg2rad(degrees) for joint, degrees in joint_degrees.items() + } + self.joints_task = self.solver.add_joints_task() + self.joints_task.set_joints(joint_radians) + self.joints_task.configure("joints", "soft", 1.0) + + # Placing the robot in the initial position + print("Placing the robot in the initial position...") + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + print("Initial position reached") + + print(self.get_angles()) + # exit() + + # Creating the FootstepsPlanner + self.repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive( + self.parameters + ) + self.d_x = 0.0 + self.d_y = 0.0 + self.d_theta = 0.0 + self.nb_steps = 5 + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + + # Creating the pattern generator and making an initial plan + self.walk = placo.WalkPatternGenerator(self.robot, self.parameters) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + self.start = None + self.initial_delay = -1.0 + # self.initial_delay = 0 + self.t = self.initial_delay + self.last_replan = 0 + + # TODO remove startend_double_support_duration() when starting and ending ? + self.period = ( + 2 * self.parameters.single_support_duration + + 2 * self.parameters.double_support_duration() + ) + + def load_defaults(self, filename): + with open(filename, "r") as f: + data = json.load(f) + params = self.parameters + load_parameters(data) + + def load_parameters(self, data): + params = self.parameters + params.double_support_ratio = data.get( + "double_support_ratio", params.double_support_ratio + ) + params.startend_double_support_ratio = data.get( + "startend_double_support_ratio", params.startend_double_support_ratio + ) + params.planned_timesteps = data.get( + "planned_timesteps", params.planned_timesteps + ) + params.replan_timesteps = data.get("replan_timesteps", params.replan_timesteps) + params.walk_com_height = data.get("walk_com_height", params.walk_com_height) + params.walk_foot_height = data.get("walk_foot_height", params.walk_foot_height) + params.walk_trunk_pitch = np.deg2rad( + data.get("walk_trunk_pitch", np.rad2deg(params.walk_trunk_pitch)) + ) + params.walk_foot_rise_ratio = data.get( + "walk_foot_rise_ratio", params.walk_foot_rise_ratio + ) + params.single_support_duration = data.get( + "single_support_duration", params.single_support_duration + ) + params.single_support_timesteps = data.get( + "single_support_timesteps", params.single_support_timesteps + ) + params.foot_length = data.get("foot_length", params.foot_length) + params.feet_spacing = data.get("feet_spacing", params.feet_spacing) + params.zmp_margin = data.get("zmp_margin", params.zmp_margin) + params.foot_zmp_target_x = data.get( + "foot_zmp_target_x", params.foot_zmp_target_x + ) + params.foot_zmp_target_y = data.get( + "foot_zmp_target_y", params.foot_zmp_target_y + ) + params.walk_max_dtheta = data.get("walk_max_dtheta", params.walk_max_dtheta) + params.walk_max_dy = data.get("walk_max_dy", params.walk_max_dy) + params.walk_max_dx_forward = data.get( + "walk_max_dx_forward", params.walk_max_dx_forward + ) + params.walk_max_dx_backward = data.get( + "walk_max_dx_backward", params.walk_max_dx_backward + ) + params.joints = data.get("joints", []) + params.joint_angles = data.get("joint_angles", []) + if "trunk_mode" in data: + params.trunk_mode = data.get("trunk_mode") + + def get_angles(self, ignore=[]): + angles = {joint: self.robot.get_joint(joint) for joint in self.joints} + for joint in ignore: + angles.pop(joint, None) + return angles + + def reset(self): + self.t = self.initial_delay + self.start = None + self.last_replan = 0 + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + def set_traj(self, d_x, d_y, d_theta): + self.d_x = d_x + self.d_y = d_y + self.d_theta = d_theta + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + def get_footsteps_in_world(self): + footsteps = self.trajectory.get_supports() + footsteps_in_world = [] + for footstep in footsteps: + if not footstep.is_both(): + footsteps_in_world.append(footstep.frame()) + + for i in range(len(footsteps_in_world)): + footsteps_in_world[i][:3, 3][1] += self.parameters.feet_spacing / 2 + + return footsteps_in_world + + def get_footsteps_in_robot_frame(self): + T_world_fbase = self.robot.get_T_world_fbase() + + footsteps = self.trajectory.get_supports() + footsteps_in_robot_frame = [] + for footstep in footsteps: + if not footstep.is_both(): + T_world_footstepFrame = footstep.frame().copy() + T_fbase_footstepFrame = ( + np.linalg.inv(T_world_fbase) @ T_world_footstepFrame + ) + T_fbase_footstepFrame = placo.flatten_on_floor(T_fbase_footstepFrame) + T_fbase_footstepFrame[:3, 3][2] = -T_world_fbase[:3, 3][2] + + footsteps_in_robot_frame.append(T_fbase_footstepFrame) + + return footsteps_in_robot_frame + + def get_current_support_phase(self): + if self.trajectory.support_is_both(self.t): + return "both" + + return self.trajectory.support_side(self.t) + + def tick(self, dt, left_contact=True, right_contact=True): + if self.start is None: + self.start = time.time() + + if not self.ignore_feet_contact: + if left_contact: + self.time_since_last_left_contact = 0.0 + if right_contact: + self.time_since_last_right_contact = 0.0 + + falling = not self.ignore_feet_contact and ( + self.time_since_last_left_contact > self.parameters.single_support_duration + or self.time_since_last_right_contact + > self.parameters.single_support_duration + ) + + for k in range(REFINE): + # Updating the QP tasks from planned trajectory + if not falling: + self.tasks.update_tasks_from_trajectory( + self.trajectory, self.t - dt + k * dt / REFINE + ) + + self.robot.update_kinematics() + _ = self.solver.solve(True) + + # If enough time elapsed and we can replan, do the replanning + if ( + self.t - self.last_replan + > self.parameters.replan_timesteps * self.parameters.dt() + and self.walk.can_replan_supports(self.trajectory, self.t) + ): + self.last_replan = self.t + + # Replanning footsteps from current trajectory + self.supports = self.walk.replan_supports( + self.repetitive_footsteps_planner, self.trajectory, self.t + ) + + # Replanning CoM trajectory, yielding a new trajectory we can switch to + self.trajectory = self.walk.replan(self.supports, self.trajectory, self.t) + + self.time_since_last_left_contact += dt + self.time_since_last_right_contact += dt + self.t += dt + + # while time.time() < self.start_t + self.t: + # time.sleep(1e-3) diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py.bak b/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py.bak new file mode 100644 index 0000000..4eccafc --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py.bak @@ -0,0 +1,303 @@ +import time +import warnings + +import numpy as np +import placo + +warnings.filterwarnings("ignore") + +DT = 0.01 +REFINE = 10 + + +class PlacoWalkEngine: + def __init__( + self, + model_filename: str = "../../robots/bdx/robot.urdf", + ignore_feet_contact: bool = False, + ) -> None: + self.model_filename = model_filename + self.ignore_feet_contact = ignore_feet_contact + + # Loading the robot + self.robot = placo.HumanoidRobot(model_filename) + + # Walk parameters - if double_support_ratio is not set to 0, should be greater than replan_frequency + self.parameters = placo.HumanoidParameters() + + self.parameters.double_support_ratio = ( + 0.2 # Ratio of double support (0.0 to 1.0) + ) + self.parameters.startend_double_support_ratio = ( + 1.5 # Ratio duration of supports for starting and stopping walk + ) + self.parameters.planned_timesteps = 48 # Number of timesteps planned ahead + self.parameters.replan_timesteps = 10 # Replanning each n timesteps + # parameters.zmp_reference_weight = 1e-6 + + # Posture parameters + # self.parameters.walk_com_height = 0.16 # Constant height for the CoM [m] + self.parameters.walk_com_height = 0.165 # Constant height for the CoM [m] + self.parameters.walk_foot_height = ( + 0.025 # Height of foot rising while walking [m] # 3 + ) + # self.parameters.walk_trunk_pitch = 0 # Trunk pitch angle [rad] + self.parameters.walk_trunk_pitch = np.deg2rad(-5) # Trunk pitch angle [rad] + self.parameters.walk_foot_rise_ratio = ( + 0.2 # Time ratio for the foot swing plateau (0.0 to 1.0) + ) + self.parameters.single_support_duration = ( + 0.3 # Duration of single support phase [s] + ) + self.parameters.single_support_timesteps = ( + 10 # Number of planning timesteps per single support phase + ) + + # Feet parameters + self.parameters.foot_length = 0.06 # Foot length [m] + # self.parameters.foot_width = 0.006 # Foot width [m] + self.parameters.feet_spacing = 0.14 # Lateral feet spacing [m] # 12 + self.parameters.zmp_margin = 0.00 # ZMP margin [m] + self.parameters.foot_zmp_target_x = ( + 0.0 # Reference target ZMP position in the foot [m] + ) + self.parameters.foot_zmp_target_y = ( + 0.0 # Reference target ZMP position in the foot [m] + ) + + # Limit parameters + self.parameters.walk_max_dtheta = 1 # Maximum dtheta per step [rad] + self.parameters.walk_max_dy = 0.1 # Maximum dy per step [m] + self.parameters.walk_max_dx_forward = 0.08 # Maximum dx per step forward [m] + self.parameters.walk_max_dx_backward = 0.03 # Maximum dx per step backward [m] + + # Creating the kinematics solver + self.solver = placo.KinematicsSolver(self.robot) + self.solver.enable_velocity_limits(True) + self.robot.set_velocity_limits(12.0) + self.solver.enable_joint_limits(False) + self.solver.dt = DT / REFINE + + self.robot.set_joint_limits("left_knee", -2, -0.01) + self.robot.set_joint_limits("right_knee", -2, -0.01) + + # Creating the walk QP tasks + self.tasks = placo.WalkTasks() + # tasks.trunk_mode = True + # self.tasks.com_x = -0.015 + self.tasks.com_x = 0.0 + self.tasks.initialize_tasks(self.solver, self.robot) + self.tasks.left_foot_task.orientation().mask.set_axises("yz", "local") + self.tasks.right_foot_task.orientation().mask.set_axises("yz", "local") + # tasks.trunk_orientation_task.configure("trunk_orientation", "soft", 1e-4) + # tasks.left_foot_task.orientation().configure("left_foot_orientation", "soft", 1e-6) + # tasks.right_foot_task.orientation().configure("right_foot_orientation", "soft", 1e-6) + + # # Creating a joint task to assign DoF values for upper body + self.joints_task = self.solver.add_joints_task() + self.joints_task.set_joints( + { + "head_pitch": np.deg2rad(-10), + "head_yaw": 0.0, + "neck_pitch": np.deg2rad(-10), + "left_antenna": np.deg2rad(0), + "right_antenna": np.deg2rad(0), + # "right_knee": np.deg2rad(-10), + # "left_knee": np.deg2rad(-10), + } + ) + self.joints_task.configure("joints", "soft", 1.0) + + # Placing the robot in the initial position + print("Placing the robot in the initial position...") + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + print("Initial position reached") + + print(self.get_angles()) + # exit() + + # Creating the FootstepsPlanner + self.repetitive_footsteps_planner = placo.FootstepsPlannerRepetitive( + self.parameters + ) + self.d_x = 0.0 + self.d_y = 0.0 + self.d_theta = 0.0 + self.nb_steps = 5 + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + + # Creating the pattern generator and making an initial plan + self.walk = placo.WalkPatternGenerator(self.robot, self.parameters) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + self.start = None + self.initial_delay = -1.0 + # self.initial_delay = 0 + self.t = self.initial_delay + self.last_replan = 0 + + # TODO remove startend_double_support_duration() when starting and ending ? + self.period = ( + 2 * self.parameters.single_support_duration + + 2 * self.parameters.double_support_duration() + ) + + def get_angles(self): + angles = { + "left_hip_yaw": self.robot.get_joint("left_hip_yaw"), + "left_hip_roll": self.robot.get_joint("left_hip_roll"), + "left_hip_pitch": self.robot.get_joint("left_hip_pitch"), + "left_knee": self.robot.get_joint("left_knee"), + "left_ankle": self.robot.get_joint("left_ankle"), + "neck_pitch": self.robot.get_joint("neck_pitch"), + "head_pitch": self.robot.get_joint("head_pitch"), + "head_yaw": self.robot.get_joint("head_yaw"), + "left_antenna": self.robot.get_joint("left_antenna"), + "right_antenna": self.robot.get_joint("right_antenna"), + "right_hip_yaw": self.robot.get_joint("right_hip_yaw"), + "right_hip_roll": self.robot.get_joint("right_hip_roll"), + "right_hip_pitch": self.robot.get_joint("right_hip_pitch"), + "right_knee": self.robot.get_joint("right_knee"), + "right_ankle": self.robot.get_joint("right_ankle"), + } + + return angles + + def reset(self): + self.t = self.initial_delay + self.start = None + self.last_replan = 0 + self.time_since_last_right_contact = 0.0 + self.time_since_last_left_contact = 0.0 + + self.tasks.reach_initial_pose( + np.eye(4), + self.parameters.feet_spacing, + self.parameters.walk_com_height, + self.parameters.walk_trunk_pitch, + ) + + # Planning footsteps + self.T_world_left = placo.flatten_on_floor(self.robot.get_T_world_left()) + self.T_world_right = placo.flatten_on_floor(self.robot.get_T_world_right()) + self.footsteps = self.repetitive_footsteps_planner.plan( + placo.HumanoidRobot_Side.left, self.T_world_left, self.T_world_right + ) + + self.supports = placo.FootstepsPlanner.make_supports( + self.footsteps, True, self.parameters.has_double_support(), True + ) + self.trajectory = self.walk.plan(self.supports, self.robot.com_world(), 0.0) + + def set_traj(self, d_x, d_y, d_theta): + self.d_x = d_x + self.d_y = d_y + self.d_theta = d_theta + self.repetitive_footsteps_planner.configure( + self.d_x, self.d_y, self.d_theta, self.nb_steps + ) + + def get_footsteps_in_world(self): + footsteps = self.trajectory.get_supports() + footsteps_in_world = [] + for footstep in footsteps: + if not footstep.is_both(): + footsteps_in_world.append(footstep.frame()) + + for i in range(len(footsteps_in_world)): + footsteps_in_world[i][:3, 3][1] += self.parameters.feet_spacing / 2 + + return footsteps_in_world + + def get_footsteps_in_robot_frame(self): + T_world_fbase = self.robot.get_T_world_fbase() + + footsteps = self.trajectory.get_supports() + footsteps_in_robot_frame = [] + for footstep in footsteps: + if not footstep.is_both(): + T_world_footstepFrame = footstep.frame().copy() + T_fbase_footstepFrame = ( + np.linalg.inv(T_world_fbase) @ T_world_footstepFrame + ) + T_fbase_footstepFrame = placo.flatten_on_floor(T_fbase_footstepFrame) + T_fbase_footstepFrame[:3, 3][2] = -T_world_fbase[:3, 3][2] + + footsteps_in_robot_frame.append(T_fbase_footstepFrame) + + return footsteps_in_robot_frame + + def get_current_support_phase(self): + if self.trajectory.support_is_both(self.t): + return "both" + + return self.trajectory.support_side(self.t) + + def tick(self, dt, left_contact=True, right_contact=True): + if self.start is None: + self.start = time.time() + + if not self.ignore_feet_contact: + if left_contact: + self.time_since_last_left_contact = 0.0 + if right_contact: + self.time_since_last_right_contact = 0.0 + + falling = not self.ignore_feet_contact and ( + self.time_since_last_left_contact > self.parameters.single_support_duration + or self.time_since_last_right_contact + > self.parameters.single_support_duration + ) + + for k in range(REFINE): + # Updating the QP tasks from planned trajectory + if not falling: + self.tasks.update_tasks_from_trajectory( + self.trajectory, self.t - dt + k * dt / REFINE + ) + + self.robot.update_kinematics() + _ = self.solver.solve(True) + + # If enough time elapsed and we can replan, do the replanning + if ( + self.t - self.last_replan + > self.parameters.replan_timesteps * self.parameters.dt() + and self.walk.can_replan_supports(self.trajectory, self.t) + ): + self.last_replan = self.t + + # Replanning footsteps from current trajectory + self.supports = self.walk.replan_supports( + self.repetitive_footsteps_planner, self.trajectory, self.t + ) + + # Replanning CoM trajectory, yielding a new trajectory we can switch to + self.trajectory = self.walk.replan(self.supports, self.trajectory, self.t) + + self.time_since_last_left_contact += dt + self.time_since_last_right_contact += dt + self.t += dt + + # while time.time() < self.start_t + self.t: + # time.sleep(1e-3) diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py new file mode 100644 index 0000000..faf8926 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py @@ -0,0 +1 @@ +from .poly_spline import PolySpline diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/mujoco_utils.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/mujoco_utils.py new file mode 100644 index 0000000..8edbfa2 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/mujoco_utils.py @@ -0,0 +1,70 @@ +import mujoco +import numpy as np + + +def check_contact(data, model, body1_name, body2_name): + body1_id = data.body(body1_name).id + body2_id = data.body(body2_name).id + + for i in range(data.ncon): + try: + contact = data.contact[i] + except Exception as e: + return False + + if ( + model.geom_bodyid[contact.geom1] == body1_id + and model.geom_bodyid[contact.geom2] == body2_id + ) or ( + model.geom_bodyid[contact.geom1] == body2_id + and model.geom_bodyid[contact.geom2] == body1_id + ): + return True + + return False + + +def get_contact_force(data, model, body1_name, body2_name): + body1_id = data.body(body1_name).id + body2_id = data.body(body2_name).id + + contacts = [] + for i in range(data.ncon): + try: + contact = data.contact[i] + except Exception as e: + return 0 + + if ( + model.geom_bodyid[contact.geom1] == body1_id + and model.geom_bodyid[contact.geom2] == body2_id + ) or ( + model.geom_bodyid[contact.geom1] == body2_id + and model.geom_bodyid[contact.geom2] == body1_id + ): + contacts.append((i, contact)) + + if len(contacts) == 0: + return 0 + + force = 0 + for i, con in contacts: + c_array = np.zeros(6, dtype=np.float64) + mujoco.mj_contactForce(model, data, i, c_array) + force += np.linalg.norm(c_array) + + return force + + +def get_actuator_name(model, index: int) -> str: + return mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_ACTUATOR, index) + + +def get_actuator_index(model, name: str) -> int: + return mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) + + +def list_actuators(model): + for i in range(20): + name = get_actuator_name(model, i) + print(i, name) diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/poly_spline.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/poly_spline.py new file mode 100644 index 0000000..ad00ed3 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/poly_spline.py @@ -0,0 +1,149 @@ +import numpy as np + + +class Point: + def __init__(self, position: float, value: float, delta: float): + self.position = position + self.value = value + self.delta = delta + + +class Points: + def __init__(self): + self.points = [] + + +class Polynom: + def __init__(self, a: float, b: float, c: float, d: float): + self.a = a + self.b = b + self.c = c + self.d = d + + +class Spline: + def __init__(self, poly: Polynom, min: float, max: float): + self.poly = poly + self.min = min + self.max = max + + +class Splines: + def __init__(self): + self.splines = [] + + +class PolySpline: + def __init__(self): + self._points = [] + self._splines = [] + + def add_point(self, position: float, value: float, delta: float): + if len(self._points) > 0 and position <= self._points[-1].position: + raise Exception( + "Trying to add a point in a cublic spline before a previous one" + ) + self._points.append(Point(position, value, delta)) + + self.compute_splines() + + def copy(self): + poly_spline = PolySpline() + for point in self._points: + poly_spline.add_point(point.position, point.value, point.delta) + return poly_spline + + def get(self, x: float): + return self.interpolation(x, "value") + + def get_vel(self, x: float): + return self.interpolation(x, "speed") + + def get_mod(self, x: float): + if x < 0.0: + x = 1.0 + (x - ((int(x) / 1))) + elif x > 1.0: + x = x - ((int(x) / 1)) + return self.get(x) + + def clear(self): + self._points = [] + self._splines = [] + + def compute_splines(self): + self._splines = [] + if len(self._points) < 2: + return + + for i in range(1, len(self._points)): + if ( + np.abs(self._points[i - 1].position - self._points[i].position) + < 0.00001 + ): + continue + poly = self.polynom_fit( + self._points[i - 1].value, + self._points[i - 1].delta, + self._points[i].value, + self._points[i].delta, + ) + spline = Spline( + poly, self._points[i - 1].position, self._points[i].position + ) + self._splines.append(spline) + + def polynom_fit(self, val1: float, delta1: float, val2: float, delta2: float): + a = 2.0 * val1 + delta1 + delta2 - 2.0 * val2 + b = 3.0 * val2 - 2.0 * delta1 - 3.0 * val1 - delta2 + c = delta1 + d = val1 + return Polynom(a, b, c, d) + + def interpolation( + self, x: float, value_type="value" + ): # value_type can be "value" or "speed" + if value_type not in ["value", "speed"]: + raise Exception("Invalid value_type") + + if len(self._points) == 0: + return 0.0 + elif len(self._points) == 1: + if value_type == "value": + return self._points[0].value + else: + return self._points[0].delta + else: + if x < self._splines[0].min: + x = self._splines[0].min + if x > self._splines[-1].max: + x = self._splines[-1].max + + for i in range(len(self._splines)): + if x >= self._splines[i].min and x <= self._splines[i].max: + xi = (x - self._splines[i].min) / ( + self._splines[i].max - self._splines[i].min + ) + if value_type == "value": + return self.polynom_value(xi, self._splines[i].poly) + elif value_type == "speed": + return self.polynom_diff(xi, self._splines[i].poly) + return 0.0 + + def polynom_value(self, t: float, p: Polynom): + return p.d + t * (t * (p.a * t + p.b) + p.c) + + def polynom_diff(self, t: float, p: Polynom): + return t * (3 * p.a * t + 2 * p.b) + p.c + + +if __name__ == "__main__": + poly_spline = PolySpline() + poly_spline.add_point(0.0, 0.0, 0.0) + poly_spline.add_point(1.0, 1.0, 0.0) + poly_spline.add_point(2.0, 0.0, 0.0) + import matplotlib.pyplot as plt + + x = np.linspace(-1, 3, 100) + y = [poly_spline.get(i) for i in x] + plt.plot(x, y) + plt.show() diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/rl_utils.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/rl_utils.py new file mode 100644 index 0000000..a5a0ae9 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/rl_utils.py @@ -0,0 +1,120 @@ +# This is the joints order when loading using IsaacGymEnvs +# ['left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna', 'right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle'] +# This is the "standard" order (from mujoco) +# ['right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle', 'left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna'] +# +# We need to reorder the joints to match the IsaacGymEnvs order +# + +mujoco_joints_order = [ + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", +] + +isaac_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + + +def isaac_to_mujoco(joints): + new_joints = [ + # right leg + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + # left leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # head + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + ] + + return new_joints + + +def mujoco_to_isaac(joints): + new_joints = [ + # left leg + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + # head + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + # right leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + ] + return new_joints + + +def test(joints): + new_joints = [ + # right leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # head + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + # left leg + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + ] + return new_joints + + +def action_to_pd_targets(action, pd_action_offset, pd_action_scale): + return pd_action_offset + pd_action_scale * action diff --git a/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/xbox_controller.py b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/xbox_controller.py new file mode 100644 index 0000000..bd0b640 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/xbox_controller.py @@ -0,0 +1,130 @@ +import math +import threading + +from inputs import get_gamepad + + +class XboxController(object): + MAX_TRIG_VAL = math.pow(2, 8) + MAX_JOY_VAL = math.pow(2, 15) + + def __init__(self): + + self.LeftJoystickY = 0 + self.LeftJoystickX = 0 + self.RightJoystickY = 0 + self.RightJoystickX = 0 + self.LeftTrigger = 0 + self.RightTrigger = 0 + self.LeftBumper = 0 + self.RightBumper = 0 + self.A = 0 + self.X = 0 + self.Y = 0 + self.B = 0 + self.LeftThumb = 0 + self.RightThumb = 0 + self.Back = 0 + self.Start = 0 + self.LeftDPad = 0 + self.RightDPad = 0 + self.UpDPad = 0 + self.DownDPad = 0 + + self._monitor_thread = threading.Thread( + target=self._monitor_controller, args=() + ) + self._monitor_thread.daemon = True + self._monitor_thread.start() + + def deadzone(self, val, range): + if abs(val) < range: + return 0 + return val + + def read(self): # return the buttons/triggers that you care about in this method + ret = { + "l_x": round(self.deadzone(self.LeftJoystickX, 0.15), 2), + "l_y": round(self.deadzone(self.LeftJoystickY, 0.15), 2), + "r_x": round(self.deadzone(self.RightJoystickX, 0.15), 2), + "r_y": round(self.deadzone(self.RightJoystickY, 0.15), 2), + "dpad_left": self.LeftDPad, + "dpad_right": self.RightDPad, + "dpad_up": self.UpDPad, + "dpad_down": self.DownDPad, + "l_trigger": round(self.LeftTrigger / 4.0, 2), + "r_trigger": round(self.RightTrigger / 4.0, 2), + "l_bumper": self.LeftBumper, + "r_bumper": self.RightBumper, + "a": self.A, + "x": self.X, + "y": self.Y, + "b": self.B, + "start": self.Start, + "back": self.Back, + } + + return ret + + def _monitor_controller(self): + while True: + events = get_gamepad() + for event in events: + if event.code == "ABS_Y": + self.LeftJoystickY = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_X": + self.LeftJoystickX = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RY": + self.RightJoystickY = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_RX": + self.RightJoystickX = ( + event.state / XboxController.MAX_JOY_VAL + ) # normalize between -1 and 1 + elif event.code == "ABS_Z": + self.LeftTrigger = ( + event.state / XboxController.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "ABS_RZ": + self.RightTrigger = ( + event.state / XboxController.MAX_TRIG_VAL + ) # normalize between 0 and 1 + elif event.code == "BTN_TL": + self.LeftBumper = event.state + elif event.code == "BTN_TR": + self.RightBumper = event.state + elif event.code == "BTN_SOUTH": + self.A = event.state + elif event.code == "BTN_NORTH": + self.X = event.state # previously switched with Y + elif event.code == "BTN_WEST": + self.Y = event.state # previously switched with X + elif event.code == "BTN_EAST": + self.B = event.state + elif event.code == "BTN_THUMBL": + self.LeftThumb = event.state + elif event.code == "BTN_THUMBR": + self.RightThumb = event.state + elif event.code == "BTN_SELECT": + self.Back = event.state + elif event.code == "BTN_START": + self.Start = event.state + elif event.code == "BTN_TRIGGER_HAPPY1": + self.LeftDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY2": + self.RightDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY3": + self.UpDPad = event.state + elif event.code == "BTN_TRIGGER_HAPPY4": + self.DownDPad = event.state + + +if __name__ == "__main__": + joy = XboxController() + while True: + print(joy.read()) diff --git a/Open_Duck_Mini-2/mini_bdx/robots/.gitignore b/Open_Duck_Mini-2/mini_bdx/robots/.gitignore new file mode 100644 index 0000000..9c9cd7b --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/.gitignore @@ -0,0 +1 @@ +old_bdx \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/.gitignore~ b/Open_Duck_Mini-2/mini_bdx/robots/.gitignore~ new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part new file mode 100644 index 0000000..f06679f --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MRjqS1rfFrE9mVebb", + "isStandardContent": false, + "name": "39281023.prt <1>", + "partId": "JFn", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl new file mode 100644 index 0000000..3ca5977 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part new file mode 100644 index 0000000..3d84899 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "Mme4otj1W/8+N1ByX", + "isStandardContent": false, + "name": "antenna <1>", + "partId": "RNCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl new file mode 100644 index 0000000..957ed1d Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.part new file mode 100644 index 0000000..c089398 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MnM0IlZpoFlIWBoQO", + "isStandardContent": false, + "name": "antenna_holder_left <1>", + "partId": "RSDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl new file mode 100644 index 0000000..c014a7b Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.part new file mode 100644 index 0000000..446e932 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MCFYWU6z+m7i7uuh3", + "isStandardContent": false, + "name": "antenna_holder_right <1>", + "partId": "R8BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl new file mode 100644 index 0000000..cb55a2d Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.part new file mode 100644 index 0000000..5673957 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MQd2hqMci0z3nsWR2", + "isStandardContent": false, + "name": "antenna_motor_holder <1>", + "partId": "RpCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl new file mode 100644 index 0000000..4510f41 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part new file mode 100644 index 0000000..28a1b78 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MDIl/o7j9ZkP0F1Lu", + "isStandardContent": false, + "name": "antenna_tip <1>", + "partId": "RQCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl new file mode 100644 index 0000000..10ffbe9 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.part new file mode 100644 index 0000000..90f64f3 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "Mq59OqMGpBdxKaLN8", + "isStandardContent": false, + "name": "axis_to_axis <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl new file mode 100644 index 0000000..8bd53af Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.part new file mode 100644 index 0000000..a8e0332 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "M9iQmeJTc2SjWPNZy", + "isStandardContent": false, + "name": "block_to_axis <1>", + "partId": "JND", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl new file mode 100644 index 0000000..b811f43 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part new file mode 100644 index 0000000..5adf5a2 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MqxaAqvh7r8oVUe/M", + "isStandardContent": false, + "name": "bms <1>", + "partId": "JSD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl new file mode 100644 index 0000000..4598b81 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part new file mode 100644 index 0000000..834b727 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "58e35a434b8265113623a1c6", + "fullConfiguration": "default", + "id": "McEQHWuSIpb4LzRnT", + "isStandardContent": false, + "name": "BNO055 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl new file mode 100644 index 0000000..047b620 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part new file mode 100644 index 0000000..dacfd4b --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MTEKAmJCtStJHQZcz", + "isStandardContent": false, + "name": "Board <1>", + "partId": "JFf", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl new file mode 100644 index 0000000..ebdecfe Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part new file mode 100644 index 0000000..bf178fb --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MgtEBF9hk9yBKNuFr", + "isStandardContent": false, + "name": "body_part <1>", + "partId": "RUBD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl new file mode 100644 index 0000000..cd219cd Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part new file mode 100644 index 0000000..3dcb855 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MtGjXc2STrbHqs0TF", + "isStandardContent": false, + "name": "cage_back <1>", + "partId": "RfBH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl new file mode 100644 index 0000000..4616af9 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part new file mode 100644 index 0000000..082d252 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "M3PrPkym5OxucEDLY", + "isStandardContent": false, + "name": "cage_bottom <1>", + "partId": "R5DD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl new file mode 100644 index 0000000..8cdbc2e Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.part new file mode 100644 index 0000000..6476567 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MlZeSTv4GhNntgplW", + "isStandardContent": false, + "name": "cage_bottom_battery_hold <1>", + "partId": "R5DH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl new file mode 100644 index 0000000..2ac1ca6 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part new file mode 100644 index 0000000..c5c118b --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MMhqY41MtSO0BMQmM", + "isStandardContent": false, + "name": "cage_top <1>", + "partId": "RRDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl new file mode 100644 index 0000000..1c35e18 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part new file mode 100644 index 0000000..0c5f048 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "Mx1gCi95bgVKmQGMH", + "isStandardContent": false, + "name": "cell <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl new file mode 100644 index 0000000..20e4cc3 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json b/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json new file mode 100644 index 0000000..49a9eac --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json @@ -0,0 +1,5 @@ +{ + "documentId": "a18ff8cc622a533762a3a6f5", + "outputFormat": "urdf", + "assemblyName": "BDX", +} diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.part new file mode 100644 index 0000000..2f84b4d --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MeE9kg4oOZhPqrFKc", + "isStandardContent": false, + "name": "DC15_A01_CASE_B_DUMMY <1>", + "partId": "JFn", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl new file mode 100644 index 0000000..8bc42f3 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.part new file mode 100644 index 0000000..3aa9005 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MMeN4IWguTKJDjvpk", + "isStandardContent": false, + "name": "DC15_A01_CASE_F_DUMMY <1>", + "partId": "JFr", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl new file mode 100644 index 0000000..211a2ee Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.part new file mode 100644 index 0000000..a92d5fa --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MVGfJzCaRc159i+a2", + "isStandardContent": false, + "name": "DC15_A01_CASE_M_DUMMY <1>", + "partId": "JFv", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl new file mode 100644 index 0000000..7f4c118 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.part new file mode 100644 index 0000000..2240b09 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MS6UeJfX4Xsr7Qkr6", + "isStandardContent": false, + "name": "DC15_A01_HORN_DUMMY <1>", + "partId": "JFj", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl new file mode 100644 index 0000000..c56ae4a Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.part new file mode 100644 index 0000000..1e70692 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "05360d05e74c3d8f57c2ed2c", + "fullConfiguration": "default", + "id": "MvkpKlw0tvUizYc+v", + "isStandardContent": false, + "name": "DC15_A01_HORN_IDLE2_DUMMY <1>", + "partId": "JFT", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl new file mode 100644 index 0000000..d36e079 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part new file mode 100644 index 0000000..aa6358d --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MGLU297Hzzd9cmJbM", + "isStandardContent": false, + "name": "double_U <1>", + "partId": "R2BD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl new file mode 100644 index 0000000..c31e309 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part new file mode 100644 index 0000000..9b3b38b --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "bdbd17662073b7a524b273fe", + "fullConfiguration": "default", + "id": "MabiF1g8dKex+GSYq", + "isStandardContent": false, + "name": "foot <1>", + "partId": "JwD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl new file mode 100644 index 0000000..360e25d Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part new file mode 100644 index 0000000..98cd544 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "bdbd17662073b7a524b273fe", + "fullConfiguration": "default", + "id": "MacgSWoiJ3m0BAOhr", + "isStandardContent": false, + "name": "foot_contact <1>", + "partId": "JwH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl new file mode 100644 index 0000000..0675f8b Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part new file mode 100644 index 0000000..366aa6d --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MgOjcYXWnsH30ASqy", + "isStandardContent": false, + "name": "front_cover <1>", + "partId": "JmD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl new file mode 100644 index 0000000..6a26fd1 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part new file mode 100644 index 0000000..e5d1eee --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MJY1ejhDKCoaIDnR4", + "isStandardContent": false, + "name": "head <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl new file mode 100644 index 0000000..87e00df Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.part new file mode 100644 index 0000000..c1b82d8 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "3637c78db5929abd08602abb", + "fullConfiguration": "default", + "id": "MQrACtZhtBd++0zIE", + "isStandardContent": false, + "name": "head_roll_pitch <1>", + "partId": "JlD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl new file mode 100644 index 0000000..b18e1a2 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.part new file mode 100644 index 0000000..0272703 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MZ4X5IZKAFwcUFWJL", + "isStandardContent": false, + "name": "hip_left_roll_to_pitch <1>", + "partId": "RmCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl new file mode 100644 index 0000000..ba4bf36 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.part new file mode 100644 index 0000000..4d2c985 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MqpX4oSaGt0fcUeEq", + "isStandardContent": false, + "name": "hip_right_roll_to_pitch <1>", + "partId": "JXD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl new file mode 100644 index 0000000..ea5c3a4 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part new file mode 100644 index 0000000..9224de8 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MhtdHOvIsBT6us1tg", + "isStandardContent": false, + "name": "holder <1>", + "partId": "JLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl new file mode 100644 index 0000000..0d3f382 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.part new file mode 100644 index 0000000..1288771 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MwzAB4u2uoHpcOUsB", + "isStandardContent": false, + "name": "holder_U2D2_power_hub <1>", + "partId": "RdED", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl new file mode 100644 index 0000000..446c015 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.part new file mode 100644 index 0000000..b23e220 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MqKlaiko0aCWNOvid", + "isStandardContent": false, + "name": "JST-B3B-EH-A.PRT.1 <2>", + "partId": "JFb", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl new file mode 100644 index 0000000..2262446 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.part new file mode 100644 index 0000000..62eca13 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MiQsWzqt1weSpH7Ee", + "isStandardContent": false, + "name": "JST-B3B-EH-A.PRT <1>", + "partId": "JFX", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl new file mode 100644 index 0000000..2262446 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.part new file mode 100644 index 0000000..53a4820 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mjo+l+YE4yXewvi5b", + "isStandardContent": false, + "name": "JST-B4B-EH-A.PRT.1 <2>", + "partId": "JFj", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl new file mode 100644 index 0000000..a957dc2 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.part new file mode 100644 index 0000000..ebdbae7 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mkf/5vUS2ezdkcZAV", + "isStandardContent": false, + "name": "JST-B4B-EH-A.PRT <1>", + "partId": "JFT", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl new file mode 100644 index 0000000..a957dc2 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.part new file mode 100644 index 0000000..3d5cc52 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mk62vUHK7F3maBAlW", + "isStandardContent": false, + "name": "JST-B4B-PH-K-S.PRT <1>", + "partId": "JFP", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl new file mode 100644 index 0000000..3f02215 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.part new file mode 100644 index 0000000..c0d975f --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "Mzqj3Q49Pss2LoqJX", + "isStandardContent": false, + "name": "MICRO_USB_2_0_CONNECTOR__AB_REC.PRT <1>", + "partId": "JFL", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl new file mode 100644 index 0000000..10c20a8 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.part new file mode 100644 index 0000000..6974d48 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MnAngNatw3dhkwz+s", + "isStandardContent": false, + "name": "PCB_U2D2.PRT <1>", + "partId": "JFH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl new file mode 100644 index 0000000..d620752 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part new file mode 100644 index 0000000..98053ba --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "8bfad62b97860b67f75e3376", + "fullConfiguration": "default", + "id": "M99VFeL41Pi5byGXl", + "isStandardContent": false, + "name": "power_switch <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl new file mode 100644 index 0000000..12a17d9 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part new file mode 100644 index 0000000..d14b889 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MzdR3wa2Pikx8l7un", + "isStandardContent": false, + "name": "rasp_spacer <3>", + "partId": "R3DD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl new file mode 100644 index 0000000..5cd723a Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part new file mode 100644 index 0000000..e93fb09 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "9f45233997c8070932aec904", + "fullConfiguration": "default", + "id": "MchLzfPC746ZAzlWi", + "isStandardContent": false, + "name": "RaspberryPiZeroW <2>", + "partId": "JFH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl new file mode 100644 index 0000000..93e9d8a Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.part new file mode 100644 index 0000000..50ef50a --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "M9cyMohQTfT3UyQa8", + "isStandardContent": false, + "name": "renfort_head_link <1>", + "partId": "RJDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl new file mode 100644 index 0000000..d335e81 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part new file mode 100644 index 0000000..4ddbedc --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "41cb17acd87e9a291dc66083", + "fullConfiguration": "default", + "id": "MEjG5RS5osemC0kEN", + "isStandardContent": false, + "name": "renfort_leg <1>", + "partId": "JUD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl new file mode 100644 index 0000000..63a1691 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf b/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf new file mode 100644 index 0000000..16f9f19 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf @@ -0,0 +1,2284 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml b/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml new file mode 100644 index 0000000..403e3ae --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml @@ -0,0 +1,305 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml b/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml new file mode 100644 index 0000000..01f1a02 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part new file mode 100644 index 0000000..cbe4d1f --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "349d7eaab34e22a2c029ddc9", + "fullConfiguration": "default", + "id": "MuCWOV8+Toix8DMeR", + "isStandardContent": false, + "name": "sg90 <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl new file mode 100644 index 0000000..581f49c Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part new file mode 100644 index 0000000..dba4834 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "ecfc18694e27c0842b7fd6e5", + "fullConfiguration": "default", + "id": "MpwO0az74Ch9pB7z/", + "isStandardContent": false, + "name": "spacer <1>", + "partId": "RCCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl new file mode 100644 index 0000000..3e96b26 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.part new file mode 100644 index 0000000..6859ce6 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "5645a2ee62732108708ff18c", + "fullConfiguration": "default", + "id": "MNRmb843uWiCvcYRP", + "isStandardContent": false, + "name": "U2D2_CASING.PRT <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl new file mode 100644 index 0000000..fb32481 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.part b/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.part new file mode 100644 index 0000000..2955805 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "0e5460059362b6d56fca6403", + "elementId": "1a4a827101fb7d1d1b3cc8de", + "fullConfiguration": "default", + "id": "MOdHUYaAV7HrozYl7", + "isStandardContent": false, + "name": "usb_c_charger <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl b/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl new file mode 100644 index 0000000..b2483b9 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.part new file mode 100644 index 0000000..1a8f3a4 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MmXLbiyIJZ1T9tiX3", + "isStandardContent": false, + "name": "antenna <1>", + "partId": "RuOD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl new file mode 100644 index 0000000..07eb706 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.part new file mode 100644 index 0000000..c42dd15 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MvOQ01tQlVV+zSv63", + "isStandardContent": false, + "name": "battery_pack_lid <1>", + "partId": "R5OD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl new file mode 100644 index 0000000..076eb25 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.part new file mode 100644 index 0000000..d5e0fd2 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MqEGDvIKFH4SpUaCn", + "isStandardContent": false, + "name": "bms <1>", + "partId": "JSD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl new file mode 100644 index 0000000..4598b81 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.part new file mode 100644 index 0000000..31c1864 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "58e35a434b8265113623a1c6", + "fullConfiguration": "default", + "id": "MCK/voA1KPjJiuZRD", + "isStandardContent": false, + "name": "BNO055 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl new file mode 100644 index 0000000..6de1ee7 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.part new file mode 100644 index 0000000..dbd8cbc --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "fbc3ecd1071c724f4d859dd6", + "fullConfiguration": "default", + "id": "MxRZVYwGEp1Sig2/i", + "isStandardContent": false, + "name": "Board <1>", + "partId": "JFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl new file mode 100644 index 0000000..f2f3da5 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.part new file mode 100644 index 0000000..0cac428 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MfCA08JspCUYCoOzy", + "isStandardContent": false, + "name": "body_back <1>", + "partId": "RqKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl new file mode 100644 index 0000000..b6692ac Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.part new file mode 100644 index 0000000..6bcf261 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MAILAHna+4EMDjwEB", + "isStandardContent": false, + "name": "body_front <1>", + "partId": "RbKD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl new file mode 100644 index 0000000..fa76bb4 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.part new file mode 100644 index 0000000..93a8273 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mi7/ZBgz7j8FhNN8n", + "isStandardContent": false, + "name": "body_middle_bottom <1>", + "partId": "R/KH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl new file mode 100644 index 0000000..9c8ccee Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.part new file mode 100644 index 0000000..b39e827 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MwLs2m7WqJdIp5rCM", + "isStandardContent": false, + "name": "body_middle_top <1>", + "partId": "R/KD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl new file mode 100644 index 0000000..5530f0b Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.part new file mode 100644 index 0000000..c7c9417 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "Mx1gCi95bgVKmQGMH", + "isStandardContent": false, + "name": "cell <2>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl new file mode 100644 index 0000000..20e4cc3 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/config.json b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/config.json new file mode 100644 index 0000000..a1032a4 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/config.json @@ -0,0 +1,5 @@ +{ + "documentId": "64074dfcfa379b37d8a47762", + "outputFormat": "urdf", + "assemblyName": "Open_Duck_Mini_v2", +} diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.part new file mode 100644 index 0000000..b940a1a --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MHjgEclEhEZQK/p6r", + "isStandardContent": false, + "name": "drive_palonier <1>", + "partId": "JJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl new file mode 100644 index 0000000..66e60e4 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.part new file mode 100644 index 0000000..e087dd5 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MbWaXbNFhwXrvfPr1", + "isStandardContent": false, + "name": "foot_bottom_pla <1>", + "partId": "R7GH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl new file mode 100644 index 0000000..66feed7 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.part new file mode 100644 index 0000000..22b27eb --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MSRJjnMROc2yqGlBV", + "isStandardContent": false, + "name": "foot_bottom_tpu <1>", + "partId": "R7GD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl new file mode 100644 index 0000000..4f05326 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.part new file mode 100644 index 0000000..55842e9 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M9QvdrmpVd96BQQ3J", + "isStandardContent": false, + "name": "foot_side <1>", + "partId": "RsGD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl new file mode 100644 index 0000000..20a434e Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.part new file mode 100644 index 0000000..8981480 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MryNg763VjsN4xBWU", + "isStandardContent": false, + "name": "foot_top <1>", + "partId": "RsGH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl new file mode 100644 index 0000000..0d08c19 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.part new file mode 100644 index 0000000..1395a55 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MGPnRRHSCLRuHbf9s", + "isStandardContent": false, + "name": "head <1>", + "partId": "RXMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl new file mode 100644 index 0000000..2a101fd Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.part new file mode 100644 index 0000000..4184b67 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MB88Zh5nm8uQRQJB5", + "isStandardContent": false, + "name": "head_bot_sheet <1>", + "partId": "RxMD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl new file mode 100644 index 0000000..c565ba6 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.part new file mode 100644 index 0000000..7892e94 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MMYrJ8GU6Tulh9Ezl", + "isStandardContent": false, + "name": "head_pitch_to_yaw <1>", + "partId": "RGCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl new file mode 100644 index 0000000..1de8aef Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.part new file mode 100644 index 0000000..01b0ff2 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MRmftgtax43WG3TM4", + "isStandardContent": false, + "name": "head_roll_mount <1>", + "partId": "RMND", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl new file mode 100644 index 0000000..e6efec9 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.part new file mode 100644 index 0000000..df7aa08 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MSSQrz89LHCpx23/c", + "isStandardContent": false, + "name": "head_yaw_to_roll <1>", + "partId": "RyCD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl new file mode 100644 index 0000000..14a2122 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.part new file mode 100644 index 0000000..cd32e37 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "20dad50bf4a254b818c813f7", + "fullConfiguration": "default", + "id": "MhtdHOvIsBT6us1tg", + "isStandardContent": false, + "name": "holder <1>", + "partId": "JLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl new file mode 100644 index 0000000..0d3f382 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.part new file mode 100644 index 0000000..b56531f --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MqpShx/iKn+WMc6nz", + "isStandardContent": false, + "name": "left_antenna_holder <1>", + "partId": "RfPL", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl new file mode 100644 index 0000000..5b20ae7 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.part new file mode 100644 index 0000000..b68c120 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mh206kD+LDwOu9+3I", + "isStandardContent": false, + "name": "left_cache <1>", + "partId": "RELD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl new file mode 100644 index 0000000..a927bc9 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.part new file mode 100644 index 0000000..c5f56e0 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MkexR4anxdIDvoNnP", + "isStandardContent": false, + "name": "left_knee_to_ankle_left_sheet <1>", + "partId": "RyDD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.stl new file mode 100644 index 0000000..e46725d Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.part new file mode 100644 index 0000000..72193f8 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MFSYykuSOyFPl8MEE", + "isStandardContent": false, + "name": "left_knee_to_ankle_right_sheet <1>", + "partId": "RyDH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.stl new file mode 100644 index 0000000..a1fc36e Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.part new file mode 100644 index 0000000..8036293 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mg7gXWiWLFdTkX/WZ", + "isStandardContent": false, + "name": "left_roll_to_pitch <1>", + "partId": "RRFD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl new file mode 100644 index 0000000..11ee400 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.part new file mode 100644 index 0000000..7ca518f --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M1AJs3nTP2bD6DgAF", + "isStandardContent": false, + "name": "leg_spacer <1>", + "partId": "RfED", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl new file mode 100644 index 0000000..336c0b3 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.part new file mode 100644 index 0000000..db821c4 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M8Je0JZqWOimCS7cY", + "isStandardContent": false, + "name": "neck_left_sheet <1>", + "partId": "RgLD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl new file mode 100644 index 0000000..04825be Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.part new file mode 100644 index 0000000..422d393 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M8fJckSPn4u+ct8pO", + "isStandardContent": false, + "name": "neck_right_sheet <1>", + "partId": "RgLH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl new file mode 100644 index 0000000..867149e Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.part new file mode 100644 index 0000000..c3fe8bf --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MtwZA0wRrU1lvONjK", + "isStandardContent": false, + "name": "passive_palonier <1>", + "partId": "JVD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl new file mode 100644 index 0000000..234dd7e Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.part new file mode 100644 index 0000000..65df91a --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "8bfad62b97860b67f75e3376", + "fullConfiguration": "default", + "id": "MoxN2/6DvG8o5Khzi", + "isStandardContent": false, + "name": "power_switch <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl new file mode 100644 index 0000000..a72c233 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.part new file mode 100644 index 0000000..769abe7 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "9f45233997c8070932aec904", + "fullConfiguration": "default", + "id": "MT4+RWBYZDvB/ifWw", + "isStandardContent": false, + "name": "RaspberryPiZeroW <1>", + "partId": "JFH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl new file mode 100644 index 0000000..c47c605 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.part new file mode 100644 index 0000000..43bbafc --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "M/p8PGArLA1nrXwe8", + "isStandardContent": false, + "name": "right_antenna_holder <1>", + "partId": "RcOD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl new file mode 100644 index 0000000..3bf85d6 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.part new file mode 100644 index 0000000..3f165a0 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MBtFsLan/52XkruX7", + "isStandardContent": false, + "name": "right_cache <1>", + "partId": "RfPH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl new file mode 100644 index 0000000..fa40ec6 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.part new file mode 100644 index 0000000..5480194 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Mvz4SlAytL1YUO5L4", + "isStandardContent": false, + "name": "right_roll_to_pitch <1>", + "partId": "RfPD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl new file mode 100644 index 0000000..38dd671 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.urdf b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.urdf new file mode 100644 index 0000000..2b90f41 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.urdf @@ -0,0 +1,2269 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml new file mode 100644 index 0000000..05b788c --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml @@ -0,0 +1,303 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot_motors.xml b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot_motors.xml new file mode 100644 index 0000000..d4ccdb3 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot_motors.xml @@ -0,0 +1,1086 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.part new file mode 100644 index 0000000..566900c --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "Ma27pDNdkzSRCakAZ", + "isStandardContent": false, + "name": "roll_bearing <1>", + "partId": "RtHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl new file mode 100644 index 0000000..3210d4e Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.part new file mode 100644 index 0000000..a824c1a --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MjsDdlagt/LopvMjR", + "isStandardContent": false, + "name": "roll_motor_bottom <1>", + "partId": "RGHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl new file mode 100644 index 0000000..a36dff5 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.part new file mode 100644 index 0000000..7233b6c --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MoZYOvTkbbGLGW0df", + "isStandardContent": false, + "name": "roll_motor_top <1>", + "partId": "RyHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl new file mode 100644 index 0000000..99f314c Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml new file mode 100644 index 0000000..d4f7b0c --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml new file mode 100644 index 0000000..31b5f63 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.part new file mode 100644 index 0000000..c02443b --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "349d7eaab34e22a2c029ddc9", + "fullConfiguration": "default", + "id": "MkrkdH7oZvjAepXlL", + "isStandardContent": false, + "name": "sg90 <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl new file mode 100644 index 0000000..bac6639 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.part new file mode 100644 index 0000000..2929658 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MMYHrJ4hqH24cpa8I", + "isStandardContent": false, + "name": "trunk_bottom <1>", + "partId": "RvJH", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl new file mode 100644 index 0000000..48afbc7 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.part new file mode 100644 index 0000000..f5fad37 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "560456e30aa4a28c1f2fba59", + "fullConfiguration": "default", + "id": "MNg8avQSkDpa0UkRX", + "isStandardContent": false, + "name": "trunk_top <1>", + "partId": "RvJD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl new file mode 100644 index 0000000..760665c Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.part new file mode 100644 index 0000000..8cb8bd9 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.part @@ -0,0 +1,14 @@ +{ + "configuration": "default", + "documentId": "a18ff8cc622a533762a3a6f5", + "documentMicroversion": "f5e1d9417006aed4e8eece17", + "documentVersion": "94881018f8994675184c1f42", + "elementId": "1a4a827101fb7d1d1b3cc8de", + "fullConfiguration": "default", + "id": "MXjBB69uv3FCgauqS", + "isStandardContent": false, + "name": "usb_c_charger <1>", + "partId": "JHD", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl new file mode 100644 index 0000000..2760f99 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.part new file mode 100644 index 0000000..342f518 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MOFyAb/RZuW0kTpzn", + "isStandardContent": false, + "name": "WJ-WK00-0122TOPCABINETCASE_95 <1>", + "partId": "JFb", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl new file mode 100644 index 0000000..984d136 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.part new file mode 100644 index 0000000..ffcc8ab --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "M1HwhnHIG/iEEwnW3", + "isStandardContent": false, + "name": "WJ-WK00-0123MIDDLECASE_56 <1>", + "partId": "JFP", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl new file mode 100644 index 0000000..098c62c Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl differ diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.part b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.part new file mode 100644 index 0000000..3497c02 --- /dev/null +++ b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.part @@ -0,0 +1,13 @@ +{ + "configuration": "default", + "documentId": "64074dfcfa379b37d8a47762", + "documentMicroversion": "827c48cdf99b59691ea6bc9c", + "elementId": "d386542214948bc90a7bc881", + "fullConfiguration": "default", + "id": "MOFR6bFTIen7rmARW", + "isStandardContent": false, + "name": "WJ-WK00-0124BOTTOMCASE_45 <1>", + "partId": "JFT", + "suppressed": false, + "type": "Part" +} \ No newline at end of file diff --git a/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl new file mode 100644 index 0000000..b5c7064 Binary files /dev/null and b/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl differ diff --git a/Open_Duck_Mini-2/print/battery_pack_lid.stl b/Open_Duck_Mini-2/print/battery_pack_lid.stl new file mode 100644 index 0000000..c2ba5f8 Binary files /dev/null and b/Open_Duck_Mini-2/print/battery_pack_lid.stl differ diff --git a/Open_Duck_Mini-2/print/body_back.stl b/Open_Duck_Mini-2/print/body_back.stl new file mode 100644 index 0000000..564b8c2 Binary files /dev/null and b/Open_Duck_Mini-2/print/body_back.stl differ diff --git a/Open_Duck_Mini-2/print/body_front.stl b/Open_Duck_Mini-2/print/body_front.stl new file mode 100644 index 0000000..ec3a048 Binary files /dev/null and b/Open_Duck_Mini-2/print/body_front.stl differ diff --git a/Open_Duck_Mini-2/print/body_middle_bottom.stl b/Open_Duck_Mini-2/print/body_middle_bottom.stl new file mode 100644 index 0000000..b4207cc Binary files /dev/null and b/Open_Duck_Mini-2/print/body_middle_bottom.stl differ diff --git a/Open_Duck_Mini-2/print/body_middle_top.stl b/Open_Duck_Mini-2/print/body_middle_top.stl new file mode 100644 index 0000000..96dd935 Binary files /dev/null and b/Open_Duck_Mini-2/print/body_middle_top.stl differ diff --git a/Open_Duck_Mini-2/print/bulb.stl b/Open_Duck_Mini-2/print/bulb.stl new file mode 100644 index 0000000..93f7d48 Binary files /dev/null and b/Open_Duck_Mini-2/print/bulb.stl differ diff --git a/Open_Duck_Mini-2/print/flash_light_module.stl b/Open_Duck_Mini-2/print/flash_light_module.stl new file mode 100644 index 0000000..97c3f49 Binary files /dev/null and b/Open_Duck_Mini-2/print/flash_light_module.stl differ diff --git a/Open_Duck_Mini-2/print/flash_reflector_interface.stl b/Open_Duck_Mini-2/print/flash_reflector_interface.stl new file mode 100644 index 0000000..1919e29 Binary files /dev/null and b/Open_Duck_Mini-2/print/flash_reflector_interface.stl differ diff --git a/Open_Duck_Mini-2/print/foot_bottom_pla.stl b/Open_Duck_Mini-2/print/foot_bottom_pla.stl new file mode 100644 index 0000000..b53e0d5 Binary files /dev/null and b/Open_Duck_Mini-2/print/foot_bottom_pla.stl differ diff --git a/Open_Duck_Mini-2/print/foot_bottom_tpu.stl b/Open_Duck_Mini-2/print/foot_bottom_tpu.stl new file mode 100644 index 0000000..2aed4b8 Binary files /dev/null and b/Open_Duck_Mini-2/print/foot_bottom_tpu.stl differ diff --git a/Open_Duck_Mini-2/print/foot_side.stl b/Open_Duck_Mini-2/print/foot_side.stl new file mode 100644 index 0000000..55bb289 Binary files /dev/null and b/Open_Duck_Mini-2/print/foot_side.stl differ diff --git a/Open_Duck_Mini-2/print/foot_top.stl b/Open_Duck_Mini-2/print/foot_top.stl new file mode 100644 index 0000000..134284e Binary files /dev/null and b/Open_Duck_Mini-2/print/foot_top.stl differ diff --git a/Open_Duck_Mini-2/print/head.stl b/Open_Duck_Mini-2/print/head.stl new file mode 100644 index 0000000..ccf7211 Binary files /dev/null and b/Open_Duck_Mini-2/print/head.stl differ diff --git a/Open_Duck_Mini-2/print/head_bot_sheet.stl b/Open_Duck_Mini-2/print/head_bot_sheet.stl new file mode 100644 index 0000000..95c939e Binary files /dev/null and b/Open_Duck_Mini-2/print/head_bot_sheet.stl differ diff --git a/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl b/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl new file mode 100644 index 0000000..e64ad2f Binary files /dev/null and b/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl differ diff --git a/Open_Duck_Mini-2/print/head_roll_mount.stl b/Open_Duck_Mini-2/print/head_roll_mount.stl new file mode 100644 index 0000000..93aec87 Binary files /dev/null and b/Open_Duck_Mini-2/print/head_roll_mount.stl differ diff --git a/Open_Duck_Mini-2/print/head_yaw_to_roll.stl b/Open_Duck_Mini-2/print/head_yaw_to_roll.stl new file mode 100644 index 0000000..29edd44 Binary files /dev/null and b/Open_Duck_Mini-2/print/head_yaw_to_roll.stl differ diff --git a/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl b/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl new file mode 100644 index 0000000..e8a82ef Binary files /dev/null and b/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl b/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl new file mode 100644 index 0000000..8242f8c Binary files /dev/null and b/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/print/left_antenna_holder.stl b/Open_Duck_Mini-2/print/left_antenna_holder.stl new file mode 100644 index 0000000..2df311c Binary files /dev/null and b/Open_Duck_Mini-2/print/left_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/print/left_cache.stl b/Open_Duck_Mini-2/print/left_cache.stl new file mode 100644 index 0000000..0ce5ca2 Binary files /dev/null and b/Open_Duck_Mini-2/print/left_cache.stl differ diff --git a/Open_Duck_Mini-2/print/left_eye.stl b/Open_Duck_Mini-2/print/left_eye.stl new file mode 100644 index 0000000..6281ea1 Binary files /dev/null and b/Open_Duck_Mini-2/print/left_eye.stl differ diff --git a/Open_Duck_Mini-2/print/left_roll_to_pitch.stl b/Open_Duck_Mini-2/print/left_roll_to_pitch.stl new file mode 100644 index 0000000..9a60364 Binary files /dev/null and b/Open_Duck_Mini-2/print/left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/print/leg_spacer.stl b/Open_Duck_Mini-2/print/leg_spacer.stl new file mode 100644 index 0000000..c57f8d2 Binary files /dev/null and b/Open_Duck_Mini-2/print/leg_spacer.stl differ diff --git a/Open_Duck_Mini-2/print/neck_left_sheet.stl b/Open_Duck_Mini-2/print/neck_left_sheet.stl new file mode 100644 index 0000000..e91778a Binary files /dev/null and b/Open_Duck_Mini-2/print/neck_left_sheet.stl differ diff --git a/Open_Duck_Mini-2/print/neck_right_sheet.stl b/Open_Duck_Mini-2/print/neck_right_sheet.stl new file mode 100644 index 0000000..c05919e Binary files /dev/null and b/Open_Duck_Mini-2/print/neck_right_sheet.stl differ diff --git a/Open_Duck_Mini-2/print/right_antenna_holder.stl b/Open_Duck_Mini-2/print/right_antenna_holder.stl new file mode 100644 index 0000000..3797e0c Binary files /dev/null and b/Open_Duck_Mini-2/print/right_antenna_holder.stl differ diff --git a/Open_Duck_Mini-2/print/right_cache.stl b/Open_Duck_Mini-2/print/right_cache.stl new file mode 100644 index 0000000..16c24f0 Binary files /dev/null and b/Open_Duck_Mini-2/print/right_cache.stl differ diff --git a/Open_Duck_Mini-2/print/right_eye.stl b/Open_Duck_Mini-2/print/right_eye.stl new file mode 100644 index 0000000..185500a Binary files /dev/null and b/Open_Duck_Mini-2/print/right_eye.stl differ diff --git a/Open_Duck_Mini-2/print/right_roll_to_pitch.stl b/Open_Duck_Mini-2/print/right_roll_to_pitch.stl new file mode 100644 index 0000000..d57105d Binary files /dev/null and b/Open_Duck_Mini-2/print/right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini-2/print/roll_motor_bottom.stl b/Open_Duck_Mini-2/print/roll_motor_bottom.stl new file mode 100644 index 0000000..a8dcb05 Binary files /dev/null and b/Open_Duck_Mini-2/print/roll_motor_bottom.stl differ diff --git a/Open_Duck_Mini-2/print/roll_motor_top.stl b/Open_Duck_Mini-2/print/roll_motor_top.stl new file mode 100644 index 0000000..91ac599 Binary files /dev/null and b/Open_Duck_Mini-2/print/roll_motor_top.stl differ diff --git a/Open_Duck_Mini-2/print/speaker_interface.stl b/Open_Duck_Mini-2/print/speaker_interface.stl new file mode 100644 index 0000000..dd7225e Binary files /dev/null and b/Open_Duck_Mini-2/print/speaker_interface.stl differ diff --git a/Open_Duck_Mini-2/print/speaker_stand.stl b/Open_Duck_Mini-2/print/speaker_stand.stl new file mode 100644 index 0000000..4265223 Binary files /dev/null and b/Open_Duck_Mini-2/print/speaker_stand.stl differ diff --git a/Open_Duck_Mini-2/print/trunk_bottom.stl b/Open_Duck_Mini-2/print/trunk_bottom.stl new file mode 100644 index 0000000..a9545d1 Binary files /dev/null and b/Open_Duck_Mini-2/print/trunk_bottom.stl differ diff --git a/Open_Duck_Mini-2/print/trunk_top.stl b/Open_Duck_Mini-2/print/trunk_top.stl new file mode 100644 index 0000000..4fd8711 Binary files /dev/null and b/Open_Duck_Mini-2/print/trunk_top.stl differ diff --git a/Open_Duck_Mini-2/pyproject.toml b/Open_Duck_Mini-2/pyproject.toml new file mode 100644 index 0000000..7fd26b9 --- /dev/null +++ b/Open_Duck_Mini-2/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" \ No newline at end of file diff --git a/Open_Duck_Mini-2/setup.cfg b/Open_Duck_Mini-2/setup.cfg new file mode 100644 index 0000000..a50de1d --- /dev/null +++ b/Open_Duck_Mini-2/setup.cfg @@ -0,0 +1,41 @@ +[metadata] +name = mini-bdx +version = 0.0.1 +author = Antoine Pirrone +author_email = antoine.pirrone@gmail.com +url = https://github.com/apirrone/mini_BDX +description = Making a mini version of the BDX droid +long_description = file: README.md +long_description_content_type = text/markdown + + +[options] +packages = find: +zip_safe = True +include_package_data = True +package_dir= + =mini_bdx +install_requires = +[options.packages.find] +where=mini_bdx + +[options.package_data] + +[options.extras_require] +all = + mujoco==3.1.5 + mujoco-python-viewer==0.1.4 + onshape-to-robot==0.3.25 + gymnasium[mujoco] == 0.29.1 + stable-baselines3[extra]==2.3.2 + sb3_contrib==2.3.0 + placo==0.5.0 + FramesViewer + inputs==0.5 + imitation==1.0.0 + h5py==3.11.0 +robot = placo==0.5.0 + pypot @ git+https://github.com/apirrone/pypot@support_4B_registers + +[options.entry_points] +console_scripts = diff --git a/Open_Duck_Mini-2/thanks.md b/Open_Duck_Mini-2/thanks.md new file mode 100644 index 0000000..30f984e --- /dev/null +++ b/Open_Duck_Mini-2/thanks.md @@ -0,0 +1,10 @@ +People involved in the project. (I'll try to update this list when I think of a name, so not to forget anyone) + +- Mimir Reynisson +- Grégoire Passault +- Augustin Crampette +- Michel Aractingi +- Mankaran Singh +- Steve N'Guyen +- Pierre Rouanet +- Thomas Wolf diff --git a/Open_Duck_Mini_Runtime-2/.gitignore b/Open_Duck_Mini_Runtime-2/.gitignore new file mode 100644 index 0000000..2798d1f --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/.gitignore @@ -0,0 +1,4 @@ +__pycache__/ +mini_bdx_runtime.egg-info/ +build +dist \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/.gitignore b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/.gitignore new file mode 100644 index 0000000..2798d1f --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/.gitignore @@ -0,0 +1,4 @@ +__pycache__/ +mini_bdx_runtime.egg-info/ +build +dist \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/README.md b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/README.md new file mode 100644 index 0000000..a5025ef --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/README.md @@ -0,0 +1,182 @@ +# Open Duck Mini Runtime + +## Raspberry Pi zero 2W setup + +### Install Raspberry Pi OS + +Download Raspberry Pi OS Lite (64-bit) from here : https://www.raspberrypi.com/software/operating-systems/ + +Follow the instructions here to install the OS on the SD card : https://www.raspberrypi.com/documentation/computers/getting-started.html + +With the Raspberry Pi Imager, you can pre-configure session, wifi and ssh. Do it like below : + +![imager_setup](https://github.com/user-attachments/assets/7a4987b2-de83-41dd-ab7f-585259685f16) + +> Tip: I configure the rasp to connect to my phone's hotspot, this way I can connect to it from anywhere. + +### Setup SSH (If not setup during the installation) + +When first booting on the rasp, you will need to connect a screen and a keyboard. The first thing you should do is connect to a wifi network and enable SSH. + +To do so, you can follow this guide : https://www.raspberrypi.com/documentation/computers/configuration.html#setting-up-wifi + +Then, you can connect to your rasp using SSH without having to plug a screen and a keyboard. + +### Update the system and install necessary stuff + +```bash +sudo apt update +sudo apt upgrade +sudo apt install git +sudo apt install python3-pip +sudo apt install python3-virtualenvwrapper +(optional) sudo apt install python3-picamzero + +``` + +Add this to the end of the `.bashrc`: + +```bash +export WORKON_HOME=$HOME/.virtualenvs +export PROJECT_HOME=$HOME/Devel +source /usr/share/virtualenvwrapper/virtualenvwrapper.sh +``` + +### Enable I2C + +`sudo raspi-config` -> `Interface Options` -> `I2C` + +TODO set 400KHz ? + +### Set the usbserial latency timer + +```bash +cd /etc/udev/rules.d/ +sudo touch 99-usb-serial.rules +sudo nano 99-usb-serial.rules +# copy the following line in the file +SUBSYSTEM=="usb-serial", DRIVER=="ftdi_sio", ATTR{latency_timer}="1" +``` + +### Set the udev rules for the motor control board + +TODO + + +### Setup xbox one controller over bluetooth + +Turn your xbox one controller on and set it in pairing mode by long pressing the sync button on the top of the controller. + +Run the following commands on the rasp : + +```bash +bluetoothctl +scan on +``` + +Wait for the controller to appear in the list, then run : + +```bash +pair +trust +connect +``` + +The led on the controller should stop blinking and stay on. + +You can test that it's working by running + +```bash +python3 mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py +``` + +## Speaker wiring and configuration +Follow this tutorial + +> For now, don't activate `/dev/zero` when they ask + +https://learn.adafruit.com/adafruit-max98357-i2s-class-d-mono-amp?view=all + + +## Install the runtime + +### Make a virtual environment and activate it + +```bash +mkvirtualenv -p python3 open-duck-mini-runtime +workon open-duck-mini-runtime +``` + +Clone this repository on your rasp, cd into the repo, then : + +```bash +git clone https://github.com/apirrone/Open_Duck_Mini_Runtime +cd Open_Duck_Mini_Runtime +git checkout v2 +pip install -e . +``` + +In Raspberry Pi 5, you need to perform the following operations + +```bash +pip uninstall -y RPi.GPIO +pip install lgpio +``` + + +## Test the IMU + +```bash +python3 mini_bdx_runtime/mini_bdx_runtime/raw_imu.py +``` + +You can also run `python3 scripts/imu_server.py` on the robot and `python3 scripts/imu_client.py --ip ` on your computer to check that the frame is oriented correctly. + +> To find the ip address of the robot, run `ifconfig` on the robot + +## Test motors + +This will allow you to verify all your motors are connected and configured. + +```bash +python3 scripts/check_motors.py +``` + +## Make your duck_config.json + +Copy `example_config.json` in the home directory of your duck and rename it `duck_config.json`. + +`cp example_config.json ~/duck_config.json` + +In this file, you can configure some stuff, like registering if you installed the expression features, installed the imu upside down or and other stuff. You also write the joints offsets of your duck here + +## Find the joints offsets + +This script will guide you through finding the joints offsets of your robot that you can then write in your `duck_config.json` + +> This procedure won't be necessary in the future as we will be flashing the offsets directly in each motor's eeprom. + +```bash +cd scripts/ +python find_soft_offsets.py +``` + +## Run the walk ! + +Download the [latest policy checkpoint ](https://github.com/apirrone/Open_Duck_Mini/blob/v2/BEST_WALK_ONNX_2.onnx) and copy it to your duck. + +`cd scripts/` + +`python v2_rl_walk_mujoco.py --onnx_model_path /BEST_WALK_ONNX_2.onnx` + + + +``` +- The commands are : +- A to pause/unpause +- X to turn on/off the projector +- B to play a random sound +- Y to turn on/off head control (very experimental, I don't recommend trying that, it can break your duck's head) +- left and right triggers to control the left and right antennas +- LB (new!) press and hold to increase the walking frequency, kind of a sprint mode 🙂 +``` \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/TODO.md b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/TODO.md new file mode 100644 index 0000000..d7aae06 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/TODO.md @@ -0,0 +1,3 @@ +- [] Better handle xbox controller + - It's a little bit of a mess right now, how we handle directions and buttons etc +- [] Make the offsets flashing work. This will be in the motor configuration script \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/checklist.md b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/checklist.md new file mode 100644 index 0000000..d68d340 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/checklist.md @@ -0,0 +1,15 @@ +# Checklist + +To check that all works and is properly configured before running a policy. + +TODO (Antoine) +Make a script that goes through all this automatically +- joints positions and offsets +- imu orientation +- feet switches + +make optional +- eyes/projector +- antennas +- speaker +- camera diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/example_config.json b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/example_config.json new file mode 100644 index 0000000..7c7e381 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/example_config.json @@ -0,0 +1,29 @@ +{ + "start_paused": false, + "imu_upside_down": false, + "phase_frequency_factor_offset": 0.0, + "expression_features": { + "eyes": false, + "projector": false, + "antennas": false, + "speaker": false, + "microphone": false, + "camera": false + }, + "joints_offsets": { + "left_hip_yaw": 0.0, + "left_hip_roll": 0.0, + "left_hip_pitch": 0.0, + "left_knee": 0.0, + "left_ankle": 0.0, + "neck_pitch": 0.0, + "head_pitch": 0.0, + "head_yaw": 0.0, + "head_roll": 0.00, + "right_hip_yaw": 0.0, + "right_hip_roll": 0.0, + "right_hip_pitch": 0.0, + "right_knee": 0.0, + "right_ankle": 0.0 + } +} \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/__init__.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/__init__.py @@ -0,0 +1 @@ + diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep1.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep1.wav new file mode 100644 index 0000000..f6b7450 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep1.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep2.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep2.wav new file mode 100644 index 0000000..aea7ea3 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep2.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy1.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy1.wav new file mode 100644 index 0000000..f7174db Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy1.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy2.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy2.wav new file mode 100644 index 0000000..ff772ab Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy2.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy3.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy3.wav new file mode 100644 index 0000000..7f3124f Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy3.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp.wav new file mode 100644 index 0000000..a211b92 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp2.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp2.wav new file mode 100644 index 0000000..70bac5e Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp2.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp3.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp3.wav new file mode 100644 index 0000000..946b446 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp3.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/motor.wav b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/motor.wav new file mode 100644 index 0000000..5337bd0 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/motor.wav differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/__init__.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/__init__.py new file mode 100644 index 0000000..0d1570c --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/__init__.py @@ -0,0 +1 @@ +from .onnx_infer import OnnxInfer diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/antennas.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/antennas.py new file mode 100644 index 0000000..9f041c9 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/antennas.py @@ -0,0 +1,64 @@ +import board +import pwmio +import math +import time + +LEFT_ANTENNA_PIN = board.D13 +RIGHT_ANTENNA_PIN = board.D12 +LEFT_SIGN = 1 +RIGHT_SIGN = -1 +MIN_UPDATE_INTERVAL = 1 / 50 # 20ms + + +def value_to_duty_cycle(v): + pulse_width_ms = 1.5 + (v * 0.5) # 1ms to 2ms + duty_cycle = int((pulse_width_ms / 20) * 65535) + return min(max(duty_cycle, 3277), 6553) + + +class Antennas: + def __init__(self): + neutral_duty = value_to_duty_cycle(0) + self.pwm_left = pwmio.PWMOut(LEFT_ANTENNA_PIN, frequency=50, duty_cycle=neutral_duty) + self.pwm_right = pwmio.PWMOut(RIGHT_ANTENNA_PIN, frequency=50, duty_cycle=neutral_duty) + + def set_position_left(self, position): + self.set_position(self.pwm_left, position, LEFT_SIGN) + + def set_position_right(self, position): + self.set_position(self.pwm_right, position, RIGHT_SIGN) + + def set_position(self, pwm, value, sign=1): + # if value == 0: + # return + if -1 <= value <= 1: + duty_cycle = value_to_duty_cycle(value * sign) # Convert value to duty cycle (1ms-2ms) + pwm.duty_cycle = duty_cycle + else: + print("Invalid input! Enter a value between -1 and 1.") + + def stop(self): + time.sleep(MIN_UPDATE_INTERVAL) + self.set_position_left(0) + self.set_position_right(0) + time.sleep(MIN_UPDATE_INTERVAL) + self.pwm_left.deinit() + self.pwm_right.deinit() + + +if __name__ == "__main__": + antennas = Antennas() + + try: + start_time = time.monotonic() + current_time = start_time + + while current_time - start_time < 5: + value = math.sin(2 * math.pi * 1 * current_time) + antennas.set_position_left(value) + antennas.set_position_right(value) + time.sleep(MIN_UPDATE_INTERVAL) + current_time = time.monotonic() + + finally: + antennas.stop() \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/buttons.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/buttons.py new file mode 100644 index 0000000..5f2d828 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/buttons.py @@ -0,0 +1,85 @@ +import time + + +class Button: + def __init__(self): + self.last_pressed_time = time.time() + self.timeout = 0.2 + self.is_pressed = False + self.triggered = False + self.released = True + + def update(self, value): + if self.is_pressed and not value: + self.released = True + self.is_pressed = value + if ( + self.released + and self.is_pressed + and (time.time() - self.last_pressed_time > self.timeout) + ): + self.triggered = True + self.last_pressed_time = time.time() + else: + self.triggered = False + + if self.is_pressed: + self.released = False + + +class Buttons: + def __init__(self): + self.A = Button() + self.B = Button() + self.X = Button() + self.Y = Button() + self.LB = Button() + self.RB = Button() + self.dpad_up = Button() + self.dpad_down = Button() + + def update(self, A, B, X, Y, LB, RB, dpad_up, dpad_down): + self.A.update(A) + self.B.update(B) + self.X.update(X) + self.Y.update(Y) + self.LB.update(LB) + self.RB.update(RB) + self.dpad_up.update(dpad_up) + self.dpad_down.update(dpad_down) + + +if __name__ == "__main__": + from mini_bdx_runtime.xbox_controller import XBoxController + + xbox_controller = XBoxController(30) + buttons = Buttons() + while True: + + ( + _, + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + _, + _, + up_down, + ) = xbox_controller.get_last_command() + + buttons.update( + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + up_down == 1, + up_down == -1, + ) + + print(buttons.dpad_up.triggered, buttons.dpad_up.is_pressed, buttons.dpad_down.triggered, buttons.dpad_down.is_pressed) + + time.sleep(0.05) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/camera.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/camera.py new file mode 100644 index 0000000..68b59b7 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/camera.py @@ -0,0 +1,30 @@ +from picamzero import Camera +import cv2 +import base64 +import os + +class Cam: + def __init__(self): + self.cam = Camera() + + def get_encoded_image(self): + im = self.cam.capture_array() + im = cv2.resize(im, (512, 512)) + im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB) + im = cv2.rotate(im, cv2.ROTATE_90_CLOCKWISE) + + cv2.imwrite("/home/bdxv2/aze.jpg", im) + + return self.encode_image("/home/bdxv2/aze.jpg") + + + + # def encode_image(self, image): + # return base64.b64encode(image).decode("utf-8") + + def encode_image(self, image_path: str): + # check if the image exists + if not os.path.exists(image_path): + raise FileNotFoundError(f"Image file not found: {image_path}") + with open(image_path, "rb") as image_file: + return base64.b64encode(image_file.read()).decode("utf-8") diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/duck_config.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/duck_config.py new file mode 100644 index 0000000..c570c15 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/duck_config.py @@ -0,0 +1,83 @@ +import json +from typing import Optional +import os + +HOME_DIR = os.path.expanduser("~") + + +class DuckConfig: + + def __init__( + self, + config_json_path: Optional[str] = f"{HOME_DIR}/duck_config.json", + ignore_default: bool = False, + ): + """ + Looks for duck_config.json in the home directory by default. + If not found, uses default values. + """ + self.default = False + try: + self.json_config = ( + json.load(open(config_json_path, "r")) if config_json_path else {} + ) + except FileNotFoundError: + print( + f"Warning : didn't find the config json file at {config_json_path}, using default values" + ) + self.json_config = {} + self.default = True + + if config_json_path is None: + print("Warning : didn't provide a config json path, using default values") + self.default = True + + if self.default and not ignore_default: + print("") + print("") + print("") + print("") + print("======") + print( + "WARNING : Running with default values probably won't work well. Please make a duck_config.json file and set the parameters." + ) + res = input("Do you still want to run ? (y/N)") + if res.lower() != "y": + print("Exiting...") + exit(1) + + self.start_paused = self.json_config.get("start_paused", False) + self.imu_upside_down = self.json_config.get("imu_upside_down", False) + self.phase_frequency_factor_offset = self.json_config.get( + "phase_frequency_factor_offset", 0.0 + ) + + expression_features = self.json_config.get("expression_features", {}) + + self.eyes = expression_features.get("eyes", False) + self.projector = expression_features.get("projector", False) + self.antennas = expression_features.get("antennas", False) + self.speaker = expression_features.get("speaker", False) + self.microphone = expression_features.get("microphone", False) + self.camera = expression_features.get("camera", False) + + # default joints offsets are 0.0 + self.joints_offset = self.json_config.get( + "joints_offsets", + { + "left_hip_yaw": 0.0, + "left_hip_roll": 0.0, + "left_hip_pitch": 0.0, + "left_knee": 0.0, + "left_ankle": 0.0, + "neck_pitch": 0.0, + "head_pitch": 0.0, + "head_yaw": 0.0, + "head_roll": 0.00, + "right_hip_yaw": 0.0, + "right_hip_roll": 0.0, + "right_hip_pitch": 0.0, + "right_knee": 0.0, + "right_ankle": 0.0, + }, + ) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/eyes.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/eyes.py new file mode 100644 index 0000000..b07446b --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/eyes.py @@ -0,0 +1,58 @@ +import board +import digitalio +import random +import time +from threading import Thread, Event + +LEFT_EYE_PIN = board.D24 +RIGHT_EYE_PIN = board.D23 + + +class Eyes: + def __init__(self, blink_duration=0.1, min_interval=1.0, max_interval=4.0): + self.left_eye = digitalio.DigitalInOut(LEFT_EYE_PIN) + self.left_eye.direction = digitalio.Direction.OUTPUT + + self.right_eye = digitalio.DigitalInOut(RIGHT_EYE_PIN) + self.right_eye.direction = digitalio.Direction.OUTPUT + + self.blink_duration = blink_duration + self.min_interval = min_interval + self.max_interval = max_interval + + self._stop_event = Event() + self._thread = Thread(target=self.run, daemon=True) + self._thread.start() + + def _set_eyes(self, state): + self.left_eye.value = state + self.right_eye.value = state + + def run(self): + try: + while not self._stop_event.is_set(): + self._set_eyes(False) + time.sleep(self.blink_duration) + self._set_eyes(True) + next_blink = random.uniform(self.min_interval, self.max_interval) + time.sleep(next_blink) + except Exception as err: + print(f"Error in eye thread: {err}") + self._stop_event.set() + + def stop(self): + self._stop_event.set() + self._thread.join() + self._set_eyes(False) + self.left_eye.deinit() + self.right_eye.deinit() + + +if __name__ == "__main__": + e = Eyes() + try: + while True: + time.sleep(1) + finally: + e.stop() + diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py new file mode 100644 index 0000000..87c2d2e --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py @@ -0,0 +1,34 @@ +import board +import digitalio +import time + +LEFT_FOOT_PIN = board.D22 +RIGHT_FOOT_PIN = board.D27 + +class FeetContacts: + def __init__(self): + self.left_foot = digitalio.DigitalInOut(LEFT_FOOT_PIN) + self.left_foot.direction = digitalio.Direction.INPUT + self.left_foot.pull = digitalio.Pull.UP + + self.right_foot = digitalio.DigitalInOut(RIGHT_FOOT_PIN) + self.right_foot.direction = digitalio.Direction.INPUT + self.right_foot.pull = digitalio.Pull.UP + + def get(self): + left = not self.left_foot.value + right = not self.right_foot.value + return [left, right] + + def stop(self): + self.left_foot.deinit() + self.right_foot.deinit() + +if __name__ == "__main__": + feet_contacts = FeetContacts() + try: + while True: + print(feet_contacts.get()) + time.sleep(0.05) + finally: + feet_contacts.stop() diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/imu.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/imu.py new file mode 100644 index 0000000..bcdcb55 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/imu.py @@ -0,0 +1,161 @@ +import adafruit_bno055 +import board +import busio +import numpy as np +import pickle +import os + +# import serial + +from queue import Queue +from threading import Thread +import time +from scipy.spatial.transform import Rotation as R + + +# TODO filter spikes +class Imu: + def __init__( + self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True + ): + self.sampling_freq = sampling_freq + self.user_pitch_bias = user_pitch_bias + self.nominal_pitch_bias = 0 + self.calibrate = calibrate + + # self.uart = serial.Serial("/dev/ttyS0", baudrate=9600) + # self.imu = adafruit_bno055.BNO055_UART(self.uart) + + i2c = busio.I2C(board.SCL, board.SDA) + self.imu = adafruit_bno055.BNO055_I2C(i2c) + + self.imu.mode = adafruit_bno055.IMUPLUS_MODE + # self.imu.mode = adafruit_bno055.ACCGYRO_MODE + # self.imu.mode = adafruit_bno055.GYRONLY_MODE + # self.imu.mode = adafruit_bno055.NDOF_MODE + # self.imu.mode = adafruit_bno055.NDOF_FMC_OFF_MODE + + if upside_down: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + ) + else: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + ) + + self.pitch_bias = self.nominal_pitch_bias + self.user_pitch_bias + + if self.calibrate: + self.imu.mode = adafruit_bno055.NDOF_MODE + calibrated = self.imu.calibrated + while not calibrated: + print("Calibration status: ", self.imu.calibration_status) + print("Calibrated : ", self.imu.calibrated) + calibrated = self.imu.calibrated + time.sleep(0.1) + print("CALIBRATION DONE") + offsets_accelerometer = self.imu.offsets_accelerometer + offsets_gyroscope = self.imu.offsets_gyroscope + offsets_magnetometer = self.imu.offsets_magnetometer + + imu_calib_data = { + "offsets_accelerometer": offsets_accelerometer, + "offsets_gyroscope": offsets_gyroscope, + "offsets_magnetometer": offsets_magnetometer, + } + for k, v in imu_calib_data.items(): + print(k, v) + + pickle.dump(imu_calib_data, open("imu_calib_data.pkl", "wb")) + + print("Saved", "imu_calib_data.pkl") + exit() + + if os.path.exists("imu_calib_data.pkl"): + imu_calib_data = pickle.load(open("imu_calib_data.pkl", "rb")) + self.imu.mode = adafruit_bno055.CONFIG_MODE + time.sleep(0.1) + self.imu.offsets_accelerometer = imu_calib_data["offsets_accelerometer"] + self.imu.offsets_gyroscope = imu_calib_data["offsets_gyroscope"] + self.imu.offsets_magnetometer = imu_calib_data["offsets_magnetometer"] + self.imu.mode = adafruit_bno055.IMUPLUS_MODE + time.sleep(0.1) + else: + print("imu_calib_data.pkl not found") + print("Imu is running uncalibrated") + + self.last_imu_data = [0, 0, 0, 0] + self.imu_queue = Queue(maxsize=1) + Thread(target=self.imu_worker, daemon=True).start() + + def convert_axes(self, euler): + euler = [np.pi + euler[1], euler[0], euler[2]] + return euler + + def imu_worker(self): + while True: + s = time.time() + try: + # imu returns scalar first + raw_orientation = np.array(self.imu.quaternion).copy() # quat + euler = ( + R.from_quat(raw_orientation, scalar_first=True) + .as_euler("xyz") + .copy() + ) + except Exception as e: + print("[IMU]:", e) + continue + + # Converting to correct axes + # euler = self.convert_axes(euler) + euler[1] -= np.deg2rad(self.pitch_bias) + # euler[2] = 0 # ignoring yaw + + # gives scalar last, which is what isaac wants + final_orientation_quat = R.from_euler("xyz", euler).as_quat() + + self.imu_queue.put(final_orientation_quat.copy()) + took = time.time() - s + time.sleep(max(0, 1 / self.sampling_freq - took)) + + def get_data(self, euler=False, mat=False): + try: + self.last_imu_data = self.imu_queue.get(False) # non blocking + except Exception: + pass + + try: + if not euler and not mat: + return self.last_imu_data + elif euler: + return R.from_quat(self.last_imu_data).as_euler("xyz") + elif mat: + return R.from_quat(self.last_imu_data).as_matrix() + + except Exception as e: + print("[IMU]: ", e) + return None + + +if __name__ == "__main__": + imu = Imu(50, calibrate=True, upside_down=False) + # imu = Imu(50, upside_down=False) + while True: + data = imu.get_data() + # print(data) + print("gyro", np.around(data["gyro"], 3)) + print("accelero", np.around(data["accelero"], 3)) + print("---") + time.sleep(1 / 25) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/onnx_infer.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/onnx_infer.py new file mode 100644 index 0000000..85f0ddd --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/onnx_infer.py @@ -0,0 +1,43 @@ +import onnxruntime + + +class OnnxInfer: + def __init__(self, onnx_model_path, input_name="obs", awd=False): + self.onnx_model_path = onnx_model_path + self.ort_session = onnxruntime.InferenceSession( + self.onnx_model_path, providers=["CPUExecutionProvider"] + ) + self.input_name = input_name + self.awd = awd + + def infer(self, inputs): + if self.awd: + outputs = self.ort_session.run(None, {self.input_name: [inputs]}) + return outputs[0][0] + else: + outputs = self.ort_session.run( + None, {self.input_name: inputs.astype("float32")} + ) + return outputs[0] + + +if __name__ == "__main__": + import argparse + import numpy as np + import time + + parser = argparse.ArgumentParser() + parser.add_argument("-o", "--onnx_model_path", type=str, required=True) + args = parser.parse_args() + + oi = OnnxInfer(args.onnx_model_path, awd=True) + inputs = np.random.uniform(size=54).astype(np.float32) + inputs = np.arange(47).astype(np.float32) + times = [] + for i in range(1000): + start = time.time() + print(oi.infer(inputs)) + times.append(time.time() - start) + + print("Average time: ", sum(times) / len(times)) + print("Average fps: ", 1 / (sum(times) / len(times))) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/poly_reference_motion.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/poly_reference_motion.py new file mode 100644 index 0000000..b5d5961 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/poly_reference_motion.py @@ -0,0 +1,118 @@ +import numpy as np +import pickle + +class PolyReferenceMotion: + def __init__(self, polynomial_coefficients: str): + data = pickle.load(open(polynomial_coefficients, "rb")) + self.dx_range = [0, 0] + self.dy_range = [0, 0] + self.dtheta_range = [0, 0] + self.dxs = [] + self.dys = [] + self.dthetas = [] + self.data_array = [] + self.period = None + self.fps = None + self.frame_offsets = None + self.startend_double_support_ratio = None + self.start_offset = None + self.nb_steps_in_period = None + + self.process(data) + + def process(self, data): + print("[Poly ref data] Processing ...") + _data = {} + for name in data.keys(): + split = name.split("_") + dx = float(split[0]) + dy = float(split[1]) + dtheta = float(split[2]) + + if self.period is None: + self.period = data[name]["period"] + self.fps = data[name]["fps"] + self.frame_offsets = data[name]["frame_offsets"] + self.startend_double_support_ratio = data[name][ + "startend_double_support_ratio" + ] + self.start_offset = int(self.startend_double_support_ratio * self.fps) + self.nb_steps_in_period = int(self.period * self.fps) + + if dx not in self.dxs: + self.dxs.append(dx) + + if dy not in self.dys: + self.dys.append(dy) + + if dtheta not in self.dthetas: + self.dthetas.append(dtheta) + + self.dx_range = [min(dx, self.dx_range[0]), max(dx, self.dx_range[1])] + self.dy_range = [min(dy, self.dy_range[0]), max(dy, self.dy_range[1])] + self.dtheta_range = [ + min(dtheta, self.dtheta_range[0]), + max(dtheta, self.dtheta_range[1]), + ] + + if dx not in _data: + _data[dx] = {} + + if dy not in _data[dx]: + _data[dx][dy] = {} + + if dtheta not in _data[dx][dy]: + _data[dx][dy][dtheta] = data[name] + + _coeffs = data[name]["coefficients"] + + coeffs = [] + for k, v in _coeffs.items(): + coeffs.append(v) + _data[dx][dy][dtheta] = coeffs + + self.dxs = sorted(self.dxs) + self.dys = sorted(self.dys) + self.dthetas = sorted(self.dthetas) + + nb_dx = len(self.dxs) + nb_dy = len(self.dys) + nb_dtheta = len(self.dthetas) + + self.data_array = nb_dx * [None] + for x, dx in enumerate(self.dxs): + self.data_array[x] = nb_dy * [None] + for y, dy in enumerate(self.dys): + self.data_array[x][y] = nb_dtheta * [None] + for th, dtheta in enumerate(self.dthetas): + self.data_array[x][y][th] = _data[dx][dy][dtheta] + + self.data_array = self.data_array + + print("[Poly ref data] Done processing") + + def vel_to_index(self, dx, dy, dtheta): + + dx = np.clip(dx, self.dx_range[0], self.dx_range[1]) + dy = np.clip(dy, self.dy_range[0], self.dy_range[1]) + dtheta = np.clip(dtheta, self.dtheta_range[0], self.dtheta_range[1]) + + ix = np.argmin(np.abs(np.array(self.dxs) - dx)) + iy = np.argmin(np.abs(np.array(self.dys) - dy)) + itheta = np.argmin(np.abs(np.array(self.dthetas) - dtheta)) + + return int(ix), int(iy), int(itheta) + + def sample_polynomial(self, t, coeffs): + ret = [] + for c in coeffs: + ret.append(np.polyval(np.flip(c), t)) + + return ret + + def get_reference_motion(self, dx, dy, dtheta, i): + ix, iy, itheta = self.vel_to_index(dx, dy, dtheta) + t = i % self.nb_steps_in_period / self.nb_steps_in_period + t = np.clip(t, 0.0, 1.0) # safeguard + ret = self.sample_polynomial(t, self.data_array[ix][iy][itheta]) + return ret \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/projector.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/projector.py new file mode 100644 index 0000000..6ba4d39 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/projector.py @@ -0,0 +1,31 @@ +import board +import digitalio +import time + +PROJECTOR_GPIO = board.D25 + + +class Projector: + def __init__(self): + self.project = digitalio.DigitalInOut(PROJECTOR_GPIO) + self.project.direction = digitalio.Direction.OUTPUT + self.on = False + + def switch(self): + self.on = not self.on + + self.project.value = self.on + + def stop(self): + self.project.value = False + self.project.deinit() + + +if __name__ == "__main__": + p = Projector() + try: + while True: + p.switch() + time.sleep(1) + finally: + p.stop() diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py new file mode 100644 index 0000000..ce24627 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py @@ -0,0 +1,167 @@ +import adafruit_bno055 +import board +import busio +import numpy as np +import os +import pickle + +from queue import Queue +from threading import Thread +import time + + +# TODO filter spikes +class Imu: + def __init__( + self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True + ): + self.sampling_freq = sampling_freq + self.calibrate = calibrate + + i2c = busio.I2C(board.SCL, board.SDA) + self.imu = adafruit_bno055.BNO055_I2C(i2c) + + # self.imu.mode = adafruit_bno055.IMUPLUS_MODE + # self.imu.mode = adafruit_bno055.ACCGYRO_MODE + # self.imu.mode = adafruit_bno055.GYRONLY_MODE + self.imu.mode = adafruit_bno055.NDOF_MODE + # self.imu.mode = adafruit_bno055.NDOF_FMC_OFF_MODE + + if upside_down: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + ) + + else: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + ) + + if self.calibrate: + self.imu.mode = adafruit_bno055.NDOF_MODE + calibrated = self.imu.calibrated + while not calibrated: + print("Calibration status: ", self.imu.calibration_status) + print("Calibrated : ", self.imu.calibrated) + calibrated = self.imu.calibrated + time.sleep(0.1) + print("CALIBRATION DONE") + offsets_accelerometer = self.imu.offsets_accelerometer + offsets_gyroscope = self.imu.offsets_gyroscope + offsets_magnetometer = self.imu.offsets_magnetometer + + imu_calib_data = { + "offsets_accelerometer": offsets_accelerometer, + "offsets_gyroscope": offsets_gyroscope, + "offsets_magnetometer": offsets_magnetometer, + } + for k, v in imu_calib_data.items(): + print(k, v) + + pickle.dump(imu_calib_data, open("imu_calib_data.pkl", "wb")) + + print("Saved", "imu_calib_data.pkl") + exit() + + if os.path.exists("imu_calib_data.pkl"): + imu_calib_data = pickle.load(open("imu_calib_data.pkl", "rb")) + self.imu.mode = adafruit_bno055.CONFIG_MODE + time.sleep(0.1) + self.imu.offsets_accelerometer = imu_calib_data["offsets_accelerometer"] + self.imu.offsets_gyroscope = imu_calib_data["offsets_gyroscope"] + self.imu.offsets_magnetometer = imu_calib_data["offsets_magnetometer"] + self.imu.mode = adafruit_bno055.NDOF_MODE + time.sleep(0.1) + else: + print("imu_calib_data.pkl not found") + print("Imu is running uncalibrated") + + self.x_offset = 0 + + # self.tare_x() + + self.last_imu_data = [0, 0, 0, 0] + self.last_imu_data = { + "gyro": [0, 0, 0], + "accelero": [0, 0, 0], + } + self.imu_queue = Queue(maxsize=1) + Thread(target=self.imu_worker, daemon=True).start() + + def tare_x(self): + print("Taring x ...") + x_values = [] + num_values = 100 + ok = False + while not ok: + x_values.append(np.array(self.imu.acceleration)[0]) + + x_values = x_values[-num_values:] + + if len(x_values) == num_values: + mean = np.mean(x_values) + std = np.std(x_values) + if std < 0.05: + ok = True + self.x_offset = mean + print("Tare x done") + else: + print(std) + + time.sleep(0.01) + + def imu_worker(self): + while True: + s = time.time() + try: + gyro = np.array(self.imu.gyro).copy() + accelero = np.array(self.imu.acceleration).copy() + except Exception as e: + print("[IMU]:", e) + continue + + if gyro is None or accelero is None: + continue + + if gyro.any() is None or accelero.any() is None: + continue + + accelero[0] -= self.x_offset + + data = { + "gyro": gyro, + "accelero": accelero, + } + + self.imu_queue.put(data) + took = time.time() - s + time.sleep(max(0, 1 / self.sampling_freq - took)) + + def get_data(self): + try: + self.last_imu_data = self.imu_queue.get(False) # non blocking + except Exception: + pass + + return self.last_imu_data + + +if __name__ == "__main__": + imu = Imu(50, upside_down=False) + while True: + data = imu.get_data() + # print(data) + print("gyro", np.around(data["gyro"], 3)) + print("accelero", np.around(data["accelero"], 3)) + print("---") + time.sleep(1 / 25) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rl_utils.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rl_utils.py new file mode 100644 index 0000000..2296507 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rl_utils.py @@ -0,0 +1,161 @@ +# This is the joints order when loading using IsaacGymEnvs +# ['left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna', 'right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle'] +# This is the "standard" order (from mujoco) +# ['right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle', 'left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna'] +# +# We need to reorder the joints to match the IsaacGymEnvs order +# +import numpy as np + +mujoco_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + +isaac_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + + +def isaac_to_mujoco(joints): + new_joints = [ + # left leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # right leg + joints[11], + joints[12], + joints[13], + joints[14], + joints[15], + # head + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + joints[10], + ] + return new_joints + + +def mujoco_to_isaac(joints): + new_joints = [ + # left leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # head + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + joints[15], + # right leg + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + ] + return new_joints + + +# TODO ADD BACK +def action_to_pd_targets(action, offset, scale): + return offset + scale * action + + +def make_action_dict(action, joints_order): + action_dict = {} + for i, a in enumerate(action): + if "antenna" not in joints_order[i]: + action_dict[joints_order[i]] = a + + return action_dict + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +class ActionFilter: + def __init__(self, window_size=10): + self.window_size = window_size + self.action_buffer = [] + + def push(self, action): + self.action_buffer.append(action) + if len(self.action_buffer) > self.window_size: + self.action_buffer.pop(0) + + def get_filtered_action(self): + return np.mean(self.action_buffer, axis=0) + + +class LowPassActionFilter: + def __init__(self, control_freq, cutoff_frequency=30.0): + self.last_action = 0 + self.current_action = 0 + self.control_freq = float(control_freq) + self.cutoff_frequency = float(cutoff_frequency) + self.alpha = self.compute_alpha() + + def compute_alpha(self): + return (1.0 / self.cutoff_frequency) / ( + 1.0 / self.control_freq + 1.0 / self.cutoff_frequency + ) + + def push(self, action): + self.current_action = action + + def get_filtered_action(self): + self.last_action = ( + self.alpha * self.last_action + (1 - self.alpha) * self.current_action + ) + return self.last_action diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py new file mode 100644 index 0000000..c065c8f --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py @@ -0,0 +1,166 @@ +import time + +import numpy as np +import rustypot +from mini_bdx_runtime.duck_config import DuckConfig + + +class HWI: + def __init__(self, duck_config: DuckConfig, usb_port: str = "/dev/ttyACM0"): + + self.duck_config = duck_config + + # Order matters here + self.joints = { + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + # "left_antenna": None, + # "right_antenna": None, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, + } + + self.zero_pos = { + "left_hip_yaw": 0, + "left_hip_roll": 0, + "left_hip_pitch": 0, + "left_knee": 0, + "left_ankle": 0, + "neck_pitch": 0, + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + # "left_antenna":0, + # "right_antenna":0, + "right_hip_yaw": 0, + "right_hip_roll": 0, + "right_hip_pitch": 0, + "right_knee": 0, + "right_ankle": 0, + } + + self.init_pos = { + "left_hip_yaw": 0.002, + "left_hip_roll": 0.053, + "left_hip_pitch": -0.63, + "left_knee": 1.368, + "left_ankle": -0.784, + "neck_pitch": 0.0, + "head_pitch": 0.0, + "head_yaw": 0, + "head_roll": 0, + # "left_antenna": 0, + # "right_antenna": 0, + "right_hip_yaw": -0.003, + "right_hip_roll": -0.065, + "right_hip_pitch": 0.635, + "right_knee": 1.379, + "right_ankle": -0.796, + } + + self.joints_offsets = self.duck_config.joints_offset + + self.kps = np.ones(len(self.joints)) * 32 # default kp + self.kds = np.ones(len(self.joints)) * 0 # default kd + self.low_torque_kps = np.ones(len(self.joints)) * 2 + + self.io = rustypot.feetech(usb_port, 1000000) + + def set_kps(self, kps): + self.kps = kps + self.io.set_kps(list(self.joints.values()), self.kps) + + def set_kds(self, kds): + self.kds = kds + self.io.set_kds(list(self.joints.values()), self.kds) + + def set_kp(self, id, kp): + self.io.set_kps([id], [kp]) + + def turn_on(self): + self.io.set_kps(list(self.joints.values()), self.low_torque_kps) + print("turn on : low KPS set") + time.sleep(1) + + self.set_position_all(self.init_pos) + print("turn on : init pos set") + + time.sleep(1) + + self.io.set_kps(list(self.joints.values()), self.kps) + print("turn on : high kps") + + def turn_off(self): + self.io.disable_torque(list(self.joints.values())) + + def set_position(self, joint_name, pos): + """ + pos is in radians + """ + id = self.joints[joint_name] + pos = pos + self.joints_offsets[joint_name] + self.io.write_goal_position([id], [pos]) + + def set_position_all(self, joints_positions): + """ + joints_positions is a dictionary with joint names as keys and joint positions as values + Warning: expects radians + """ + ids_positions = { + self.joints[joint]: position + self.joints_offsets[joint] + for joint, position in joints_positions.items() + } + + self.io.write_goal_position( + list(self.joints.values()), list(ids_positions.values()) + ) + + def get_present_positions(self, ignore=[]): + """ + Returns the present positions in radians + """ + + try: + present_positions = self.io.read_present_position( + list(self.joints.values()) + ) + except Exception as e: + print(e) + return None + + present_positions = [ + pos - self.joints_offsets[joint] + for joint, pos in zip(self.joints.keys(), present_positions) + if joint not in ignore + ] + return np.array(np.around(present_positions, 3)) + + def get_present_velocities(self, rad_s=True, ignore=[]): + """ + Returns the present velocities in rad/s (default) or rev/min + """ + try: + present_velocities = self.io.read_present_velocity( + list(self.joints.values()) + ) + except Exception as e: + print(e) + return None + + present_velocities = [ + vel + for joint, vel in zip(self.joints.keys(), present_velocities) + if joint not in ignore + ] + + return np.array(np.around(present_velocities, 3)) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/sounds.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/sounds.py new file mode 100644 index 0000000..7cc1e3a --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/sounds.py @@ -0,0 +1,58 @@ +import pygame +import time +import os +import random + + +class Sounds: + def __init__(self, volume=1.0, sound_directory="./"): + pygame.mixer.init() + pygame.mixer.music.set_volume(volume) + self.sounds = {} + self.ok = True + try: + for file in os.listdir(sound_directory): + if file.endswith(".wav"): + sound_path = os.path.join(sound_directory, file) + try: + self.sounds[file] = pygame.mixer.Sound(sound_path) + print(f"Loaded: {file}") + except pygame.error as e: + print(f"Failed to load {file}: {e}") + except FileNotFoundError: + print(f"Directory {sound_directory} not found.") + self.ok = False + if len(self.sounds) == 0: + print("No sound files found in the directory.") + self.ok = False + + def play(self, sound_name): + if not self.ok: + print("Sounds not initialized properly.") + return + if sound_name in self.sounds: + self.sounds[sound_name].play() + print(f"Playing: {sound_name}") + else: + print(f"Sound '{sound_name}' not found!") + + def play_random_sound(self): + if not self.ok: + print("Sounds not initialized properly.") + return + sound_name = random.choice(list(self.sounds.keys())) + self.sounds[sound_name].play() + + def play_happy(self): + self.sounds["happy1.wav"].play() + + + +# Example usage +if __name__ == "__main__": + sound_player = Sounds(1.0, "../assets/") + time.sleep(1) + while True: + # sound_player.play_random_sound() + sound_player.play_happy() + time.sleep(3) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py new file mode 100644 index 0000000..cd74b54 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py @@ -0,0 +1,220 @@ +import pygame +from threading import Thread +from queue import Queue +import time +import numpy as np +from mini_bdx_runtime.buttons import Buttons + + +X_RANGE = [-0.15, 0.15] +Y_RANGE = [-0.2, 0.2] +YAW_RANGE = [-1.0, 1.0] + +# rads +NECK_PITCH_RANGE = [-0.34, 1.1] +HEAD_PITCH_RANGE = [-0.78, 0.3] +HEAD_YAW_RANGE = [-0.5, 0.5] +HEAD_ROLL_RANGE = [-0.5, 0.5] + + +class XBoxController: + def __init__(self, command_freq, only_head_control=False): + self.command_freq = command_freq + self.head_control_mode = only_head_control + self.only_head_control = only_head_control + + self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + self.last_left_trigger = 0.0 + self.last_right_trigger = 0.0 + pygame.init() + self.p1 = pygame.joystick.Joystick(0) + self.p1.init() + print(f"Loaded joystick with {self.p1.get_numaxes()} axes.") + self.cmd_queue = Queue(maxsize=1) + + self.A_pressed = False + self.B_pressed = False + self.X_pressed = False + self.Y_pressed = False + self.LB_pressed = False + self.RB_pressed = False + + self.buttons = Buttons() + + Thread(target=self.commands_worker, daemon=True).start() + + def commands_worker(self): + while True: + self.cmd_queue.put(self.get_commands()) + time.sleep(1 / self.command_freq) + + def get_commands(self): + last_commands = self.last_commands + left_trigger = self.last_left_trigger + right_trigger = self.last_right_trigger + + l_x = -1 * self.p1.get_axis(0) + l_y = -1 * self.p1.get_axis(1) + r_x = -1 * self.p1.get_axis(2) + r_y = -1 * self.p1.get_axis(3) + + right_trigger = np.around((self.p1.get_axis(4) + 1) / 2, 3) + left_trigger = np.around((self.p1.get_axis(5) + 1) / 2, 3) + + if left_trigger < 0.1: + left_trigger = 0 + if right_trigger < 0.1: + right_trigger = 0 + + if not self.head_control_mode: + lin_vel_y = l_x + lin_vel_x = l_y + ang_vel = r_x + if lin_vel_x >= 0: + lin_vel_x *= np.abs(X_RANGE[1]) + else: + lin_vel_x *= np.abs(X_RANGE[0]) + + if lin_vel_y >= 0: + lin_vel_y *= np.abs(Y_RANGE[1]) + else: + lin_vel_y *= np.abs(Y_RANGE[0]) + + if ang_vel >= 0: + ang_vel *= np.abs(YAW_RANGE[1]) + else: + ang_vel *= np.abs(YAW_RANGE[0]) + + last_commands[0] = lin_vel_x + last_commands[1] = lin_vel_y + last_commands[2] = ang_vel + else: + last_commands[0] = 0.0 + last_commands[1] = 0.0 + last_commands[2] = 0.0 + last_commands[3] = 0.0 # neck pitch 0 for now + + head_yaw = l_x + head_pitch = l_y + head_roll = r_x + + if head_yaw >= 0: + head_yaw *= np.abs(HEAD_YAW_RANGE[0]) + else: + head_yaw *= np.abs(HEAD_YAW_RANGE[1]) + + if head_pitch >= 0: + head_pitch *= np.abs(HEAD_PITCH_RANGE[0]) + else: + head_pitch *= np.abs(HEAD_PITCH_RANGE[1]) + + if head_roll >= 0: + head_roll *= np.abs(HEAD_ROLL_RANGE[0]) + else: + head_roll *= np.abs(HEAD_ROLL_RANGE[1]) + + last_commands[4] = head_pitch + last_commands[5] = head_yaw + last_commands[6] = head_roll + + for event in pygame.event.get(): + if event.type == pygame.JOYBUTTONDOWN: + + if self.p1.get_button(0): # A button + self.A_pressed = True + + if self.p1.get_button(1): # B button + self.B_pressed = True + + if self.p1.get_button(3): # X button + self.X_pressed = True + + if self.p1.get_button(4): # Y button + self.Y_pressed = True + if not self.only_head_control: + self.head_control_mode = not self.head_control_mode + + if self.p1.get_button(6): # LB button + self.LB_pressed = True + + if self.p1.get_button(7): # RB button + self.RB_pressed = True + + if event.type == pygame.JOYBUTTONUP: + self.A_pressed = False + self.B_pressed = False + self.X_pressed = False + self.Y_pressed = False + self.LB_pressed = False + self.RB_pressed = False + + # for i in range(10): + # if self.p1.get_button(i): + # print(f"Button {i} pressed") + + up_down = self.p1.get_hat(0)[1] + pygame.event.pump() # process event queue + + return ( + np.around(last_commands, 3), + self.A_pressed, + self.B_pressed, + self.X_pressed, + self.Y_pressed, + self.LB_pressed, + self.RB_pressed, + left_trigger, + right_trigger, + up_down, + ) + + def get_last_command(self): + A_pressed = False + B_pressed = False + X_pressed = False + Y_pressed = False + LB_pressed = False + RB_pressed = False + up_down = 0 + try: + ( + self.last_commands, + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + self.last_left_trigger, + self.last_right_trigger, + up_down, + ) = self.cmd_queue.get( + False + ) # non blocking + except Exception: + pass + + self.buttons.update( + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + up_down == 1, + up_down == -1, + ) + + return ( + self.last_commands, + self.buttons, + self.last_left_trigger, + self.last_right_trigger, + ) + +if __name__ == "__main__": + controller = XBoxController(20) + + while True: + print(controller.get_last_command()) + time.sleep(0.05) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/pyproject.toml b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/pyproject.toml new file mode 100644 index 0000000..7fd26b9 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/antennas_controller_test.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/antennas_controller_test.py new file mode 100644 index 0000000..b0edddd --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/antennas_controller_test.py @@ -0,0 +1,18 @@ +from mini_bdx_runtime.xbox_controller import XBoxController +from mini_bdx_runtime.antennas import Antennas +import time + +controller = XBoxController(60) + +antennas = Antennas() + + +while True: + + _, _, left_trigger, right_trigger = controller.get_last_command() + + antennas.set_position_left(right_trigger) + antennas.set_position_right(left_trigger) + + # print(left_trigger, right_trigger) + time.sleep(1 / 50) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/calibrate_imu.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/calibrate_imu.py new file mode 100644 index 0000000..73a531d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/calibrate_imu.py @@ -0,0 +1,5 @@ + +from mini_bdx_runtime.raw_imu import Imu + +if __name__ == "__main__": + imu = Imu(50, calibrate=True, upside_down=False) \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/cam_test.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/cam_test.py new file mode 100644 index 0000000..b75f311 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/cam_test.py @@ -0,0 +1,18 @@ +from picamzero import Camera +import cv2 + +print("Initializing camera ...") +cam = Camera() +# cam.still_size = (512, 512) +print("Camera initialized") + +im = cam.capture_array() +im = cv2.resize(im, (512, 512)) +im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB) + +cv2.imwrite("/home/bdxv2/aze.jpg", im) +# cv2.imshow("Image", im) +# cv2.waitKey(0) +# cv2.destroyAllWindows() + +# cam.take_photo("/home/bdxv2/aze.jpg") diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/check_motors.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/check_motors.py new file mode 100644 index 0000000..9bc3f26 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/check_motors.py @@ -0,0 +1,168 @@ +""" +Debug script to check all motors in the robot. +Verifies each motor is accessible and allows testing movement. +""" + +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time +import numpy as np +import traceback + +def main(): + print("Initializing hardware interface...") + try: + # Initialize with default USB port - you might need to modify this + print("Attempting to connect to motor controller...") + duck_config = DuckConfig() # Create default configuration + + # Initialize with duck_config + print("Attempting to connect to motor controller...") + hwi = HWI(duck_config=duck_config) + print("Successfully connected to hardware!") + except Exception as e: + print(f"Error connecting to hardware: {e}") + print(f"Error details: {traceback.format_exc()}") + print("Check that the robot is powered on and USB connection is correct.") + return + + # Turn on with low torque for safety - ONE BY ONE + print("\nTurning on motors with low torque (one by one)...") + unresponsive_motors = [] + + for joint_name, joint_id in hwi.joints.items(): + try: + print(f"Setting low torque for motor '{joint_name}' (ID: {joint_id})...") + hwi.io.set_kps([joint_id], [hwi.low_torque_kps[0]]) + print(f"✓ Low torque set successfully for motor '{joint_name}' (ID: {joint_id}).") + except Exception as e: + print(f"✗ Error setting low torque for motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details: {traceback.format_exc()}") + unresponsive_motors.append((joint_name, joint_id)) + + # Check if all motors are responsive + print("\nChecking if all motors are responsive...") + + for joint_name, joint_id in hwi.joints.items(): + # Skip motors that already failed + if (joint_name, joint_id) in unresponsive_motors: + print(f"Skipping previously unresponsive motor: '{joint_name}' (ID: {joint_id})") + continue + + print(f"Attempting to read position from motor '{joint_name}' (ID: {joint_id})...") + try: + # Try to read the position to check if motor is responsive + position = hwi.io.read_present_position([joint_id]) + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) is responsive. Position: {position[0]:.3f}") + except Exception as e: + print(f"✗ Error accessing motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details for motor {joint_id}: {traceback.format_exc()}") + unresponsive_motors.append((joint_name, joint_id)) + + if unresponsive_motors: + print("\nWARNING: Some motors are not responsive!") + print("Unresponsive motors:", unresponsive_motors) + continue_anyway = input("Do you want to continue anyway? (y/n): ").lower() + if continue_anyway != 'y': + print("Exiting...") + try: + print("Attempting to turn off responsive motors before exiting...") + for joint_name, joint_id in hwi.joints.items(): + if (joint_name, joint_id) not in unresponsive_motors: + try: + hwi.io.disable_torque([joint_id]) + print(f"Disabled torque for motor '{joint_name}' (ID: {joint_id})") + except: + pass + except: + pass + return + + # Test moving each motor individually + print("\n--- Motor Movement Test ---") + print("This will move each motor by a small amount to check if it's working correctly.") + input("Press Enter to begin the movement test...") + + for joint_name, joint_id in hwi.joints.items(): + # Skip unresponsive motors + if (joint_name, joint_id) in unresponsive_motors: + print(f"Skipping unresponsive motor: '{joint_name}' (ID: {joint_id})") + continue + + print(f"\nTesting motor: '{joint_name}' (ID: {joint_id})") + test_this_motor = input(f"Test this motor? (Enter/y for yes, n to skip, q to quit): ").lower() + + if test_this_motor == 'q': + print("Exiting movement test...") + break + + if test_this_motor == 'n': + print(f"Skipping '{joint_name}' (ID: {joint_id})") + continue + + try: + # Get current position + print(f"Reading current position from motor '{joint_name}' (ID: {joint_id})...") + current_position = hwi.io.read_present_position([joint_id])[0] + print(f"Current position: {current_position:.3f}") + + # Calculate test position (move by 0.1 radians) + test_position = current_position + 0.1 + + # Move to test position + print(f"Moving motor '{joint_name}' (ID: {joint_id}) to test position: {test_position:.3f}...") + hwi.io.write_goal_position([joint_id], [test_position]) + time.sleep(1) # Wait for movement + + # Read new position + print(f"Reading new position from motor '{joint_name}' (ID: {joint_id})...") + new_position = hwi.io.read_present_position([joint_id])[0] + print(f"New position: {new_position:.3f}") + + # Return to original position + print(f"Returning motor '{joint_name}' (ID: {joint_id}) to original position...") + hwi.io.write_goal_position([joint_id], [current_position]) + time.sleep(1) # Wait for movement + + # No confirmation question, just assume success + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) movement test completed.") + + except Exception as e: + print(f"Error testing motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details: {traceback.format_exc()}") + + # Turn off motors + print("\nTurning off motors one by one...") + for joint_name, joint_id in hwi.joints.items(): + if (joint_name, joint_id) in unresponsive_motors: + print(f"Skipping turning off unresponsive motor: '{joint_name}' (ID: {joint_id})") + continue + + try: + print(f"Disabling torque for motor '{joint_name}' (ID: {joint_id})...") + hwi.io.disable_torque([joint_id]) + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) turned off successfully.") + except Exception as e: + print(f"✗ Error turning off motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details: {traceback.format_exc()}") + + print("\nMotor test completed.") + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\nScript interrupted by user. Attempting to turn off motors...") + try: + print("Initializing HWI to turn off motors...") + hwi = HWI() + for joint_name, joint_id in hwi.joints.items(): + try: + print(f"Turning off motor '{joint_name}' (ID: {joint_id})...") + hwi.io.disable_torque([joint_id]) + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) turned off successfully.") + except Exception as e: + print(f"✗ Error turning off motor '{joint_name}' (ID: {joint_id}): {e}") + except Exception as e: + print(f"Error initializing HWI to turn off motors: {e}") + print(f"Error details: {traceback.format_exc()}") \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/check_voltage.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/check_voltage.py new file mode 100644 index 0000000..89d4430 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/check_voltage.py @@ -0,0 +1,31 @@ +from pypot.feetech import FeetechSTS3215IO + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) + +joints = { + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + # "left_antenna": None, + # "right_antenna": None, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} + + +voltages = io.get_present_voltage(list(joints.values())) +for i, name in enumerate(joints.keys()): + print(name, round(voltages[i] * 0.1, 2), "V") \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/configure_all_motors.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/configure_all_motors.py new file mode 100644 index 0000000..5d391cc --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/configure_all_motors.py @@ -0,0 +1,40 @@ +from pypot.feetech import FeetechSTS3215IO +import time + +joints = { + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} + +joints_inv = {v: k for k, v in joints.items()} + +ids = list(joints.values()) +io = FeetechSTS3215IO("/dev/ttyACM0") +for current_id in ids: + print("Configuring", joints_inv[current_id]) + io.set_lock({current_id: 0}) + # io.set_mode({current_id: 0}) + io.set_maximum_acceleration({current_id: 0}) + io.set_acceleration({current_id: 0}) + # io.set_maximum_velocity({current_id: 0}) + # io.set_goal_speed({current_id: 0}) + io.set_P_coefficient({current_id: 32}) + io.set_I_coefficient({current_id: 0}) + io.set_D_coefficient({current_id: 0}) + + io.set_lock({current_id: 1}) + time.sleep(1) + input("Press any key to set this dof to 0 position ... Or press Ctrl+C to cancel") + io.set_goal_position({current_id: 0}) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/configure_motor.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/configure_motor.py new file mode 100644 index 0000000..c95639f --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/configure_motor.py @@ -0,0 +1,89 @@ +from pypot.feetech import FeetechSTS3215IO +import argparse +import time + +DEFAULT_ID = 1 # A brand new motor should have id 1 + +parser = argparse.ArgumentParser() +parser.add_argument( + "--port", + help="The port the motor is connected to. Default is /dev/ttyACM0. Use `ls /dev/tty* | grep usb` to find the port.", + default="/dev/ttyACM0", +) +parser.add_argument("--id", help="The id to set to the motor.", type=str, required=True) +args = parser.parse_args() +io = FeetechSTS3215IO(args.port) + +current_id = DEFAULT_ID + + +def scan(): + id = None + for i in range(255): + + print(f"scanning for id {i} ...") + try: + io.get_present_position([i]) + id = i + print(f"Found motor with id {id}") + break + except Exception: + pass + return id + + +try: + io.get_present_position([DEFAULT_ID]) +except Exception: + print( + f"Could not find motor with default id ({DEFAULT_ID}). Scanning for motor ..." + ) + res = scan() + if res is not None: + current_id = res + else: + print("Could not find motor. Exiting ...") + exit() + + +# print("current id: ", current_id) + +kp = io.get_P_coefficient([current_id]) +ki = io.get_I_coefficient([current_id]) +kd = io.get_D_coefficient([current_id]) +max_acceleration = io.get_maximum_acceleration([current_id]) +acceleration = io.get_acceleration([current_id]) +mode = io.get_mode([current_id]) + +# print(f"PID : {kp}, {ki}, {kd}") +# print(f"max_acceleration: {max_acceleration}") +# print(f"acceleration: {acceleration}") +# print(f"mode: {mode}") + +io.set_lock({current_id: 0}) +io.set_mode({current_id: 0}) +io.set_maximum_acceleration({current_id: 0}) +io.set_acceleration({current_id: 0}) +io.set_P_coefficient({current_id: 32}) +io.set_I_coefficient({current_id: 0}) +io.set_D_coefficient({current_id: 0}) +io.change_id({current_id: int(args.id)}) + +current_id = int(args.id) + +time.sleep(1) + +io.set_goal_position({current_id: 0}) + +time.sleep(1) + +print("===") +print("Done configuring motor.") +print(f"Motor id: {current_id}") +print(f"P coefficient : {io.get_P_coefficient([current_id])}") +print(f"I coefficient : {io.get_I_coefficient([current_id])}") +print(f"D coefficient : {io.get_D_coefficient([current_id])}") +print(f"acceleration: {io.get_acceleration([current_id])}") +print(f"max_acceleration: {io.get_maximum_acceleration([current_id])}") +print(f"mode: {io.get_mode([current_id])}") +print("===") diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/fc_test.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/fc_test.py new file mode 100644 index 0000000..3eed49d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/fc_test.py @@ -0,0 +1,302 @@ +from openai import OpenAI +import time +import json +import os +from io import BytesIO +import base64 + +from v2_rl_walk_mujoco import RLWalk +from threading import Thread +import cv2 +from mini_bdx_runtime.camera import Cam + +# TODO mission : find an object ? + + +# Your Tools class +class Tools: + def __init__(self): + self.cam = Cam() + self.rl_walk = RLWalk( + "/home/bdxv2/BEST_WALK_ONNX_2.onnx", + cutoff_frequency=40, + ) + + Thread(target=self.rl_walk.run, daemon=True).start() + + # def upload_image(self, image_path: str): + # image_name = os.path.basename(image_path) + # im = cv2.imread(image_path) + # im = cv2.resize(im, (512, 512)) + # cv2.imwrite(image_path, im) + # # command = ( + # # f"scp {image_path} apirrone@s-nguyen.net:/home/apirrone/webserv/images/" + # # ) + # command = ( + # f"scp {image_path} apirrone@192.168.10.103:/home/apirrone/webserv/images/" + # ) + # print(command) + # url = f"http://s-nguyen.net:4444/images/{image_name}" + # os.system(command) + # return url + + + def move_forward(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Moving forward for {seconds} seconds") + self.rl_walk.last_commands[0] = 0.15 + time.sleep(seconds) + self.rl_walk.last_commands[0] = 0.0 + print("Stopped moving forward") + return f"Moved forward for {seconds} seconds successfully" + + def turn_left(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Turning left for {seconds} seconds") + self.rl_walk.last_commands[2] = 1.0 + time.sleep(seconds) + self.rl_walk.last_commands[2] = 0.0 + print("Stopped turning left") + return f"Turned left for {seconds} seconds successfully" + + def turn_right(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Turning right for {seconds} seconds") + self.rl_walk.last_commands[2] = -1.0 + time.sleep(seconds) + self.rl_walk.last_commands[2] = 0.0 + print("Stopped turning right") + return f"Turned right for {seconds} seconds successfully" + + def move_backward(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Moving backward for {seconds} seconds") + self.rl_walk.last_commands[0] = -0.15 + time.sleep(seconds) + self.rl_walk.last_commands[0] = 0.0 + print("Stopped moving backward") + return f"Moved backward for {seconds} seconds successfully" + + def take_picture(self): + # https://projects.raspberrypi.org/en/projects/getting-started-with-picamera/5 + print("Taking a picture...") + image64 = self.cam.get_encoded_image() + url = ("data:image/jpeg;base64," + image64,) + time.sleep(1) + print("Picture taken") + return url + + def play_happy_sound(self): + self.rl_walk.sounds.play_happy() + return "Played happy sound" + +# Tool instance +tools_instance = Tools() + +openai_tools = [ + { + "type": "function", + "name": "move_forward", + "description": "Move forward for a number of seconds", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to move forward (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "move_backward", + "description": "Move backward for a number of seconds", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to move backward (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "turn_left", + "description": "Turn left on the spot", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to turn left (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "turn_right", + "description": "Turn right on the spot", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to turn right (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "take_picture", + "description": "Take a picture", + "parameters": { + "type": "object", + "properties": {}, + # No required properties for taking a picture + }, + }, + { + "type": "function", + "name": "play_happy_sound", + "description": "Play a happy sound", + "parameters": { + "type": "object", + "properties": {}, + # No required properties for playing a sound + }, + } +] + +# Mapping function names to actual methods +function_map = { + "move_forward": tools_instance.move_forward, + "move_backward": tools_instance.move_backward, + "turn_left": tools_instance.turn_left, + "turn_right": tools_instance.turn_right, + "take_picture": tools_instance.take_picture, +} + +messages = [ + { + "role": "system", + "content": ( + "You are a cute little biped robot that can move around using the following tools: " + "`move_forward`, `move_backward`, `turn_left`, `turn_right`, 'play_happy_sound' and 'take_picture'. " + "moving forward for 1 second will make you move forward by about 15 centimeters" + "turning for 1 second will make you turn about 45 degrees" + "You can only perform one action at a time. If multiple actions are needed, call them step by step." + "Explain what you are doing along the way" + "Always start by taking a picture of the environment so you can see where you are. " + "When you take a picture, describe what you see in the image. " + "make sure not to hit any walls or objects. Take pictures regularly to know where you are." + "Maybe it's a good idea to turn 360 degrees to check all directions. (no need if you already found it)" + "When given a goal to find something, if you found it, navigate to be in front of it, facing it. You want to be about 1 meter close to it" + "When you are 1 meter close to the object, play the happy sound" + "" + ), + }, + # { + # "role": "user", + # "content": "Find the yellow vaccum cleaner !", + # }, + { + "role": "user", + "content": "Find the waste bin and turn around it. Play the happy sound when you are done", + }, +] + + +# Mapping function names to actual methods +function_map = { + "move_forward": tools_instance.move_forward, + "move_backward": tools_instance.move_backward, + "turn_left": tools_instance.turn_left, + "turn_right": tools_instance.turn_right, + "take_picture": tools_instance.take_picture, + "play_happy_sound": tools_instance.play_happy_sound, +} + + +client = OpenAI() + + +def call_function(name, args): + if name == "move_forward": + return function_map[name](args["seconds"]) + elif name == "move_backward": + return function_map[name](args["seconds"]) + elif name == "turn_left": + return function_map[name](args["seconds"]) + elif name == "turn_right": + return function_map[name](args["seconds"]) + elif name == "take_picture": + return function_map[name]() + elif name == "play_happy_sound": + return function_map[name]() + else: + raise ValueError(f"Unknown function name: {name}") + + +while True: + response = client.responses.create( + model="gpt-4o-mini", + input=messages, + tools=openai_tools, + ) + + if len(response.output) == 1 and response.output[0].type == "function_call": + print("Only function call, no text response") + else: + try: + print(response.output[0].content[0].text) + except: + print("Error occurred while processing response") + for tool_call in response.output: + if tool_call.type != "function_call": + continue + + name = tool_call.name + args = json.loads(tool_call.arguments) + + result = call_function(name, args)[0] + messages.append(tool_call) + if tool_call.name == "take_picture": + # result is an image URL + + # Add an optional prompt or let GPT interpret the image + messages.append( + { + "role": "user", + "content": [{"type": "input_image", "image_url": result}], + } + ) + + messages.append( + { + "type": "function_call_output", + "call_id": tool_call.call_id, + "output": "Image taken and provided above.", + } + ) + else: + + messages.append( + { + "type": "function_call_output", + "call_id": tool_call.call_id, + "output": str(result), + } + ) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/find_soft_offsets.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/find_soft_offsets.py new file mode 100644 index 0000000..a3e7b66 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/find_soft_offsets.py @@ -0,0 +1,84 @@ +""" +Find the offsets to set in self.joints_offsets in hwi_feetech_pwm_control.py +""" + +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time + +dummy_config = DuckConfig(config_json_path=None, ignore_default=True) + +print("======") +print( + "Warning : this script will move the robot to its zero position quiclky, make sure it is safe to do so" +) +print("======") +print("") +input( + "Press any key to start. The robot will move to its zero position. Make sure it is safe to do so. At any time, press ctrl+c to stop, the motors will be turned off." +) + +hwi = HWI(dummy_config) + + +hwi.init_pos = hwi.zero_pos +hwi.set_kds([0] * len(hwi.joints)) +hwi.turn_on() +print("") +print("") +print("") +print("") +hwi.set_position_all(hwi.zero_pos) +time.sleep(1) +try: + for i, joint_name in enumerate(hwi.joints.keys()): + joint_id = hwi.joints[joint_name] + ok = False + while not ok: + res = input(f" === Setting up {joint_name} === (Y/(s)kip : ").lower() + if res == "s": + break + hwi.set_position_all(hwi.zero_pos) + time.sleep(0.5) + current_pos = hwi.get_present_positions()[i] + if current_pos is None: + continue + # hwi.control.kps[i] = 0 + hwi.io.disable_torque([joint_id]) + input( + f"{joint_name} is now turned off. Move it to the desired zero position and press any key to confirm the offset" + ) + new_pos = hwi.get_present_positions()[i] + offset = new_pos - current_pos + print(f" ---> Offset is {offset}") + hwi.joints_offsets[joint_name] = offset + input( + "Press any key to move the motor to its zero position with offset taken into account" + ) + hwi.set_position_all(hwi.zero_pos) + time.sleep(0.5) + hwi.io.enable_torque([joint_id]) + # hwi.control.kps[i] = 32 + res = input("Is that ok ? (Y/n)").lower() + if res == "y" or res == "": + print("Ok, setting offset") + hwi.joints_offsets[joint_name] = offset + ok = True + print("------") + print("Current offsets : ") + for k, v in hwi.joints_offsets.items(): + print(f"{k} : {v}") + print("------") + print("") + else: + print("Ok, let's try again") + hwi.joints_offsets[joint_name] = 0 + + print("===") + + print("Done ! ") + print("Now you can copy the offsets in your duck_config.json") + + +except KeyboardInterrupt: + hwi.turn_off() diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/head_puppet.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/head_puppet.py new file mode 100644 index 0000000..4df65b0 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/head_puppet.py @@ -0,0 +1,103 @@ +""" +Sets up the robot in init position, you control the head with the xbox controller +""" + +import time +import numpy as np +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +from mini_bdx_runtime.xbox_controller import XBoxController + + +from mini_bdx_runtime.eyes import Eyes +from mini_bdx_runtime.sounds import Sounds +from mini_bdx_runtime.antennas import Antennas +from mini_bdx_runtime.projector import Projector + +duck_config = DuckConfig() + +xbox_controller = XBoxController(50, only_head_control=True) + +if duck_config.speaker: + sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/") +if duck_config.antennas: + antennas = Antennas() +if duck_config.eyes: + eyes = Eyes() +if duck_config.projector: + projector = Projector() + +hwi = HWI(duck_config) + +kps = [8] * 14 +kds = [0] * 14 + +hwi.set_kps(kps) +hwi.set_kds(kds) +hwi.turn_on() + +limits = { + "neck_pitch": [-20, 60], + "head_pitch": [-60, 45], + "head_yaw": [-60, 60], + "head_roll": [-20, 20], +} + +try: + while True: + + last_commands, buttons, left_trigger, right_trigger = ( + xbox_controller.get_last_command() + ) + + l_x = last_commands[5] + l_y = last_commands[4] + r_x = last_commands[6] + # r_y = last_commands[3] + + head_yaw_deg = ( + l_x * (limits["head_yaw"][1] - limits["head_yaw"][0]) / 2 + + (limits["head_yaw"][1] + limits["head_yaw"][0]) / 2 + ) + head_yaw_pos_rad = np.deg2rad(head_yaw_deg) + + head_roll_deg = ( + r_x * (limits["head_roll"][1] - limits["head_roll"][0]) / 2 + + (limits["head_roll"][1] + limits["head_roll"][0]) / 2 + ) + head_roll_pos_rad = np.deg2rad(head_roll_deg) + + head_pitch_deg = ( + l_y * (limits["head_pitch"][1] - limits["head_pitch"][0]) / 2 + + (limits["head_pitch"][1] + limits["head_pitch"][0]) / 2 + ) + head_pitch_pos_rad = np.deg2rad(head_pitch_deg) + + # neck_pitch_deg = ( + # -r_y * (limits["neck_pitch"][1] - limits["neck_pitch"][0]) / 2 + # + (limits["neck_pitch"][1] + limits["neck_pitch"][0]) / 2 + # ) + # neck_pitch_pos_rad = np.deg2rad(neck_pitch_deg) + + hwi.set_position("head_yaw", head_yaw_pos_rad) + hwi.set_position("head_roll", head_roll_pos_rad) + hwi.set_position("head_pitch", head_pitch_pos_rad) + # hwi.set_position("neck_pitch", neck_pitch_pos_rad) + + if duck_config.antennas: + antennas.set_position_left(right_trigger) + antennas.set_position_right(left_trigger) + + if buttons.B.triggered: + if duck_config.speaker: + sounds.play_random_sound() + + if buttons.X.triggered: + if duck_config.projector: + projector.switch() + + # pygame.event.pump() # process event queue + time.sleep(1 / 60) +except KeyboardInterrupt: + if duck_config.antennas: + antennas.stop() diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/imu_client.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/imu_client.py new file mode 100644 index 0000000..6cc367b --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/imu_client.py @@ -0,0 +1,76 @@ +import socket +import time +import numpy as np +import pickle +from queue import Queue +from threading import Thread +from scipy.spatial.transform import Rotation as R +from FramesViewer.viewer import Viewer +import argparse + + +class IMUClient: + def __init__(self, host, port=1234, freq=30): + self.host = host + self.port = port + self.freq = freq + self.client_socket = socket.socket() + self.connected = False + while not self.connected: + try: + self.client_socket.connect((self.host, self.port)) + self.connected = True + except Exception as e: + print(e) + time.sleep(0.5) + self.imu_queue = Queue(maxsize=1) + self.last_imu = [0, 0, 0, 0] + + Thread(target=self.imu_worker, daemon=True).start() + + def imu_worker(self): + while True: + try: + data = self.client_socket.recv(1024) # receive response + data = pickle.loads(data) + + self.imu_queue.put(data) + except: + print("missed imu") + + time.sleep(1 / self.freq) + + def get_imu(self): + try: + self.last_imu = self.imu_queue.get(False) + except: + pass + + return self.last_imu + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser() + parser.add_argument("--ip", type=str, required=True, help="IP address of the robot") + args = parser.parse_args() + + client = IMUClient(args.ip) + + fv = Viewer() + fv.start() + pose = np.eye(4) + pose[:3, 3] = [0.1, 0.1, 0.1] + try: + while True: + quat = client.get_imu() + try: + rot_mat = R.from_quat(quat).as_matrix() + pose[:3, :3] = rot_mat + fv.pushFrame(pose, "pose") + except Exception as e: + print("error", e) + pass + time.sleep(1 / 30) + except KeyboardInterrupt: + pass diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/imu_server.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/imu_server.py new file mode 100644 index 0000000..e537eec --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/imu_server.py @@ -0,0 +1,62 @@ +import socket +import time +import pickle +from mini_bdx_runtime.imu import Imu +from threading import Thread +import time + +import argparse + + +class IMUServer: + def __init__(self, imu=None): + self.host = "0.0.0.0" + self.port = 1234 + + self.server_socket = socket.socket() + self.server_socket.setsockopt( + socket.SOL_SOCKET, socket.SO_REUSEADDR, 1 + ) # enable address reuse + + self.server_socket.bind((self.host, self.port)) + + if imu is None: + self.imu = Imu(50, user_pitch_bias=args.pitch_bias, upside_down=False) + else: + self.imu = imu + self.stop = False + + Thread(target=self.run, daemon=True).start() + + def run(self): + while not self.stop: + self.server_socket.listen(1) + conn, address = self.server_socket.accept() # accept new connection + print("Connection from: " + str(address)) + try: + while True: + data = self.imu.get_data() + data = pickle.dumps(data) + conn.send(data) # send data to the client + time.sleep(1 / 30) + except: + pass + + self.server_socket.close() + print("thread closed") + time.sleep(1) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--pitch_bias", type=float, default=0, help="deg") + args = parser.parse_args() + imu_server = IMUServer() + try: + while True: + time.sleep(0.01) + except KeyboardInterrupt: + print("Closing server") + imu_server.stop = True + + time.sleep(2) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/new_record_data.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/new_record_data.py new file mode 100644 index 0000000..548839d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/new_record_data.py @@ -0,0 +1,104 @@ +from pypot.feetech import FeetechSTS3215IO +import pickle +import numpy as np +import time +import argparse + +parser = argparse.ArgumentParser(description="Record data from Feetech motor") +parser.add_argument( + "--new_firmware", + action="store_true", + help="Use new firmware for the motor", + default=False, +) +args = parser.parse_args() +io = FeetechSTS3215IO("/dev/ttyACM0") + + +def convert_load(raw_load): + sign = -1 + if raw_load > 1023: + raw_load -= 1024 + sign = 1 + return sign * raw_load * 0.001 + + +id = 1 if args.new_firmware else 11 +kp = 32 +kd = 0 +acceleration = 0 +maximum_acceleration = 0 +maximum_velocity = 0 +goal_speed = 0 + +time.sleep(1) +io.set_maximum_acceleration({id: maximum_acceleration}) +io.set_acceleration({id: acceleration}) +io.set_maximum_velocity({id: maximum_velocity}) +io.set_goal_speed({id: goal_speed}) +io.set_P_coefficient({id: kp}) +io.set_D_coefficient({id: kd}) +time.sleep(1) + +goal_position = 90 + +io.set_goal_position({id: 0}) +time.sleep(3) + +exit() + +times = [] +positions = [] +goal_positions = [] +speeds = [] +loads = [] +currents = [] + +io.set_goal_position({id: goal_position}) + +s = time.time() +set = False +while True: + t = time.time() - s + # goal_position = np.rad2deg(np.sin(t**2)) + io.set_goal_position({id: goal_position}) + present_position = np.deg2rad(io.get_present_position([id])[0]) + present_speed = np.deg2rad(io.get_present_speed([id])[0]) + present_load = convert_load(io.get_present_load([id])[0]) + present_current = io.get_present_current([id])[0] + + times.append(t) + positions.append(present_position) + goal_positions.append(np.deg2rad(goal_position)) + speeds.append(present_speed) + loads.append(present_load) + currents.append(present_current) + + if t > 3: + break + + time.sleep(0.01) + +fw_version = "new" if args.new_firmware else "old" +data = { + "maximum_acceleration": maximum_acceleration, + "acceleration": acceleration, + "maximum_velocity": maximum_velocity, + "kp": kp, + "kd": kd, + "times": times, + "positions": positions, + "goal_positions": goal_positions, + "speeds": speeds, + "loads": loads, + "currents": currents, + "fw_version": fw_version, +} + +pickle.dump( + data, + open( + f"data_KP_{kp}_{fw_version}.pkl", + "wb", + ), +) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/plot_recorded_data.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/plot_recorded_data.py new file mode 100644 index 0000000..82f7d0d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/plot_recorded_data.py @@ -0,0 +1,35 @@ +import pickle +import argparse +import matplotlib.pyplot as plt + +parser = argparse.ArgumentParser() +parser.add_argument("-f", "--file", type=str, required=True) +args = parser.parse_args() + +data = pickle.load(open(args.file, "rb")) + + +plt.figure() + +plt.subplot(4, 1, 1) +plt.plot(data["times"], data["positions"], label="positions (rad)") +plt.plot(data["times"], data["goal_positions"], label="goal_positions (rad)") +plt.legend() + +plt.subplot(4, 1, 2) +plt.plot(data["times"], data["speeds"], label="speed (rad/s)") +plt.legend() + +plt.subplot(4, 1, 3) +plt.plot(data["times"], data["loads"], label="load") +plt.legend() + +plt.subplot(4, 1, 4) +plt.plot(data["times"], data["currents"], label="current (mA)") +plt.legend() + +# plt.suptitle(f"Acceleration: {data['acceleration']}, KP: {data['kp']}, KD: {data['kd']}") +plt.suptitle(f"KP: {data['kp']}, firmware: {data['fw_version']}") +plt.tight_layout() + +plt.show() diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/polynomial_coefficients.pkl b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/polynomial_coefficients.pkl new file mode 100644 index 0000000..ebfef40 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/polynomial_coefficients.pkl differ diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/record_data.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/record_data.py new file mode 100644 index 0000000..76b6a85 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/record_data.py @@ -0,0 +1,100 @@ +from pypot.feetech import FeetechSTS3215IO +import pickle +import numpy as np +import time + +io = FeetechSTS3215IO("/dev/ttyACM0") + +# accelerations = [0, 10, 50, 100, 200, 255] +accelerations = [0] +switch=False +# kps = [4, 8, 16, 32] +# kds = [0, 4, 8, 16, 32] + +# accelerations = [0] +kps = [32] +kds = [0] + +for acceleration in accelerations: + for kp in kps: + for kd in kds: + print(f"acceleration: {acceleration}, kp: {kp}, kd: {kd}") + + # acceleration = 50 + # kp = 32 + # kd = 0 + + # io.set_mode({1: 0}) + # io.set_lock({1: 1}) + # time.sleep(1) + # io.set_maximum_acceleration({1: acceleration}) + # io.set_acceleration({1: acceleration}) + # time.sleep(1) + io.set_P_coefficient({1: kp}) + io.set_D_coefficient({1: kd}) + + # time.sleep(1) + goal_position = 90 + + io.set_goal_position({1: 0}) + time.sleep(3) + + times = [] + positions = [] + goal_positions = [] + speeds = [] + loads = [] + currents = [] + + def convert_load(raw_load): + sign = -1 + if raw_load > 1023: + raw_load -= 1024 + sign = 1 + return sign * raw_load * 0.001 + + io.set_goal_position({1: goal_position}) + s = time.time() + set = False + while True: + t = time.time() - s + # goal_position = np.rad2deg(np.sin(t**2)) + io.set_goal_position({1: goal_position}) + present_position = np.deg2rad(io.get_present_position([1])[0]) + present_speed = np.deg2rad(io.get_present_speed([1])[0]) + present_load = convert_load(io.get_present_load([1])[0]) + present_current = io.get_present_current([1])[0] + + times.append(t) + positions.append(present_position) + goal_positions.append(np.deg2rad(goal_position)) + speeds.append(present_speed) + loads.append(present_load) + currents.append(present_current) + + # if switch and t > 0.2 and not set: + # goal_position = -goal_position + # io.set_goal_position({1: goal_position}) + # set = True + + if t > 3: + break + + time.sleep(0.01) + + data = { + "acceleration": acceleration, + "kp": kp, + "kd": kd, + "times": times, + "positions": positions, + "goal_positions": goal_positions, + "speeds": speeds, + "loads": loads, + "currents": currents, + } + + pickle.dump( + data, + open(f"data_{'switch_' if switch else ''}acceleration_{acceleration}_kp_{kp}_kd_{kd}.pkl", "wb"), + ) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/turn_off.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/turn_off.py new file mode 100644 index 0000000..20d215c --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/turn_off.py @@ -0,0 +1,9 @@ +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time + +duck_config = DuckConfig() + +hwi = HWI(duck_config) +hwi.turn_off() +time.sleep(1) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/turn_on.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/turn_on.py new file mode 100644 index 0000000..33edad3 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/turn_on.py @@ -0,0 +1,9 @@ +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time + +duck_config = DuckConfig() + +hwi = HWI(duck_config) +hwi.turn_on() +time.sleep(1) diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/v2_rl_walk_mujoco.py b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/v2_rl_walk_mujoco.py new file mode 100644 index 0000000..530ebb9 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/scripts/v2_rl_walk_mujoco.py @@ -0,0 +1,400 @@ +import time +import pickle + +import numpy as np +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.onnx_infer import OnnxInfer + +from mini_bdx_runtime.raw_imu import Imu +from mini_bdx_runtime.poly_reference_motion import PolyReferenceMotion +from mini_bdx_runtime.xbox_controller import XBoxController +from mini_bdx_runtime.feet_contacts import FeetContacts +from mini_bdx_runtime.eyes import Eyes +from mini_bdx_runtime.sounds import Sounds +from mini_bdx_runtime.antennas import Antennas +from mini_bdx_runtime.projector import Projector +from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter +from mini_bdx_runtime.duck_config import DuckConfig + +import os + +HOME_DIR = os.path.expanduser("~") + + +class RLWalk: + def __init__( + self, + onnx_model_path: str, + duck_config_path: str = f"{HOME_DIR}/duck_config.json", + serial_port: str = "/dev/ttyACM0", + control_freq: float = 50, + pid=[30, 0, 0], + action_scale=0.25, + commands=False, + pitch_bias=0, + save_obs=False, + replay_obs=None, + cutoff_frequency=None, + ): + + self.duck_config = DuckConfig(config_json_path=duck_config_path) + + self.commands = commands + self.pitch_bias = pitch_bias + + self.onnx_model_path = onnx_model_path + self.policy = OnnxInfer(self.onnx_model_path, awd=True) + + self.num_dofs = 14 + self.max_motor_velocity = 5.24 # rad/s + + # Control + self.control_freq = control_freq + self.pid = pid + + self.save_obs = save_obs + if self.save_obs: + self.saved_obs = [] + + self.replay_obs = replay_obs + if self.replay_obs is not None: + self.replay_obs = pickle.load(open(self.replay_obs, "rb")) + + self.action_filter = None + if cutoff_frequency is not None: + self.action_filter = LowPassActionFilter( + self.control_freq, cutoff_frequency + ) + + self.hwi = HWI(self.duck_config, serial_port) + + self.start() + + self.imu = Imu( + sampling_freq=int(self.control_freq), + user_pitch_bias=self.pitch_bias, + upside_down=self.duck_config.imu_upside_down, + ) + + self.feet_contacts = FeetContacts() + + # Scales + self.action_scale = action_scale + + self.last_action = np.zeros(self.num_dofs) + self.last_last_action = np.zeros(self.num_dofs) + self.last_last_last_action = np.zeros(self.num_dofs) + + self.init_pos = list(self.hwi.init_pos.values()) + + self.motor_targets = np.array(self.init_pos.copy()) + self.prev_motor_targets = np.array(self.init_pos.copy()) + + self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + self.paused = self.duck_config.start_paused + + self.command_freq = 20 # hz + if self.commands: + self.xbox_controller = XBoxController(self.command_freq) + + # Reference motion, but we only really need the length of one phase + # TODO + self.PRM = PolyReferenceMotion("./polynomial_coefficients.pkl") + self.imitation_i = 0 + self.imitation_phase = np.array([0, 0]) + self.phase_frequency_factor = 1.0 + self.phase_frequency_factor_offset = ( + self.duck_config.phase_frequency_factor_offset + ) + + # Optional expression features + if self.duck_config.eyes: + self.eyes = Eyes() + if self.duck_config.projector: + self.projector = Projector() + if self.duck_config.speaker: + self.sounds = Sounds( + volume=1.0, sound_directory="../mini_bdx_runtime/assets/" + ) + if self.duck_config.antennas: + self.antennas = Antennas() + + def get_obs(self): + + imu_data = self.imu.get_data() + + dof_pos = self.hwi.get_present_positions( + ignore=[ + "left_antenna", + "right_antenna", + ] + ) # rad + + dof_vel = self.hwi.get_present_velocities( + ignore=[ + "left_antenna", + "right_antenna", + ] + ) # rad/s + + if dof_pos is None or dof_vel is None: + return None + + if len(dof_pos) != self.num_dofs: + print(f"ERROR len(dof_pos) != {self.num_dofs}") + return None + + if len(dof_vel) != self.num_dofs: + print(f"ERROR len(dof_vel) != {self.num_dofs}") + return None + + cmds = self.last_commands + + feet_contacts = self.feet_contacts.get() + + obs = np.concatenate( + [ + imu_data["gyro"], + imu_data["accelero"], + cmds, + dof_pos - self.init_pos, + dof_vel * 0.05, + self.last_action, + self.last_last_action, + self.last_last_last_action, + self.motor_targets, + feet_contacts, + self.imitation_phase, + ] + ) + + return obs + + def start(self): + kps = [self.pid[0]] * 14 + kds = [self.pid[2]] * 14 + + # lower head kps + kps[5:9] = [8, 8, 8, 8] + + self.hwi.set_kps(kps) + self.hwi.set_kds(kds) + self.hwi.turn_on() + + time.sleep(2) + + def get_phase_frequency_factor(self, x_velocity): + + max_phase_frequency = 1.2 + min_phase_frequency = 1.0 + + # Perform linear interpolation + freq = min_phase_frequency + (abs(x_velocity) / 0.15) * ( + max_phase_frequency - min_phase_frequency + ) + + return freq + + def run(self): + i = 0 + try: + print("Starting") + start_t = time.time() + while True: + left_trigger = 0 + right_trigger = 0 + t = time.time() + + if self.commands: + self.last_commands, self.buttons, left_trigger, right_trigger = ( + self.xbox_controller.get_last_command() + ) + if self.buttons.dpad_up.triggered: + self.phase_frequency_factor_offset += 0.05 + print( + f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}" + ) + + if self.buttons.dpad_down.triggered: + self.phase_frequency_factor_offset -= 0.05 + print( + f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}" + ) + + if self.buttons.LB.is_pressed: + self.phase_frequency_factor = 1.3 + else: + self.phase_frequency_factor = 1.0 + + if self.buttons.X.triggered: + if self.duck_config.projector: + self.projector.switch() + + if self.buttons.B.triggered: + if self.duck_config.speaker: + self.sounds.play_random_sound() + + if self.duck_config.antennas: + self.antennas.set_position_left(right_trigger) + self.antennas.set_position_right(left_trigger) + + if self.buttons.A.triggered: + self.paused = not self.paused + if self.paused: + print("PAUSE") + else: + print("UNPAUSE") + + if self.paused: + time.sleep(0.1) + continue + + obs = self.get_obs() + if obs is None: + continue + + self.imitation_i += 1 * ( + self.phase_frequency_factor + self.phase_frequency_factor_offset + ) + self.imitation_i = self.imitation_i % self.PRM.nb_steps_in_period + self.imitation_phase = np.array( + [ + np.cos( + self.imitation_i / self.PRM.nb_steps_in_period * 2 * np.pi + ), + np.sin( + self.imitation_i / self.PRM.nb_steps_in_period * 2 * np.pi + ), + ] + ) + + if self.save_obs: + self.saved_obs.append(obs) + + if self.replay_obs is not None: + if i < len(self.replay_obs): + obs = self.replay_obs[i] + else: + print("BREAKING ") + break + + action = self.policy.infer(obs) + + self.last_last_last_action = self.last_last_action.copy() + self.last_last_action = self.last_action.copy() + self.last_action = action.copy() + + # action = np.zeros(10) + + self.motor_targets = self.init_pos + action * self.action_scale + + # self.motor_targets = np.clip( + # self.motor_targets, + # self.prev_motor_targets + # - self.max_motor_velocity * (1 / self.control_freq), # control dt + # self.prev_motor_targets + # + self.max_motor_velocity * (1 / self.control_freq), # control dt + # ) + + if self.action_filter is not None: + self.action_filter.push(self.motor_targets) + filtered_motor_targets = self.action_filter.get_filtered_action() + if ( + time.time() - start_t > 1 + ): # give time to the filter to stabilize + self.motor_targets = filtered_motor_targets + + self.prev_motor_targets = self.motor_targets.copy() + + head_motor_targets = self.last_commands[3:] + self.motor_targets[5:9] + self.motor_targets[5:9] = head_motor_targets + + action_dict = make_action_dict( + self.motor_targets, list(self.hwi.joints.keys()) + ) + + self.hwi.set_position_all(action_dict) + + i += 1 + + took = time.time() - t + # print("Full loop took", took, "fps : ", np.around(1 / took, 2)) + if (1 / self.control_freq - took) < 0: + print( + "Policy control budget exceeded by", + np.around(took - 1 / self.control_freq, 3), + ) + time.sleep(max(0, 1 / self.control_freq - took)) + + except KeyboardInterrupt: + if self.duck_config.antennas: + self.antennas.stop() + if self.duck_config.eyes: + self.eyes.stop() + if self.duck_config.projector: + self.projector.stop() + self.feet_contacts.stop() + + if self.save_obs: + pickle.dump(self.saved_obs, open("robot_saved_obs.pkl", "wb")) + print("TURNING OFF") + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("--onnx_model_path", type=str, required=True) + parser.add_argument( + "--duck_config_path", + type=str, + required=False, + default=f"{HOME_DIR}/duck_config.json", + ) + parser.add_argument("-a", "--action_scale", type=float, default=0.25) + parser.add_argument("-p", type=int, default=30) + parser.add_argument("-i", type=int, default=0) + parser.add_argument("-d", type=int, default=0) + parser.add_argument("-c", "--control_freq", type=int, default=50) + parser.add_argument("--pitch_bias", type=float, default=0, help="deg") + parser.add_argument( + "--commands", + action="store_true", + default=True, + help="external commands, keyboard or gamepad. Launch control_server.py on host computer", + ) + parser.add_argument( + "--save_obs", + type=str, + required=False, + default=False, + help="save the run's observations", + ) + parser.add_argument( + "--replay_obs", + type=str, + required=False, + default=None, + help="replay the observations from a previous run (can be from the robot or from mujoco)", + ) + parser.add_argument("--cutoff_frequency", type=float, default=None) + + args = parser.parse_args() + pid = [args.p, args.i, args.d] + + print("Done parsing args") + rl_walk = RLWalk( + args.onnx_model_path, + duck_config_path=args.duck_config_path, + action_scale=args.action_scale, + pid=pid, + control_freq=args.control_freq, + commands=args.commands, + pitch_bias=args.pitch_bias, + save_obs=args.save_obs, + replay_obs=args.replay_obs, + cutoff_frequency=args.cutoff_frequency, + ) + print("Done instantiating RLWalk") + rl_walk.run() diff --git a/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/setup.cfg b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/setup.cfg new file mode 100644 index 0000000..0eb07fa --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/Open_Duck_Mini_Runtime-2/setup.cfg @@ -0,0 +1,38 @@ +[metadata] +name = mini-bdx-runtime +version = 0.0.1 +author = Antoine Pirrone +author_email = antoine.pirrone@gmail.com +url = https://github.com/apirrone/mini_BDX_runtime +description = Runtime code for mini BDX +long_description = file: README.md +long_description_content_type = text/markdown + + +[options] +packages = find: +zip_safe = True +include_package_data = True +package_dir= + =mini_bdx_runtime +install_requires = + rustypot==0.1.0 + onnxruntime==1.18.1 + numpy==1.26.4 + adafruit-circuitpython-bno055==5.4.13 + scipy==1.15.1 + pygame==2.6.0 + pypot @ git+https://github.com/pollen-robotics/pypot@support-feetech-sts3215 + openai==1.70.0 + + # adafruit_extended_bus + +[options.packages.find] +where=mini_bdx_runtime + +[options.package_data] + +[options.extras_require] + +[options.entry_points] +console_scripts = diff --git a/Open_Duck_Mini_Runtime-2/README.md b/Open_Duck_Mini_Runtime-2/README.md new file mode 100644 index 0000000..a5025ef --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/README.md @@ -0,0 +1,182 @@ +# Open Duck Mini Runtime + +## Raspberry Pi zero 2W setup + +### Install Raspberry Pi OS + +Download Raspberry Pi OS Lite (64-bit) from here : https://www.raspberrypi.com/software/operating-systems/ + +Follow the instructions here to install the OS on the SD card : https://www.raspberrypi.com/documentation/computers/getting-started.html + +With the Raspberry Pi Imager, you can pre-configure session, wifi and ssh. Do it like below : + +![imager_setup](https://github.com/user-attachments/assets/7a4987b2-de83-41dd-ab7f-585259685f16) + +> Tip: I configure the rasp to connect to my phone's hotspot, this way I can connect to it from anywhere. + +### Setup SSH (If not setup during the installation) + +When first booting on the rasp, you will need to connect a screen and a keyboard. The first thing you should do is connect to a wifi network and enable SSH. + +To do so, you can follow this guide : https://www.raspberrypi.com/documentation/computers/configuration.html#setting-up-wifi + +Then, you can connect to your rasp using SSH without having to plug a screen and a keyboard. + +### Update the system and install necessary stuff + +```bash +sudo apt update +sudo apt upgrade +sudo apt install git +sudo apt install python3-pip +sudo apt install python3-virtualenvwrapper +(optional) sudo apt install python3-picamzero + +``` + +Add this to the end of the `.bashrc`: + +```bash +export WORKON_HOME=$HOME/.virtualenvs +export PROJECT_HOME=$HOME/Devel +source /usr/share/virtualenvwrapper/virtualenvwrapper.sh +``` + +### Enable I2C + +`sudo raspi-config` -> `Interface Options` -> `I2C` + +TODO set 400KHz ? + +### Set the usbserial latency timer + +```bash +cd /etc/udev/rules.d/ +sudo touch 99-usb-serial.rules +sudo nano 99-usb-serial.rules +# copy the following line in the file +SUBSYSTEM=="usb-serial", DRIVER=="ftdi_sio", ATTR{latency_timer}="1" +``` + +### Set the udev rules for the motor control board + +TODO + + +### Setup xbox one controller over bluetooth + +Turn your xbox one controller on and set it in pairing mode by long pressing the sync button on the top of the controller. + +Run the following commands on the rasp : + +```bash +bluetoothctl +scan on +``` + +Wait for the controller to appear in the list, then run : + +```bash +pair +trust +connect +``` + +The led on the controller should stop blinking and stay on. + +You can test that it's working by running + +```bash +python3 mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py +``` + +## Speaker wiring and configuration +Follow this tutorial + +> For now, don't activate `/dev/zero` when they ask + +https://learn.adafruit.com/adafruit-max98357-i2s-class-d-mono-amp?view=all + + +## Install the runtime + +### Make a virtual environment and activate it + +```bash +mkvirtualenv -p python3 open-duck-mini-runtime +workon open-duck-mini-runtime +``` + +Clone this repository on your rasp, cd into the repo, then : + +```bash +git clone https://github.com/apirrone/Open_Duck_Mini_Runtime +cd Open_Duck_Mini_Runtime +git checkout v2 +pip install -e . +``` + +In Raspberry Pi 5, you need to perform the following operations + +```bash +pip uninstall -y RPi.GPIO +pip install lgpio +``` + + +## Test the IMU + +```bash +python3 mini_bdx_runtime/mini_bdx_runtime/raw_imu.py +``` + +You can also run `python3 scripts/imu_server.py` on the robot and `python3 scripts/imu_client.py --ip ` on your computer to check that the frame is oriented correctly. + +> To find the ip address of the robot, run `ifconfig` on the robot + +## Test motors + +This will allow you to verify all your motors are connected and configured. + +```bash +python3 scripts/check_motors.py +``` + +## Make your duck_config.json + +Copy `example_config.json` in the home directory of your duck and rename it `duck_config.json`. + +`cp example_config.json ~/duck_config.json` + +In this file, you can configure some stuff, like registering if you installed the expression features, installed the imu upside down or and other stuff. You also write the joints offsets of your duck here + +## Find the joints offsets + +This script will guide you through finding the joints offsets of your robot that you can then write in your `duck_config.json` + +> This procedure won't be necessary in the future as we will be flashing the offsets directly in each motor's eeprom. + +```bash +cd scripts/ +python find_soft_offsets.py +``` + +## Run the walk ! + +Download the [latest policy checkpoint ](https://github.com/apirrone/Open_Duck_Mini/blob/v2/BEST_WALK_ONNX_2.onnx) and copy it to your duck. + +`cd scripts/` + +`python v2_rl_walk_mujoco.py --onnx_model_path /BEST_WALK_ONNX_2.onnx` + + + +``` +- The commands are : +- A to pause/unpause +- X to turn on/off the projector +- B to play a random sound +- Y to turn on/off head control (very experimental, I don't recommend trying that, it can break your duck's head) +- left and right triggers to control the left and right antennas +- LB (new!) press and hold to increase the walking frequency, kind of a sprint mode 🙂 +``` \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/TODO.md b/Open_Duck_Mini_Runtime-2/TODO.md new file mode 100644 index 0000000..d7aae06 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/TODO.md @@ -0,0 +1,3 @@ +- [] Better handle xbox controller + - It's a little bit of a mess right now, how we handle directions and buttons etc +- [] Make the offsets flashing work. This will be in the motor configuration script \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/checklist.md b/Open_Duck_Mini_Runtime-2/checklist.md new file mode 100644 index 0000000..d68d340 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/checklist.md @@ -0,0 +1,15 @@ +# Checklist + +To check that all works and is properly configured before running a policy. + +TODO (Antoine) +Make a script that goes through all this automatically +- joints positions and offsets +- imu orientation +- feet switches + +make optional +- eyes/projector +- antennas +- speaker +- camera diff --git a/Open_Duck_Mini_Runtime-2/example_config.json b/Open_Duck_Mini_Runtime-2/example_config.json new file mode 100644 index 0000000..7c7e381 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/example_config.json @@ -0,0 +1,29 @@ +{ + "start_paused": false, + "imu_upside_down": false, + "phase_frequency_factor_offset": 0.0, + "expression_features": { + "eyes": false, + "projector": false, + "antennas": false, + "speaker": false, + "microphone": false, + "camera": false + }, + "joints_offsets": { + "left_hip_yaw": 0.0, + "left_hip_roll": 0.0, + "left_hip_pitch": 0.0, + "left_knee": 0.0, + "left_ankle": 0.0, + "neck_pitch": 0.0, + "head_pitch": 0.0, + "head_yaw": 0.0, + "head_roll": 0.00, + "right_hip_yaw": 0.0, + "right_hip_roll": 0.0, + "right_hip_pitch": 0.0, + "right_knee": 0.0, + "right_ankle": 0.0 + } +} \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/__init__.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/__init__.py @@ -0,0 +1 @@ + diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep1.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep1.wav new file mode 100644 index 0000000..f6b7450 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep1.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep2.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep2.wav new file mode 100644 index 0000000..aea7ea3 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/beep2.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy1.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy1.wav new file mode 100644 index 0000000..f7174db Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy1.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy2.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy2.wav new file mode 100644 index 0000000..ff772ab Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy2.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy3.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy3.wav new file mode 100644 index 0000000..7f3124f Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/happy3.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp.wav new file mode 100644 index 0000000..a211b92 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp2.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp2.wav new file mode 100644 index 0000000..70bac5e Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp2.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp3.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp3.wav new file mode 100644 index 0000000..946b446 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/lamp3.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/motor.wav b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/motor.wav new file mode 100644 index 0000000..5337bd0 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/assets/motor.wav differ diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/__init__.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/__init__.py new file mode 100644 index 0000000..0d1570c --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/__init__.py @@ -0,0 +1 @@ +from .onnx_infer import OnnxInfer diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/antennas.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/antennas.py new file mode 100644 index 0000000..9f041c9 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/antennas.py @@ -0,0 +1,64 @@ +import board +import pwmio +import math +import time + +LEFT_ANTENNA_PIN = board.D13 +RIGHT_ANTENNA_PIN = board.D12 +LEFT_SIGN = 1 +RIGHT_SIGN = -1 +MIN_UPDATE_INTERVAL = 1 / 50 # 20ms + + +def value_to_duty_cycle(v): + pulse_width_ms = 1.5 + (v * 0.5) # 1ms to 2ms + duty_cycle = int((pulse_width_ms / 20) * 65535) + return min(max(duty_cycle, 3277), 6553) + + +class Antennas: + def __init__(self): + neutral_duty = value_to_duty_cycle(0) + self.pwm_left = pwmio.PWMOut(LEFT_ANTENNA_PIN, frequency=50, duty_cycle=neutral_duty) + self.pwm_right = pwmio.PWMOut(RIGHT_ANTENNA_PIN, frequency=50, duty_cycle=neutral_duty) + + def set_position_left(self, position): + self.set_position(self.pwm_left, position, LEFT_SIGN) + + def set_position_right(self, position): + self.set_position(self.pwm_right, position, RIGHT_SIGN) + + def set_position(self, pwm, value, sign=1): + # if value == 0: + # return + if -1 <= value <= 1: + duty_cycle = value_to_duty_cycle(value * sign) # Convert value to duty cycle (1ms-2ms) + pwm.duty_cycle = duty_cycle + else: + print("Invalid input! Enter a value between -1 and 1.") + + def stop(self): + time.sleep(MIN_UPDATE_INTERVAL) + self.set_position_left(0) + self.set_position_right(0) + time.sleep(MIN_UPDATE_INTERVAL) + self.pwm_left.deinit() + self.pwm_right.deinit() + + +if __name__ == "__main__": + antennas = Antennas() + + try: + start_time = time.monotonic() + current_time = start_time + + while current_time - start_time < 5: + value = math.sin(2 * math.pi * 1 * current_time) + antennas.set_position_left(value) + antennas.set_position_right(value) + time.sleep(MIN_UPDATE_INTERVAL) + current_time = time.monotonic() + + finally: + antennas.stop() \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/buttons.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/buttons.py new file mode 100644 index 0000000..5f2d828 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/buttons.py @@ -0,0 +1,85 @@ +import time + + +class Button: + def __init__(self): + self.last_pressed_time = time.time() + self.timeout = 0.2 + self.is_pressed = False + self.triggered = False + self.released = True + + def update(self, value): + if self.is_pressed and not value: + self.released = True + self.is_pressed = value + if ( + self.released + and self.is_pressed + and (time.time() - self.last_pressed_time > self.timeout) + ): + self.triggered = True + self.last_pressed_time = time.time() + else: + self.triggered = False + + if self.is_pressed: + self.released = False + + +class Buttons: + def __init__(self): + self.A = Button() + self.B = Button() + self.X = Button() + self.Y = Button() + self.LB = Button() + self.RB = Button() + self.dpad_up = Button() + self.dpad_down = Button() + + def update(self, A, B, X, Y, LB, RB, dpad_up, dpad_down): + self.A.update(A) + self.B.update(B) + self.X.update(X) + self.Y.update(Y) + self.LB.update(LB) + self.RB.update(RB) + self.dpad_up.update(dpad_up) + self.dpad_down.update(dpad_down) + + +if __name__ == "__main__": + from mini_bdx_runtime.xbox_controller import XBoxController + + xbox_controller = XBoxController(30) + buttons = Buttons() + while True: + + ( + _, + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + _, + _, + up_down, + ) = xbox_controller.get_last_command() + + buttons.update( + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + up_down == 1, + up_down == -1, + ) + + print(buttons.dpad_up.triggered, buttons.dpad_up.is_pressed, buttons.dpad_down.triggered, buttons.dpad_down.is_pressed) + + time.sleep(0.05) diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/camera.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/camera.py new file mode 100644 index 0000000..68b59b7 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/camera.py @@ -0,0 +1,30 @@ +from picamzero import Camera +import cv2 +import base64 +import os + +class Cam: + def __init__(self): + self.cam = Camera() + + def get_encoded_image(self): + im = self.cam.capture_array() + im = cv2.resize(im, (512, 512)) + im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB) + im = cv2.rotate(im, cv2.ROTATE_90_CLOCKWISE) + + cv2.imwrite("/home/bdxv2/aze.jpg", im) + + return self.encode_image("/home/bdxv2/aze.jpg") + + + + # def encode_image(self, image): + # return base64.b64encode(image).decode("utf-8") + + def encode_image(self, image_path: str): + # check if the image exists + if not os.path.exists(image_path): + raise FileNotFoundError(f"Image file not found: {image_path}") + with open(image_path, "rb") as image_file: + return base64.b64encode(image_file.read()).decode("utf-8") diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/duck_config.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/duck_config.py new file mode 100644 index 0000000..c570c15 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/duck_config.py @@ -0,0 +1,83 @@ +import json +from typing import Optional +import os + +HOME_DIR = os.path.expanduser("~") + + +class DuckConfig: + + def __init__( + self, + config_json_path: Optional[str] = f"{HOME_DIR}/duck_config.json", + ignore_default: bool = False, + ): + """ + Looks for duck_config.json in the home directory by default. + If not found, uses default values. + """ + self.default = False + try: + self.json_config = ( + json.load(open(config_json_path, "r")) if config_json_path else {} + ) + except FileNotFoundError: + print( + f"Warning : didn't find the config json file at {config_json_path}, using default values" + ) + self.json_config = {} + self.default = True + + if config_json_path is None: + print("Warning : didn't provide a config json path, using default values") + self.default = True + + if self.default and not ignore_default: + print("") + print("") + print("") + print("") + print("======") + print( + "WARNING : Running with default values probably won't work well. Please make a duck_config.json file and set the parameters." + ) + res = input("Do you still want to run ? (y/N)") + if res.lower() != "y": + print("Exiting...") + exit(1) + + self.start_paused = self.json_config.get("start_paused", False) + self.imu_upside_down = self.json_config.get("imu_upside_down", False) + self.phase_frequency_factor_offset = self.json_config.get( + "phase_frequency_factor_offset", 0.0 + ) + + expression_features = self.json_config.get("expression_features", {}) + + self.eyes = expression_features.get("eyes", False) + self.projector = expression_features.get("projector", False) + self.antennas = expression_features.get("antennas", False) + self.speaker = expression_features.get("speaker", False) + self.microphone = expression_features.get("microphone", False) + self.camera = expression_features.get("camera", False) + + # default joints offsets are 0.0 + self.joints_offset = self.json_config.get( + "joints_offsets", + { + "left_hip_yaw": 0.0, + "left_hip_roll": 0.0, + "left_hip_pitch": 0.0, + "left_knee": 0.0, + "left_ankle": 0.0, + "neck_pitch": 0.0, + "head_pitch": 0.0, + "head_yaw": 0.0, + "head_roll": 0.00, + "right_hip_yaw": 0.0, + "right_hip_roll": 0.0, + "right_hip_pitch": 0.0, + "right_knee": 0.0, + "right_ankle": 0.0, + }, + ) diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/eyes.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/eyes.py new file mode 100644 index 0000000..b07446b --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/eyes.py @@ -0,0 +1,58 @@ +import board +import digitalio +import random +import time +from threading import Thread, Event + +LEFT_EYE_PIN = board.D24 +RIGHT_EYE_PIN = board.D23 + + +class Eyes: + def __init__(self, blink_duration=0.1, min_interval=1.0, max_interval=4.0): + self.left_eye = digitalio.DigitalInOut(LEFT_EYE_PIN) + self.left_eye.direction = digitalio.Direction.OUTPUT + + self.right_eye = digitalio.DigitalInOut(RIGHT_EYE_PIN) + self.right_eye.direction = digitalio.Direction.OUTPUT + + self.blink_duration = blink_duration + self.min_interval = min_interval + self.max_interval = max_interval + + self._stop_event = Event() + self._thread = Thread(target=self.run, daemon=True) + self._thread.start() + + def _set_eyes(self, state): + self.left_eye.value = state + self.right_eye.value = state + + def run(self): + try: + while not self._stop_event.is_set(): + self._set_eyes(False) + time.sleep(self.blink_duration) + self._set_eyes(True) + next_blink = random.uniform(self.min_interval, self.max_interval) + time.sleep(next_blink) + except Exception as err: + print(f"Error in eye thread: {err}") + self._stop_event.set() + + def stop(self): + self._stop_event.set() + self._thread.join() + self._set_eyes(False) + self.left_eye.deinit() + self.right_eye.deinit() + + +if __name__ == "__main__": + e = Eyes() + try: + while True: + time.sleep(1) + finally: + e.stop() + diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py new file mode 100644 index 0000000..87c2d2e --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/feet_contacts.py @@ -0,0 +1,34 @@ +import board +import digitalio +import time + +LEFT_FOOT_PIN = board.D22 +RIGHT_FOOT_PIN = board.D27 + +class FeetContacts: + def __init__(self): + self.left_foot = digitalio.DigitalInOut(LEFT_FOOT_PIN) + self.left_foot.direction = digitalio.Direction.INPUT + self.left_foot.pull = digitalio.Pull.UP + + self.right_foot = digitalio.DigitalInOut(RIGHT_FOOT_PIN) + self.right_foot.direction = digitalio.Direction.INPUT + self.right_foot.pull = digitalio.Pull.UP + + def get(self): + left = not self.left_foot.value + right = not self.right_foot.value + return [left, right] + + def stop(self): + self.left_foot.deinit() + self.right_foot.deinit() + +if __name__ == "__main__": + feet_contacts = FeetContacts() + try: + while True: + print(feet_contacts.get()) + time.sleep(0.05) + finally: + feet_contacts.stop() diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/imu.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/imu.py new file mode 100644 index 0000000..bcdcb55 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/imu.py @@ -0,0 +1,161 @@ +import adafruit_bno055 +import board +import busio +import numpy as np +import pickle +import os + +# import serial + +from queue import Queue +from threading import Thread +import time +from scipy.spatial.transform import Rotation as R + + +# TODO filter spikes +class Imu: + def __init__( + self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True + ): + self.sampling_freq = sampling_freq + self.user_pitch_bias = user_pitch_bias + self.nominal_pitch_bias = 0 + self.calibrate = calibrate + + # self.uart = serial.Serial("/dev/ttyS0", baudrate=9600) + # self.imu = adafruit_bno055.BNO055_UART(self.uart) + + i2c = busio.I2C(board.SCL, board.SDA) + self.imu = adafruit_bno055.BNO055_I2C(i2c) + + self.imu.mode = adafruit_bno055.IMUPLUS_MODE + # self.imu.mode = adafruit_bno055.ACCGYRO_MODE + # self.imu.mode = adafruit_bno055.GYRONLY_MODE + # self.imu.mode = adafruit_bno055.NDOF_MODE + # self.imu.mode = adafruit_bno055.NDOF_FMC_OFF_MODE + + if upside_down: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + ) + else: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + ) + + self.pitch_bias = self.nominal_pitch_bias + self.user_pitch_bias + + if self.calibrate: + self.imu.mode = adafruit_bno055.NDOF_MODE + calibrated = self.imu.calibrated + while not calibrated: + print("Calibration status: ", self.imu.calibration_status) + print("Calibrated : ", self.imu.calibrated) + calibrated = self.imu.calibrated + time.sleep(0.1) + print("CALIBRATION DONE") + offsets_accelerometer = self.imu.offsets_accelerometer + offsets_gyroscope = self.imu.offsets_gyroscope + offsets_magnetometer = self.imu.offsets_magnetometer + + imu_calib_data = { + "offsets_accelerometer": offsets_accelerometer, + "offsets_gyroscope": offsets_gyroscope, + "offsets_magnetometer": offsets_magnetometer, + } + for k, v in imu_calib_data.items(): + print(k, v) + + pickle.dump(imu_calib_data, open("imu_calib_data.pkl", "wb")) + + print("Saved", "imu_calib_data.pkl") + exit() + + if os.path.exists("imu_calib_data.pkl"): + imu_calib_data = pickle.load(open("imu_calib_data.pkl", "rb")) + self.imu.mode = adafruit_bno055.CONFIG_MODE + time.sleep(0.1) + self.imu.offsets_accelerometer = imu_calib_data["offsets_accelerometer"] + self.imu.offsets_gyroscope = imu_calib_data["offsets_gyroscope"] + self.imu.offsets_magnetometer = imu_calib_data["offsets_magnetometer"] + self.imu.mode = adafruit_bno055.IMUPLUS_MODE + time.sleep(0.1) + else: + print("imu_calib_data.pkl not found") + print("Imu is running uncalibrated") + + self.last_imu_data = [0, 0, 0, 0] + self.imu_queue = Queue(maxsize=1) + Thread(target=self.imu_worker, daemon=True).start() + + def convert_axes(self, euler): + euler = [np.pi + euler[1], euler[0], euler[2]] + return euler + + def imu_worker(self): + while True: + s = time.time() + try: + # imu returns scalar first + raw_orientation = np.array(self.imu.quaternion).copy() # quat + euler = ( + R.from_quat(raw_orientation, scalar_first=True) + .as_euler("xyz") + .copy() + ) + except Exception as e: + print("[IMU]:", e) + continue + + # Converting to correct axes + # euler = self.convert_axes(euler) + euler[1] -= np.deg2rad(self.pitch_bias) + # euler[2] = 0 # ignoring yaw + + # gives scalar last, which is what isaac wants + final_orientation_quat = R.from_euler("xyz", euler).as_quat() + + self.imu_queue.put(final_orientation_quat.copy()) + took = time.time() - s + time.sleep(max(0, 1 / self.sampling_freq - took)) + + def get_data(self, euler=False, mat=False): + try: + self.last_imu_data = self.imu_queue.get(False) # non blocking + except Exception: + pass + + try: + if not euler and not mat: + return self.last_imu_data + elif euler: + return R.from_quat(self.last_imu_data).as_euler("xyz") + elif mat: + return R.from_quat(self.last_imu_data).as_matrix() + + except Exception as e: + print("[IMU]: ", e) + return None + + +if __name__ == "__main__": + imu = Imu(50, calibrate=True, upside_down=False) + # imu = Imu(50, upside_down=False) + while True: + data = imu.get_data() + # print(data) + print("gyro", np.around(data["gyro"], 3)) + print("accelero", np.around(data["accelero"], 3)) + print("---") + time.sleep(1 / 25) diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/onnx_infer.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/onnx_infer.py new file mode 100644 index 0000000..85f0ddd --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/onnx_infer.py @@ -0,0 +1,43 @@ +import onnxruntime + + +class OnnxInfer: + def __init__(self, onnx_model_path, input_name="obs", awd=False): + self.onnx_model_path = onnx_model_path + self.ort_session = onnxruntime.InferenceSession( + self.onnx_model_path, providers=["CPUExecutionProvider"] + ) + self.input_name = input_name + self.awd = awd + + def infer(self, inputs): + if self.awd: + outputs = self.ort_session.run(None, {self.input_name: [inputs]}) + return outputs[0][0] + else: + outputs = self.ort_session.run( + None, {self.input_name: inputs.astype("float32")} + ) + return outputs[0] + + +if __name__ == "__main__": + import argparse + import numpy as np + import time + + parser = argparse.ArgumentParser() + parser.add_argument("-o", "--onnx_model_path", type=str, required=True) + args = parser.parse_args() + + oi = OnnxInfer(args.onnx_model_path, awd=True) + inputs = np.random.uniform(size=54).astype(np.float32) + inputs = np.arange(47).astype(np.float32) + times = [] + for i in range(1000): + start = time.time() + print(oi.infer(inputs)) + times.append(time.time() - start) + + print("Average time: ", sum(times) / len(times)) + print("Average fps: ", 1 / (sum(times) / len(times))) diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/poly_reference_motion.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/poly_reference_motion.py new file mode 100644 index 0000000..b5d5961 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/poly_reference_motion.py @@ -0,0 +1,118 @@ +import numpy as np +import pickle + +class PolyReferenceMotion: + def __init__(self, polynomial_coefficients: str): + data = pickle.load(open(polynomial_coefficients, "rb")) + self.dx_range = [0, 0] + self.dy_range = [0, 0] + self.dtheta_range = [0, 0] + self.dxs = [] + self.dys = [] + self.dthetas = [] + self.data_array = [] + self.period = None + self.fps = None + self.frame_offsets = None + self.startend_double_support_ratio = None + self.start_offset = None + self.nb_steps_in_period = None + + self.process(data) + + def process(self, data): + print("[Poly ref data] Processing ...") + _data = {} + for name in data.keys(): + split = name.split("_") + dx = float(split[0]) + dy = float(split[1]) + dtheta = float(split[2]) + + if self.period is None: + self.period = data[name]["period"] + self.fps = data[name]["fps"] + self.frame_offsets = data[name]["frame_offsets"] + self.startend_double_support_ratio = data[name][ + "startend_double_support_ratio" + ] + self.start_offset = int(self.startend_double_support_ratio * self.fps) + self.nb_steps_in_period = int(self.period * self.fps) + + if dx not in self.dxs: + self.dxs.append(dx) + + if dy not in self.dys: + self.dys.append(dy) + + if dtheta not in self.dthetas: + self.dthetas.append(dtheta) + + self.dx_range = [min(dx, self.dx_range[0]), max(dx, self.dx_range[1])] + self.dy_range = [min(dy, self.dy_range[0]), max(dy, self.dy_range[1])] + self.dtheta_range = [ + min(dtheta, self.dtheta_range[0]), + max(dtheta, self.dtheta_range[1]), + ] + + if dx not in _data: + _data[dx] = {} + + if dy not in _data[dx]: + _data[dx][dy] = {} + + if dtheta not in _data[dx][dy]: + _data[dx][dy][dtheta] = data[name] + + _coeffs = data[name]["coefficients"] + + coeffs = [] + for k, v in _coeffs.items(): + coeffs.append(v) + _data[dx][dy][dtheta] = coeffs + + self.dxs = sorted(self.dxs) + self.dys = sorted(self.dys) + self.dthetas = sorted(self.dthetas) + + nb_dx = len(self.dxs) + nb_dy = len(self.dys) + nb_dtheta = len(self.dthetas) + + self.data_array = nb_dx * [None] + for x, dx in enumerate(self.dxs): + self.data_array[x] = nb_dy * [None] + for y, dy in enumerate(self.dys): + self.data_array[x][y] = nb_dtheta * [None] + for th, dtheta in enumerate(self.dthetas): + self.data_array[x][y][th] = _data[dx][dy][dtheta] + + self.data_array = self.data_array + + print("[Poly ref data] Done processing") + + def vel_to_index(self, dx, dy, dtheta): + + dx = np.clip(dx, self.dx_range[0], self.dx_range[1]) + dy = np.clip(dy, self.dy_range[0], self.dy_range[1]) + dtheta = np.clip(dtheta, self.dtheta_range[0], self.dtheta_range[1]) + + ix = np.argmin(np.abs(np.array(self.dxs) - dx)) + iy = np.argmin(np.abs(np.array(self.dys) - dy)) + itheta = np.argmin(np.abs(np.array(self.dthetas) - dtheta)) + + return int(ix), int(iy), int(itheta) + + def sample_polynomial(self, t, coeffs): + ret = [] + for c in coeffs: + ret.append(np.polyval(np.flip(c), t)) + + return ret + + def get_reference_motion(self, dx, dy, dtheta, i): + ix, iy, itheta = self.vel_to_index(dx, dy, dtheta) + t = i % self.nb_steps_in_period / self.nb_steps_in_period + t = np.clip(t, 0.0, 1.0) # safeguard + ret = self.sample_polynomial(t, self.data_array[ix][iy][itheta]) + return ret \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/projector.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/projector.py new file mode 100644 index 0000000..6ba4d39 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/projector.py @@ -0,0 +1,31 @@ +import board +import digitalio +import time + +PROJECTOR_GPIO = board.D25 + + +class Projector: + def __init__(self): + self.project = digitalio.DigitalInOut(PROJECTOR_GPIO) + self.project.direction = digitalio.Direction.OUTPUT + self.on = False + + def switch(self): + self.on = not self.on + + self.project.value = self.on + + def stop(self): + self.project.value = False + self.project.deinit() + + +if __name__ == "__main__": + p = Projector() + try: + while True: + p.switch() + time.sleep(1) + finally: + p.stop() diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py new file mode 100644 index 0000000..ce24627 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/raw_imu.py @@ -0,0 +1,167 @@ +import adafruit_bno055 +import board +import busio +import numpy as np +import os +import pickle + +from queue import Queue +from threading import Thread +import time + + +# TODO filter spikes +class Imu: + def __init__( + self, sampling_freq, user_pitch_bias=0, calibrate=False, upside_down=True + ): + self.sampling_freq = sampling_freq + self.calibrate = calibrate + + i2c = busio.I2C(board.SCL, board.SDA) + self.imu = adafruit_bno055.BNO055_I2C(i2c) + + # self.imu.mode = adafruit_bno055.IMUPLUS_MODE + # self.imu.mode = adafruit_bno055.ACCGYRO_MODE + # self.imu.mode = adafruit_bno055.GYRONLY_MODE + self.imu.mode = adafruit_bno055.NDOF_MODE + # self.imu.mode = adafruit_bno055.NDOF_FMC_OFF_MODE + + if upside_down: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + ) + + else: + self.imu.axis_remap = ( + adafruit_bno055.AXIS_REMAP_Y, + adafruit_bno055.AXIS_REMAP_X, + adafruit_bno055.AXIS_REMAP_Z, + adafruit_bno055.AXIS_REMAP_NEGATIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + adafruit_bno055.AXIS_REMAP_POSITIVE, + ) + + if self.calibrate: + self.imu.mode = adafruit_bno055.NDOF_MODE + calibrated = self.imu.calibrated + while not calibrated: + print("Calibration status: ", self.imu.calibration_status) + print("Calibrated : ", self.imu.calibrated) + calibrated = self.imu.calibrated + time.sleep(0.1) + print("CALIBRATION DONE") + offsets_accelerometer = self.imu.offsets_accelerometer + offsets_gyroscope = self.imu.offsets_gyroscope + offsets_magnetometer = self.imu.offsets_magnetometer + + imu_calib_data = { + "offsets_accelerometer": offsets_accelerometer, + "offsets_gyroscope": offsets_gyroscope, + "offsets_magnetometer": offsets_magnetometer, + } + for k, v in imu_calib_data.items(): + print(k, v) + + pickle.dump(imu_calib_data, open("imu_calib_data.pkl", "wb")) + + print("Saved", "imu_calib_data.pkl") + exit() + + if os.path.exists("imu_calib_data.pkl"): + imu_calib_data = pickle.load(open("imu_calib_data.pkl", "rb")) + self.imu.mode = adafruit_bno055.CONFIG_MODE + time.sleep(0.1) + self.imu.offsets_accelerometer = imu_calib_data["offsets_accelerometer"] + self.imu.offsets_gyroscope = imu_calib_data["offsets_gyroscope"] + self.imu.offsets_magnetometer = imu_calib_data["offsets_magnetometer"] + self.imu.mode = adafruit_bno055.NDOF_MODE + time.sleep(0.1) + else: + print("imu_calib_data.pkl not found") + print("Imu is running uncalibrated") + + self.x_offset = 0 + + # self.tare_x() + + self.last_imu_data = [0, 0, 0, 0] + self.last_imu_data = { + "gyro": [0, 0, 0], + "accelero": [0, 0, 0], + } + self.imu_queue = Queue(maxsize=1) + Thread(target=self.imu_worker, daemon=True).start() + + def tare_x(self): + print("Taring x ...") + x_values = [] + num_values = 100 + ok = False + while not ok: + x_values.append(np.array(self.imu.acceleration)[0]) + + x_values = x_values[-num_values:] + + if len(x_values) == num_values: + mean = np.mean(x_values) + std = np.std(x_values) + if std < 0.05: + ok = True + self.x_offset = mean + print("Tare x done") + else: + print(std) + + time.sleep(0.01) + + def imu_worker(self): + while True: + s = time.time() + try: + gyro = np.array(self.imu.gyro).copy() + accelero = np.array(self.imu.acceleration).copy() + except Exception as e: + print("[IMU]:", e) + continue + + if gyro is None or accelero is None: + continue + + if gyro.any() is None or accelero.any() is None: + continue + + accelero[0] -= self.x_offset + + data = { + "gyro": gyro, + "accelero": accelero, + } + + self.imu_queue.put(data) + took = time.time() - s + time.sleep(max(0, 1 / self.sampling_freq - took)) + + def get_data(self): + try: + self.last_imu_data = self.imu_queue.get(False) # non blocking + except Exception: + pass + + return self.last_imu_data + + +if __name__ == "__main__": + imu = Imu(50, upside_down=False) + while True: + data = imu.get_data() + # print(data) + print("gyro", np.around(data["gyro"], 3)) + print("accelero", np.around(data["accelero"], 3)) + print("---") + time.sleep(1 / 25) diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rl_utils.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rl_utils.py new file mode 100644 index 0000000..2296507 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rl_utils.py @@ -0,0 +1,161 @@ +# This is the joints order when loading using IsaacGymEnvs +# ['left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna', 'right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle'] +# This is the "standard" order (from mujoco) +# ['right_hip_yaw', 'right_hip_roll', 'right_hip_pitch', 'right_knee', 'right_ankle', 'left_hip_yaw', 'left_hip_roll', 'left_hip_pitch', 'left_knee', 'left_ankle', 'neck_pitch', 'head_pitch', 'head_yaw', 'left_antenna', 'right_antenna'] +# +# We need to reorder the joints to match the IsaacGymEnvs order +# +import numpy as np + +mujoco_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + +isaac_joints_order = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "left_antenna", + "right_antenna", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +] + + +def isaac_to_mujoco(joints): + new_joints = [ + # left leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # right leg + joints[11], + joints[12], + joints[13], + joints[14], + joints[15], + # head + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + joints[10], + ] + return new_joints + + +def mujoco_to_isaac(joints): + new_joints = [ + # left leg + joints[0], + joints[1], + joints[2], + joints[3], + joints[4], + # head + joints[10], + joints[11], + joints[12], + joints[13], + joints[14], + joints[15], + # right leg + joints[5], + joints[6], + joints[7], + joints[8], + joints[9], + ] + return new_joints + + +# TODO ADD BACK +def action_to_pd_targets(action, offset, scale): + return offset + scale * action + + +def make_action_dict(action, joints_order): + action_dict = {} + for i, a in enumerate(action): + if "antenna" not in joints_order[i]: + action_dict[joints_order[i]] = a + + return action_dict + + +def quat_rotate_inverse(q, v): + q = np.array(q) + v = np.array(v) + + q_w = q[-1] + q_vec = q[:3] + + a = v * (2.0 * q_w**2 - 1.0) + b = np.cross(q_vec, v) * q_w * 2.0 + c = q_vec * (np.dot(q_vec, v)) * 2.0 + + return a - b + c + + +class ActionFilter: + def __init__(self, window_size=10): + self.window_size = window_size + self.action_buffer = [] + + def push(self, action): + self.action_buffer.append(action) + if len(self.action_buffer) > self.window_size: + self.action_buffer.pop(0) + + def get_filtered_action(self): + return np.mean(self.action_buffer, axis=0) + + +class LowPassActionFilter: + def __init__(self, control_freq, cutoff_frequency=30.0): + self.last_action = 0 + self.current_action = 0 + self.control_freq = float(control_freq) + self.cutoff_frequency = float(cutoff_frequency) + self.alpha = self.compute_alpha() + + def compute_alpha(self): + return (1.0 / self.cutoff_frequency) / ( + 1.0 / self.control_freq + 1.0 / self.cutoff_frequency + ) + + def push(self, action): + self.current_action = action + + def get_filtered_action(self): + self.last_action = ( + self.alpha * self.last_action + (1 - self.alpha) * self.current_action + ) + return self.last_action diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py new file mode 100644 index 0000000..c065c8f --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/rustypot_position_hwi.py @@ -0,0 +1,166 @@ +import time + +import numpy as np +import rustypot +from mini_bdx_runtime.duck_config import DuckConfig + + +class HWI: + def __init__(self, duck_config: DuckConfig, usb_port: str = "/dev/ttyACM0"): + + self.duck_config = duck_config + + # Order matters here + self.joints = { + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + # "left_antenna": None, + # "right_antenna": None, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, + } + + self.zero_pos = { + "left_hip_yaw": 0, + "left_hip_roll": 0, + "left_hip_pitch": 0, + "left_knee": 0, + "left_ankle": 0, + "neck_pitch": 0, + "head_pitch": 0, + "head_yaw": 0, + "head_roll": 0, + # "left_antenna":0, + # "right_antenna":0, + "right_hip_yaw": 0, + "right_hip_roll": 0, + "right_hip_pitch": 0, + "right_knee": 0, + "right_ankle": 0, + } + + self.init_pos = { + "left_hip_yaw": 0.002, + "left_hip_roll": 0.053, + "left_hip_pitch": -0.63, + "left_knee": 1.368, + "left_ankle": -0.784, + "neck_pitch": 0.0, + "head_pitch": 0.0, + "head_yaw": 0, + "head_roll": 0, + # "left_antenna": 0, + # "right_antenna": 0, + "right_hip_yaw": -0.003, + "right_hip_roll": -0.065, + "right_hip_pitch": 0.635, + "right_knee": 1.379, + "right_ankle": -0.796, + } + + self.joints_offsets = self.duck_config.joints_offset + + self.kps = np.ones(len(self.joints)) * 32 # default kp + self.kds = np.ones(len(self.joints)) * 0 # default kd + self.low_torque_kps = np.ones(len(self.joints)) * 2 + + self.io = rustypot.feetech(usb_port, 1000000) + + def set_kps(self, kps): + self.kps = kps + self.io.set_kps(list(self.joints.values()), self.kps) + + def set_kds(self, kds): + self.kds = kds + self.io.set_kds(list(self.joints.values()), self.kds) + + def set_kp(self, id, kp): + self.io.set_kps([id], [kp]) + + def turn_on(self): + self.io.set_kps(list(self.joints.values()), self.low_torque_kps) + print("turn on : low KPS set") + time.sleep(1) + + self.set_position_all(self.init_pos) + print("turn on : init pos set") + + time.sleep(1) + + self.io.set_kps(list(self.joints.values()), self.kps) + print("turn on : high kps") + + def turn_off(self): + self.io.disable_torque(list(self.joints.values())) + + def set_position(self, joint_name, pos): + """ + pos is in radians + """ + id = self.joints[joint_name] + pos = pos + self.joints_offsets[joint_name] + self.io.write_goal_position([id], [pos]) + + def set_position_all(self, joints_positions): + """ + joints_positions is a dictionary with joint names as keys and joint positions as values + Warning: expects radians + """ + ids_positions = { + self.joints[joint]: position + self.joints_offsets[joint] + for joint, position in joints_positions.items() + } + + self.io.write_goal_position( + list(self.joints.values()), list(ids_positions.values()) + ) + + def get_present_positions(self, ignore=[]): + """ + Returns the present positions in radians + """ + + try: + present_positions = self.io.read_present_position( + list(self.joints.values()) + ) + except Exception as e: + print(e) + return None + + present_positions = [ + pos - self.joints_offsets[joint] + for joint, pos in zip(self.joints.keys(), present_positions) + if joint not in ignore + ] + return np.array(np.around(present_positions, 3)) + + def get_present_velocities(self, rad_s=True, ignore=[]): + """ + Returns the present velocities in rad/s (default) or rev/min + """ + try: + present_velocities = self.io.read_present_velocity( + list(self.joints.values()) + ) + except Exception as e: + print(e) + return None + + present_velocities = [ + vel + for joint, vel in zip(self.joints.keys(), present_velocities) + if joint not in ignore + ] + + return np.array(np.around(present_velocities, 3)) diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/sounds.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/sounds.py new file mode 100644 index 0000000..7cc1e3a --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/sounds.py @@ -0,0 +1,58 @@ +import pygame +import time +import os +import random + + +class Sounds: + def __init__(self, volume=1.0, sound_directory="./"): + pygame.mixer.init() + pygame.mixer.music.set_volume(volume) + self.sounds = {} + self.ok = True + try: + for file in os.listdir(sound_directory): + if file.endswith(".wav"): + sound_path = os.path.join(sound_directory, file) + try: + self.sounds[file] = pygame.mixer.Sound(sound_path) + print(f"Loaded: {file}") + except pygame.error as e: + print(f"Failed to load {file}: {e}") + except FileNotFoundError: + print(f"Directory {sound_directory} not found.") + self.ok = False + if len(self.sounds) == 0: + print("No sound files found in the directory.") + self.ok = False + + def play(self, sound_name): + if not self.ok: + print("Sounds not initialized properly.") + return + if sound_name in self.sounds: + self.sounds[sound_name].play() + print(f"Playing: {sound_name}") + else: + print(f"Sound '{sound_name}' not found!") + + def play_random_sound(self): + if not self.ok: + print("Sounds not initialized properly.") + return + sound_name = random.choice(list(self.sounds.keys())) + self.sounds[sound_name].play() + + def play_happy(self): + self.sounds["happy1.wav"].play() + + + +# Example usage +if __name__ == "__main__": + sound_player = Sounds(1.0, "../assets/") + time.sleep(1) + while True: + # sound_player.play_random_sound() + sound_player.play_happy() + time.sleep(3) diff --git a/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py new file mode 100644 index 0000000..cd74b54 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/mini_bdx_runtime/mini_bdx_runtime/xbox_controller.py @@ -0,0 +1,220 @@ +import pygame +from threading import Thread +from queue import Queue +import time +import numpy as np +from mini_bdx_runtime.buttons import Buttons + + +X_RANGE = [-0.15, 0.15] +Y_RANGE = [-0.2, 0.2] +YAW_RANGE = [-1.0, 1.0] + +# rads +NECK_PITCH_RANGE = [-0.34, 1.1] +HEAD_PITCH_RANGE = [-0.78, 0.3] +HEAD_YAW_RANGE = [-0.5, 0.5] +HEAD_ROLL_RANGE = [-0.5, 0.5] + + +class XBoxController: + def __init__(self, command_freq, only_head_control=False): + self.command_freq = command_freq + self.head_control_mode = only_head_control + self.only_head_control = only_head_control + + self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + self.last_left_trigger = 0.0 + self.last_right_trigger = 0.0 + pygame.init() + self.p1 = pygame.joystick.Joystick(0) + self.p1.init() + print(f"Loaded joystick with {self.p1.get_numaxes()} axes.") + self.cmd_queue = Queue(maxsize=1) + + self.A_pressed = False + self.B_pressed = False + self.X_pressed = False + self.Y_pressed = False + self.LB_pressed = False + self.RB_pressed = False + + self.buttons = Buttons() + + Thread(target=self.commands_worker, daemon=True).start() + + def commands_worker(self): + while True: + self.cmd_queue.put(self.get_commands()) + time.sleep(1 / self.command_freq) + + def get_commands(self): + last_commands = self.last_commands + left_trigger = self.last_left_trigger + right_trigger = self.last_right_trigger + + l_x = -1 * self.p1.get_axis(0) + l_y = -1 * self.p1.get_axis(1) + r_x = -1 * self.p1.get_axis(2) + r_y = -1 * self.p1.get_axis(3) + + right_trigger = np.around((self.p1.get_axis(4) + 1) / 2, 3) + left_trigger = np.around((self.p1.get_axis(5) + 1) / 2, 3) + + if left_trigger < 0.1: + left_trigger = 0 + if right_trigger < 0.1: + right_trigger = 0 + + if not self.head_control_mode: + lin_vel_y = l_x + lin_vel_x = l_y + ang_vel = r_x + if lin_vel_x >= 0: + lin_vel_x *= np.abs(X_RANGE[1]) + else: + lin_vel_x *= np.abs(X_RANGE[0]) + + if lin_vel_y >= 0: + lin_vel_y *= np.abs(Y_RANGE[1]) + else: + lin_vel_y *= np.abs(Y_RANGE[0]) + + if ang_vel >= 0: + ang_vel *= np.abs(YAW_RANGE[1]) + else: + ang_vel *= np.abs(YAW_RANGE[0]) + + last_commands[0] = lin_vel_x + last_commands[1] = lin_vel_y + last_commands[2] = ang_vel + else: + last_commands[0] = 0.0 + last_commands[1] = 0.0 + last_commands[2] = 0.0 + last_commands[3] = 0.0 # neck pitch 0 for now + + head_yaw = l_x + head_pitch = l_y + head_roll = r_x + + if head_yaw >= 0: + head_yaw *= np.abs(HEAD_YAW_RANGE[0]) + else: + head_yaw *= np.abs(HEAD_YAW_RANGE[1]) + + if head_pitch >= 0: + head_pitch *= np.abs(HEAD_PITCH_RANGE[0]) + else: + head_pitch *= np.abs(HEAD_PITCH_RANGE[1]) + + if head_roll >= 0: + head_roll *= np.abs(HEAD_ROLL_RANGE[0]) + else: + head_roll *= np.abs(HEAD_ROLL_RANGE[1]) + + last_commands[4] = head_pitch + last_commands[5] = head_yaw + last_commands[6] = head_roll + + for event in pygame.event.get(): + if event.type == pygame.JOYBUTTONDOWN: + + if self.p1.get_button(0): # A button + self.A_pressed = True + + if self.p1.get_button(1): # B button + self.B_pressed = True + + if self.p1.get_button(3): # X button + self.X_pressed = True + + if self.p1.get_button(4): # Y button + self.Y_pressed = True + if not self.only_head_control: + self.head_control_mode = not self.head_control_mode + + if self.p1.get_button(6): # LB button + self.LB_pressed = True + + if self.p1.get_button(7): # RB button + self.RB_pressed = True + + if event.type == pygame.JOYBUTTONUP: + self.A_pressed = False + self.B_pressed = False + self.X_pressed = False + self.Y_pressed = False + self.LB_pressed = False + self.RB_pressed = False + + # for i in range(10): + # if self.p1.get_button(i): + # print(f"Button {i} pressed") + + up_down = self.p1.get_hat(0)[1] + pygame.event.pump() # process event queue + + return ( + np.around(last_commands, 3), + self.A_pressed, + self.B_pressed, + self.X_pressed, + self.Y_pressed, + self.LB_pressed, + self.RB_pressed, + left_trigger, + right_trigger, + up_down, + ) + + def get_last_command(self): + A_pressed = False + B_pressed = False + X_pressed = False + Y_pressed = False + LB_pressed = False + RB_pressed = False + up_down = 0 + try: + ( + self.last_commands, + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + self.last_left_trigger, + self.last_right_trigger, + up_down, + ) = self.cmd_queue.get( + False + ) # non blocking + except Exception: + pass + + self.buttons.update( + A_pressed, + B_pressed, + X_pressed, + Y_pressed, + LB_pressed, + RB_pressed, + up_down == 1, + up_down == -1, + ) + + return ( + self.last_commands, + self.buttons, + self.last_left_trigger, + self.last_right_trigger, + ) + +if __name__ == "__main__": + controller = XBoxController(20) + + while True: + print(controller.get_last_command()) + time.sleep(0.05) diff --git a/Open_Duck_Mini_Runtime-2/pyproject.toml b/Open_Duck_Mini_Runtime-2/pyproject.toml new file mode 100644 index 0000000..7fd26b9 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/pyproject.toml @@ -0,0 +1,3 @@ +[build-system] +requires = ["setuptools"] +build-backend = "setuptools.build_meta" \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/scripts/antennas_controller_test.py b/Open_Duck_Mini_Runtime-2/scripts/antennas_controller_test.py new file mode 100644 index 0000000..b0edddd --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/antennas_controller_test.py @@ -0,0 +1,18 @@ +from mini_bdx_runtime.xbox_controller import XBoxController +from mini_bdx_runtime.antennas import Antennas +import time + +controller = XBoxController(60) + +antennas = Antennas() + + +while True: + + _, _, left_trigger, right_trigger = controller.get_last_command() + + antennas.set_position_left(right_trigger) + antennas.set_position_right(left_trigger) + + # print(left_trigger, right_trigger) + time.sleep(1 / 50) diff --git a/Open_Duck_Mini_Runtime-2/scripts/calibrate_imu.py b/Open_Duck_Mini_Runtime-2/scripts/calibrate_imu.py new file mode 100644 index 0000000..73a531d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/calibrate_imu.py @@ -0,0 +1,5 @@ + +from mini_bdx_runtime.raw_imu import Imu + +if __name__ == "__main__": + imu = Imu(50, calibrate=True, upside_down=False) \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/scripts/cam_test.py b/Open_Duck_Mini_Runtime-2/scripts/cam_test.py new file mode 100644 index 0000000..b75f311 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/cam_test.py @@ -0,0 +1,18 @@ +from picamzero import Camera +import cv2 + +print("Initializing camera ...") +cam = Camera() +# cam.still_size = (512, 512) +print("Camera initialized") + +im = cam.capture_array() +im = cv2.resize(im, (512, 512)) +im = cv2.cvtColor(im, cv2.COLOR_BGR2RGB) + +cv2.imwrite("/home/bdxv2/aze.jpg", im) +# cv2.imshow("Image", im) +# cv2.waitKey(0) +# cv2.destroyAllWindows() + +# cam.take_photo("/home/bdxv2/aze.jpg") diff --git a/Open_Duck_Mini_Runtime-2/scripts/check_motors.py b/Open_Duck_Mini_Runtime-2/scripts/check_motors.py new file mode 100644 index 0000000..9bc3f26 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/check_motors.py @@ -0,0 +1,168 @@ +""" +Debug script to check all motors in the robot. +Verifies each motor is accessible and allows testing movement. +""" + +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time +import numpy as np +import traceback + +def main(): + print("Initializing hardware interface...") + try: + # Initialize with default USB port - you might need to modify this + print("Attempting to connect to motor controller...") + duck_config = DuckConfig() # Create default configuration + + # Initialize with duck_config + print("Attempting to connect to motor controller...") + hwi = HWI(duck_config=duck_config) + print("Successfully connected to hardware!") + except Exception as e: + print(f"Error connecting to hardware: {e}") + print(f"Error details: {traceback.format_exc()}") + print("Check that the robot is powered on and USB connection is correct.") + return + + # Turn on with low torque for safety - ONE BY ONE + print("\nTurning on motors with low torque (one by one)...") + unresponsive_motors = [] + + for joint_name, joint_id in hwi.joints.items(): + try: + print(f"Setting low torque for motor '{joint_name}' (ID: {joint_id})...") + hwi.io.set_kps([joint_id], [hwi.low_torque_kps[0]]) + print(f"✓ Low torque set successfully for motor '{joint_name}' (ID: {joint_id}).") + except Exception as e: + print(f"✗ Error setting low torque for motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details: {traceback.format_exc()}") + unresponsive_motors.append((joint_name, joint_id)) + + # Check if all motors are responsive + print("\nChecking if all motors are responsive...") + + for joint_name, joint_id in hwi.joints.items(): + # Skip motors that already failed + if (joint_name, joint_id) in unresponsive_motors: + print(f"Skipping previously unresponsive motor: '{joint_name}' (ID: {joint_id})") + continue + + print(f"Attempting to read position from motor '{joint_name}' (ID: {joint_id})...") + try: + # Try to read the position to check if motor is responsive + position = hwi.io.read_present_position([joint_id]) + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) is responsive. Position: {position[0]:.3f}") + except Exception as e: + print(f"✗ Error accessing motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details for motor {joint_id}: {traceback.format_exc()}") + unresponsive_motors.append((joint_name, joint_id)) + + if unresponsive_motors: + print("\nWARNING: Some motors are not responsive!") + print("Unresponsive motors:", unresponsive_motors) + continue_anyway = input("Do you want to continue anyway? (y/n): ").lower() + if continue_anyway != 'y': + print("Exiting...") + try: + print("Attempting to turn off responsive motors before exiting...") + for joint_name, joint_id in hwi.joints.items(): + if (joint_name, joint_id) not in unresponsive_motors: + try: + hwi.io.disable_torque([joint_id]) + print(f"Disabled torque for motor '{joint_name}' (ID: {joint_id})") + except: + pass + except: + pass + return + + # Test moving each motor individually + print("\n--- Motor Movement Test ---") + print("This will move each motor by a small amount to check if it's working correctly.") + input("Press Enter to begin the movement test...") + + for joint_name, joint_id in hwi.joints.items(): + # Skip unresponsive motors + if (joint_name, joint_id) in unresponsive_motors: + print(f"Skipping unresponsive motor: '{joint_name}' (ID: {joint_id})") + continue + + print(f"\nTesting motor: '{joint_name}' (ID: {joint_id})") + test_this_motor = input(f"Test this motor? (Enter/y for yes, n to skip, q to quit): ").lower() + + if test_this_motor == 'q': + print("Exiting movement test...") + break + + if test_this_motor == 'n': + print(f"Skipping '{joint_name}' (ID: {joint_id})") + continue + + try: + # Get current position + print(f"Reading current position from motor '{joint_name}' (ID: {joint_id})...") + current_position = hwi.io.read_present_position([joint_id])[0] + print(f"Current position: {current_position:.3f}") + + # Calculate test position (move by 0.1 radians) + test_position = current_position + 0.1 + + # Move to test position + print(f"Moving motor '{joint_name}' (ID: {joint_id}) to test position: {test_position:.3f}...") + hwi.io.write_goal_position([joint_id], [test_position]) + time.sleep(1) # Wait for movement + + # Read new position + print(f"Reading new position from motor '{joint_name}' (ID: {joint_id})...") + new_position = hwi.io.read_present_position([joint_id])[0] + print(f"New position: {new_position:.3f}") + + # Return to original position + print(f"Returning motor '{joint_name}' (ID: {joint_id}) to original position...") + hwi.io.write_goal_position([joint_id], [current_position]) + time.sleep(1) # Wait for movement + + # No confirmation question, just assume success + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) movement test completed.") + + except Exception as e: + print(f"Error testing motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details: {traceback.format_exc()}") + + # Turn off motors + print("\nTurning off motors one by one...") + for joint_name, joint_id in hwi.joints.items(): + if (joint_name, joint_id) in unresponsive_motors: + print(f"Skipping turning off unresponsive motor: '{joint_name}' (ID: {joint_id})") + continue + + try: + print(f"Disabling torque for motor '{joint_name}' (ID: {joint_id})...") + hwi.io.disable_torque([joint_id]) + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) turned off successfully.") + except Exception as e: + print(f"✗ Error turning off motor '{joint_name}' (ID: {joint_id}): {e}") + print(f"Error details: {traceback.format_exc()}") + + print("\nMotor test completed.") + +if __name__ == "__main__": + try: + main() + except KeyboardInterrupt: + print("\nScript interrupted by user. Attempting to turn off motors...") + try: + print("Initializing HWI to turn off motors...") + hwi = HWI() + for joint_name, joint_id in hwi.joints.items(): + try: + print(f"Turning off motor '{joint_name}' (ID: {joint_id})...") + hwi.io.disable_torque([joint_id]) + print(f"✓ Motor '{joint_name}' (ID: {joint_id}) turned off successfully.") + except Exception as e: + print(f"✗ Error turning off motor '{joint_name}' (ID: {joint_id}): {e}") + except Exception as e: + print(f"Error initializing HWI to turn off motors: {e}") + print(f"Error details: {traceback.format_exc()}") \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/scripts/check_voltage.py b/Open_Duck_Mini_Runtime-2/scripts/check_voltage.py new file mode 100644 index 0000000..89d4430 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/check_voltage.py @@ -0,0 +1,31 @@ +from pypot.feetech import FeetechSTS3215IO + +io = FeetechSTS3215IO( + "/dev/ttyACM0", + baudrate=1000000, + use_sync_read=True, +) + +joints = { + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + # "left_antenna": None, + # "right_antenna": None, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} + + +voltages = io.get_present_voltage(list(joints.values())) +for i, name in enumerate(joints.keys()): + print(name, round(voltages[i] * 0.1, 2), "V") \ No newline at end of file diff --git a/Open_Duck_Mini_Runtime-2/scripts/configure_all_motors.py b/Open_Duck_Mini_Runtime-2/scripts/configure_all_motors.py new file mode 100644 index 0000000..5d391cc --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/configure_all_motors.py @@ -0,0 +1,40 @@ +from pypot.feetech import FeetechSTS3215IO +import time + +joints = { + "left_hip_yaw": 20, + "left_hip_roll": 21, + "left_hip_pitch": 22, + "left_knee": 23, + "left_ankle": 24, + "neck_pitch": 30, + "head_pitch": 31, + "head_yaw": 32, + "head_roll": 33, + "right_hip_yaw": 10, + "right_hip_roll": 11, + "right_hip_pitch": 12, + "right_knee": 13, + "right_ankle": 14, +} + +joints_inv = {v: k for k, v in joints.items()} + +ids = list(joints.values()) +io = FeetechSTS3215IO("/dev/ttyACM0") +for current_id in ids: + print("Configuring", joints_inv[current_id]) + io.set_lock({current_id: 0}) + # io.set_mode({current_id: 0}) + io.set_maximum_acceleration({current_id: 0}) + io.set_acceleration({current_id: 0}) + # io.set_maximum_velocity({current_id: 0}) + # io.set_goal_speed({current_id: 0}) + io.set_P_coefficient({current_id: 32}) + io.set_I_coefficient({current_id: 0}) + io.set_D_coefficient({current_id: 0}) + + io.set_lock({current_id: 1}) + time.sleep(1) + input("Press any key to set this dof to 0 position ... Or press Ctrl+C to cancel") + io.set_goal_position({current_id: 0}) diff --git a/Open_Duck_Mini_Runtime-2/scripts/configure_motor.py b/Open_Duck_Mini_Runtime-2/scripts/configure_motor.py new file mode 100644 index 0000000..c95639f --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/configure_motor.py @@ -0,0 +1,89 @@ +from pypot.feetech import FeetechSTS3215IO +import argparse +import time + +DEFAULT_ID = 1 # A brand new motor should have id 1 + +parser = argparse.ArgumentParser() +parser.add_argument( + "--port", + help="The port the motor is connected to. Default is /dev/ttyACM0. Use `ls /dev/tty* | grep usb` to find the port.", + default="/dev/ttyACM0", +) +parser.add_argument("--id", help="The id to set to the motor.", type=str, required=True) +args = parser.parse_args() +io = FeetechSTS3215IO(args.port) + +current_id = DEFAULT_ID + + +def scan(): + id = None + for i in range(255): + + print(f"scanning for id {i} ...") + try: + io.get_present_position([i]) + id = i + print(f"Found motor with id {id}") + break + except Exception: + pass + return id + + +try: + io.get_present_position([DEFAULT_ID]) +except Exception: + print( + f"Could not find motor with default id ({DEFAULT_ID}). Scanning for motor ..." + ) + res = scan() + if res is not None: + current_id = res + else: + print("Could not find motor. Exiting ...") + exit() + + +# print("current id: ", current_id) + +kp = io.get_P_coefficient([current_id]) +ki = io.get_I_coefficient([current_id]) +kd = io.get_D_coefficient([current_id]) +max_acceleration = io.get_maximum_acceleration([current_id]) +acceleration = io.get_acceleration([current_id]) +mode = io.get_mode([current_id]) + +# print(f"PID : {kp}, {ki}, {kd}") +# print(f"max_acceleration: {max_acceleration}") +# print(f"acceleration: {acceleration}") +# print(f"mode: {mode}") + +io.set_lock({current_id: 0}) +io.set_mode({current_id: 0}) +io.set_maximum_acceleration({current_id: 0}) +io.set_acceleration({current_id: 0}) +io.set_P_coefficient({current_id: 32}) +io.set_I_coefficient({current_id: 0}) +io.set_D_coefficient({current_id: 0}) +io.change_id({current_id: int(args.id)}) + +current_id = int(args.id) + +time.sleep(1) + +io.set_goal_position({current_id: 0}) + +time.sleep(1) + +print("===") +print("Done configuring motor.") +print(f"Motor id: {current_id}") +print(f"P coefficient : {io.get_P_coefficient([current_id])}") +print(f"I coefficient : {io.get_I_coefficient([current_id])}") +print(f"D coefficient : {io.get_D_coefficient([current_id])}") +print(f"acceleration: {io.get_acceleration([current_id])}") +print(f"max_acceleration: {io.get_maximum_acceleration([current_id])}") +print(f"mode: {io.get_mode([current_id])}") +print("===") diff --git a/Open_Duck_Mini_Runtime-2/scripts/fc_test.py b/Open_Duck_Mini_Runtime-2/scripts/fc_test.py new file mode 100644 index 0000000..3eed49d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/fc_test.py @@ -0,0 +1,302 @@ +from openai import OpenAI +import time +import json +import os +from io import BytesIO +import base64 + +from v2_rl_walk_mujoco import RLWalk +from threading import Thread +import cv2 +from mini_bdx_runtime.camera import Cam + +# TODO mission : find an object ? + + +# Your Tools class +class Tools: + def __init__(self): + self.cam = Cam() + self.rl_walk = RLWalk( + "/home/bdxv2/BEST_WALK_ONNX_2.onnx", + cutoff_frequency=40, + ) + + Thread(target=self.rl_walk.run, daemon=True).start() + + # def upload_image(self, image_path: str): + # image_name = os.path.basename(image_path) + # im = cv2.imread(image_path) + # im = cv2.resize(im, (512, 512)) + # cv2.imwrite(image_path, im) + # # command = ( + # # f"scp {image_path} apirrone@s-nguyen.net:/home/apirrone/webserv/images/" + # # ) + # command = ( + # f"scp {image_path} apirrone@192.168.10.103:/home/apirrone/webserv/images/" + # ) + # print(command) + # url = f"http://s-nguyen.net:4444/images/{image_name}" + # os.system(command) + # return url + + + def move_forward(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Moving forward for {seconds} seconds") + self.rl_walk.last_commands[0] = 0.15 + time.sleep(seconds) + self.rl_walk.last_commands[0] = 0.0 + print("Stopped moving forward") + return f"Moved forward for {seconds} seconds successfully" + + def turn_left(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Turning left for {seconds} seconds") + self.rl_walk.last_commands[2] = 1.0 + time.sleep(seconds) + self.rl_walk.last_commands[2] = 0.0 + print("Stopped turning left") + return f"Turned left for {seconds} seconds successfully" + + def turn_right(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Turning right for {seconds} seconds") + self.rl_walk.last_commands[2] = -1.0 + time.sleep(seconds) + self.rl_walk.last_commands[2] = 0.0 + print("Stopped turning right") + return f"Turned right for {seconds} seconds successfully" + + def move_backward(self, seconds=2): + seconds = max(2, min(seconds, 5)) + print(f"Moving backward for {seconds} seconds") + self.rl_walk.last_commands[0] = -0.15 + time.sleep(seconds) + self.rl_walk.last_commands[0] = 0.0 + print("Stopped moving backward") + return f"Moved backward for {seconds} seconds successfully" + + def take_picture(self): + # https://projects.raspberrypi.org/en/projects/getting-started-with-picamera/5 + print("Taking a picture...") + image64 = self.cam.get_encoded_image() + url = ("data:image/jpeg;base64," + image64,) + time.sleep(1) + print("Picture taken") + return url + + def play_happy_sound(self): + self.rl_walk.sounds.play_happy() + return "Played happy sound" + +# Tool instance +tools_instance = Tools() + +openai_tools = [ + { + "type": "function", + "name": "move_forward", + "description": "Move forward for a number of seconds", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to move forward (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "move_backward", + "description": "Move backward for a number of seconds", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to move backward (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "turn_left", + "description": "Turn left on the spot", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to turn left (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "turn_right", + "description": "Turn right on the spot", + "parameters": { + "type": "object", + "properties": { + "seconds": { + "type": "integer", + "description": "Number of seconds to turn right (min 2, max 5)", + } + }, + "required": ["seconds"], + "additionalProperties": False, + }, + }, + { + "type": "function", + "name": "take_picture", + "description": "Take a picture", + "parameters": { + "type": "object", + "properties": {}, + # No required properties for taking a picture + }, + }, + { + "type": "function", + "name": "play_happy_sound", + "description": "Play a happy sound", + "parameters": { + "type": "object", + "properties": {}, + # No required properties for playing a sound + }, + } +] + +# Mapping function names to actual methods +function_map = { + "move_forward": tools_instance.move_forward, + "move_backward": tools_instance.move_backward, + "turn_left": tools_instance.turn_left, + "turn_right": tools_instance.turn_right, + "take_picture": tools_instance.take_picture, +} + +messages = [ + { + "role": "system", + "content": ( + "You are a cute little biped robot that can move around using the following tools: " + "`move_forward`, `move_backward`, `turn_left`, `turn_right`, 'play_happy_sound' and 'take_picture'. " + "moving forward for 1 second will make you move forward by about 15 centimeters" + "turning for 1 second will make you turn about 45 degrees" + "You can only perform one action at a time. If multiple actions are needed, call them step by step." + "Explain what you are doing along the way" + "Always start by taking a picture of the environment so you can see where you are. " + "When you take a picture, describe what you see in the image. " + "make sure not to hit any walls or objects. Take pictures regularly to know where you are." + "Maybe it's a good idea to turn 360 degrees to check all directions. (no need if you already found it)" + "When given a goal to find something, if you found it, navigate to be in front of it, facing it. You want to be about 1 meter close to it" + "When you are 1 meter close to the object, play the happy sound" + "" + ), + }, + # { + # "role": "user", + # "content": "Find the yellow vaccum cleaner !", + # }, + { + "role": "user", + "content": "Find the waste bin and turn around it. Play the happy sound when you are done", + }, +] + + +# Mapping function names to actual methods +function_map = { + "move_forward": tools_instance.move_forward, + "move_backward": tools_instance.move_backward, + "turn_left": tools_instance.turn_left, + "turn_right": tools_instance.turn_right, + "take_picture": tools_instance.take_picture, + "play_happy_sound": tools_instance.play_happy_sound, +} + + +client = OpenAI() + + +def call_function(name, args): + if name == "move_forward": + return function_map[name](args["seconds"]) + elif name == "move_backward": + return function_map[name](args["seconds"]) + elif name == "turn_left": + return function_map[name](args["seconds"]) + elif name == "turn_right": + return function_map[name](args["seconds"]) + elif name == "take_picture": + return function_map[name]() + elif name == "play_happy_sound": + return function_map[name]() + else: + raise ValueError(f"Unknown function name: {name}") + + +while True: + response = client.responses.create( + model="gpt-4o-mini", + input=messages, + tools=openai_tools, + ) + + if len(response.output) == 1 and response.output[0].type == "function_call": + print("Only function call, no text response") + else: + try: + print(response.output[0].content[0].text) + except: + print("Error occurred while processing response") + for tool_call in response.output: + if tool_call.type != "function_call": + continue + + name = tool_call.name + args = json.loads(tool_call.arguments) + + result = call_function(name, args)[0] + messages.append(tool_call) + if tool_call.name == "take_picture": + # result is an image URL + + # Add an optional prompt or let GPT interpret the image + messages.append( + { + "role": "user", + "content": [{"type": "input_image", "image_url": result}], + } + ) + + messages.append( + { + "type": "function_call_output", + "call_id": tool_call.call_id, + "output": "Image taken and provided above.", + } + ) + else: + + messages.append( + { + "type": "function_call_output", + "call_id": tool_call.call_id, + "output": str(result), + } + ) diff --git a/Open_Duck_Mini_Runtime-2/scripts/find_soft_offsets.py b/Open_Duck_Mini_Runtime-2/scripts/find_soft_offsets.py new file mode 100644 index 0000000..a3e7b66 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/find_soft_offsets.py @@ -0,0 +1,84 @@ +""" +Find the offsets to set in self.joints_offsets in hwi_feetech_pwm_control.py +""" + +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time + +dummy_config = DuckConfig(config_json_path=None, ignore_default=True) + +print("======") +print( + "Warning : this script will move the robot to its zero position quiclky, make sure it is safe to do so" +) +print("======") +print("") +input( + "Press any key to start. The robot will move to its zero position. Make sure it is safe to do so. At any time, press ctrl+c to stop, the motors will be turned off." +) + +hwi = HWI(dummy_config) + + +hwi.init_pos = hwi.zero_pos +hwi.set_kds([0] * len(hwi.joints)) +hwi.turn_on() +print("") +print("") +print("") +print("") +hwi.set_position_all(hwi.zero_pos) +time.sleep(1) +try: + for i, joint_name in enumerate(hwi.joints.keys()): + joint_id = hwi.joints[joint_name] + ok = False + while not ok: + res = input(f" === Setting up {joint_name} === (Y/(s)kip : ").lower() + if res == "s": + break + hwi.set_position_all(hwi.zero_pos) + time.sleep(0.5) + current_pos = hwi.get_present_positions()[i] + if current_pos is None: + continue + # hwi.control.kps[i] = 0 + hwi.io.disable_torque([joint_id]) + input( + f"{joint_name} is now turned off. Move it to the desired zero position and press any key to confirm the offset" + ) + new_pos = hwi.get_present_positions()[i] + offset = new_pos - current_pos + print(f" ---> Offset is {offset}") + hwi.joints_offsets[joint_name] = offset + input( + "Press any key to move the motor to its zero position with offset taken into account" + ) + hwi.set_position_all(hwi.zero_pos) + time.sleep(0.5) + hwi.io.enable_torque([joint_id]) + # hwi.control.kps[i] = 32 + res = input("Is that ok ? (Y/n)").lower() + if res == "y" or res == "": + print("Ok, setting offset") + hwi.joints_offsets[joint_name] = offset + ok = True + print("------") + print("Current offsets : ") + for k, v in hwi.joints_offsets.items(): + print(f"{k} : {v}") + print("------") + print("") + else: + print("Ok, let's try again") + hwi.joints_offsets[joint_name] = 0 + + print("===") + + print("Done ! ") + print("Now you can copy the offsets in your duck_config.json") + + +except KeyboardInterrupt: + hwi.turn_off() diff --git a/Open_Duck_Mini_Runtime-2/scripts/head_puppet.py b/Open_Duck_Mini_Runtime-2/scripts/head_puppet.py new file mode 100644 index 0000000..4df65b0 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/head_puppet.py @@ -0,0 +1,103 @@ +""" +Sets up the robot in init position, you control the head with the xbox controller +""" + +import time +import numpy as np +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +from mini_bdx_runtime.xbox_controller import XBoxController + + +from mini_bdx_runtime.eyes import Eyes +from mini_bdx_runtime.sounds import Sounds +from mini_bdx_runtime.antennas import Antennas +from mini_bdx_runtime.projector import Projector + +duck_config = DuckConfig() + +xbox_controller = XBoxController(50, only_head_control=True) + +if duck_config.speaker: + sounds = Sounds(volume=1.0, sound_directory="../mini_bdx_runtime/assets/") +if duck_config.antennas: + antennas = Antennas() +if duck_config.eyes: + eyes = Eyes() +if duck_config.projector: + projector = Projector() + +hwi = HWI(duck_config) + +kps = [8] * 14 +kds = [0] * 14 + +hwi.set_kps(kps) +hwi.set_kds(kds) +hwi.turn_on() + +limits = { + "neck_pitch": [-20, 60], + "head_pitch": [-60, 45], + "head_yaw": [-60, 60], + "head_roll": [-20, 20], +} + +try: + while True: + + last_commands, buttons, left_trigger, right_trigger = ( + xbox_controller.get_last_command() + ) + + l_x = last_commands[5] + l_y = last_commands[4] + r_x = last_commands[6] + # r_y = last_commands[3] + + head_yaw_deg = ( + l_x * (limits["head_yaw"][1] - limits["head_yaw"][0]) / 2 + + (limits["head_yaw"][1] + limits["head_yaw"][0]) / 2 + ) + head_yaw_pos_rad = np.deg2rad(head_yaw_deg) + + head_roll_deg = ( + r_x * (limits["head_roll"][1] - limits["head_roll"][0]) / 2 + + (limits["head_roll"][1] + limits["head_roll"][0]) / 2 + ) + head_roll_pos_rad = np.deg2rad(head_roll_deg) + + head_pitch_deg = ( + l_y * (limits["head_pitch"][1] - limits["head_pitch"][0]) / 2 + + (limits["head_pitch"][1] + limits["head_pitch"][0]) / 2 + ) + head_pitch_pos_rad = np.deg2rad(head_pitch_deg) + + # neck_pitch_deg = ( + # -r_y * (limits["neck_pitch"][1] - limits["neck_pitch"][0]) / 2 + # + (limits["neck_pitch"][1] + limits["neck_pitch"][0]) / 2 + # ) + # neck_pitch_pos_rad = np.deg2rad(neck_pitch_deg) + + hwi.set_position("head_yaw", head_yaw_pos_rad) + hwi.set_position("head_roll", head_roll_pos_rad) + hwi.set_position("head_pitch", head_pitch_pos_rad) + # hwi.set_position("neck_pitch", neck_pitch_pos_rad) + + if duck_config.antennas: + antennas.set_position_left(right_trigger) + antennas.set_position_right(left_trigger) + + if buttons.B.triggered: + if duck_config.speaker: + sounds.play_random_sound() + + if buttons.X.triggered: + if duck_config.projector: + projector.switch() + + # pygame.event.pump() # process event queue + time.sleep(1 / 60) +except KeyboardInterrupt: + if duck_config.antennas: + antennas.stop() diff --git a/Open_Duck_Mini_Runtime-2/scripts/imu_client.py b/Open_Duck_Mini_Runtime-2/scripts/imu_client.py new file mode 100644 index 0000000..6cc367b --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/imu_client.py @@ -0,0 +1,76 @@ +import socket +import time +import numpy as np +import pickle +from queue import Queue +from threading import Thread +from scipy.spatial.transform import Rotation as R +from FramesViewer.viewer import Viewer +import argparse + + +class IMUClient: + def __init__(self, host, port=1234, freq=30): + self.host = host + self.port = port + self.freq = freq + self.client_socket = socket.socket() + self.connected = False + while not self.connected: + try: + self.client_socket.connect((self.host, self.port)) + self.connected = True + except Exception as e: + print(e) + time.sleep(0.5) + self.imu_queue = Queue(maxsize=1) + self.last_imu = [0, 0, 0, 0] + + Thread(target=self.imu_worker, daemon=True).start() + + def imu_worker(self): + while True: + try: + data = self.client_socket.recv(1024) # receive response + data = pickle.loads(data) + + self.imu_queue.put(data) + except: + print("missed imu") + + time.sleep(1 / self.freq) + + def get_imu(self): + try: + self.last_imu = self.imu_queue.get(False) + except: + pass + + return self.last_imu + + +if __name__ == "__main__": + + parser = argparse.ArgumentParser() + parser.add_argument("--ip", type=str, required=True, help="IP address of the robot") + args = parser.parse_args() + + client = IMUClient(args.ip) + + fv = Viewer() + fv.start() + pose = np.eye(4) + pose[:3, 3] = [0.1, 0.1, 0.1] + try: + while True: + quat = client.get_imu() + try: + rot_mat = R.from_quat(quat).as_matrix() + pose[:3, :3] = rot_mat + fv.pushFrame(pose, "pose") + except Exception as e: + print("error", e) + pass + time.sleep(1 / 30) + except KeyboardInterrupt: + pass diff --git a/Open_Duck_Mini_Runtime-2/scripts/imu_server.py b/Open_Duck_Mini_Runtime-2/scripts/imu_server.py new file mode 100644 index 0000000..e537eec --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/imu_server.py @@ -0,0 +1,62 @@ +import socket +import time +import pickle +from mini_bdx_runtime.imu import Imu +from threading import Thread +import time + +import argparse + + +class IMUServer: + def __init__(self, imu=None): + self.host = "0.0.0.0" + self.port = 1234 + + self.server_socket = socket.socket() + self.server_socket.setsockopt( + socket.SOL_SOCKET, socket.SO_REUSEADDR, 1 + ) # enable address reuse + + self.server_socket.bind((self.host, self.port)) + + if imu is None: + self.imu = Imu(50, user_pitch_bias=args.pitch_bias, upside_down=False) + else: + self.imu = imu + self.stop = False + + Thread(target=self.run, daemon=True).start() + + def run(self): + while not self.stop: + self.server_socket.listen(1) + conn, address = self.server_socket.accept() # accept new connection + print("Connection from: " + str(address)) + try: + while True: + data = self.imu.get_data() + data = pickle.dumps(data) + conn.send(data) # send data to the client + time.sleep(1 / 30) + except: + pass + + self.server_socket.close() + print("thread closed") + time.sleep(1) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("--pitch_bias", type=float, default=0, help="deg") + args = parser.parse_args() + imu_server = IMUServer() + try: + while True: + time.sleep(0.01) + except KeyboardInterrupt: + print("Closing server") + imu_server.stop = True + + time.sleep(2) diff --git a/Open_Duck_Mini_Runtime-2/scripts/new_record_data.py b/Open_Duck_Mini_Runtime-2/scripts/new_record_data.py new file mode 100644 index 0000000..548839d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/new_record_data.py @@ -0,0 +1,104 @@ +from pypot.feetech import FeetechSTS3215IO +import pickle +import numpy as np +import time +import argparse + +parser = argparse.ArgumentParser(description="Record data from Feetech motor") +parser.add_argument( + "--new_firmware", + action="store_true", + help="Use new firmware for the motor", + default=False, +) +args = parser.parse_args() +io = FeetechSTS3215IO("/dev/ttyACM0") + + +def convert_load(raw_load): + sign = -1 + if raw_load > 1023: + raw_load -= 1024 + sign = 1 + return sign * raw_load * 0.001 + + +id = 1 if args.new_firmware else 11 +kp = 32 +kd = 0 +acceleration = 0 +maximum_acceleration = 0 +maximum_velocity = 0 +goal_speed = 0 + +time.sleep(1) +io.set_maximum_acceleration({id: maximum_acceleration}) +io.set_acceleration({id: acceleration}) +io.set_maximum_velocity({id: maximum_velocity}) +io.set_goal_speed({id: goal_speed}) +io.set_P_coefficient({id: kp}) +io.set_D_coefficient({id: kd}) +time.sleep(1) + +goal_position = 90 + +io.set_goal_position({id: 0}) +time.sleep(3) + +exit() + +times = [] +positions = [] +goal_positions = [] +speeds = [] +loads = [] +currents = [] + +io.set_goal_position({id: goal_position}) + +s = time.time() +set = False +while True: + t = time.time() - s + # goal_position = np.rad2deg(np.sin(t**2)) + io.set_goal_position({id: goal_position}) + present_position = np.deg2rad(io.get_present_position([id])[0]) + present_speed = np.deg2rad(io.get_present_speed([id])[0]) + present_load = convert_load(io.get_present_load([id])[0]) + present_current = io.get_present_current([id])[0] + + times.append(t) + positions.append(present_position) + goal_positions.append(np.deg2rad(goal_position)) + speeds.append(present_speed) + loads.append(present_load) + currents.append(present_current) + + if t > 3: + break + + time.sleep(0.01) + +fw_version = "new" if args.new_firmware else "old" +data = { + "maximum_acceleration": maximum_acceleration, + "acceleration": acceleration, + "maximum_velocity": maximum_velocity, + "kp": kp, + "kd": kd, + "times": times, + "positions": positions, + "goal_positions": goal_positions, + "speeds": speeds, + "loads": loads, + "currents": currents, + "fw_version": fw_version, +} + +pickle.dump( + data, + open( + f"data_KP_{kp}_{fw_version}.pkl", + "wb", + ), +) diff --git a/Open_Duck_Mini_Runtime-2/scripts/plot_recorded_data.py b/Open_Duck_Mini_Runtime-2/scripts/plot_recorded_data.py new file mode 100644 index 0000000..82f7d0d --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/plot_recorded_data.py @@ -0,0 +1,35 @@ +import pickle +import argparse +import matplotlib.pyplot as plt + +parser = argparse.ArgumentParser() +parser.add_argument("-f", "--file", type=str, required=True) +args = parser.parse_args() + +data = pickle.load(open(args.file, "rb")) + + +plt.figure() + +plt.subplot(4, 1, 1) +plt.plot(data["times"], data["positions"], label="positions (rad)") +plt.plot(data["times"], data["goal_positions"], label="goal_positions (rad)") +plt.legend() + +plt.subplot(4, 1, 2) +plt.plot(data["times"], data["speeds"], label="speed (rad/s)") +plt.legend() + +plt.subplot(4, 1, 3) +plt.plot(data["times"], data["loads"], label="load") +plt.legend() + +plt.subplot(4, 1, 4) +plt.plot(data["times"], data["currents"], label="current (mA)") +plt.legend() + +# plt.suptitle(f"Acceleration: {data['acceleration']}, KP: {data['kp']}, KD: {data['kd']}") +plt.suptitle(f"KP: {data['kp']}, firmware: {data['fw_version']}") +plt.tight_layout() + +plt.show() diff --git a/Open_Duck_Mini_Runtime-2/scripts/polynomial_coefficients.pkl b/Open_Duck_Mini_Runtime-2/scripts/polynomial_coefficients.pkl new file mode 100644 index 0000000..ebfef40 Binary files /dev/null and b/Open_Duck_Mini_Runtime-2/scripts/polynomial_coefficients.pkl differ diff --git a/Open_Duck_Mini_Runtime-2/scripts/record_data.py b/Open_Duck_Mini_Runtime-2/scripts/record_data.py new file mode 100644 index 0000000..76b6a85 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/record_data.py @@ -0,0 +1,100 @@ +from pypot.feetech import FeetechSTS3215IO +import pickle +import numpy as np +import time + +io = FeetechSTS3215IO("/dev/ttyACM0") + +# accelerations = [0, 10, 50, 100, 200, 255] +accelerations = [0] +switch=False +# kps = [4, 8, 16, 32] +# kds = [0, 4, 8, 16, 32] + +# accelerations = [0] +kps = [32] +kds = [0] + +for acceleration in accelerations: + for kp in kps: + for kd in kds: + print(f"acceleration: {acceleration}, kp: {kp}, kd: {kd}") + + # acceleration = 50 + # kp = 32 + # kd = 0 + + # io.set_mode({1: 0}) + # io.set_lock({1: 1}) + # time.sleep(1) + # io.set_maximum_acceleration({1: acceleration}) + # io.set_acceleration({1: acceleration}) + # time.sleep(1) + io.set_P_coefficient({1: kp}) + io.set_D_coefficient({1: kd}) + + # time.sleep(1) + goal_position = 90 + + io.set_goal_position({1: 0}) + time.sleep(3) + + times = [] + positions = [] + goal_positions = [] + speeds = [] + loads = [] + currents = [] + + def convert_load(raw_load): + sign = -1 + if raw_load > 1023: + raw_load -= 1024 + sign = 1 + return sign * raw_load * 0.001 + + io.set_goal_position({1: goal_position}) + s = time.time() + set = False + while True: + t = time.time() - s + # goal_position = np.rad2deg(np.sin(t**2)) + io.set_goal_position({1: goal_position}) + present_position = np.deg2rad(io.get_present_position([1])[0]) + present_speed = np.deg2rad(io.get_present_speed([1])[0]) + present_load = convert_load(io.get_present_load([1])[0]) + present_current = io.get_present_current([1])[0] + + times.append(t) + positions.append(present_position) + goal_positions.append(np.deg2rad(goal_position)) + speeds.append(present_speed) + loads.append(present_load) + currents.append(present_current) + + # if switch and t > 0.2 and not set: + # goal_position = -goal_position + # io.set_goal_position({1: goal_position}) + # set = True + + if t > 3: + break + + time.sleep(0.01) + + data = { + "acceleration": acceleration, + "kp": kp, + "kd": kd, + "times": times, + "positions": positions, + "goal_positions": goal_positions, + "speeds": speeds, + "loads": loads, + "currents": currents, + } + + pickle.dump( + data, + open(f"data_{'switch_' if switch else ''}acceleration_{acceleration}_kp_{kp}_kd_{kd}.pkl", "wb"), + ) diff --git a/Open_Duck_Mini_Runtime-2/scripts/turn_off.py b/Open_Duck_Mini_Runtime-2/scripts/turn_off.py new file mode 100644 index 0000000..20d215c --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/turn_off.py @@ -0,0 +1,9 @@ +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time + +duck_config = DuckConfig() + +hwi = HWI(duck_config) +hwi.turn_off() +time.sleep(1) diff --git a/Open_Duck_Mini_Runtime-2/scripts/turn_on.py b/Open_Duck_Mini_Runtime-2/scripts/turn_on.py new file mode 100644 index 0000000..33edad3 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/turn_on.py @@ -0,0 +1,9 @@ +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.duck_config import DuckConfig +import time + +duck_config = DuckConfig() + +hwi = HWI(duck_config) +hwi.turn_on() +time.sleep(1) diff --git a/Open_Duck_Mini_Runtime-2/scripts/v2_rl_walk_mujoco.py b/Open_Duck_Mini_Runtime-2/scripts/v2_rl_walk_mujoco.py new file mode 100644 index 0000000..530ebb9 --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/scripts/v2_rl_walk_mujoco.py @@ -0,0 +1,400 @@ +import time +import pickle + +import numpy as np +from mini_bdx_runtime.rustypot_position_hwi import HWI +from mini_bdx_runtime.onnx_infer import OnnxInfer + +from mini_bdx_runtime.raw_imu import Imu +from mini_bdx_runtime.poly_reference_motion import PolyReferenceMotion +from mini_bdx_runtime.xbox_controller import XBoxController +from mini_bdx_runtime.feet_contacts import FeetContacts +from mini_bdx_runtime.eyes import Eyes +from mini_bdx_runtime.sounds import Sounds +from mini_bdx_runtime.antennas import Antennas +from mini_bdx_runtime.projector import Projector +from mini_bdx_runtime.rl_utils import make_action_dict, LowPassActionFilter +from mini_bdx_runtime.duck_config import DuckConfig + +import os + +HOME_DIR = os.path.expanduser("~") + + +class RLWalk: + def __init__( + self, + onnx_model_path: str, + duck_config_path: str = f"{HOME_DIR}/duck_config.json", + serial_port: str = "/dev/ttyACM0", + control_freq: float = 50, + pid=[30, 0, 0], + action_scale=0.25, + commands=False, + pitch_bias=0, + save_obs=False, + replay_obs=None, + cutoff_frequency=None, + ): + + self.duck_config = DuckConfig(config_json_path=duck_config_path) + + self.commands = commands + self.pitch_bias = pitch_bias + + self.onnx_model_path = onnx_model_path + self.policy = OnnxInfer(self.onnx_model_path, awd=True) + + self.num_dofs = 14 + self.max_motor_velocity = 5.24 # rad/s + + # Control + self.control_freq = control_freq + self.pid = pid + + self.save_obs = save_obs + if self.save_obs: + self.saved_obs = [] + + self.replay_obs = replay_obs + if self.replay_obs is not None: + self.replay_obs = pickle.load(open(self.replay_obs, "rb")) + + self.action_filter = None + if cutoff_frequency is not None: + self.action_filter = LowPassActionFilter( + self.control_freq, cutoff_frequency + ) + + self.hwi = HWI(self.duck_config, serial_port) + + self.start() + + self.imu = Imu( + sampling_freq=int(self.control_freq), + user_pitch_bias=self.pitch_bias, + upside_down=self.duck_config.imu_upside_down, + ) + + self.feet_contacts = FeetContacts() + + # Scales + self.action_scale = action_scale + + self.last_action = np.zeros(self.num_dofs) + self.last_last_action = np.zeros(self.num_dofs) + self.last_last_last_action = np.zeros(self.num_dofs) + + self.init_pos = list(self.hwi.init_pos.values()) + + self.motor_targets = np.array(self.init_pos.copy()) + self.prev_motor_targets = np.array(self.init_pos.copy()) + + self.last_commands = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + self.paused = self.duck_config.start_paused + + self.command_freq = 20 # hz + if self.commands: + self.xbox_controller = XBoxController(self.command_freq) + + # Reference motion, but we only really need the length of one phase + # TODO + self.PRM = PolyReferenceMotion("./polynomial_coefficients.pkl") + self.imitation_i = 0 + self.imitation_phase = np.array([0, 0]) + self.phase_frequency_factor = 1.0 + self.phase_frequency_factor_offset = ( + self.duck_config.phase_frequency_factor_offset + ) + + # Optional expression features + if self.duck_config.eyes: + self.eyes = Eyes() + if self.duck_config.projector: + self.projector = Projector() + if self.duck_config.speaker: + self.sounds = Sounds( + volume=1.0, sound_directory="../mini_bdx_runtime/assets/" + ) + if self.duck_config.antennas: + self.antennas = Antennas() + + def get_obs(self): + + imu_data = self.imu.get_data() + + dof_pos = self.hwi.get_present_positions( + ignore=[ + "left_antenna", + "right_antenna", + ] + ) # rad + + dof_vel = self.hwi.get_present_velocities( + ignore=[ + "left_antenna", + "right_antenna", + ] + ) # rad/s + + if dof_pos is None or dof_vel is None: + return None + + if len(dof_pos) != self.num_dofs: + print(f"ERROR len(dof_pos) != {self.num_dofs}") + return None + + if len(dof_vel) != self.num_dofs: + print(f"ERROR len(dof_vel) != {self.num_dofs}") + return None + + cmds = self.last_commands + + feet_contacts = self.feet_contacts.get() + + obs = np.concatenate( + [ + imu_data["gyro"], + imu_data["accelero"], + cmds, + dof_pos - self.init_pos, + dof_vel * 0.05, + self.last_action, + self.last_last_action, + self.last_last_last_action, + self.motor_targets, + feet_contacts, + self.imitation_phase, + ] + ) + + return obs + + def start(self): + kps = [self.pid[0]] * 14 + kds = [self.pid[2]] * 14 + + # lower head kps + kps[5:9] = [8, 8, 8, 8] + + self.hwi.set_kps(kps) + self.hwi.set_kds(kds) + self.hwi.turn_on() + + time.sleep(2) + + def get_phase_frequency_factor(self, x_velocity): + + max_phase_frequency = 1.2 + min_phase_frequency = 1.0 + + # Perform linear interpolation + freq = min_phase_frequency + (abs(x_velocity) / 0.15) * ( + max_phase_frequency - min_phase_frequency + ) + + return freq + + def run(self): + i = 0 + try: + print("Starting") + start_t = time.time() + while True: + left_trigger = 0 + right_trigger = 0 + t = time.time() + + if self.commands: + self.last_commands, self.buttons, left_trigger, right_trigger = ( + self.xbox_controller.get_last_command() + ) + if self.buttons.dpad_up.triggered: + self.phase_frequency_factor_offset += 0.05 + print( + f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}" + ) + + if self.buttons.dpad_down.triggered: + self.phase_frequency_factor_offset -= 0.05 + print( + f"Phase frequency factor offset {round(self.phase_frequency_factor_offset, 3)}" + ) + + if self.buttons.LB.is_pressed: + self.phase_frequency_factor = 1.3 + else: + self.phase_frequency_factor = 1.0 + + if self.buttons.X.triggered: + if self.duck_config.projector: + self.projector.switch() + + if self.buttons.B.triggered: + if self.duck_config.speaker: + self.sounds.play_random_sound() + + if self.duck_config.antennas: + self.antennas.set_position_left(right_trigger) + self.antennas.set_position_right(left_trigger) + + if self.buttons.A.triggered: + self.paused = not self.paused + if self.paused: + print("PAUSE") + else: + print("UNPAUSE") + + if self.paused: + time.sleep(0.1) + continue + + obs = self.get_obs() + if obs is None: + continue + + self.imitation_i += 1 * ( + self.phase_frequency_factor + self.phase_frequency_factor_offset + ) + self.imitation_i = self.imitation_i % self.PRM.nb_steps_in_period + self.imitation_phase = np.array( + [ + np.cos( + self.imitation_i / self.PRM.nb_steps_in_period * 2 * np.pi + ), + np.sin( + self.imitation_i / self.PRM.nb_steps_in_period * 2 * np.pi + ), + ] + ) + + if self.save_obs: + self.saved_obs.append(obs) + + if self.replay_obs is not None: + if i < len(self.replay_obs): + obs = self.replay_obs[i] + else: + print("BREAKING ") + break + + action = self.policy.infer(obs) + + self.last_last_last_action = self.last_last_action.copy() + self.last_last_action = self.last_action.copy() + self.last_action = action.copy() + + # action = np.zeros(10) + + self.motor_targets = self.init_pos + action * self.action_scale + + # self.motor_targets = np.clip( + # self.motor_targets, + # self.prev_motor_targets + # - self.max_motor_velocity * (1 / self.control_freq), # control dt + # self.prev_motor_targets + # + self.max_motor_velocity * (1 / self.control_freq), # control dt + # ) + + if self.action_filter is not None: + self.action_filter.push(self.motor_targets) + filtered_motor_targets = self.action_filter.get_filtered_action() + if ( + time.time() - start_t > 1 + ): # give time to the filter to stabilize + self.motor_targets = filtered_motor_targets + + self.prev_motor_targets = self.motor_targets.copy() + + head_motor_targets = self.last_commands[3:] + self.motor_targets[5:9] + self.motor_targets[5:9] = head_motor_targets + + action_dict = make_action_dict( + self.motor_targets, list(self.hwi.joints.keys()) + ) + + self.hwi.set_position_all(action_dict) + + i += 1 + + took = time.time() - t + # print("Full loop took", took, "fps : ", np.around(1 / took, 2)) + if (1 / self.control_freq - took) < 0: + print( + "Policy control budget exceeded by", + np.around(took - 1 / self.control_freq, 3), + ) + time.sleep(max(0, 1 / self.control_freq - took)) + + except KeyboardInterrupt: + if self.duck_config.antennas: + self.antennas.stop() + if self.duck_config.eyes: + self.eyes.stop() + if self.duck_config.projector: + self.projector.stop() + self.feet_contacts.stop() + + if self.save_obs: + pickle.dump(self.saved_obs, open("robot_saved_obs.pkl", "wb")) + print("TURNING OFF") + + +if __name__ == "__main__": + import argparse + + parser = argparse.ArgumentParser() + parser.add_argument("--onnx_model_path", type=str, required=True) + parser.add_argument( + "--duck_config_path", + type=str, + required=False, + default=f"{HOME_DIR}/duck_config.json", + ) + parser.add_argument("-a", "--action_scale", type=float, default=0.25) + parser.add_argument("-p", type=int, default=30) + parser.add_argument("-i", type=int, default=0) + parser.add_argument("-d", type=int, default=0) + parser.add_argument("-c", "--control_freq", type=int, default=50) + parser.add_argument("--pitch_bias", type=float, default=0, help="deg") + parser.add_argument( + "--commands", + action="store_true", + default=True, + help="external commands, keyboard or gamepad. Launch control_server.py on host computer", + ) + parser.add_argument( + "--save_obs", + type=str, + required=False, + default=False, + help="save the run's observations", + ) + parser.add_argument( + "--replay_obs", + type=str, + required=False, + default=None, + help="replay the observations from a previous run (can be from the robot or from mujoco)", + ) + parser.add_argument("--cutoff_frequency", type=float, default=None) + + args = parser.parse_args() + pid = [args.p, args.i, args.d] + + print("Done parsing args") + rl_walk = RLWalk( + args.onnx_model_path, + duck_config_path=args.duck_config_path, + action_scale=args.action_scale, + pid=pid, + control_freq=args.control_freq, + commands=args.commands, + pitch_bias=args.pitch_bias, + save_obs=args.save_obs, + replay_obs=args.replay_obs, + cutoff_frequency=args.cutoff_frequency, + ) + print("Done instantiating RLWalk") + rl_walk.run() diff --git a/Open_Duck_Mini_Runtime-2/setup.cfg b/Open_Duck_Mini_Runtime-2/setup.cfg new file mode 100644 index 0000000..0eb07fa --- /dev/null +++ b/Open_Duck_Mini_Runtime-2/setup.cfg @@ -0,0 +1,38 @@ +[metadata] +name = mini-bdx-runtime +version = 0.0.1 +author = Antoine Pirrone +author_email = antoine.pirrone@gmail.com +url = https://github.com/apirrone/mini_BDX_runtime +description = Runtime code for mini BDX +long_description = file: README.md +long_description_content_type = text/markdown + + +[options] +packages = find: +zip_safe = True +include_package_data = True +package_dir= + =mini_bdx_runtime +install_requires = + rustypot==0.1.0 + onnxruntime==1.18.1 + numpy==1.26.4 + adafruit-circuitpython-bno055==5.4.13 + scipy==1.15.1 + pygame==2.6.0 + pypot @ git+https://github.com/pollen-robotics/pypot@support-feetech-sts3215 + openai==1.70.0 + + # adafruit_extended_bus + +[options.packages.find] +where=mini_bdx_runtime + +[options.package_data] + +[options.extras_require] + +[options.entry_points] +console_scripts =