commit 4cc91d770815027cd53123a3b9c250066c3b357c Author: “jiawei” <“1073198597@qq.com”> Date: Sat Aug 9 15:53:07 2025 +0800 first commit diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/.gitignore b/Open_Duck_Mini/Open_Duck_Mini-2/.gitignore new file mode 100644 index 0000000..786bdaf --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/FUNDING.yml b/Open_Duck_Mini/Open_Duck_Mini-2/FUNDING.yml new file mode 100644 index 0000000..22a38ad --- /dev/null +++ b/Open_Duck_Mini/Open_Duck_Mini-2/FUNDING.yml @@ -0,0 +1,3 @@ +# These are supported funding model platforms + +github: apirrone diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/LICENSE b/Open_Duck_Mini/Open_Duck_Mini-2/LICENSE new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/README.md b/Open_Duck_Mini/Open_Duck_Mini-2/README.md new file mode 100644 index 0000000..0becdbe --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/assembly_guide.md b/Open_Duck_Mini/Open_Duck_Mini-2/docs/assembly_guide.md new file mode 100644 index 0000000..e264fe5 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/configure_motors.md b/Open_Duck_Mini/Open_Duck_Mini-2/docs/configure_motors.md new file mode 100644 index 0000000..a2bf15a --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/feetech_identification.md b/Open_Duck_Mini/Open_Duck_Mini-2/docs/feetech_identification.md new file mode 100644 index 0000000..c9dbd45 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/head_schematic.xcf b/Open_Duck_Mini/Open_Duck_Mini-2/docs/head_schematic.xcf new file mode 100644 index 0000000..0141713 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/docs/head_schematic.xcf differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio @@ -0,0 +1,396 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/docs/prepare_robot.md b/Open_Duck_Mini/Open_Duck_Mini-2/docs/prepare_robot.md new file mode 100644 index 0000000..4a4f0c7 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/print_guide.md b/Open_Duck_Mini/Open_Duck_Mini-2/docs/print_guide.md new file mode 100644 index 0000000..aa64eba --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/sim2real.md b/Open_Duck_Mini/Open_Duck_Mini-2/docs/sim2real.md new file mode 100644 index 0000000..1cce56b --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/docs/wiring.drawio b/Open_Duck_Mini/Open_Duck_Mini-2/docs/wiring.drawio new file mode 100644 index 0000000..f533332 --- /dev/null +++ b/Open_Duck_Mini/Open_Duck_Mini-2/docs/wiring.drawio @@ -0,0 +1,503 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/docs/wiring.png b/Open_Duck_Mini/Open_Duck_Mini-2/docs/wiring.png new file mode 100644 index 0000000..2c60f39 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/docs/wiring.png differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/experiments/LeRobot/README.md b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/LeRobot/README.md new file mode 100644 index 0000000..3ce568b --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py new file mode 100644 index 0000000..c7f56ff --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/README.md b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/README.md new file mode 100644 index 0000000..6e99a1d --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/env.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/env.py new file mode 100644 index 0000000..de51cc4 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/env_humanoid.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/env_humanoid.py new file mode 100644 index 0000000..0b68c2c --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/.gitignore b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/.gitignore new file mode 100644 index 0000000..314f02b --- /dev/null +++ b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/.gitignore @@ -0,0 +1 @@ +*.txt \ No newline at end of file diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/env.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/env.py new file mode 100644 index 0000000..16bd3fb --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/eval.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/eval.py new file mode 100644 index 0000000..8085b15 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py new file mode 100644 index 0000000..c168b41 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py new file mode 100644 index 0000000..d8d9e91 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py new file mode 100644 index 0000000..5a2dfa2 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py new file mode 100644 index 0000000..126ec58 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py new file mode 100644 index 0000000..9a8ca50 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py new file mode 100644 index 0000000..e898ddc --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py new file mode 100644 index 0000000..b4eb728 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/RL/new/simple_env.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/simple_env.py new file mode 100644 index 0000000..8d600ec --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/new/train.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/new/train.py new file mode 100644 index 0000000..fb7dee7 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/old_test.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/old_test.py new file mode 100644 index 0000000..a28bcdc --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/play_policy.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/play_policy.py new file mode 100644 index 0000000..f62c360 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py new file mode 100644 index 0000000..9dc98b0 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py new file mode 100644 index 0000000..3aaae72 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py new file mode 100644 index 0000000..7007671 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/record_episodes.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/record_episodes.py new file mode 100644 index 0000000..0fa2250 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/replay_episodes.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/replay_episodes.py new file mode 100644 index 0000000..2b09382 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/train.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/train.py new file mode 100644 index 0000000..ffa1f8d --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/RL/view_hdf5.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/RL/view_hdf5.py new file mode 100644 index 0000000..27e7086 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py new file mode 100644 index 0000000..b65c0fe --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/identification/check_speed.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/identification/check_speed.py new file mode 100644 index 0000000..ce20a01 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/identification/get_data.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/identification/get_data.py new file mode 100644 index 0000000..0fdca7a --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/identification/plot.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/identification/plot.py new file mode 100644 index 0000000..0772021 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py new file mode 100644 index 0000000..96692a9 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/identification/plot_obs.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/identification/plot_obs.py new file mode 100644 index 0000000..f9f12a1 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/identification/plot_speeds.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/identification/plot_speeds.py new file mode 100644 index 0000000..a5c48cc --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/identification/utils.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/identification/utils.py new file mode 100644 index 0000000..2919a7b --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/mujoco/.gitignore b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/mujoco/.gitignore new file mode 100644 index 0000000..fac783a --- /dev/null +++ b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/mujoco/.gitignore @@ -0,0 +1 @@ +*.onnx \ No newline at end of file diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py new file mode 100644 index 0000000..04c6cbf --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py new file mode 100644 index 0000000..35e7e27 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py new file mode 100644 index 0000000..59f0603 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py new file mode 100644 index 0000000..fe445b7 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/.gitignore b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/.gitignore new file mode 100644 index 0000000..8587f59 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py new file mode 100644 index 0000000..da2b1fb --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/bdx_walk.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/bdx_walk.py new file mode 100644 index 0000000..5391892 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py new file mode 100644 index 0000000..eea7eeb --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py new file mode 100644 index 0000000..62bcabe --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py new file mode 100644 index 0000000..56cfb7a --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py new file mode 100644 index 0000000..6a8aee3 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py new file mode 100644 index 0000000..5c12b53 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/replay_amp.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/replay_amp.py new file mode 100644 index 0000000..150e48c --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/test_bdx.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/test_bdx.py new file mode 100644 index 0000000..3479ba3 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py new file mode 100644 index 0000000..a9eb1f4 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py new file mode 100644 index 0000000..5bb5f64 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/move_test.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/move_test.py new file mode 100644 index 0000000..a097192 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/new_run.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/new_run.py new file mode 100644 index 0000000..32f826c --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/plot.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/plot.py new file mode 100644 index 0000000..d466892 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py new file mode 100644 index 0000000..a667469 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py new file mode 100644 index 0000000..3add6cd --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py new file mode 100644 index 0000000..1329134 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py new file mode 100644 index 0000000..5b3f99e --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py new file mode 100644 index 0000000..a9e0de6 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/run.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/run.py new file mode 100644 index 0000000..ead6d90 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/real_robot/utils.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/real_robot/utils.py new file mode 100644 index 0000000..f3ecf4c --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/bench_com_time.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/bench_com_time.py new file mode 100644 index 0000000..7da04e8 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/configure_motors.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/configure_motors.py new file mode 100644 index 0000000..141b67a --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/identification.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/identification.py new file mode 100644 index 0000000..f19f605 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py new file mode 100644 index 0000000..9bb8de3 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py new file mode 100644 index 0000000..b47a717 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/v2/params_m6.json b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/params_m6.json new file mode 100644 index 0000000..684ca71 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/placo_defaults.json b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/placo_defaults.json new file mode 100644 index 0000000..cba3d00 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py new file mode 100644 index 0000000..0dd057f --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py new file mode 100644 index 0000000..b3ec714 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/plot.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/plot.py new file mode 100644 index 0000000..ea07bc0 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py new file mode 100644 index 0000000..14035a9 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ new file mode 100644 index 0000000..e67326d --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py new file mode 100644 index 0000000..b9ee743 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/test.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/test.py new file mode 100644 index 0000000..6198845 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/v2/test_understand_isaac_mujoco_transfer_motor_control.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/experiments/v2/walk_test.py b/Open_Duck_Mini/Open_Duck_Mini-2/experiments/v2/walk_test.py new file mode 100644 index 0000000..4ab6f54 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/__init__.py b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/mini_bdx/__init__.py b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/mini_bdx/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/__init__.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/old_walk_engine/walk_engine.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/__init__.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/placo_walk_engine/placo_walk_engine.py.bak b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/__init__.py new file mode 100644 index 0000000..faf8926 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/mujoco_utils.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/poly_spline.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/rl_utils.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/mini_bdx/utils/xbox_controller.py b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/.gitignore b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/.gitignore new file mode 100644 index 0000000..9c9cd7b --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/.gitignore~ b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/.gitignore~ new file mode 100644 index 0000000..e69de29 diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.part new file mode 100644 index 0000000..f06679f --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/39281023.prt.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.part new file mode 100644 index 0000000..3d84899 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_left.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_holder_right.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_motor_holder.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.part new file mode 100644 index 0000000..28a1b78 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/antenna_tip.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/axis_to_axis.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/block_to_axis.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.part new file mode 100644 index 0000000..5adf5a2 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/bms.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.part new file mode 100644 index 0000000..834b727 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/bno055.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.part new file mode 100644 index 0000000..dacfd4b --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/board.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.part new file mode 100644 index 0000000..bf178fb --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/body_part.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.part new file mode 100644 index 0000000..3dcb855 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_back.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.part new file mode 100644 index 0000000..082d252 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_bottom_battery_hold.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.part new file mode 100644 index 0000000..c5c118b --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cage_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.part new file mode 100644 index 0000000..0c5f048 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/cell.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/config.json new file mode 100644 index 0000000..49a9eac --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_b_dummy.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_f_dummy.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_case_m_dummy.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_dummy.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/dc15_a01_horn_idle2_dummy.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.part new file mode 100644 index 0000000..aa6358d --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/double_u.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.part new file mode 100644 index 0000000..9b3b38b --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.part new file mode 100644 index 0000000..98cd544 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/foot_contact.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.part new file mode 100644 index 0000000..366aa6d --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/front_cover.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.part new file mode 100644 index 0000000..e5d1eee --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/head.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/head_roll_pitch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/hip_right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.part new file mode 100644 index 0000000..9224de8 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/holder_u2d2_power_hub.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.1.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b3b-eh-a.prt.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.1.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-eh-a.prt.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/jst-b4b-ph-k-s.prt.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/micro_usb_2_0_connector__ab_rec.prt.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/pcb_u2d2.prt.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.part new file mode 100644 index 0000000..98053ba --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/power_switch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.part new file mode 100644 index 0000000..d14b889 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/rasp_spacer.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.part new file mode 100644 index 0000000..e93fb09 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/raspberrypizerow.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_head_link.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.part new file mode 100644 index 0000000..4ddbedc --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/renfort_leg.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.urdf new file mode 100644 index 0000000..16f9f19 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml new file mode 100644 index 0000000..403e3ae --- /dev/null +++ b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/robot.xml @@ -0,0 +1,305 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml new file mode 100644 index 0000000..01f1a02 --- /dev/null +++ b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/scene.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.part new file mode 100644 index 0000000..cbe4d1f --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/sg90.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part b/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.part new file mode 100644 index 0000000..dba4834 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/spacer.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/u2d2_casing.prt.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/bdx/usb_c_charger.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/antenna.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/battery_pack_lid.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bms.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/bno055.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/board.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_back.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_front.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_bottom.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/body_middle_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/cell.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/config.json b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/drive_palonier.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_pla.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_bottom_tpu.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_side.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/foot_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_bot_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_pitch_to_yaw.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_roll_mount.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/head_yaw_to_roll.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/holder.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_antenna_holder.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_cache.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_left_sheet.stl b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_knee_to_ankle_right_sheet.stl b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/leg_spacer.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_left_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/neck_right_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/passive_palonier.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/power_switch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/raspberrypizerow.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_antenna_holder.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_cache.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.urdf b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot.xml @@ -0,0 +1,303 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/robot_motors.xml b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_bearing.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_bottom.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/roll_motor_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/scene_position.xml @@ -0,0 +1,32 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/sg90.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_bottom.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/trunk_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/usb_c_charger.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0122topcabinetcase_95.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0123middlecase_56.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.part b/Open_Duck_Mini/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/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/mini_bdx/robots/open_duck_mini_v2/wj-wk00-0124bottomcase_45.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/battery_pack_lid.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/battery_pack_lid.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/body_back.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/body_back.stl new file mode 100644 index 0000000..564b8c2 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/body_back.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/body_front.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/body_front.stl new file mode 100644 index 0000000..ec3a048 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/body_front.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/body_middle_bottom.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/body_middle_bottom.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/body_middle_top.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/body_middle_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/bulb.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/bulb.stl new file mode 100644 index 0000000..93f7d48 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/bulb.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/flash_light_module.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/flash_light_module.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/flash_reflector_interface.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/flash_reflector_interface.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_bottom_pla.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/foot_bottom_pla.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_bottom_tpu.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/foot_bottom_tpu.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_side.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_side.stl new file mode 100644 index 0000000..55bb289 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_side.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_top.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_top.stl new file mode 100644 index 0000000..134284e Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/foot_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/head.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/head.stl new file mode 100644 index 0000000..ccf7211 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/head.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/head_bot_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/head_bot_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/head_pitch_to_yaw.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/head_roll_mount.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/head_roll_mount.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/head_yaw_to_roll.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/head_yaw_to_roll.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/knee_to_ankle_left_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/knee_to_ankle_right_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/left_antenna_holder.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/left_antenna_holder.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/left_cache.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/left_cache.stl new file mode 100644 index 0000000..0ce5ca2 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/left_cache.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/left_eye.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/left_eye.stl new file mode 100644 index 0000000..6281ea1 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/left_eye.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/left_roll_to_pitch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/left_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/leg_spacer.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/leg_spacer.stl new file mode 100644 index 0000000..c57f8d2 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/leg_spacer.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/neck_left_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/neck_left_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/neck_right_sheet.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/neck_right_sheet.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/right_antenna_holder.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/right_antenna_holder.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/right_cache.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/right_cache.stl new file mode 100644 index 0000000..16c24f0 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/right_cache.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/right_eye.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/right_eye.stl new file mode 100644 index 0000000..185500a Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/right_eye.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/right_roll_to_pitch.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/right_roll_to_pitch.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/roll_motor_bottom.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/roll_motor_bottom.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/roll_motor_top.stl b/Open_Duck_Mini/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/Open_Duck_Mini-2/print/roll_motor_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/speaker_interface.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/speaker_interface.stl new file mode 100644 index 0000000..dd7225e Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/speaker_interface.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/speaker_stand.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/speaker_stand.stl new file mode 100644 index 0000000..4265223 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/speaker_stand.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/trunk_bottom.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/trunk_bottom.stl new file mode 100644 index 0000000..a9545d1 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/trunk_bottom.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/print/trunk_top.stl b/Open_Duck_Mini/Open_Duck_Mini-2/print/trunk_top.stl new file mode 100644 index 0000000..4fd8711 Binary files /dev/null and b/Open_Duck_Mini/Open_Duck_Mini-2/print/trunk_top.stl differ diff --git a/Open_Duck_Mini/Open_Duck_Mini-2/pyproject.toml b/Open_Duck_Mini/Open_Duck_Mini-2/pyproject.toml new file mode 100644 index 0000000..7fd26b9 --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/setup.cfg b/Open_Duck_Mini/Open_Duck_Mini-2/setup.cfg new file mode 100644 index 0000000..a50de1d --- /dev/null +++ b/Open_Duck_Mini/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/Open_Duck_Mini-2/thanks.md b/Open_Duck_Mini/Open_Duck_Mini-2/thanks.md new file mode 100644 index 0000000..30f984e --- /dev/null +++ b/Open_Duck_Mini/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