ZhaiShuaiShuai/OPEN_DUCK_MINI
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit

Browse Source
This commit is contained in:
翟帅帅 2025-07-28 12:35:44 +08:00
commit 693921b622
802 changed files with 47534 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
Open_Duck_Mini-2/.gitignore vendored Normal file
View File

@ -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

3
Open_Duck_Mini-2/FUNDING.yml Normal file
View File

@ -0,0 +1,3 @@
# These are supported funding model platforms
github: apirrone

201
Open_Duck_Mini-2/LICENSE Normal file
View File

@ -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.

11
Open_Duck_Mini-2/Open_Duck_Mini-2/.gitignore vendored Normal file
View File

@ -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

3
Open_Duck_Mini-2/Open_Duck_Mini-2/FUNDING.yml Normal file
View File

@ -0,0 +1,3 @@
# These are supported funding model platforms
github: apirrone

201
Open_Duck_Mini-2/Open_Duck_Mini-2/LICENSE Normal file
View File

@ -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.

97
Open_Duck_Mini-2/Open_Duck_Mini-2/README.md Normal file
View File

@ -0,0 +1,97 @@
# Open Duck Mini v2
<table>
<tr>
<td> <img src="https://github.com/user-attachments/assets/2a407765-70ad-48dd-8a5d-488f82503716" alt="1" width="300px" ></td>
<td> <img src="https://github.com/user-attachments/assets/3b8fe350-73a9-4c9f-ad29-efc781be7aee" alt="2" width="300px" ></td>
<td> <img src="https://github.com/user-attachments/assets/fd7e5949-1492-4d31-851f-feaa9b695557" alt="3" width="300px" ></td>
</tr>
</table>
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 !

251
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/assembly_guide.md Normal file
View File

@ -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.
<img src="https://github.com/user-attachments/assets/9ed8591a-7c96-4410-8d7e-9b7d88c6bd1f" alt="1" width="500px" >
Then assamble `trunk_bottom` and `trunk_top`, and screw them together with 2 `M3x10` screws through these holes
<img src="https://github.com/user-attachments/assets/ae36b396-a34a-4691-8e62-fd916cd1f76c" alt="1" width="500px" >
Mount the middle motor like so and screw it with the plastic screws that came with the motors :
<img src="https://github.com/user-attachments/assets/d4a6ba6c-852f-440e-afb7-3b9ca66ad3dc" alt="1" width="500px" >
Insert `roll_motor_bottom` like this
<img src="https://github.com/user-attachments/assets/31c031b7-58b8-4c4c-a3fa-1f14a310c2f2" alt="1" width="500px" >
### Assemble the feet
Both feet are the same.
First, assemble `foot_bottom_tpu` with `foot_bottom_pla`. Insert M3 inserts in these holes :
<img src="https://github.com/user-attachments/assets/6749a5ba-cea9-4b0a-ac32-f32e130fd057" alt="1" width="500px" >
And screw the two parts together with two `M3x6` screws.
Then, insert M3 inserts in these holes in `foot_top` here :
<img src="https://github.com/user-attachments/assets/1a77f2f8-56ea-43d2-91c7-78130456c45b" alt="1" width="500px" >
And assemble everything like so. Make sure the driver side of the motor is on the `foot_top` part side :
<table>
<tr>
<td> <img src="https://github.com/user-attachments/assets/197cd9b7-05a6-46ed-9af5-d2def37970c8" alt="1" width="500px" > </td>
<td> <img src="https://github.com/user-attachments/assets/f532fbcc-100b-4715-96a8-66697e4fda26" alt="1" width="500px" > </td>
</tr>
</table>
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
<table>
<tr>
<td> <img src="https://github.com/user-attachments/assets/71b9ad91-864e-423f-97a3-911d65182ab9" alt="1" width="500px" > </td>
<td> <img src="https://github.com/user-attachments/assets/59e15c7c-3515-4408-bfe9-80e30c4cd007" alt="1" width="500px" > </td>
</tr>
</table>
### Assemble the shins
Insert M3 Inserts in these holes of `leg_spacer` (on both sides. Insert 4 M3 inserts in total) :
<img src="https://github.com/user-attachments/assets/41eb01a2-d6f9-43b1-a83e-d14785907425" alt="1" width="500px" >
Then, first plug the motor cable in the foot's motor, and make it go through the `right_sheet` like so
<img src="https://github.com/user-attachments/assets/a1432505-6b6c-4765-aea6-0a7a0b8b220b" alt="1" width="500px" >
Then assemble like below:
<img src="https://github.com/user-attachments/assets/54615217-fec9-46dd-8b40-e21a4f527b55" alt="1" width="500px" >
### 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)
<img src="https://github.com/user-attachments/assets/951157a4-26dc-41bb-b97f-18dbbb0c1cd3" alt="1" width="500px" >
### 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.
<img src="https://github.com/user-attachments/assets/368a85ad-1c58-4db2-bdd0-812334b6c784" alt="1" width="500px" >
Mount `roll_motor_top` to the `hip_yaw` servo (screw from the bottom). Don't mount the servo to the trunk yet.
<img src="https://github.com/user-attachments/assets/768f1664-030c-4432-9071-c52d79d3ef6b" alt="1" width="500px" >
Then mount `hip_roll` like this
<img src="https://github.com/user-attachments/assets/e1287f67-320c-42cf-a5dc-84f4c64abb17" alt="1" width="500px" >
And insert the sub assembly like this
<img src="https://github.com/user-attachments/assets/dba458ae-e9cb-4e16-a82e-cbe74d2350fa" alt="1" width="500px" >
Screw everything you can (with the plastic screws provided with the servos)
You can now mount the leg like this :
<img src="https://github.com/user-attachments/assets/a7169791-9f0a-4827-aea7-00ab1d0f6212" alt="1" width="500px" >
And do the same for the other leg :)
Your duck should now look like this
<img src="https://github.com/user-attachments/assets/4921b29b-b38b-423d-9da1-1c8f84689e84" alt="1" width="500px" >
### Assemble the neck
You know the drill
<img src="https://github.com/user-attachments/assets/f96fe44f-a7bc-423e-a925-4aef3e7bf568" alt="1" width="500px" >
### Assemble the head mechanism
First, mount `head_pitch_to_yaw` like this
<img src="https://github.com/user-attachments/assets/a4dba395-eb0d-4150-8980-8e14f1173d02" alt="1" width="500px" >
Then, independently mount `head_yaw_to_roll` and `head_roll_mount` to `head_roll dof`
<img src="https://github.com/user-attachments/assets/4648cd6c-391e-41e4-9617-4ecebfa9215b" alt="1" width="500px" >
(You can insert `head_bot_plate` and `body_middle_top` now too to avoid having to disassamble the head later)
Then
<img src="https://github.com/user-attachments/assets/03f3bdae-06c3-4c37-b68f-14587edd6123" alt="1" width="500px" >
Then
<img src="https://github.com/user-attachments/assets/04a219d8-bbaf-4910-b5d3-7ca67bce466c" alt="1" width="500px" >
Your duck should now look like this
<img src="https://github.com/user-attachments/assets/71545d40-f0f5-411d-a8d5-1cd676a74e75" alt="1" width="500px" >
### 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
<table>
<tr>
<td> <img src="https://github.com/user-attachments/assets/8772996d-5906-48fa-8cbe-6b6823982375" alt="1" width="500px" ></td>
<td> <img src="https://github.com/user-attachments/assets/172f44c7-c7c6-47f8-894a-14024abd24f4" alt="2" width="500px" ></td>
</tr>
</table>
## Electronics
Here is the global electonics schematic for reference
<table>
<tr>
<td> <img src="open_duck_mini_v2_wiring_diagram.png" alt="1" width="500px" ></td>
<td> <img src="wiring.png" alt="2" width="500px" ></td>
</tr>
</table>
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.
<table>
<tr>
<td> <img src="https://github.com/user-attachments/assets/371db809-7cb3-47e1-b277-7fc2bdf21025" alt="1" width="500px" ></td>
<td> <img src="https://github.com/user-attachments/assets/3acfa4e6-ecf7-41f6-a965-35a3040f52fb" alt="2" width="500px" ></td>
</tr>
</table>
### 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
<table>
<tr>
<td> <img src="https://github.com/user-attachments/assets/679a9cd2-89ca-41e8-9d03-f93fc040068b" alt="1" width="500px" ></td>
<td> <img src="https://github.com/user-attachments/assets/ca292c48-1c72-4649-b7d1-4d3c9eac62ac" alt="2" width="500px" ></td>
</tr>
</table>
Et voila :)
<table>
<tr>
<td> <img src="https://github.com/user-attachments/assets/312ec747-8eb4-4145-92eb-c1c3ddba80da" alt="1" width="500px" ></td>
<td> <img src="https://github.com/user-attachments/assets/864d9dd7-3daf-4e63-913c-2e99157e4aaf" alt="2" width="500px" ></td>
</tr>
</table>
> 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)

40
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/configure_motors.md Normal file
View File

@ -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 <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,
}
```

29
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/feetech_identification.md Normal file
View File

@ -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 }$

BIN
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/head_schematic.xcf Normal file
View File

Binary file not shown.

BIN
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v1_wiring_diagram.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 50 KiB

396
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.drawio Normal file
View File

@ -0,0 +1,396 @@
<mxfile host="app.diagrams.net" agent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/131.0.0.0 Safari/537.36" version="26.0.4">
<diagram name="Page-1" id="qsV2s4hs_2FrfZJGvVU3">
<mxGraphModel dx="1838" dy="1045" grid="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" page="1" pageScale="1" pageWidth="827" pageHeight="1169" math="0" shadow="0">
<root>
<mxCell id="0" />
<mxCell id="1" parent="0" />
<mxCell id="-YBQGZOib8_rkUtJKQvK-69" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1421" y="40" width="679" height="720" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-67" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1421" y="780" width="679" height="720" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-45" value="&amp;nbsp;" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="380" y="540" width="685" height="210" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-7" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;entryX=0;entryY=0.25;entryDx=0;entryDy=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-13" target="-YBQGZOib8_rkUtJKQvK-19" edge="1">
<mxGeometry relative="1" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-8" value="+" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-7" vertex="1" connectable="0">
<mxGeometry x="-0.7381" y="-1" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-9" value="B+" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-7" vertex="1" connectable="0">
<mxGeometry x="0.7286" y="-1" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-10" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.75;exitDx=0;exitDy=0;entryX=0;entryY=0.75;entryDx=0;entryDy=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-13" target="-YBQGZOib8_rkUtJKQvK-19" edge="1">
<mxGeometry relative="1" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-11" value="-" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-10" vertex="1" connectable="0">
<mxGeometry x="-0.7286" y="-2" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-12" value="B-" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-10" vertex="1" connectable="0">
<mxGeometry x="0.6714" y="-1" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-13" value="18650 x2 series" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="430" y="670" width="120" height="60" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-14" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.25;exitDx=0;exitDy=0;entryX=0;entryY=0.25;entryDx=0;entryDy=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-19" target="-YBQGZOib8_rkUtJKQvK-27" edge="1">
<mxGeometry relative="1" as="geometry">
<mxPoint x="1442.2399999999998" y="684.6800000000001" as="targetPoint" />
<Array as="points">
<mxPoint x="1230" y="685" />
<mxPoint x="1230" y="675" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-15" value="P+" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-14" vertex="1" connectable="0">
<mxGeometry x="-0.6714" relative="1" as="geometry">
<mxPoint x="-90" as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-54" value="P+" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-14" vertex="1" connectable="0">
<mxGeometry x="-0.9566" y="-1" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-17" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.75;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;" parent="1" target="-YBQGZOib8_rkUtJKQvK-38" edge="1">
<mxGeometry relative="1" as="geometry">
<mxPoint x="790" y="714.52" as="sourcePoint" />
<mxPoint x="910" y="714.52" as="targetPoint" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-18" value="P-" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-17" vertex="1" connectable="0">
<mxGeometry x="-0.6333" y="1" relative="1" as="geometry">
<mxPoint x="-16" y="1" as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-19" value="BMS" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="670" y="670" width="120" height="60" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-20" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.75;exitDx=0;exitDy=0;strokeWidth=4;fillColor=#d5e8d4;strokeColor=#82b366;" parent="1" source="-YBQGZOib8_rkUtJKQvK-24" edge="1">
<mxGeometry relative="1" as="geometry">
<mxPoint x="820" y="715" as="targetPoint" />
<mxPoint x="890" y="600" as="sourcePoint" />
<Array as="points">
<mxPoint x="910" y="600" />
<mxPoint x="821" y="600" />
<mxPoint x="821" y="715" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-21" value="&amp;nbsp;-&amp;nbsp;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-20" vertex="1" connectable="0">
<mxGeometry x="-0.8049" y="-1" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-22" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=-0.017;exitY=0.326;exitDx=0;exitDy=0;exitPerimeter=0;strokeWidth=4;fillColor=#d5e8d4;strokeColor=#82b366;" parent="1" source="-YBQGZOib8_rkUtJKQvK-24" edge="1">
<mxGeometry relative="1" as="geometry">
<mxPoint x="810" y="685" as="targetPoint" />
<mxPoint x="890" y="570" as="sourcePoint" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-23" value="&amp;nbsp;+&amp;nbsp;" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-22" vertex="1" connectable="0">
<mxGeometry x="-0.8129" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-24" value="USB-C charger" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="930" y="550" width="120" height="60" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-27" value="UBEC" style="rounded=0;whiteSpace=wrap;html=1;fillColor=#d5e8d4;strokeColor=#82b366;" parent="1" vertex="1">
<mxGeometry x="1460" y="660" width="120" height="60" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-28" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=0;exitY=0.25;exitDx=0;exitDy=0;entryX=0.921;entryY=0.007;entryDx=0;entryDy=0;entryPerimeter=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-13" target="-YBQGZOib8_rkUtJKQvK-19" edge="1">
<mxGeometry relative="1" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-29" value="BM" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-28" vertex="1" connectable="0">
<mxGeometry x="0.8678" y="2" relative="1" as="geometry">
<mxPoint as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-30" value="Between the two cells" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-28" vertex="1" connectable="0">
<mxGeometry x="-0.9577" y="-1" relative="1" as="geometry">
<mxPoint x="20" y="-24" as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-32" value="Raspberry pi zero 2W" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1">
<mxGeometry x="1520" y="310" width="340" height="140" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-33" value="7.4V" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
<mxGeometry x="580" y="685" width="60" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-34" value="7.4V out" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
<mxGeometry x="1070" y="685" width="60" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-36" style="edgeStyle=orthogonalEdgeStyle;rounded=0;orthogonalLoop=1;jettySize=auto;html=1;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.75;entryDx=0;entryDy=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-38" target="-YBQGZOib8_rkUtJKQvK-27" edge="1">
<mxGeometry relative="1" as="geometry">
<mxPoint x="900" y="715" as="targetPoint" />
<Array as="points">
<mxPoint x="1240" y="715" />
<mxPoint x="1240" y="705" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-43" value="micro usb" style="rounded=1;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1659.9" y="430" width="60.1" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-44" value="micro usb" style="rounded=1;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1559.9" y="430" width="60.1" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-46" value="&lt;font style=&quot;font-size: 18px;&quot;&gt;Battery pack&lt;/font&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
<mxGeometry x="360" y="540" width="170" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-47" value="usb-c" style="rounded=1;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1040" y="560" width="40" height="40" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-48" value="Motor control board" style="whiteSpace=wrap;html=1;aspect=fixed;fillColor=#d5e8d4;strokeColor=#82b366;" parent="1" vertex="1">
<mxGeometry x="1460" y="800" width="160" height="160" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-49" value="" style="rounded=1;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1590" y="830" width="20" height="40" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-50" value="" style="rounded=1;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1590" y="890" width="20" height="40" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-51" value="usb-c" style="rounded=1;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="1440" y="820" width="40" height="40" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-52" value="7.4V DC in" style="shape=process;whiteSpace=wrap;html=1;backgroundOutline=1;" parent="1" vertex="1">
<mxGeometry x="1440" y="900" width="80" height="40" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-53" value="" style="endArrow=classic;html=1;rounded=0;entryX=0;entryY=1;entryDx=0;entryDy=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;dashed=1;" parent="1" target="-YBQGZOib8_rkUtJKQvK-52" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1320" y="710" as="sourcePoint" />
<mxPoint x="840" y="890" as="targetPoint" />
<Array as="points">
<mxPoint x="1320" y="940" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-55" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.009;entryY=0.154;entryDx=0;entryDy=0;entryPerimeter=0;fillColor=#d5e8d4;strokeColor=#82b366;strokeWidth=4;dashed=1;" parent="1" target="-YBQGZOib8_rkUtJKQvK-52" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1340" y="670" as="sourcePoint" />
<mxPoint x="1470" y="905" as="targetPoint" />
<Array as="points">
<mxPoint x="1340" y="905" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-60" value="" style="endArrow=none;html=1;rounded=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;exitX=0;exitY=0.5;exitDx=0;exitDy=0;strokeColor=#9673a6;strokeWidth=4;fillColor=#e1d5e7;dashed=1;" parent="1" source="-YBQGZOib8_rkUtJKQvK-51" target="-YBQGZOib8_rkUtJKQvK-43" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1430" y="680" as="sourcePoint" />
<mxPoint x="1480" y="630" as="targetPoint" />
<Array as="points">
<mxPoint x="1370" y="840" />
<mxPoint x="1370" y="510" />
<mxPoint x="1690" y="510" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-61" value="IMU" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1">
<mxGeometry x="1460" y="1000" width="120" height="90" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-68" value="&lt;font style=&quot;font-size: 24px;&quot;&gt;Body&lt;/font&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
<mxGeometry x="2030" y="790" width="60" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-70" value="&lt;font style=&quot;font-size: 24px;&quot;&gt;Head&lt;/font&gt;" style="text;html=1;align=center;verticalAlign=middle;whiteSpace=wrap;rounded=0;" parent="1" vertex="1">
<mxGeometry x="2030" y="60" width="60" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-72" value="" style="endArrow=none;html=1;rounded=0;exitX=0;exitY=0.5;exitDx=0;exitDy=0;entryX=0.129;entryY=-0.018;entryDx=0;entryDy=0;entryPerimeter=0;strokeColor=#9673a6;strokeWidth=4;fillColor=#e1d5e7;dashed=1;" parent="1" source="-YBQGZOib8_rkUtJKQvK-61" target="-YBQGZOib8_rkUtJKQvK-32" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1860" y="850" as="sourcePoint" />
<mxPoint x="1910" y="800" as="targetPoint" />
<Array as="points">
<mxPoint x="1280" y="1045" />
<mxPoint x="1280" y="270" />
<mxPoint x="1564" y="270" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-73" value="&lt;font style=&quot;font-size: 18px;&quot;&gt;Left leg&lt;/font&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1750" y="960" width="90" height="450" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-74" value="&lt;font style=&quot;font-size: 18px;&quot;&gt;Right leg&lt;/font&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1970" y="960" width="90" height="450" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-75" value="&lt;font style=&quot;font-size: 18px;&quot;&gt;Neck&lt;/font&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1970" y="190" width="90" height="450" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-76" value="" style="endArrow=none;html=1;rounded=0;strokeWidth=4;fillColor=#1ba1e2;strokeColor=#006EAF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="1" source="-YBQGZOib8_rkUtJKQvK-49" target="-YBQGZOib8_rkUtJKQvK-73" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1840" y="890" as="sourcePoint" />
<mxPoint x="1890" y="840" as="targetPoint" />
<Array as="points">
<mxPoint x="1795" y="850" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-77" value="" style="endArrow=none;html=1;rounded=0;strokeWidth=4;fillColor=#1ba1e2;strokeColor=#006EAF;entryX=0.5;entryY=0;entryDx=0;entryDy=0;" parent="1" target="-YBQGZOib8_rkUtJKQvK-74" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1610" y="910" as="sourcePoint" />
<mxPoint x="1795" y="1020" as="targetPoint" />
<Array as="points">
<mxPoint x="2015" y="910" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-81" value="" style="endArrow=none;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0.844;entryY=0.015;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=4;fillColor=#d5e8d4;strokeColor=#82b366;" parent="1" source="-YBQGZOib8_rkUtJKQvK-27" target="-YBQGZOib8_rkUtJKQvK-32" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1885" y="615" as="sourcePoint" />
<mxPoint x="1935" y="565" as="targetPoint" />
<Array as="points">
<mxPoint x="1920" y="690" />
<mxPoint x="1920" y="270" />
<mxPoint x="1807" y="270" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-82" value="5V out" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];" parent="-YBQGZOib8_rkUtJKQvK-81" vertex="1" connectable="0">
<mxGeometry x="-0.9226" y="4" relative="1" as="geometry">
<mxPoint y="4" as="offset" />
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-84" value="&lt;font style=&quot;font-size: 18px;&quot;&gt;Antenna 1&lt;/font&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1790" y="70" width="110" height="80" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-85" value="&lt;font style=&quot;font-size: 18px;&quot;&gt;Antenna 1&lt;/font&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1910" y="70" width="110" height="80" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-86" value="" style="endArrow=none;html=1;rounded=0;exitX=0.721;exitY=0.008;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.675;entryY=1.016;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;entryPerimeter=0;" parent="1" source="-YBQGZOib8_rkUtJKQvK-32" target="-YBQGZOib8_rkUtJKQvK-84" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1820" y="600" as="sourcePoint" />
<mxPoint x="1870" y="550" as="targetPoint" />
<Array as="points">
<mxPoint x="1780" y="230" />
<mxPoint x="1890" y="220" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-87" value="" style="endArrow=none;html=1;rounded=0;exitX=0.773;exitY=0.008;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-32" target="-YBQGZOib8_rkUtJKQvK-85" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1790" y="309" as="sourcePoint" />
<mxPoint x="1793" y="150" as="targetPoint" />
<Array as="points">
<mxPoint x="1790" y="240" />
<mxPoint x="1900" y="240" />
<mxPoint x="1920" y="170" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-88" value="switch" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1">
<mxGeometry x="1750" y="1410" width="90" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-90" value="switch" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#e1d5e7;strokeColor=#9673a6;" parent="1" vertex="1">
<mxGeometry x="1970" y="1410" width="90" height="30" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-91" value="" style="endArrow=none;html=1;rounded=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.205;entryY=-0.001;entryDx=0;entryDy=0;entryPerimeter=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;dashed=1;" parent="1" source="-YBQGZOib8_rkUtJKQvK-88" target="-YBQGZOib8_rkUtJKQvK-32" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1660" y="1000" as="sourcePoint" />
<mxPoint x="1710" y="950" as="targetPoint" />
<Array as="points">
<mxPoint x="1795" y="1460" />
<mxPoint x="1160" y="1460" />
<mxPoint x="1160" y="200" />
<mxPoint x="1590" y="200" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-92" value="" style="endArrow=none;html=1;rounded=0;exitX=0.5;exitY=1;exitDx=0;exitDy=0;entryX=0.25;entryY=0;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;dashed=1;" parent="1" target="-YBQGZOib8_rkUtJKQvK-32" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="2015" y="1440" as="sourcePoint" />
<mxPoint x="1810" y="310" as="targetPoint" />
<Array as="points">
<mxPoint x="2015" y="1480" />
<mxPoint x="1180" y="1480" />
<mxPoint x="1180" y="210" />
<mxPoint x="1605" y="210" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-93" value="&lt;span style=&quot;font-size: 18px;&quot;&gt;flashlight&lt;/span&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1549.9" y="70" width="110" height="80" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-94" value="" style="endArrow=none;html=1;rounded=0;exitX=0.721;exitY=0.008;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.675;entryY=1.016;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;entryPerimeter=0;" parent="1" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1654" y="310" as="sourcePoint" />
<mxPoint x="1653" y="150" as="targetPoint" />
<Array as="points">
<mxPoint x="1654" y="169" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-95" value="" style="rounded=0;whiteSpace=wrap;html=1;" parent="1" vertex="1">
<mxGeometry x="947.5" y="730" width="15" height="40" as="geometry" />
</mxCell>
<mxCell id="-YBQGZOib8_rkUtJKQvK-38" value="power switch" style="whiteSpace=wrap;html=1;aspect=fixed;" parent="1" vertex="1">
<mxGeometry x="930" y="690" width="50" height="50" as="geometry" />
</mxCell>
<mxCell id="vN0w1t6Y51rWQMxhLmNh-1" value="&lt;span style=&quot;font-size: 18px;&quot;&gt;Speaker&lt;/span&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1429.8" y="110" width="110" height="40" as="geometry" />
</mxCell>
<mxCell id="vN0w1t6Y51rWQMxhLmNh-2" value="" style="endArrow=none;html=1;rounded=0;exitX=0.326;exitY=0.012;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.75;entryY=1;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-32" target="vN0w1t6Y51rWQMxhLmNh-1" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1664" y="320" as="sourcePoint" />
<mxPoint x="1663" y="160" as="targetPoint" />
<Array as="points">
<mxPoint x="1630" y="170" />
<mxPoint x="1512" y="170" />
</Array>
</mxGeometry>
</mxCell>
<mxCell id="QuHCFTCMFr1V1-7T-APS-1" value="&lt;span style=&quot;font-size: 18px;&quot;&gt;Camera&lt;/span&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1430" y="330" width="80" height="100" as="geometry" />
</mxCell>
<mxCell id="QuHCFTCMFr1V1-7T-APS-3" value="&lt;span style=&quot;font-size: 18px;&quot;&gt;Eye LED&lt;/span&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1670" y="80" width="50" height="40" as="geometry" />
</mxCell>
<mxCell id="QuHCFTCMFr1V1-7T-APS-5" value="&lt;span style=&quot;font-size: 18px;&quot;&gt;Eye LED&lt;/span&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1730" y="80" width="50" height="40" as="geometry" />
</mxCell>
<mxCell id="QuHCFTCMFr1V1-7T-APS-6" value="" style="endArrow=none;html=1;rounded=0;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;" parent="1" source="-YBQGZOib8_rkUtJKQvK-32" target="QuHCFTCMFr1V1-7T-APS-3" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1664" y="320" as="sourcePoint" />
<mxPoint x="1663" y="160" as="targetPoint" />
<Array as="points" />
</mxGeometry>
</mxCell>
<mxCell id="QuHCFTCMFr1V1-7T-APS-7" value="" style="endArrow=none;html=1;rounded=0;exitX=0.5;exitY=0;exitDx=0;exitDy=0;entryX=0.5;entryY=1;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;" parent="1" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1745" y="310" as="sourcePoint" />
<mxPoint x="1750" y="120" as="targetPoint" />
<Array as="points" />
</mxGeometry>
</mxCell>
<mxCell id="QuHCFTCMFr1V1-7T-APS-8" value="" style="endArrow=none;html=1;rounded=0;exitX=1;exitY=0.5;exitDx=0;exitDy=0;entryX=0;entryY=0.5;entryDx=0;entryDy=0;fillColor=#e1d5e7;strokeColor=#9673a6;strokeWidth=4;" parent="1" source="QuHCFTCMFr1V1-7T-APS-1" target="-YBQGZOib8_rkUtJKQvK-32" edge="1">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1700" y="320" as="sourcePoint" />
<mxPoint x="1705" y="130" as="targetPoint" />
<Array as="points" />
</mxGeometry>
</mxCell>
<mxCell id="49t0Fgb0Qvd13kqSiM8I-1" value="&lt;span style=&quot;font-size: 18px;&quot;&gt;Mic&lt;/span&gt;" style="rounded=1;whiteSpace=wrap;html=1;fillColor=#dae8fc;strokeColor=#6c8ebf;" parent="1" vertex="1">
<mxGeometry x="1429.8" y="70" width="110" height="40" as="geometry" />
</mxCell>
<mxCell id="9v2C5uotGtLQ3AJdS-sX-1" value="" style="endArrow=none;html=1;rounded=0;strokeWidth=4;fillColor=#1ba1e2;strokeColor=#006EAF;entryX=0.5;entryY=1;entryDx=0;entryDy=0;dashed=1;" edge="1" parent="1" target="-YBQGZOib8_rkUtJKQvK-75">
<mxGeometry width="50" height="50" relative="1" as="geometry">
<mxPoint x="1790" y="1410" as="sourcePoint" />
<mxPoint x="2220" y="710" as="targetPoint" />
<Array as="points">
<mxPoint x="1900" y="1410" />
<mxPoint x="1900" y="720" />
<mxPoint x="2015" y="720" />
</Array>
</mxGeometry>
</mxCell>
</root>
</mxGraphModel>
</diagram>
</mxfile>

BIN
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/open_duck_mini_v2_wiring_diagram.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 195 KiB

123
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/prepare_robot.md Normal file
View File

@ -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
<robot name="...">
<mujoco>
<compiler fusestatic="false"/>
</mujoco>
...
</robot>
```
# 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
<actuator>
<position name="left_hip_yaw" joint="left_hip_yaw" inheritrange="1"/>
<position name="left_hip_roll" joint="left_hip_roll" inheritrange="1"/>
<position name="left_hip_pitch" joint="left_hip_pitch" inheritrange="1"/>
<position name="left_knee" joint="left_knee" inheritrange="1"/>
<position name="left_ankle" joint="left_ankle" inheritrange="1"/>
<position name="right_hip_roll" joint="right_hip_roll" inheritrange="1"/>
<position name="right_hip_yaw" joint="right_hip_yaw" inheritrange="1"/>
<position name="right_hip_pitch" joint="right_hip_pitch" inheritrange="1"/>
<position name="right_knee" joint="right_knee" inheritrange="1"/>
<position name="right_ankle" joint="right_ankle" inheritrange="1"/>
<position name="head_pitch1" joint="head_pitch1" inheritrange="1"/>
<position name="head_pitch2" joint="head_pitch2" inheritrange="1"/>
<position name="head_yaw" joint="head_yaw" inheritrange="1"/>
</actuator>
```
## Add a freejoint
encapsulate the body in a freejoint
```xml
<worldbody>
<body>
<freejoint />
...
...
</body>
</worldbody>
```
## (Optional) Constrain closing loop
Add the following to the mjcf file
```xml
<equality>
<connect body1="closing_<...>_1" body2="closing_<...>_2" anchor="x y z" />
</equality>
```
the x, y, z values can be found in the .urdf
```xml
<joint name="closing_<...>_1_frame" type="fixed">
<origin xyz="x y z" rpy="r p y" />
...
</joint>
```
## Setup collision groups, damping and friction
/!\ remove actuatorfrcrange in joints
Put that inside the <mujoco> bracket
```xml
<mujoco>
<default>
<geom contype="1" conaffinity="1" solref=".004 1" />
<joint damping="0.09" frictionloss="0.1"/>
<position kp="10" forcerange="-5.0 5.0"/>
</default>
...
...
</mujoco>
```
still need to add :
- change frames to sites
## Visualize
```bash
$ python3 -m mujoco.viewer --mjcf=<path>/scene.xml
```
or
```bash
$ <path_to_mujoco_bin>/bin/simulate <path>/scene.xml
```

43
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/print_guide.md Normal file
View File

@ -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

45
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/sim2real.md Normal file
View File

@ -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.

503
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.drawio Normal file
View File

File diff suppressed because one or more lines are too long

BIN
Open_Duck_Mini-2/Open_Duck_Mini-2/docs/wiring.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 8.0 MiB

4
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/README.md Normal file
View File

@ -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.

166
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/new_record_episodes_hdf5_NOT_WORKING.py Normal file
View File

@ -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()

232
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/LeRobot/record_episodes_hdf5.py Normal file
View File

@ -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()

3
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/README.md Normal file
View File

@ -0,0 +1,3 @@
# Experiments
These are undocumented experiments, some of them very old. I wouldn't recommend trying to run them :)

401
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env.py Normal file
View File

@ -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],
]
)

218
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/env_humanoid.py Normal file
View File

@ -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

1
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
*.txt

389
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/env.py Normal file
View File

@ -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],
]
)

130
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval.py Normal file
View File

@ -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)

142
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/eval_simple.py Normal file
View File

@ -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)

384
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/footsteps_env.py Normal file
View File

@ -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(),
]
)

3
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/get_last_from_coach.sh Normal file
View File

@ -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/

258
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/placo_imitate_env.py Normal file
View File

@ -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,
]
)

41
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_bc.py Normal file
View File

@ -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)

90
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/pretrain_gail.py Normal file
View File

@ -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")

79
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes.py Normal file
View File

@ -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)

152
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp.py Normal file
View File

@ -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

132
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/record_episodes_amp_old.py Normal file
View File

@ -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

320
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/simple_env.py Normal file
View File

@ -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(),
]
)

151
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/new/train.py Normal file
View File

@ -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,
)

83
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/old_test.py Normal file
View File

@ -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)

118
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/play_policy.py Normal file
View File

@ -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)

41
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_bc.py Normal file
View File

@ -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)

64
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_dbrm.py Normal file
View File

@ -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")

89
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/pretrain_gail.py Normal file
View File

@ -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")

194
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/record_episodes.py Normal file
View File

@ -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()

54
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/replay_episodes.py Normal file
View File

@ -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()

150
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/train.py Normal file
View File

@ -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,
)

11
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/RL/view_hdf5.py Normal file
View File

@ -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])

116
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/anti_gravity_leg/anti_gravity.py Normal file
View File

@ -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

59
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/check_speed.py Normal file
View File

@ -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"))

146
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/get_data.py Normal file
View File

@ -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)

71
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot.py Normal file
View File

@ -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()

204
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_action_obs.py Normal file
View File

@ -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()

121
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_obs.py Normal file
View File

@ -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()

22
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/plot_speeds.py Normal file
View File

@ -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()

42
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/identification/utils.py Normal file
View File

@ -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()}

1
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
*.onnx

137
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_placo_walk_engine_demo.py Normal file
View File

@ -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()

159
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_record_amp.py Normal file
View File

@ -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

189
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/mujoco_walk_engine.py Normal file
View File

@ -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()

297
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_for_hardware_mujoco.py Normal file
View File

@ -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"))

278
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/onnx_AMP_mujoco.py Normal file
View File

@ -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"))

15
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/mujoco/plot_latent.py Normal file
View File

@ -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()

2
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
test/
placo_walk_examples/

45
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/auto_placo_record_amp.py Normal file
View File

@ -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)

322
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/bdx_walk.py Normal file
View File

@ -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)

56
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/generate_orientations_amp.py Normal file
View File

@ -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)

223
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp.py Normal file
View File

@ -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))

178
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_record_amp_moves.py Normal file
View File

@ -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)

114
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_mujoco.py Normal file
View File

@ -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

51
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/placo_walk_engine_test.py Normal file
View File

@ -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

85
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/replay_amp.py Normal file
View File

@ -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()

88
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_bdx.py Normal file
View File

@ -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)

18
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/placo/test_placo_hwi_tmp.py Normal file
View File

@ -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)

78
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/imu_gyro.py Normal file
View File

@ -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)

45
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/move_test.py Normal file
View File

@ -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"))

38
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/new_run.py Normal file
View File

@ -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

45
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot.py Normal file
View File

@ -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()

33
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/plot_imu.py Normal file
View File

@ -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()

15
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/pressure_test.py Normal file
View File

@ -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)

50
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/raw_imu_gyro.py Normal file
View File

@ -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

44
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/replay_imu_data.py Normal file
View File

@ -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)

103
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/rl_walk.py Normal file
View File

@ -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"))

166
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/run.py Normal file
View File

@ -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

15
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/real_robot/utils.py Normal file
View File

@ -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)

25
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/bench_com_time.py Normal file
View File

@ -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))

123
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/configure_motors.py Normal file
View File

@ -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])

80
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/identification.py Normal file
View File

@ -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"))

86
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/mujoco_placo_walk.py Normal file
View File

@ -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)

243
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco.py Normal file
View File

@ -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"))

262
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/onnx_AWD_mujoco_motor_control.py Normal file
View File

@ -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)

1
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/params_m6.json Normal file
View File

@ -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"}

52
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_defaults.json Normal file
View File

@ -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
}
}

167
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_fun_moves.py Normal file
View File

@ -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)

114
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/placo_walk_real_robot.py Normal file
View File

@ -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)

40
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot.py Normal file
View File

@ -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()

11
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py Normal file
View File

@ -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()

11
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/plot_adaptation_module_latent.py~ Normal file
View File

@ -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()

142
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/pwm_control_test.py Normal file
View File

@ -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"))

39
Open_Duck_Mini-2/Open_Duck_Mini-2/experiments/v2/test.py Normal file
View File

@ -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()

Some files were not shown because too many files have changed in this diff Show More