commit 556ddba8d6218a9c9599750284f7dd79b2c50d93 Author: Sutou Kouhei Date: Wed Jul 23 15:33:37 2025 +0900 Import diff --git a/.github/ISSUE_TEMPLATE/1-bug-report.yml b/.github/ISSUE_TEMPLATE/1-bug-report.yml new file mode 100644 index 0000000..cd35d2a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/1-bug-report.yml @@ -0,0 +1,25 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. + +name: Bug Report +description: File a bug report. +type: Bug +body: + - type: textarea + id: description + attributes: + label: Describe the problem you got + description: It's better that you provide information as much as possible. + validations: + required: true diff --git a/.github/ISSUE_TEMPLATE/2-feature-request.yml b/.github/ISSUE_TEMPLATE/2-feature-request.yml new file mode 100644 index 0000000..5c17335 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/2-feature-request.yml @@ -0,0 +1,25 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. + +name: Feature Request +description: Request a feature. +type: Feature +body: + - type: textarea + id: description + attributes: + label: Describe the feature you want + description: It's better that you also provide your use case. + validations: + required: true diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 0000000..2cdb5fa --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,22 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. + +blank_issues_enabled: false +contact_links: + - name: Discord + url: https://discord.gg/FsZaZ4z3We + about: Please ask and answer questions here. + - name: GitHub Discussions + url: https://github.com/enactic/openarm_teleop/discussions + about: If you prefer GitHub Discussions to Discord, you can use GitHub Discussions too. diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..8802b7a --- /dev/null +++ b/.gitignore @@ -0,0 +1,21 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. + +build/ +.vscode +CMakeCache.txt +ament_cmake_core/ +build.ninja +cmake_install.cmake +CMakeFiles/ diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..16fa235 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,77 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. + +cmake_minimum_required(VERSION 3.22) +project(openarm_teleop) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# ----------------------------- +# Find external packages +# ----------------------------- +find_package(OpenArmCAN REQUIRED) +# find_package(urdf REQUIRED) +find_package(orocos_kdl REQUIRED) +find_package(kdl_parser REQUIRED) +find_package(Eigen3 REQUIRED) +find_package(urdfdom REQUIRED) +find_package(urdfdom_headers REQUIRED) +find_package(yaml-cpp REQUIRED) + +# ----------------------------- +# Create static library +# ----------------------------- +add_library(openarm_teleop_lib STATIC + src/controller/dynamics.cpp + src/controller/control.cpp + src/openarm_port/openarm_init.cpp + src/openarm_port/joint_mapper.cpp +) + +target_include_directories( + openarm_teleop_lib PUBLIC + ${EIGEN3_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/src +) + +target_link_libraries(openarm_teleop_lib + OpenArmCAN::openarm_can + ${EIGEN3_LIBRARIES} + ${orocos_kdl_LIBRARIES} + kdl_parser + urdfdom_model + yaml-cpp +) + +# ----------------------------- +# Executables +# ----------------------------- +add_executable(gravity_comp control/gravity_compasation.cpp) +add_executable(comm_test control/openarm_communication_test.cpp) +add_executable(unilateral_control control/openarm_unilateral_control.cpp) +add_executable(bilateral_control control/openarm_bilateral_control.cpp) + +target_link_libraries(gravity_comp openarm_teleop_lib) +target_link_libraries(comm_test openarm_teleop_lib) +target_link_libraries(unilateral_control openarm_teleop_lib) +target_link_libraries(bilateral_control openarm_teleop_lib) + +target_include_directories(gravity_comp PRIVATE ${EIGEN3_INCLUDE_DIRS}) +target_include_directories(unilateral_control PRIVATE ${EIGEN3_INCLUDE_DIRS}) +target_include_directories(bilateral_control PRIVATE ${EIGEN3_INCLUDE_DIRS}) \ No newline at end of file diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 0000000..bee83e9 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,133 @@ + +# Contributor Covenant Code of Conduct + +## Our Pledge + +We as members, contributors, and leaders pledge to make participation in our +community a harassment-free experience for everyone, regardless of age, body +size, visible or invisible disability, ethnicity, sex characteristics, gender +identity and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, caste, color, religion, or sexual +identity and orientation. + +We pledge to act and interact in ways that contribute to an open, welcoming, +diverse, inclusive, and healthy community. + +## Our Standards + +Examples of behavior that contributes to a positive environment for our +community include: + +* Demonstrating empathy and kindness toward other people +* Being respectful of differing opinions, viewpoints, and experiences +* Giving and gracefully accepting constructive feedback +* Accepting responsibility and apologizing to those affected by our mistakes, + and learning from the experience +* Focusing on what is best not just for us as individuals, but for the overall + community + +Examples of unacceptable behavior include: + +* The use of sexualized language or imagery, and sexual attention or advances of + any kind +* Trolling, insulting or derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or email address, + without their explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Enforcement Responsibilities + +Community leaders are responsible for clarifying and enforcing our standards of +acceptable behavior and will take appropriate and fair corrective action in +response to any behavior that they deem inappropriate, threatening, offensive, +or harmful. + +Community leaders have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, and will communicate reasons for moderation +decisions when appropriate. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official email address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported to the community leaders responsible for enforcement at +hi_public@reazon.jp. +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Enforcement Guidelines + +Community leaders will follow these Community Impact Guidelines in determining +the consequences for any action they deem in violation of this Code of Conduct: + +### 1. Correction + +**Community Impact**: Use of inappropriate language or other behavior deemed +unprofessional or unwelcome in the community. + +**Consequence**: A private, written warning from community leaders, providing +clarity around the nature of the violation and an explanation of why the +behavior was inappropriate. A public apology may be requested. + +### 2. Warning + +**Community Impact**: A violation through a single incident or series of +actions. + +**Consequence**: A warning with consequences for continued behavior. No +interaction with the people involved, including unsolicited interaction with +those enforcing the Code of Conduct, for a specified period of time. This +includes avoiding interactions in community spaces as well as external channels +like social media. Violating these terms may lead to a temporary or permanent +ban. + +### 3. Temporary Ban + +**Community Impact**: A serious violation of community standards, including +sustained inappropriate behavior. + +**Consequence**: A temporary ban from any sort of interaction or public +communication with the community for a specified period of time. No public or +private interaction with the people involved, including unsolicited interaction +with those enforcing the Code of Conduct, is allowed during this period. +Violating these terms may lead to a permanent ban. + +### 4. Permanent Ban + +**Community Impact**: Demonstrating a pattern of violation of community +standards, including sustained inappropriate behavior, harassment of an +individual, or aggression toward or disparagement of classes of individuals. + +**Consequence**: A permanent ban from any sort of public interaction within the +community. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 2.1, available at +[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. + +Community Impact Guidelines were inspired by +[Mozilla's code of conduct enforcement ladder][Mozilla CoC]. + +For answers to common questions about this code of conduct, see the FAQ at +[https://www.contributor-covenant.org/faq][FAQ]. Translations are available at +[https://www.contributor-covenant.org/translations][translations]. + +[homepage]: https://www.contributor-covenant.org +[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html +[Mozilla CoC]: https://github.com/mozilla/diversity +[FAQ]: https://www.contributor-covenant.org/faq +[translations]: https://www.contributor-covenant.org/translations diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..ffc531a --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,19 @@ +# How to contribute + +## Did you find a bug? + +Please report it to [GitHub Issues](https://github.com/enactic/openarm_teleop/issues/new?template=1-bug-report.yml)! + +## Did you have a feature request? + +Please share it to [GitHub Issues](https://github.com/enactic/openarm_teleop/issues/new?template=2-feature-request.yml)! + +## Did you write a patch? + +Please open a pull request with it! + +Please make sure to review [our license](https://github.com/enactic/openarm_teleop/blob/main/LICENSE.txt) before you open a pull request. + +## Others? + +Please share it on [Discord](https://discord.gg/FsZaZ4z3We) or [GitHub Discussions](https://github.com/enactic/openarm_teleop/discussions)! diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..261eeb9 --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md new file mode 100644 index 0000000..a825a5a --- /dev/null +++ b/README.md @@ -0,0 +1,19 @@ +# OpenArm Teleop + +OpenArm supports 1:1 teleoperation from a leader arm to a follower arm in two control modes. See the [documentation](https://docs.openarm.dev/teleop/) for details. + +## Related links + +- πŸ“š Read the [documentation](https://docs.openarm.dev/teleop/) +- πŸ’¬ Join the community on [Discord](https://discord.gg/FsZaZ4z3We) +- πŸ“¬ Contact us through + +## License + +Licensed under the Apache License 2.0. See [LICENSE.txt](LICENSE.txt) for details. + +Copyright 2025 Enactic, Inc. + +## Code of Conduct + +All participation in the OpenArm project is governed by our [Code of Conduct](CODE_OF_CONDUCT.md). diff --git a/config/follower.yaml b/config/follower.yaml new file mode 100644 index 0000000..76df2b6 --- /dev/null +++ b/config/follower.yaml @@ -0,0 +1,34 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. + + # --------------------------------------------------------- + # Shared gains: + # β€’ Kp/Kd are shared between bilateral and unilateral control. + # β€’ Increasing Kp/Kd makes contact feel more "real" but also heavier. + # + # Friction model (tanh): + # Ο„_fric(Ο‰) = Fo + FvΒ·Ο‰ + FcΒ·tanh(kΒ·Ο‰) + # Fc : Coulomb friction magnitude [Nm] + # k : tanh steepness near zero velocity + # Fv : Viscous friction coefficient [NmΒ·s/rad] + # Fo : Constant bias/offset torque [Nm] + # ------------------------------------------------------- + +FollowerArmParam: + Kp: [240.0, 240.0, 240.0, 240.0, 24.0, 31.0, 25.0, 16.0] + Kd: [3.0, 3.0, 3.0, 3.0, 0.2, 0.2, 0.2, 0.2] + Fc: [0.306, 0.306, 0.40, 0.166, 0.050, 0.093, 0.172, 0.0512] + k: [28.417, 28.417, 29.065, 130.038, 151.771, 242.287, 7.888, 4.000] + Fv: [0.063, 0.0630, 0.604, 0.813, 0.029, 0.072, 0.084, 0.084] + Fo: [0.088, 0.088, 0.008, -0.058, 0.005, 0.009, -0.059, -0.050] diff --git a/config/leader.yaml b/config/leader.yaml new file mode 100644 index 0000000..40309d9 --- /dev/null +++ b/config/leader.yaml @@ -0,0 +1,34 @@ +# Copyright 2025 Enactic, Inc. +# +# 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. + + # --------------------------------------------------------- + # Shared gains: + # β€’ Kp/Kd are shared between bilateral and unilateral control. + # β€’ Increasing Kp/Kd makes contact feel more "real" but also heavier. + # + # Friction model (tanh): + # Ο„_fric(Ο‰) = Fo + FvΒ·Ο‰ + FcΒ·tanh(kΒ·Ο‰) + # Fc : Coulomb friction magnitude [Nm] + # k : tanh steepness near zero velocity + # Fv : Viscous friction coefficient [NmΒ·s/rad] + # Fo : Constant bias/offset torque [Nm] + # ------------------------------------------------------- + +LeaderArmParam: + Kp: [240.0, 240.0, 240.0, 240.0, 24.0, 31.0, 25.0, 16.0] + Kd: [3.0, 3.0, 3.0, 3.0, 0.2, 0.2, 0.2, 0.2] + Fc: [0.306, 0.306, 0.40, 0.166, 0.050, 0.083, 0.172, 0.0512] + k: [28.417, 28.417, 29.065, 130.038, 151.771, 242.287, 7.888, 4.000] + Fv: [0.063, 0.0630, 0.604, 0.813, 0.029, 0.072, 0.084, 0.084] + Fo: [0.088, 0.088, 0.008, -0.058, 0.005, 0.009, -0.059, -0.050] diff --git a/control/gravity_compasation.cpp b/control/gravity_compasation.cpp new file mode 100644 index 0000000..387ab66 --- /dev/null +++ b/control/gravity_compasation.cpp @@ -0,0 +1,142 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::atomic keep_running(true); + +void signal_handler(int signal) { + if (signal == SIGINT) { + std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; + keep_running = false; + } +} + +int main(int argc, char** argv) { + try { + std::signal(SIGINT, signal_handler); + + std::string arm_side = "right_arm"; + std::string can_interface = "can0"; + + + if (argc < 4) { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + std::cerr << "Example: " << argv[0] << " right_arm can0 /tmp/v10_bimanual.urdf" << std::endl; + return 1; + } + + arm_side = argv[1]; + can_interface = argv[2]; + std::string urdf_path = argv[3]; + + if (arm_side != "left_arm" && arm_side != "right_arm") { + std::cerr << "[ERROR] Invalid arm_side: " << arm_side << ". Must be 'left_arm' or 'right_arm'." << std::endl; + return 1; + } + + if (!std::filesystem::exists(urdf_path)) { + std::cerr << "[ERROR] URDF file not found: " << urdf_path << std::endl; + return 1; + } + + std::cout << "=== OpenArm Gravity Compensation ===" << std::endl; + std::cout << "Arm side : " << arm_side << std::endl; + std::cout << "CAN interface : " << can_interface << std::endl; + std::cout << "URDF path : " << urdf_path << std::endl; + + std::string root_link = "openarm_body_link0"; + std::string leaf_link = (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; + + Dynamics arm_dynamics(urdf_path, root_link, leaf_link); + arm_dynamics.Init(); + + std::cout << "=== Initializing Leader OpenArm ===" << std::endl; + openarm::can::socket::OpenArm *openarm = + openarm_init::OpenArmInitializer::initialize_openarm(can_interface, true); + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + auto start_time = std::chrono::high_resolution_clock::now(); + auto last_hz_display = start_time; + int frame_count = 0; + + std::vector arm_joint_positions(openarm->get_arm().get_motors().size(), 0.0); + std::vector arm_joint_velocities(openarm->get_arm().get_motors().size(), 0.0); + + std::vector gripper_joint_positions(openarm->get_gripper().get_motors().size(), 0.0); + std::vector gripper_joint_velocities(openarm->get_gripper().get_motors().size(), 0.0); + + std::vector grav_torques(openarm->get_arm().get_motors().size(), 0.0); + + while(keep_running){ + + frame_count++; + auto current_time = std::chrono::high_resolution_clock::now(); + + // Calculate and display Hz every second + auto time_since_last_display = std::chrono::duration_cast(current_time - last_hz_display).count(); + if (time_since_last_display >= 1000) { // Every 1000ms (1 second) + auto total_time = std::chrono::duration_cast(current_time - start_time).count(); + double hz = (frame_count * 1000.0) / total_time; + std::cout << "=== Loop Frequency: " << hz << " Hz ===" << std::endl; + last_hz_display = current_time; + } + + auto motors = openarm->get_arm().get_motors(); + for (size_t i = 0; i < motors.size(); ++i) { + arm_joint_positions[i] = motors[i].get_position(); + arm_joint_velocities[i] = motors[i].get_velocity(); + } + + arm_dynamics.GetGravity(arm_joint_positions.data(), grav_torques.data()); + + for(size_t i = 0; i < openarm->get_arm().get_motors().size(); ++i){ + // std::cout << "grav_torques[" << i << "] = " << grav_torques[i] << std::endl; + } + + std::vector cmds; + cmds.reserve(grav_torques.size()); + + std::transform(grav_torques.begin(), grav_torques.end(), std::back_inserter(cmds), + [](double t) { return openarm::damiao_motor::MITParam{0, 0, 0, 0, t}; }); + + openarm->get_arm().mit_control_all(cmds); + + openarm->recv_all(); + + } + + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + openarm->disable_all(); + openarm->recv_all(); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return -1; + } + + return 0; +} diff --git a/control/openarm_bilateral_control.cpp b/control/openarm_bilateral_control.cpp new file mode 100644 index 0000000..d0e8463 --- /dev/null +++ b/control/openarm_bilateral_control.cpp @@ -0,0 +1,321 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +std::atomic keep_running(true); + + +void signal_handler(int signal) { + if (signal == SIGINT) { + std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; + keep_running = false; + } +} + + +class LeaderArmThread : public PeriodicTimerThread { + public: + LeaderArmThread(std::shared_ptr robot_state, Control *control_l, double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l){} + + protected: + + void before_start() override { + std::cout << "leader start thread " << std::endl; + } + + void after_stop() override { + std::cout << "leader stop thread " << std::endl; + } + + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + + control_l_->bilateral_step(); + + auto now = std::chrono::steady_clock::now(); + + auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + + // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; + } + + private: + std::shared_ptr robot_state_; + Control *control_l_; + + }; + + +class FollowerArmThread : public PeriodicTimerThread { + public: + FollowerArmThread(std::shared_ptr robot_state, Control *control_f, double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} + + protected: + void before_start() override { + std::cout << "follower start thread " << std::endl; + } + + void after_stop() override { + std::cout << "follower stop thread " << std::endl; + } + + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + + control_f_->bilateral_step(); + + auto now = std::chrono::steady_clock::now(); + + auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + + // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; + } + + private: + std::shared_ptr robot_state_; + Control *control_f_; + + }; + + + +class AdminThread : public PeriodicTimerThread { + public: + AdminThread(std::shared_ptr leader_state, + std::shared_ptr follower_state, + Control *control_l, + Control *control_f, + double hz = 500.0) + : PeriodicTimerThread(hz), leader_state_(leader_state), follower_state_(follower_state), control_l_(control_l), control_f_(control_f) {} + + protected: + void before_start() override { + std::cout << "admin start thread " << std::endl; + } + + void after_stop() override { + std::cout << "admin stop thread " << std::endl; + } + + void on_timer() override { + + static auto prev_time = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); + + // get response + auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); + auto follower_arm_resp = follower_state_->arm_state().get_all_responses(); + + auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); + auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); + + //set referense + leader_state_->arm_state().set_all_references(follower_arm_resp); + leader_state_->hand_state().set_all_references(follower_hand_resp); + + follower_state_->arm_state().set_all_references(leader_arm_resp); + follower_state_->hand_state().set_all_references(leader_hand_resp); + + auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + + // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; + } + + private: + std::shared_ptr leader_state_; + std::shared_ptr follower_state_; + Control *control_l_; + Control *control_f_; + + }; + + +int main(int argc, char** argv) { + try { + std::signal(SIGINT, signal_handler); + + std::string arm_side = "right_arm"; + std::string leader_urdf_path; + std::string follower_urdf_path; + std::string leader_can_interface = "can0"; + std::string follower_can_interface = "can2"; + + if (argc < 3) { + std::cerr << "Usage: " << argv[0] << " [arm_side] [leader_can] [follower_can]" << std::endl; + return 1; + } + + // Required: URDF paths + leader_urdf_path = argv[1]; + follower_urdf_path = argv[2]; + + // Optional: arm_side + if (argc >= 4) { + arm_side = argv[3]; + if (arm_side != "left_arm" && arm_side != "right_arm") { + std::cerr << "[ERROR] Invalid arm_side: " << arm_side << ". Must be 'left_arm' or 'right_arm'." << std::endl; + return 1; + } + } + + // Optional: CAN interfaces + if (argc >= 6) { + leader_can_interface = argv[4]; + follower_can_interface = argv[5]; + } + + // URDF file existence check + if (!std::filesystem::exists(leader_urdf_path)) { + std::cerr << "[ERROR] Leader URDF not found: " << leader_urdf_path << std::endl; + return 1; + } + if (!std::filesystem::exists(follower_urdf_path)) { + std::cerr << "[ERROR] Follower URDF not found: " << follower_urdf_path << std::endl; + return 1; + } + + // Setup dynamics + std::string root_link = "openarm_body_link0"; + std::string leaf_link = (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; + + // Output confirmation + std::cout << "=== OpenArm Bilateral Control ===" << std::endl; + std::cout << "Arm side : " << arm_side << std::endl; + std::cout << "Leader CAN : " << leader_can_interface << std::endl; + std::cout << "Follower CAN : " << follower_can_interface << std::endl; + std::cout << "Leader URDF path : " << leader_urdf_path << std::endl; + std::cout << "Follower URDF path: " << follower_urdf_path << std::endl; + std::cout << "Root link : " << root_link << std::endl; + std::cout << "Leaf link : " << leaf_link << std::endl; + + YamlLoader leader_loader("config/leader.yaml"); + YamlLoader follower_loader("config/follower.yaml"); + + // Leader parameters + std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); + std::vector leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); + std::vector leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); + std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); + std::vector leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); + std::vector leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); + + // Follower parameters + std::vector follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); + std::vector follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); + std::vector follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc"); + std::vector follower_k = follower_loader.get_vector("FollowerArmParam", "k"); + std::vector follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv"); + std::vector follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo"); + + Dynamics *leader_arm_dynamics = new Dynamics(leader_urdf_path, root_link, leaf_link); + leader_arm_dynamics->Init(); + + Dynamics *follower_arm_dynamics = new Dynamics(follower_urdf_path, root_link, leaf_link); + follower_arm_dynamics->Init(); + + std::cout << "=== Initializing Leader OpenArm ===" << std::endl; + openarm::can::socket::OpenArm *leader_openarm = + openarm_init::OpenArmInitializer::initialize_openarm(leader_can_interface, true); + + std::cout << "=== Initializing Follower OpenArm ===" << std::endl; + openarm::can::socket::OpenArm *follower_openarm = + openarm_init::OpenArmInitializer::initialize_openarm(follower_can_interface, true); + + size_t leader_arm_motor_num = leader_openarm->get_arm().get_motors().size(); + size_t follower_arm_motor_num = follower_openarm->get_arm().get_motors().size(); + size_t leader_hand_motor_num = leader_openarm->get_gripper().get_motors().size(); + size_t follower_hand_motor_num = follower_openarm->get_gripper().get_motors().size(); + + std::cout << "leader arm motor num : " << leader_arm_motor_num << std::endl; + std::cout << "follower arm motor num : " << follower_arm_motor_num << std::endl; + std::cout << "leader hand motor num : " << leader_hand_motor_num << std::endl; + std::cout << "follower hand motor num : " << follower_hand_motor_num << std::endl; + + // Declare robot_state (Joint and motor counts are assumed to be equal) + std::shared_ptr leader_state = + std::make_shared(leader_arm_motor_num, leader_hand_motor_num); + + std::shared_ptr follower_state = + std::make_shared(follower_arm_motor_num, follower_hand_motor_num); + + Control* control_leader = new Control(leader_openarm,leader_arm_dynamics,follower_arm_dynamics, leader_state, 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); + Control* control_follower = new Control(follower_openarm,leader_arm_dynamics,follower_arm_dynamics, follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, follower_arm_motor_num, follower_hand_motor_num); + + //set parameter + control_leader->SetParameter( + leader_kp, leader_kd, + leader_Fc, leader_k, leader_Fv, leader_Fo); + + control_follower->SetParameter( + follower_kp, follower_kd, + follower_Fc, follower_k, follower_Fv, follower_Fo); + + //set home postion + std::thread thread_l(&Control::AdjustPosition, control_leader); + std::thread thread_f(&Control::AdjustPosition, control_follower); + thread_l.join(); + thread_f.join(); + + // Start control process + LeaderArmThread leader_thread(leader_state ,control_leader, FREQUENCY); + FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); + AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, FREQUENCY); + + // thread start in control + leader_thread.start_thread(); + follower_thread.start_thread(); + admin_thread.start_thread(); + + while (keep_running) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + leader_thread.stop_thread(); + follower_thread.stop_thread(); + admin_thread.stop_thread(); + + leader_openarm->disable_all(); + follower_openarm->disable_all(); + + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + } + + return 0; +} \ No newline at end of file diff --git a/control/openarm_communication_test.cpp b/control/openarm_communication_test.cpp new file mode 100644 index 0000000..e7553c5 --- /dev/null +++ b/control/openarm_communication_test.cpp @@ -0,0 +1,145 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +int main(int argc, char** argv) { + try { + std::cout << "=== OpenArm CAN Example ===" << std::endl; + std::cout << "This example demonstrates the OpenArm API functionality" << std::endl; + + std::string can_interface = "can0"; + if (argc > 1 ){ + can_interface = argv[1]; + } + + std::cout << "[INFO] Using CAN interface: " << can_interface << std::endl; + + + // Initialize OpenArm with CAN interface and enable CAN-FD + std::cout << "Initializing OpenArm CAN..." << std::endl; + openarm::can::socket::OpenArm openarm(can_interface, true); // Use CAN-FD on can0 interface + + // Initialize arm motors + std::vector motor_types = { + openarm::damiao_motor::MotorType::DM8009, openarm::damiao_motor::MotorType::DM8009, + openarm::damiao_motor::MotorType::DM4340, openarm::damiao_motor::MotorType::DM4340, + openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310, + openarm::damiao_motor::MotorType::DM4310 + }; + + std::vector send_can_ids = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; + std::vector recv_can_ids = {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}; + openarm.init_arm_motors(motor_types, send_can_ids, recv_can_ids); + + // Initialize gripper + std::cout << "Initializing gripper..." << std::endl; + openarm.init_gripper_motor(openarm::damiao_motor::MotorType::DM4310, 0x08, 0x18); + + // Set callback mode to ignore and refresh all motors + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::IGNORE); + openarm.refresh_all(); + openarm.recv_all(); + + // Enable all motors + std::cout << "\n=== Enabling Motors ===" << std::endl; + openarm.enable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm.recv_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Set device mode to param and query motor id + std::cout << "\n=== Querying Motor IDs ===" << std::endl; + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::PARAM); + openarm.query_param_all(static_cast(openarm::damiao_motor::RID::MST_ID)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + openarm.recv_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Access motors through components + for (const auto& motor : openarm.get_arm().get_motors()) { + std::cout << "Arm Motor: " << motor.get_send_can_id() << " ID: " + << motor.get_param(static_cast(openarm::damiao_motor::RID::MST_ID)) + << std::endl; + } + for (const auto& motor : openarm.get_gripper().get_motors()) { + std::cout << "Gripper Motor: " << motor.get_send_can_id() << " ID: " + << motor.get_param(static_cast(openarm::damiao_motor::RID::MST_ID)) + << std::endl; + } + + // Set device mode to state and control motor + std::cout << "\n=== Controlling Motors ===" << std::endl; + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); + + // Control arm motors + openarm.get_arm().mit_control_all({openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0} + }); + + openarm.get_gripper().mit_control_all({openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}}); + + openarm.recv_all(); + + // Control gripper + std::cout << "Opening gripper..." << std::endl; + // openarm.get_gripper().open(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + for (int i = 0; i < 100; i++) { + openarm.refresh_all(); + openarm.recv_all(); + + // Display arm motor states + for (const auto& motor : openarm.get_arm().get_motors()) { + std::cout << "Arm Motor: " << motor.get_send_can_id() + << " position: " << motor.get_position() << std::endl; + } + // Display gripper state + for (const auto& motor : openarm.get_gripper().get_motors()) { + std::cout << "Gripper Motor: " << motor.get_send_can_id() + << " position: " << motor.get_position() << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + // Test gripper close + std::cout << "Closing gripper..." << std::endl; + // openarm.get_gripper().close(); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + openarm.disable_all(); + openarm.recv_all(); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return -1; + } + + return 0; +} diff --git a/control/openarm_unilateral_control.cpp b/control/openarm_unilateral_control.cpp new file mode 100644 index 0000000..c8bac95 --- /dev/null +++ b/control/openarm_unilateral_control.cpp @@ -0,0 +1,319 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +std::atomic keep_running(true); + + +void signal_handler(int signal) { + if (signal == SIGINT) { + std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; + keep_running = false; + } +} + + +class LeaderArmThread : public PeriodicTimerThread { + public: + LeaderArmThread(std::shared_ptr robot_state, Control *control_l, double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l){} + + protected: + + void before_start() override { + std::cout << "leader start thread " << std::endl; + } + + void after_stop() override { + std::cout << "leader stop thread " << std::endl; + } + + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + + control_l_->unilateral_step(); + + auto now = std::chrono::steady_clock::now(); + + auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + + // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; + } + + private: + std::shared_ptr robot_state_; + Control *control_l_; + + }; + + +class FollowerArmThread : public PeriodicTimerThread { + public: + FollowerArmThread(std::shared_ptr robot_state, Control *control_f, double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} + + protected: + void before_start() override { + std::cout << "follower start thread " << std::endl; + } + + void after_stop() override { + std::cout << "follower stop thread " << std::endl; + } + + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + + control_f_->unilateral_step(); + + auto now = std::chrono::steady_clock::now(); + + auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + + // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; + } + + private: + std::shared_ptr robot_state_; + Control *control_f_; + + }; + + +class AdminThread : public PeriodicTimerThread { + public: + AdminThread(std::shared_ptr leader_state, + std::shared_ptr follower_state, + Control *control_l, + Control *control_f, + double hz = 500.0) + : PeriodicTimerThread(hz), leader_state_(leader_state), follower_state_(follower_state), control_l_(control_l), control_f_(control_f) {} + + protected: + void before_start() override { + std::cout << "admin start thread " << std::endl; + } + + void after_stop() override { + std::cout << "admin stop thread " << std::endl; + } + + void on_timer() override { + + static auto prev_time = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); + + // get response + auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); + auto follower_arm_resp = follower_state_->arm_state().get_all_responses(); + + auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); + auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); + + //set referense + leader_state_->arm_state().set_all_references(follower_arm_resp); + leader_state_->hand_state().set_all_references(follower_hand_resp); + + follower_state_->arm_state().set_all_references(leader_arm_resp); + follower_state_->hand_state().set_all_references(leader_hand_resp); + + auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + + // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; + } + + private: + std::shared_ptr leader_state_; + std::shared_ptr follower_state_; + Control *control_l_; + Control *control_f_; + + }; + + +int main(int argc, char** argv) { + try { + std::signal(SIGINT, signal_handler); + + // default configration + std::string arm_side = "right_arm"; + std::string leader_urdf_path; + std::string follower_urdf_path; + std::string leader_can_interface = "can0"; + std::string follower_can_interface = "can2"; + + if (argc < 3) { + std::cerr << "Usage: " << argv[0] << " [arm_side] [leader_can] [follower_can]" << std::endl; + return 1; + } + + // Required: URDF paths + leader_urdf_path = argv[1]; + follower_urdf_path = argv[2]; + + // Optional: arm_side + if (argc >= 4) { + arm_side = argv[3]; + if (arm_side != "left_arm" && arm_side != "right_arm") { + std::cerr << "[ERROR] Invalid arm_side: " << arm_side << ". Must be 'left_arm' or 'right_arm'." << std::endl; + return 1; + } + } + + // Optional: CAN interfaces + if (argc >= 6) { + leader_can_interface = argv[4]; + follower_can_interface = argv[5]; + } + + // URDF file existence check + if (!std::filesystem::exists(leader_urdf_path)) { + std::cerr << "[ERROR] Leader URDF not found: " << leader_urdf_path << std::endl; + return 1; + } + if (!std::filesystem::exists(follower_urdf_path)) { + std::cerr << "[ERROR] Follower URDF not found: " << follower_urdf_path << std::endl; + return 1; + } + + // Setup dynamics + std::string root_link = "openarm_body_link0"; + std::string leaf_link = (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; + + // Output confirmation + std::cout << "=== OpenArm Unilateral Control ===" << std::endl; + std::cout << "Arm side : " << arm_side << std::endl; + std::cout << "Leader CAN : " << leader_can_interface << std::endl; + std::cout << "Follower CAN : " << follower_can_interface << std::endl; + std::cout << "Leader URDF path : " << leader_urdf_path << std::endl; + std::cout << "Follower URDF path: " << follower_urdf_path << std::endl; + std::cout << "Root link : " << root_link << std::endl; + std::cout << "Leaf link : " << leaf_link << std::endl; + + YamlLoader leader_loader("config/leader.yaml"); + YamlLoader follower_loader("config/follower.yaml"); + + // Leader parameters + std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); + std::vector leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); + std::vector leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); + std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); + std::vector leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); + std::vector leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); + + // Follower parameters + std::vector follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); + std::vector follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); + std::vector follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc"); + std::vector follower_k = follower_loader.get_vector("FollowerArmParam", "k"); + std::vector follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv"); + std::vector follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo"); + + Dynamics *leader_arm_dynamics = new Dynamics(leader_urdf_path, root_link, leaf_link); + leader_arm_dynamics->Init(); + + Dynamics *follower_arm_dynamics = new Dynamics(follower_urdf_path, root_link, leaf_link); + follower_arm_dynamics->Init(); + + std::cout << "=== Initializing Leader OpenArm ===" << std::endl; + openarm::can::socket::OpenArm *leader_openarm = + openarm_init::OpenArmInitializer::initialize_openarm(leader_can_interface, true); + + std::cout << "=== Initializing Follower OpenArm ===" << std::endl; + openarm::can::socket::OpenArm *follower_openarm = + openarm_init::OpenArmInitializer::initialize_openarm(follower_can_interface, true); + + size_t leader_arm_motor_num = leader_openarm->get_arm().get_motors().size(); + size_t follower_arm_motor_num = follower_openarm->get_arm().get_motors().size(); + size_t leader_hand_motor_num = leader_openarm->get_gripper().get_motors().size(); + size_t follower_hand_motor_num = follower_openarm->get_gripper().get_motors().size(); + + std::cout << "leader arm motor num : " << leader_arm_motor_num << std::endl; + std::cout << "follower arm motor num : " << follower_arm_motor_num << std::endl; + std::cout << "leader hand motor num : " << leader_hand_motor_num << std::endl; + std::cout << "follower hand motor num : " << follower_hand_motor_num << std::endl; + + // Declare robot_state (Joint and motor counts are assumed to be equal) + std::shared_ptr leader_state = + std::make_shared(leader_arm_motor_num, leader_hand_motor_num); + + std::shared_ptr follower_state = + std::make_shared(follower_arm_motor_num, follower_hand_motor_num); + + Control* control_leader = new Control(leader_openarm,leader_arm_dynamics,follower_arm_dynamics, leader_state, 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); + Control* control_follower = new Control(follower_openarm,leader_arm_dynamics,follower_arm_dynamics, follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, follower_arm_motor_num, follower_hand_motor_num); + + control_leader->SetParameter( + leader_kp, leader_kd , + leader_Fc, leader_k, leader_Fv, leader_Fo); + + control_follower->SetParameter( + follower_kp, follower_kd, + follower_Fc, follower_k, follower_Fv, follower_Fo); + + //set home postion + std::thread thread_l(&Control::AdjustPosition, control_leader); + std::thread thread_f(&Control::AdjustPosition, control_follower); + thread_l.join(); + thread_f.join(); + + // Start control process + LeaderArmThread leader_thread(leader_state ,control_leader, FREQUENCY); + FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); + AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, FREQUENCY); + + leader_thread.start_thread(); + follower_thread.start_thread(); + admin_thread.start_thread(); + + while (keep_running) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + leader_thread.stop_thread(); + follower_thread.stop_thread(); + admin_thread.stop_thread(); + + leader_openarm->disable_all(); + follower_openarm->disable_all(); + + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + } + + return 0; +} \ No newline at end of file diff --git a/script/launch_bilateral.sh b/script/launch_bilateral.sh new file mode 100755 index 0000000..f7ffc41 --- /dev/null +++ b/script/launch_bilateral.sh @@ -0,0 +1,105 @@ +#!/bin/bash +# +# Copyright 2025 Enactic, Inc. +# +# 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. + +# ========= Configuration ========= +ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm +LEADER_CAN_IF=$2 # Optional: leader CAN interface +FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface +ARM_TYPE="v10" # Fixed for now +TMPDIR="/tmp/openarm_urdf_gen" + +# Validate arm side +if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then + echo "[ERROR] Invalid arm_side: $ARM_SIDE" + echo "Usage: $0 [leader_can_if] [follower_can_if]" + exit 1 +fi + +# Set default CAN interfaces if not provided +if [ -z "$LEADER_CAN_IF" ]; then + if [ "$ARM_SIDE" = "right_arm" ]; then + LEADER_CAN_IF="can0" + else + LEADER_CAN_IF="can1" + fi +fi + +if [ -z "$FOLLOWER_CAN_IF" ]; then + if [ "$ARM_SIDE" = "right_arm" ]; then + FOLLOWER_CAN_IF="can2" + else + FOLLOWER_CAN_IF="can3" + fi +fi + +# File paths +LEADER_URDF_PATH="$TMPDIR/${ARM_TYPE}_leader.urdf" +FOLLOWER_URDF_PATH="$TMPDIR/${ARM_TYPE}_follower.urdf" +XACRO_FILE="$ARM_TYPE.urdf.xacro" +WS_DIR=~/openarm_ros2_ws +XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" +BIN_PATH=~/openarm_teleop_tmp/build/bilateral_control +echo $BIN_PATH +# ================================ +# Check workspace +if [ ! -d "$WS_DIR" ]; then + echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 + echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 + echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 + exit 1 +fi + +# Check openarm_description package +if [ ! -d "$WS_DIR/src/openarm_description" ]; then + echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 + echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 + exit 1 +fi + +# Check xacro +if [ ! -f "$XACRO_PATH" ]; then + echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 + exit 1 +fi + + +# Check binary +if [ ! -f "$BIN_PATH" ]; then + echo "[ERROR] Compiled binary not found at: $BIN_PATH" + exit 1 +fi + +# Source ROS 2 +source "$WS_DIR/install/setup.bash" + +# Generate URDFs +echo "[INFO] Generating URDFs using xacro..." +mkdir -p "$TMPDIR" +xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH" +cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" + +if [ $? -ne 0 ]; then + echo "[ERROR] Failed to generate URDFs." + exit 1 +fi + +# Run binary +echo "[INFO] Launching bilateral control..." +"$BIN_PATH" "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" "$ARM_SIDE" "$LEADER_CAN_IF" "$FOLLOWER_CAN_IF" + +# Cleanup +echo "[INFO] Cleaning up temporary files..." +rm -rf "$TMPDIR" diff --git a/script/launch_grav_comp.sh b/script/launch_grav_comp.sh new file mode 100755 index 0000000..3ca44cf --- /dev/null +++ b/script/launch_grav_comp.sh @@ -0,0 +1,75 @@ +#!/bin/bash +# +# Copyright 2025 Enactic, Inc. +# +# 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. + +# ======== Configuration ======== +ARM_SIDE=${1:-right_arm} +CAN_IF=${2:-can0} +ARM_TYPE=${3:-v10} +TMPDIR="/tmp/openarm_urdf_gen" +URDF_NAME="${ARM_TYPE}_bimanual.urdf" +XACRO_FILE="${ARM_TYPE}.urdf.xacro" +WS_DIR=~/openarm_ros2_ws +XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" +URDF_OUT="$TMPDIR/$URDF_NAME" +BIN_PATH=~/openarm_teleop/build/gravity_comp # adjust if needed +# =============================== +# Check workspace +if [ ! -d "$WS_DIR" ]; then + echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 + echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 + echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 + exit 1 +fi + +# Check openarm_description package +if [ ! -d "$WS_DIR/src/openarm_description" ]; then + echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 + echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 + exit 1 +fi + +# Check xacro +if [ ! -f "$XACRO_PATH" ]; then + echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 + exit 1 +fi + + +# Check build binary +if [ ! -f "$BIN_PATH" ]; then + echo "[ERROR] Compiled binary not found at: $BIN_PATH" + exit 1 +fi + +# Generate URDF +echo "[INFO] Generating URDF using xacro..." +source $WS_DIR/install/setup.bash + +mkdir -p "$TMPDIR" +xacro "$XACRO_PATH" bimanual:=true -o "$URDF_OUT" + +if [ $? -ne 0 ]; then + echo "[ERROR] Failed to generate URDF." + exit 1 +fi + +# Run gravity compensation binary +echo "[INFO] Launching gravity compensation..." +"$BIN_PATH" "$ARM_SIDE" "$CAN_IF" "$URDF_OUT" + +# Cleanup +echo "[INFO] Cleaning up tmp dir..." +rm -rf "$TMPDIR" diff --git a/script/launch_unilateral.sh b/script/launch_unilateral.sh new file mode 100755 index 0000000..43c8b0a --- /dev/null +++ b/script/launch_unilateral.sh @@ -0,0 +1,105 @@ +#!/bin/bash +# +# Copyright 2025 Enactic, Inc. +# +# 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. + +# ========= Configuration ========= +ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm +LEADER_CAN_IF=$2 # Optional: leader CAN interface +FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface +ARM_TYPE="v10" # Fixed for now +TMPDIR="/tmp/openarm_urdf_gen" + +# Validate arm side +if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then + echo "[ERROR] Invalid arm_side: $ARM_SIDE" + echo "Usage: $0 [leader_can_if] [follower_can_if]" + exit 1 +fi + +# Set default CAN interfaces if not provided +if [ -z "$LEADER_CAN_IF" ]; then + if [ "$ARM_SIDE" = "right_arm" ]; then + LEADER_CAN_IF="can0" + else + LEADER_CAN_IF="can1" + fi +fi + +if [ -z "$FOLLOWER_CAN_IF" ]; then + if [ "$ARM_SIDE" = "right_arm" ]; then + FOLLOWER_CAN_IF="can2" + else + FOLLOWER_CAN_IF="can3" + fi +fi + +# File paths +LEADER_URDF_PATH="$TMPDIR/${ARM_TYPE}_leader.urdf" +FOLLOWER_URDF_PATH="$TMPDIR/${ARM_TYPE}_follower.urdf" +XACRO_FILE="$ARM_TYPE.urdf.xacro" +WS_DIR=~/openarm_ros2_ws +XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" +BIN_PATH=~/openarm_teleop/build/unilateral_control + +# Check workspace +if [ ! -d "$WS_DIR" ]; then + echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 + echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 + echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 + exit 1 +fi + +# Check openarm_description package +if [ ! -d "$WS_DIR/src/openarm_description" ]; then + echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 + echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 + exit 1 +fi + +# Check xacro +if [ ! -f "$XACRO_PATH" ]; then + echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 + exit 1 +fi + +# ================================ + +# Check binary +if [ ! -f "$BIN_PATH" ]; then + echo "[ERROR] Compiled binary not found at: $BIN_PATH" + exit 1 +fi + +# Source ROS 2 +source "$WS_DIR/install/setup.bash" + +# Generate URDFs +echo "[INFO] Generating URDFs using xacro..." +mkdir -p "$TMPDIR" +xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH" +cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" + +if [ $? -ne 0 ]; then + echo "[ERROR] Failed to generate URDFs." + exit 1 +fi + +# Run binary +echo "[INFO] Launching unilateral control..." +"$BIN_PATH" "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" "$ARM_SIDE" "$LEADER_CAN_IF" "$FOLLOWER_CAN_IF" + +# Cleanup +echo "[INFO] Cleaning up temporary files..." +rm -rf "$TMPDIR" diff --git a/src/controller/control.cpp b/src/controller/control.cpp new file mode 100644 index 0000000..c6c33fe --- /dev/null +++ b/src/controller/control.cpp @@ -0,0 +1,510 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, size_t arm_motor_num, size_t hand_motor_num): + openarm_(arm), dynamics_l_(dynamics_l), dynamics_f_(dynamics_f), robot_state_(robot_state), Ts_(Ts), role_(role), arm_motor_num_(arm_motor_num), hand_motor_num_(hand_motor_num) +{ + differentiator_ = new Differentiator(Ts); + openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); + openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); +} + +Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, std::string arm_type, size_t arm_motor_num, size_t hand_motor_num): + openarm_(arm), dynamics_l_(dynamics_l), dynamics_f_(dynamics_f), robot_state_(robot_state), Ts_(Ts), role_(role), arm_motor_num_(arm_motor_num), hand_motor_num_(hand_motor_num) +{ + differentiator_ = new Differentiator(Ts); + openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); + openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); + + arm_type_ = arm_type; +} + +Control::~Control() { + std::cout << "Control destructed " << std::endl; + delete openarmjointconverter_; + delete differentiator_; +} + +// bool Control::Setup(void) +// { +// // double motor_position[NMOTORS] = {0.0}; + +// // ComputeJointPosition(motor_position, response_->position.data()); + +// std::cout << "!control->Setup() finished "<< std::endl; + +// return true; +// } + +void Control::Shutdown(void){ + std::cout << "control shutdown !!!" << std::endl; + + openarm_->disable_all(); +} + +void Control::SetParameter( + const std::vector& Kp, + const std::vector& Kd, + const std::vector& Fc, + const std::vector& k, + const std::vector& Fv, + const std::vector& Fo) + { + Kp_ = Kp; + Kd_ = Kd; + Fc_ = Fc; + k_ = k; + Fv_ = Fv; + Fo_ = Fo; + } + + +bool Control::bilateral_step() +{ + // get motor status + std::vector arm_motor_states; + const auto& arm_motors = openarm_->get_arm().get_motors(); + for (size_t i = 0; i < arm_motors.size(); ++i) { + const auto& motor = arm_motors[i]; + arm_motor_states.push_back({ + motor.get_position(), + motor.get_velocity(), + 0 + }); + } + + std::vector gripper_motor_states; + const auto& gripper_motors = openarm_->get_gripper().get_motors(); + for (size_t i = 0; i < gripper_motors.size(); ++i) { + const auto& motor = gripper_motors[i]; + gripper_motor_states.push_back({ + motor.get_position(), + motor.get_velocity(), + 0 + }); + } + + // convert joint to motor + std::vector joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states); + std::vector joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); + + // set reponse + robot_state_->arm_state().set_all_responses(joint_arm_states); + robot_state_->hand_state().set_all_responses(joint_gripper_states); + + size_t arm_dof = robot_state_->arm_state().get_size(); + size_t gripper_dof = robot_state_->hand_state().get_size(); + + std::vector joint_arm_positions(arm_dof, 0.0); + std::vector joint_arm_velocities(arm_dof, 0.0); + std::vector joint_arm_efforts(arm_dof, 0.0); + + std::vector joint_gripper_positions(gripper_dof, 0.0); + std::vector joint_gripper_velocities(gripper_dof, 0.0); + std::vector joint_gripper_efforts(gripper_dof, 0.0); + + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_positions[i] = joint_arm_states[i].position; + joint_arm_velocities[i] = joint_arm_states[i].velocity; + } + + for (size_t i = 0; i < gripper_dof; ++i) { + joint_gripper_positions[i] = joint_gripper_states[i].position; + joint_gripper_velocities[i] = joint_gripper_states[i].velocity; + } + + std::vector gravity(arm_dof, 0.0); + std::vector coriolis(arm_dof, 0.0); + std::vector friction(arm_dof + gripper_dof, 0.0); + + std::vector joint_arm_states_ref = robot_state_->arm_state().get_all_references(); + std::vector joint_gripper_states_ref = robot_state_->hand_state().get_all_references(); + + std::vector joint_arm_positions_ref(arm_dof); + + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_positions_ref[i] = joint_arm_states_ref[i].position; + } + + + if (role_ == ROLE_LEADER) { + dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data()); + dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); + + } else if (role_ == ROLE_FOLLOWER) { + dynamics_f_->GetGravity(joint_arm_positions.data(), gravity.data()); + dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); + } + + + // Friction (compute joint friction) + for (size_t i = 0; i < joint_arm_velocities.size(); ++i) + ComputeFriction(joint_arm_velocities.data(), friction.data(), i); + for (size_t i = 0; i < joint_gripper_velocities.size(); ++i) + ComputeFriction(joint_gripper_velocities.data(), friction.data(), joint_arm_velocities.size() + i); + + // set gravity and friciton comp joint torque value + for(size_t i = 0 ; i < arm_dof; i++){ + joint_arm_states_ref[i].effort = gravity[i] + friction[i]; + } + + for(size_t i = 0 ; i < gripper_dof; i++){ + joint_gripper_states_ref[i].effort = friction[i + arm_dof]; + } + + std::vector motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_states_ref); + std::vector motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref); + + // kp kd q dq tau + std::vector arm_cmds; + arm_cmds.reserve(arm_dof); + for (size_t i = 0; i < arm_dof; ++i) { + arm_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity, motor_arm_states[i].effort}); + } + + // gripper command mit param + std::vector gripper_cmds; + gripper_cmds.reserve(gripper_dof); + for (size_t i = 0; i < gripper_dof; ++i) { + gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i + arm_dof],Kd_[i + arm_dof], motor_gripper_states[i].position, motor_gripper_states[i].velocity, motor_gripper_states[i].effort}); + } + + // send command to arm + openarm_->get_arm().mit_control_all(arm_cmds); + // send command to gripper + openarm_->get_gripper().mit_control_all(gripper_cmds); + + std::this_thread::sleep_for(std::chrono::microseconds(200)); + + openarm_->recv_all(220); + + return true; + + } + + + bool Control::unilateral_step(){ + + // get motor status + std::vector arm_motor_states; + for (const auto& motor : openarm_->get_arm().get_motors()) { + arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector gripper_motor_states; + for (const auto& motor : openarm_->get_gripper().get_motors()) { + gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + // convert joint to motor + std::vector joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states); + std::vector joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); + + // set reponse + robot_state_->arm_state().set_all_responses(joint_arm_states); + robot_state_->hand_state().set_all_responses(joint_gripper_states); + + size_t arm_dof = robot_state_->arm_state().get_size(); + size_t gripper_dof = robot_state_->hand_state().get_size(); + + std::vector joint_arm_positions(arm_dof, 0.0); + std::vector joint_arm_velocities(arm_dof, 0.0); + std::vector joint_gripper_positions(gripper_dof, 0.0); + std::vector joint_gripper_velocities(gripper_dof, 0.0); + + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_positions[i] = joint_arm_states[i].position; + joint_arm_velocities[i] = joint_arm_states[i].velocity; + } + + for (size_t i = 0; i < gripper_dof; ++i) { + joint_gripper_positions[i] = joint_gripper_states[i].position; + joint_gripper_velocities[i] = joint_gripper_states[i].velocity; + } + + std::vector gravity(arm_dof, 0.0); + std::vector coriolis(arm_dof, 0.0); + std::vector friction(arm_dof + gripper_dof, 0.0); + + if (role_ == ROLE_LEADER) { + + // calc dynamics + dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data()); + dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); + + for (size_t i = 0; i < joint_arm_velocities.size(); ++i) + ComputeFriction(joint_arm_velocities.data(), friction.data(), i); + + for (size_t i = 0; i < joint_gripper_velocities.size(); ++i) + ComputeFriction(joint_gripper_velocities.data(), friction.data(), arm_dof + i); + + + // arm joint state + std::vector joint_arm_state_torque(arm_dof); + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_state_torque[i].position = joint_arm_positions[i]; + joint_arm_state_torque[i].velocity = joint_arm_velocities[i]; + joint_arm_state_torque[i].effort = gravity[i] + friction[i]*0.3 + coriolis[i]*0.1; + } + + // gripper joint state + std::vector joint_gripper_state_torque(gripper_dof); + for (size_t i = 0; i < gripper_dof; ++i) { + joint_gripper_state_torque[i].position = joint_gripper_positions[i]; + joint_gripper_state_torque[i].velocity = joint_gripper_velocities[i]; + joint_gripper_state_torque[i].effort = friction[arm_dof + i]*0.3 ; + } + + std::vector motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_state_torque); + std::vector motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque); + + // arm command mit param + std::vector arm_cmds; + arm_cmds.reserve(arm_dof); + for (size_t i = 0; i < arm_dof; ++i) { + arm_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort}); + } + + // gripper command mit param + std::vector gripper_cmds; + gripper_cmds.reserve(gripper_dof); + for (size_t i = 0; i < gripper_dof; ++i) { + gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_gripper_states[i].effort}); + } + + // send command to arm + openarm_->get_arm().mit_control_all(arm_cmds); + // send command to gripper + openarm_->get_gripper().mit_control_all(gripper_cmds); + + openarm_->recv_all(200); + + return true; + + } + + else if (role_ == ROLE_FOLLOWER) { + + std::vector joint_arm_states_ref = robot_state_->arm_state().get_all_references(); + std::vector joint_hand_states_ref = robot_state_->hand_state().get_all_references(); + + // Joint β†’ Motor + std::vector arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_states_ref); + std::vector hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref); + + std::vector arm_cmds; + arm_cmds.reserve(arm_motor_refs.size()); + for (size_t i = 0; i < arm_motor_refs.size(); ++i) { + arm_cmds.emplace_back(openarm::damiao_motor::MITParam{ + Kp_[i], + Kd_[i], + arm_motor_refs[i].position, + arm_motor_refs[i].velocity, + 0.0 + }); + } + + std::vector hand_cmds; + hand_cmds.reserve(hand_motor_refs.size()); + for (size_t i = 0; i < hand_motor_refs.size(); ++i) { + hand_cmds.emplace_back(openarm::damiao_motor::MITParam{ + Kp_[i + arm_dof], + Kd_[i + arm_dof], + hand_motor_refs[i].position, + hand_motor_refs[i].velocity, + 0.0 + }); + } + + openarm_->get_arm().mit_control_all(arm_cmds); + openarm_->get_gripper().mit_control_all(hand_cmds); + + openarm_->recv_all(200); + + return true; + } + + return true; + + } + + void Control::ComputeFriction(const double* velocity, double* friction, size_t index) + { + if (TANHFRIC) { + const double amp_tmp = 1.0; + const double coef_tmp = 0.1; + + const double v = velocity[index]; + const double Fc = Fc_.at(index); + const double k = k_.at(index); + const double Fv = Fv_.at(index); + const double Fo = Fo_.at(index); + + friction[index] = amp_tmp * Fc * std::tanh(coef_tmp * k * v) + Fv * v + Fo; + } else { + friction[index] = velocity[index] * Dn_.at(index); + } + } + + bool Control::AdjustPosition(void) + { + int nstep = 220; + double alpha; + + std::vector arm_motor_states; + for (const auto& motor : openarm_->get_arm().get_motors()) { + arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector gripper_motor_states; + for (const auto& motor : openarm_->get_gripper().get_motors()) { + gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector joint_arm_now = openarmjointconverter_->motor_to_joint(arm_motor_states); + std::vector joint_hand_now = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); + + std::vector joint_arm_goal(NMOTORS-1); + for (size_t i = 0; i < NMOTORS-1; ++i) { + joint_arm_goal[i].position = INITIAL_POSITION[i]; + joint_arm_goal[i].velocity = 0.0; + joint_arm_goal[i].effort = 0.0; + } + + std::vector joint_hand_goal(joint_hand_now.size()); + for (size_t i = 0; i < joint_hand_goal.size(); ++i) { + joint_hand_goal[i].position = 0.0; + joint_hand_goal[i].velocity = 0.0; + joint_hand_goal[i].effort = 0.0; + } + + std::vector kp_arm_temp = {50, 50.0, 50.0, 50.0, 10.0, 10.0, 10.0}; + std::vector kd_arm_temp = {1.2, 1.2, 1.2, 1.2, 0.3, 0.2, 0.3}; + + std::vector kp_hand_temp = {10.0}; + std::vector kd_hand_temp = {0.5}; + + for (int step = 0; step < nstep; ++step) { + alpha = static_cast(step + 1) / nstep; + + std::vector joint_arm_interp(NMOTORS-1); + for (size_t i = 0; i < NMOTORS-1; ++i) { + joint_arm_interp[i].position = joint_arm_goal[i].position * alpha + joint_arm_now[i].position * (1.0 - alpha); + joint_arm_interp[i].velocity = 0.0; + } + + std::vector joint_hand_interp(joint_hand_goal.size()); + for (size_t i = 0; i < joint_hand_interp.size(); ++i) { + joint_hand_interp[i].position = joint_hand_goal[i].position * alpha + joint_hand_now[i].position * (1.0 - alpha); + joint_hand_interp[i].velocity = 0.0; + } + + std::vector arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_interp); + std::vector hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_interp); + + std::vector arm_cmds; + arm_cmds.reserve(arm_motor_refs.size()); + for (size_t i = 0; i < arm_motor_refs.size(); ++i) { + arm_cmds.emplace_back(openarm::damiao_motor::MITParam{ + kp_arm_temp[i], + kd_arm_temp[i], + arm_motor_refs[i].position, + arm_motor_refs[i].velocity, + 0.0 + }); + } + + std::vector hand_cmds; + hand_cmds.reserve(hand_motor_refs.size()); + for (size_t i = 0; i < hand_motor_refs.size(); ++i) { + hand_cmds.emplace_back(openarm::damiao_motor::MITParam{ + kp_hand_temp[i], + kd_hand_temp[i], + hand_motor_refs[i].position, + hand_motor_refs[i].velocity, + 0.0 + }); + } + + openarm_->get_arm().mit_control_all(arm_cmds); + openarm_->get_gripper().mit_control_all(hand_cmds); + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + openarm_->recv_all(); + } + + std::vector arm_motor_states_final; + for (const auto& motor : openarm_->get_arm().get_motors()) { + arm_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector gripper_motor_states_final; + for (const auto& motor : openarm_->get_gripper().get_motors()) { + gripper_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector joint_arm_final = openarmjointconverter_->motor_to_joint(arm_motor_states_final); + std::vector joint_hand_final = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final); + + robot_state_->arm_state().set_all_references(joint_arm_final); + robot_state_->hand_state().set_all_references(joint_hand_final); + + return true; + } + + bool Control::DetectVibration(const double* velocity, bool* what_axis) + { + bool vibration_detected = false; + + for (int i = 0; i < NJOINTS; ++i) { + what_axis[i] = false; + + velocity_buffer_[i].push_back(velocity[i]); + if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE) + velocity_buffer_[i].pop_front(); + + if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE) + continue; + + double mean = std::accumulate( + velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0 + ) / velocity_buffer_[i].size(); + + double var = 0.0; + for (double v : velocity_buffer_[i]) { + var += (v - mean) * (v - mean); + } + + double stddev = std::sqrt(var / velocity_buffer_[i].size()); + + if (stddev > VIB_THRESHOLD) { + what_axis[i] = true; + vibration_detected = true; + std::cout << "[VIBRATION] Joint " << i << " stddev: " << stddev << std::endl; + } + } + + return vibration_detected; + } \ No newline at end of file diff --git a/src/controller/control.hpp b/src/controller/control.hpp new file mode 100644 index 0000000..ed283c8 --- /dev/null +++ b/src/controller/control.hpp @@ -0,0 +1,104 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once + +// #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +class Control +{ + openarm::can::socket::OpenArm* openarm_; + + double Ts_; + int role_; + + size_t arm_motor_num_; + size_t hand_motor_num_; + + Differentiator *differentiator_; + OpenArmJointConverter *openarmjointconverter_; + OpenArmJGripperJointConverter *openarmgripperjointconverter_; + + std::shared_ptr robot_state_; + + std::string arm_type_; + + Dynamics *dynamics_f_; + Dynamics *dynamics_l_; + + double oblique_coordinates_force; + double oblique_coordinates_position; + + // for easy logging + // std::vector> velocity_log_; // (differ_velocity, motor_velocity) + // std::string log_file_path_ = "../data/velocity_comparison.csv"; + static constexpr int VEL_WINDOW_SIZE = 10; + static constexpr double VIB_THRESHOLD = 0.7; // [rad/s] + std::deque velocity_buffer_[NJOINTS]; + + public: + Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, size_t arm_joint_num, size_t hand_motor_num); + Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, std::string arm_type, size_t arm_joint_num, size_t hand_motor_num); + ~Control(); + + std::shared_ptr response_; + std::shared_ptr reference_; + + std::vector Dn_, Kp_, Kd_,Fc_, k_, Fv_, Fo_; + + // bool Setup(void); + void Setstate(int state); + void Shutdown(void); + + void SetParameter( + const std::vector& Kp, + const std::vector& Kd, + const std::vector& Fc, + const std::vector& k, + const std::vector& Fv, + const std::vector& Fo); + + bool AdjustPosition(void); + + // Compute torque based on bilateral + bool bilateral_step(); + bool unilateral_step(); + + // NOTE! Control() class operates on "joints", while the underlying + // classes operates on "actuators". The following functions map + // joints to actuators. + + void ComputeJointPosition(const double *motor_position, double *joint_position); + void ComputeJointVelocity(const double *motor_velocity, double *joint_velocity); + void ComputeMotorTorque(const double *joint_torque, double *motor_torque); + + // void ComputeFriction(const double *velocity, double *friction); + void ComputeFriction(const double* velocity, double* friction, size_t index); + void ComputeGravity(const double *position, double *gravity); + bool DetectVibration(const double* velocity, bool *what_axis); +}; diff --git a/src/controller/diff.hpp b/src/controller/diff.hpp new file mode 100644 index 0000000..19f8550 --- /dev/null +++ b/src/controller/diff.hpp @@ -0,0 +1,74 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once +// #include "controller/global.hpp" +// #include + +#include + +class Differentiator +{ + private: + double Ts_; // Sampling time + double velocity_z1_[NMOTORS] = {0.0}; // Velocity (1 step before) + double position_z1_[NMOTORS] = {0.0}; // Position (1 step before) + double acc_z1_[NMOTORS] = {0.0}; + double acc_[NMOTORS] = {0.0}; + public: + Differentiator(double Ts) : Ts_(Ts) {} + + /* + * Compute the motor speed by taking the derivative of + * the motion. + */ + void Differentiate(const double *position, double *velocity) + { + double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY); + double b = a * CUTOFF_FREQUENCY; + + for (int i = 0; i < NMOTORS; i++) { + if (position_z1_[i] == 0.0) { + position_z1_[i] = position[i]; + } + + velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]); + position_z1_[i] = position[i]; + velocity_z1_[i] = velocity[i]; + } + + } + + void Differentiate_w_obs(const double *position, double *velocity, double *mass, double *input_torque) + { + + double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY); + double b = a * CUTOFF_FREQUENCY; + + for (int i = 0; i < NMOTORS; i++) { + if (position_z1_[i] == 0.0000000) { + position_z1_[i] = position[i]; + acc_z1_[i] = acc_[i]; + } + + acc_[i] = acc_z1_[i] * a + b * (input_torque[i] / (mass[i])); + velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]) + acc_[i]; + position_z1_[i] = position[i]; + velocity_z1_[i] = velocity[i]; + acc_z1_[i] = acc_[i]; + + } + + } +}; diff --git a/src/controller/dynamics.cpp b/src/controller/dynamics.cpp new file mode 100644 index 0000000..45530b2 --- /dev/null +++ b/src/controller/dynamics.cpp @@ -0,0 +1,243 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#include + +Dynamics::Dynamics(std::string urdf_path, std::string start_link, std::string end_link) +{ + this->urdf_path = urdf_path; + this->start_link = start_link; + this->end_link = end_link; +} + +Dynamics::~Dynamics(){} + + +bool Dynamics::Init() +{ + std::ifstream file(urdf_path); + if (!file.is_open()) { + fprintf(stderr, "Failed to open URDF file: %s\n", urdf_path.c_str()); + return false; + } + + std::stringstream buffer; + buffer << file.rdbuf(); + file.close(); + + urdf_model_interface = urdf::parseURDF(buffer.str()); + if (!urdf_model_interface) { + fprintf(stderr, "Failed to parse URDF: %s\n", urdf_path.c_str()); + return false; + } + + if (!kdl_parser::treeFromUrdfModel(*urdf_model_interface, kdl_tree)) { + fprintf(stderr, "Failed to extract KDL tree: %s\n", urdf_path.c_str()); + return false; + } + + if (!kdl_tree.getChain(start_link, end_link, kdl_chain)) { + fprintf(stderr, "Failed to get KDL chain\n"); + return false; + } + + std::cout << "[GetGravity] kdl_chain.getNrOfJoints() = " << kdl_chain.getNrOfJoints() << std::endl; + + coriolis_forces.resize(kdl_chain.getNrOfJoints()); + gravity_forces.resize(kdl_chain.getNrOfJoints()); + inertia_matrix.resize(kdl_chain.getNrOfJoints()); + + + coriolis_forces.data.setZero(); + gravity_forces.data.setZero(); + inertia_matrix.data.setZero(); + + solver = std::make_unique( + kdl_chain, KDL::Vector(0, 0.0, -9.81)); + + return true; +} + +void Dynamics::GetGravity(const double *motor_position, double *gravity) +{ + + const auto njoints = kdl_chain.getNrOfJoints(); + + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + + for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + q_(i) = motor_position[i]; + } + + solver->JntToGravity(q_, gravity_forces); + for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + gravity[i] = gravity_forces(i); + } +} + +void Dynamics::GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis) { + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + KDL::JntArray q_dot(kdl_chain.getNrOfJoints()); + + for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + q_(i) = motor_position[i]; + q_dot(i) = motor_velocity[i]; + } + + solver->JntToCoriolis(q_, q_dot, coriolis_forces); + + for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + coriolis[i] = coriolis_forces(i); + } +} + +void Dynamics::GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag) +{ + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + KDL::JntSpaceInertiaMatrix inertia_matrix(kdl_chain.getNrOfJoints()); + for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + q_(i) = motor_position[i]; + } + + solver->JntToMass(q_, inertia_matrix); + + for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + inertia_diag[i] = inertia_matrix(i, i); + } + +} + +void Dynamics::GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian) +{ + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { + q_(i) = motor_position[i]; + } + + KDL::Jacobian kdl_jac(kdl_chain.getNrOfJoints()); + KDL::ChainJntToJacSolver jac_solver(kdl_chain); + jac_solver.JntToJac(q_, kdl_jac); + + jacobian = Eigen::MatrixXd(6, kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < 6; ++i) { + for (size_t j = 0; j < kdl_chain.getNrOfJoints(); ++j) { + jacobian(i, j) = kdl_jac(i, j); + } + } + +} + +void Dynamics::GetNullSpace(const double* motor_position, Eigen::MatrixXd& nullspace) { + const size_t dof = kdl_chain.getNrOfJoints(); + + bool use_stable_svd = false; + + Eigen::MatrixXd J; + GetJacobian(motor_position, J); + + Eigen::MatrixXd J_pinv; + + if (use_stable_svd) { + Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); + double tol = 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff(); + Eigen::VectorXd singularValuesInv = svd.singularValues(); + for (int i = 0; i < singularValuesInv.size(); ++i) { + singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0; + } + J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); + } else { + J_pinv = J.transpose() * (J * J.transpose()).inverse(); + } + + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); + nullspace = I - J_pinv * J; + + // std::cout << "[INFO] Null space projector computed.\n"; +} + + +void Dynamics::GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixXd& nullspace_T) +{ + const size_t dof = kdl_chain.getNrOfJoints(); + bool use_stable_svd = false; + + Eigen::MatrixXd J; + GetJacobian(motor_position, J); + + Eigen::MatrixXd J_pinv; + + if (use_stable_svd) { + Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); + double tol = 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff(); + Eigen::VectorXd singularValuesInv = svd.singularValues(); + for (int i = 0; i < singularValuesInv.size(); ++i) { + singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0; + } + J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); + } else { + J_pinv = J.transpose() * (J * J.transpose()).inverse(); + } + + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); + Eigen::MatrixXd N = I - J_pinv * J; + + nullspace_T = N.transpose(); +} + +void Dynamics::GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p) +{ + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { + q_(i) = motor_position[i]; + } + + KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); + KDL::Frame kdl_frame; + + if (fk_solver.JntToCart(q_, kdl_frame) < 0) { + // std::cerr << "[KDL] FK failed in GetEECordinate!" << std::endl; + return; + } + + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + R(i, j) = kdl_frame.M(i, j); + + p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2]; +} + +void Dynamics::GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p) +{ + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { + q_(i) = motor_position[i]; + } + + KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); + KDL::Frame kdl_frame; + + if (fk_solver.JntToCart(q_, kdl_frame, kdl_chain.getNrOfSegments() - 1) < 0) { + // std::cerr << "[KDL] FK failed in GetPreEECordinate!" << std::endl; + return; + } + + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) + R(i, j) = kdl_frame.M(i, j); + + p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2]; + + + +} diff --git a/src/controller/dynamics.hpp b/src/controller/dynamics.hpp new file mode 100644 index 0000000..2f3779e --- /dev/null +++ b/src/controller/dynamics.hpp @@ -0,0 +1,72 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +/* + * Compute gravity and inertia compensation using Orocos + * Kinematics and Dynamics Library (KDL). + */ +class Dynamics +{ + private: + std::shared_ptr urdf_model_interface; + + std::string urdf_path; + std::string start_link; + std::string end_link; + + KDL::JntSpaceInertiaMatrix inertia_matrix; + KDL::JntArray q; + KDL::JntArray q_d; + KDL::JntArray coriolis_forces; + KDL::JntArray gravity_forces; + + KDL::JntArray biasangle; + + KDL::Tree kdl_tree; + KDL::Chain kdl_chain; + std::unique_ptr solver; + + public: + Dynamics(std::string urdf_path, std::string start_link, std::string end_link); + ~Dynamics(); + + bool Init(); + void GetGravity(const double *motor_position, double *gravity); + void GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis); + void GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag); + + void GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian); + + void GetNullSpace(const double *motor_positon, Eigen::MatrixXd &nullspace); + + void GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixXd& nullspace_T); + + void GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); + + void GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); +}; diff --git a/src/joint_state_converter.hpp b/src/joint_state_converter.hpp new file mode 100644 index 0000000..cce0cd0 --- /dev/null +++ b/src/joint_state_converter.hpp @@ -0,0 +1,107 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once + +#include +#include + +// Represents the state of a single joint +// struct JointState { +// double position = 0.0; +// double velocity = 0.0; +// double effort = 0.0; +// }; + +// Represents the state of a single motor (raw values) +struct MotorState { + double position = 0.0; + double velocity = 0.0; + double effort = 0.0; +}; + +// Abstract base class for converting between motor and joint states +class MotorJointConverter { +public: + virtual ~MotorJointConverter() = default; + + // MotorState vector β†’ JointState vector + virtual std::vector motor_to_joint(const std::vector& motor_states) const = 0; + + // JointState vector β†’ MotorState vector + virtual std::vector joint_to_motor(const std::vector& joint_states) const = 0; + + virtual size_t get_joint_count() const = 0; +}; + +// assume motor num equals to joint num +class OpenArmJointConverter : public MotorJointConverter { + public: + explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) { + std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl; + } + + std::vector motor_to_joint(const std::vector& m) const override { + // std::cout << "joint num conv : " << m.size() << std::endl; + + std::vector j(m.size()); + for (size_t i = 0; i < m.size(); ++i){ + j[i] = {m[i].position, m[i].velocity, m[i].effort}; + + } + + return j; + } + + std::vector joint_to_motor(const std::vector& j) const override { + std::vector m(j.size()); + for (size_t i = 0; i < j.size(); ++i) + m[i] = {j[i].position, j[i].velocity, j[i].effort}; + return m; + } + + size_t get_joint_count() const override { return joint_count_; } + + private: + size_t joint_count_; + }; + + // assume motor num equals to joint num + class OpenArmJGripperJointConverter : public MotorJointConverter { + public: + explicit OpenArmJGripperJointConverter(size_t joint_count) : joint_count_(joint_count) { + std::cout << "Gripper joint converter joint_count is : " << joint_count << std::endl; + } + + std::vector motor_to_joint(const std::vector& m) const override { + std::vector j(m.size()); + for (size_t i = 0; i < m.size(); ++i) { + j[i] = {m[i].position, m[i].velocity, m[i].effort}; + } + return j; + } + + std::vector joint_to_motor(const std::vector& j) const override { + std::vector m(j.size()); + for (size_t i = 0; i < j.size(); ++i) { + m[i] = {j[i].position, j[i].velocity, j[i].effort}; + } + return m; + } + + size_t get_joint_count() const override { return joint_count_; } + + private: + size_t joint_count_; + }; \ No newline at end of file diff --git a/src/openarm_constants.hpp b/src/openarm_constants.hpp new file mode 100644 index 0000000..f9b3cf6 --- /dev/null +++ b/src/openarm_constants.hpp @@ -0,0 +1,113 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once + +#include +#include +#include +#include +#include + +constexpr double PI = 3.14159265358979323846; + +// 8piecies including gripper +// Joints and motors don't always have a one-to-one correspondence +#define NJOINTS 8 +#define NMOTORS 8 + +#define ROLE_LEADER 1 +#define ROLE_FOLLOWER 2 + +#define CAN0 "can0" +#define CAN1 "can1" + +#define CAN2 "can2" +#define CAN3 "can3" + +#define TANHFRIC true + +#define FREQUENCY 1000.0 +#define CUTOFF_FREQUENCY 90.0 + +#define ELBOWLIMIT 0.0 + +static const double INITIAL_POSITION[NMOTORS] = { + 0, 0, 0, PI/5.0, 0, 0, 0, 0 +}; + +// safety limit position +static const double position_limit_max_L[] = { (2.0/3.0)*PI, PI, PI/2.0, PI, PI/2.0, PI/2.0, PI/2.0, PI }; +static const double position_limit_min_L[] = { -(2.0/3.0)*PI, -PI/2.0, -PI/2.0, ELBOWLIMIT, -PI/2.0, -PI/2.0, -PI/2.0, -PI }; +static const double position_limit_max_F[] = { (2.0/3.0)*PI, PI, PI/2.0, PI, PI/2.0, PI/2.0, PI/2.0, PI }; +static const double position_limit_min_F[] = { -(2.0/3.0)*PI, -PI/2.0, -PI/2.0, ELBOWLIMIT, -PI/2.0, -PI/2.0, -PI/2.0, -PI }; + +// sefaty limit velocity +static const double velocity_limit_L[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0}; +static const double velocity_limit_F[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0}; +// sefaty limit effort +static const double effort_limit_L[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0}; +static const double effort_limit_F[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0}; + + +// Motor configuration structure +struct MotorConfig { + std::vector arm_motor_types; + std::vector arm_send_can_ids; + std::vector arm_recv_can_ids; + openarm::damiao_motor::MotorType gripper_motor_type; + uint32_t gripper_send_can_id; + uint32_t gripper_recv_can_id; +}; + +// Global default motor configuration +static const MotorConfig DEFAULT_MOTOR_CONFIG = { + // Standard 7-DOF arm motor configuration + {openarm::damiao_motor::MotorType::DM8009, + openarm::damiao_motor::MotorType::DM8009, + openarm::damiao_motor::MotorType::DM4340, + openarm::damiao_motor::MotorType::DM4340, + openarm::damiao_motor::MotorType::DM4310, + openarm::damiao_motor::MotorType::DM4310, + openarm::damiao_motor::MotorType::DM4310}, + + // Standard CAN IDs for arm motors + {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}, + {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}, + + // Standard gripper configuration + openarm::damiao_motor::MotorType::DM4310, + 0x08, + 0x18}; + +// opening function +inline void printOpenArmBanner() { + std::cout << R"( + + β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ•— + β–ˆβ–ˆβ•”β•β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•”β•β•β•β•β•β–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ•‘ + β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•”β–ˆβ–ˆβ•— β–ˆβ–ˆβ•‘β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•‘β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ•”β–ˆβ–ˆβ–ˆβ–ˆβ•”β–ˆβ–ˆβ•‘ + β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•”β•β•β•β• β–ˆβ–ˆβ•”β•β•β• β–ˆβ–ˆβ•‘β•šβ–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘β•šβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ•‘ + β•šβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘ β•šβ–ˆβ–ˆβ–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β•šβ•β• β–ˆβ–ˆβ•‘ + β•šβ•β•β•β•β•β• β•šβ•β• β•šβ•β•β•β•β•β•β•β•šβ•β• β•šβ•β•β•β•β•šβ•β• β•šβ•β•β•šβ•β• β•šβ•β•β•šβ•β• β•šβ•β• + +β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•—β–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•—β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•— β–ˆβ–ˆβ•—β–ˆβ–ˆβ•—β–ˆβ–ˆβ•—β–ˆβ–ˆβ•— +β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β•šβ•β•β–ˆβ–ˆβ•”β•β•β•β–ˆβ–ˆβ•”β•β•β•β•β•β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•”β•β•β•β•β•β–ˆβ–ˆβ•”β•β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ•‘β•šβ•β•β–ˆβ–ˆβ•”β•β•β•β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•”β•β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ +β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•”β–ˆβ–ˆβ•— β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ +β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•”β•β•β• β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘β•šβ–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•”β•β•β–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β•šβ•β•β•šβ•β•β•šβ•β•β•šβ•β• +β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ•‘β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•— β•šβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β•šβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ•‘ β•šβ–ˆβ–ˆβ–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘ β–ˆβ–ˆβ•‘β•šβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•”β•β–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ–ˆβ•—β–ˆβ–ˆβ•—β–ˆβ–ˆβ•—β–ˆβ–ˆβ•—β–ˆβ–ˆβ•— +β•šβ•β•β•β•β•β• β•šβ•β•β•šβ•β•β•β•β•β•β•β•šβ•β• β•šβ•β• β•šβ•β• β•šβ•β•β•β•β•β•β•β•šβ•β• β•šβ•β•β•šβ•β• β•šβ•β•β•šβ•β•β•β•β•β•β• β•šβ•β•β•β•β•β• β•šβ•β•β•β•β•β• β•šβ•β• β•šβ•β•β•β• β•šβ•β• β•šβ•β• β•šβ•β• β•šβ•β•β•β•β•β• β•šβ•β•β•β•β•β•β•β•šβ•β•β•šβ•β•β•šβ•β•β•šβ•β• + + )" << std::endl; +} diff --git a/src/openarm_port/joint_mapper.cpp b/src/openarm_port/joint_mapper.cpp new file mode 100644 index 0000000..d81651e --- /dev/null +++ b/src/openarm_port/joint_mapper.cpp @@ -0,0 +1,57 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#include "joint_mapper.hpp" +#include + +JointMapper::JointMapper() {} + +JointMapper::~JointMapper() {} + +// Only copying for now +void JointMapper::motor_to_joint_position(const double *motor_position, + double *joint_position) { + joint_position[0] = motor_position[0]; + joint_position[1] = motor_position[1]; + joint_position[2] = motor_position[2]; + joint_position[3] = motor_position[3]; + joint_position[4] = motor_position[4]; + joint_position[5] = motor_position[5]; + joint_position[6] = motor_position[6]; + joint_position[7] = motor_position[7]; +} + +void JointMapper::motor_to_joint_velocity(const double *motor_velocity, + double *joint_velocity) { + joint_velocity[0] = motor_velocity[0]; + joint_velocity[1] = motor_velocity[1]; + joint_velocity[2] = motor_velocity[2]; + joint_velocity[3] = motor_velocity[3]; + joint_velocity[4] = motor_velocity[4]; + joint_velocity[5] = motor_velocity[5]; + joint_velocity[6] = motor_velocity[6]; + joint_velocity[7] = motor_velocity[7]; +} + +void JointMapper::joint_to_motor_torque(const double *joint_torque, + double *motor_torque) { + motor_torque[0] = joint_torque[0]; + motor_torque[1] = joint_torque[1]; + motor_torque[2] = joint_torque[2]; + motor_torque[3] = joint_torque[3]; + motor_torque[4] = joint_torque[4]; + motor_torque[5] = joint_torque[5]; + motor_torque[6] = joint_torque[6]; + motor_torque[7] = joint_torque[7]; +} diff --git a/src/openarm_port/joint_mapper.hpp b/src/openarm_port/joint_mapper.hpp new file mode 100644 index 0000000..d27fb95 --- /dev/null +++ b/src/openarm_port/joint_mapper.hpp @@ -0,0 +1,29 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once + +#include "../openarm_constants.hpp" + +class JointMapper { +public: + JointMapper(); + ~JointMapper(); + + void motor_to_joint_position(const double *motor_position, + double *joint_position); + void motor_to_joint_velocity(const double *motor_velocity, + double *joint_velocity); + void joint_to_motor_torque(const double *joint_torque, double *motor_torque); +}; diff --git a/src/openarm_port/openarm_init.cpp b/src/openarm_port/openarm_init.cpp new file mode 100644 index 0000000..70876e4 --- /dev/null +++ b/src/openarm_port/openarm_init.cpp @@ -0,0 +1,87 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#include "openarm_init.hpp" +#include "../openarm_constants.hpp" + +namespace openarm_init { + +openarm::can::socket::OpenArm * +OpenArmInitializer::initialize_openarm(const std::string &can_device, + bool enable_debug) { + + MotorConfig config = DEFAULT_MOTOR_CONFIG; + return initialize_openarm(can_device, config, enable_debug); +} + +openarm::can::socket::OpenArm * +OpenArmInitializer::initialize_openarm(const std::string &can_device, + const MotorConfig &config, + bool enable_debug) { + + // Create OpenArm instance + openarm::can::socket::OpenArm *openarm = + new openarm::can::socket::OpenArm(can_device, enable_debug); + + // Perform common initialization + initialize_(openarm, config, enable_debug); + + return openarm; +} + +void OpenArmInitializer::initialize_(openarm::can::socket::OpenArm *openarm, + const MotorConfig &config, + bool enable_debug) { + + if (enable_debug) { + std::cout << "Initializing arm motors..." << std::endl; + } + + // Initialize arm motors + openarm->init_arm_motors(config.arm_motor_types, config.arm_send_can_ids, + config.arm_recv_can_ids); + + if (enable_debug) { + std::cout << "Initializing gripper motor..." << std::endl; + } + + // Initialize gripper motor + openarm->init_gripper_motor(config.gripper_motor_type, + config.gripper_send_can_id, + config.gripper_recv_can_id); + + // Set callback mode for all motors + openarm->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); + + if (enable_debug) { + std::cout << "Enabling motors..." << std::endl; + } + + // Enable all motors with appropriate delays + openarm->enable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm->recv_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Print motor counts for verification + if (enable_debug) { + size_t arm_motor_num = openarm->get_arm().get_motors().size(); + size_t gripper_motor_num = openarm->get_gripper().get_motors().size(); + + std::cout << "Arm motor count: " << arm_motor_num << std::endl; + std::cout << "Gripper motor count: " << gripper_motor_num << std::endl; + } +} + +} // namespace openarm_init diff --git a/src/openarm_port/openarm_init.hpp b/src/openarm_port/openarm_init.hpp new file mode 100644 index 0000000..08a652a --- /dev/null +++ b/src/openarm_port/openarm_init.hpp @@ -0,0 +1,56 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once + +#include "../openarm_constants.hpp" +#include +#include +#include +#include +#include + +namespace openarm_init { + +class OpenArmInitializer { +public: + /** + * @brief Initialize OpenArm with default configuration + * @param can_device CAN device name (e.g., "can0", "can1") + * @param enable_debug Enable debug output + * @return Initialized OpenArm pointer (caller owns memory) + */ + static openarm::can::socket::OpenArm * + initialize_openarm(const std::string &can_device, bool enable_debug = true); + + /** + * @brief Initialize OpenArm with custom motor configuration + * @param can_device CAN device name + * @param config Custom motor configuration + * @param enable_debug Enable debug output + * @return Initialized OpenArm pointer (caller owns memory) + */ + static openarm::can::socket::OpenArm * + initialize_openarm(const std::string &can_device, const MotorConfig &config, + bool enable_debug = true); + +private: + /** + * @brief Common initialization steps for OpenArm + */ + static void initialize_(openarm::can::socket::OpenArm *openarm, + const MotorConfig &config, bool enable_debug); +}; + +} // namespace openarm_init diff --git a/src/periodic_timer_thread.hpp b/src/periodic_timer_thread.hpp new file mode 100644 index 0000000..3544ed3 --- /dev/null +++ b/src/periodic_timer_thread.hpp @@ -0,0 +1,138 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include + + +class PeriodicTimerThread { +public: + explicit PeriodicTimerThread(double hz = 1000.0) + : is_running_(false) + { + if (hz <= 0.0) { + throw std::invalid_argument("Hz must be positive"); + } + period_us_.store(static_cast(1e6 / hz)); + } + + virtual ~PeriodicTimerThread() { + stop_thread(); + } + + virtual void start_thread() { + start_thread_base(); + } + + virtual void stop_thread() { + stop_thread_base(); + } + + int64_t get_elapsed_time_us() const { + return last_elapsed_us_.load(); + } + + void set_period(double hz) { + if (hz > 0.0) { + period_us_.store(static_cast(1e6 / hz)); + } + } + +protected: + virtual void on_timer() = 0; + + virtual void before_start() { + + set_thread_priority(50); + } + + virtual void after_stop() {} + + void set_thread_priority(int priority) { + struct sched_param param; + param.sched_priority = priority; + + int policy = SCHED_FIFO; + + int result = pthread_setschedparam(pthread_self(), policy, ¶m); + if (result != 0) { + std::cerr << "[WARN] Failed to set real-time priority (errno: " << result + << "). Try running with sudo or setcap." << std::endl; + } else { + std::cout << "[INFO] Real-time priority set to " << priority << std::endl; + } + } + +private: + + void start_thread_base() { + before_start(); + is_running_ = true; + pthread_create(&thread_, nullptr, &PeriodicTimerThread::thread_entry, this); + } + + void stop_thread_base() { + is_running_ = false; + if (thread_) { + pthread_join(thread_, nullptr); + thread_ = 0; + } + after_stop(); + } + + static void* thread_entry(void* arg) { + static_cast(arg)->timer_thread(); + return nullptr; + } + + void timer_thread() { + struct timespec next_time; + clock_gettime(CLOCK_MONOTONIC, &next_time); + + while (is_running_) { + auto start = std::chrono::steady_clock::now(); + + try { + on_timer(); + } catch (const std::exception& e) { + std::cerr << "[ERROR] Exception in on_timer(): " << e.what() << std::endl; + } + + auto end = std::chrono::steady_clock::now(); + last_elapsed_us_.store(std::chrono::duration_cast(end - start).count()); + + + int period_us = period_us_.load(); + next_time.tv_nsec += period_us * 1000; + while (next_time.tv_nsec >= 1000000000) { + next_time.tv_nsec -= 1000000000; + next_time.tv_sec += 1; + } + clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, nullptr); + } + } + + + pthread_t thread_{}; + std::atomic is_running_; + std::atomic period_us_; + std::atomic last_elapsed_us_{0}; +}; diff --git a/src/robot_state.hpp b/src/robot_state.hpp new file mode 100644 index 0000000..628ae7e --- /dev/null +++ b/src/robot_state.hpp @@ -0,0 +1,172 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + +#pragma once + +#include +#include + +// Represents the state of a single joint: position, velocity, and effort. +struct JointState { + double position = 0.0; + double velocity = 0.0; + double effort = 0.0; +}; + +// Manages reference and response states for a robot component (e.g., arm, hand). +class RobotState { + public: + RobotState() = default; + + explicit RobotState(size_t num_joints) + : response_(num_joints), reference_(num_joints) {} + + // --- Set/Get reference (target) joint states --- + void set_reference(size_t index, const JointState& state) { + std::lock_guard lock(mutex_); + if (index < reference_.size()) { + reference_[index] = state; + } + } + + void set_all_references(const std::vector& states) { + std::lock_guard lock(mutex_); + reference_ = states; + } + + JointState get_reference(size_t index) const { + std::lock_guard lock(mutex_); + return index < reference_.size() ? reference_[index] : JointState{}; + } + + std::vector get_all_references() const { + std::lock_guard lock(mutex_); + return reference_; + } + + void set_response(size_t index, const JointState& state) { + std::lock_guard lock(mutex_); + if (index < response_.size()) { + response_[index] = state; + } + } + + void set_all_responses(const std::vector& states) { + std::lock_guard lock(mutex_); + response_ = states; + } + + JointState get_response(size_t index) const { + std::lock_guard lock(mutex_); + return index < response_.size() ? response_[index] : JointState{}; + } + + std::vector get_all_responses() const { + std::lock_guard lock(mutex_); + return response_; + } + + void resize(size_t new_size) { + std::lock_guard lock(mutex_); + reference_.resize(new_size); + response_.resize(new_size); + } + + size_t get_size() const { + std::lock_guard lock(mutex_); + return response_.size(); // assume same size for both + } + + private: + mutable std::mutex mutex_; + std::vector response_; + std::vector reference_; + }; + + +// Manages the joint states of robot components (arm, hand). +class RobotSystemState { +public: + RobotSystemState(size_t arm_joint_count, size_t hand_joint_count) + : arm_state_(arm_joint_count), hand_state_(hand_joint_count) {} + + RobotState& arm_state() { return arm_state_; } + RobotState& hand_state() { return hand_state_; } + + const RobotState& arm_state() const { return arm_state_; } + const RobotState& hand_state() const { return hand_state_; } + + std::vector get_all_responses() const { + auto arm = arm_state_.get_all_responses(); + auto hand = hand_state_.get_all_responses(); + std::vector combined; + combined.reserve(arm.size() + hand.size()); + combined.insert(combined.end(), arm.begin(), arm.end()); + combined.insert(combined.end(), hand.begin(), hand.end()); + return combined; + } + + void set_all_references(const std::vector& all_refs) { + const size_t arm_size = arm_state_.get_size(); + const size_t hand_size = hand_state_.get_size(); + + if (all_refs.size() != arm_size + hand_size) { + throw std::runtime_error("set_all_references: size mismatch."); + } + + std::vector arm_refs(all_refs.begin(), all_refs.begin() + arm_size); + std::vector hand_refs(all_refs.begin() + arm_size, all_refs.end()); + + arm_state_.set_all_references(arm_refs); + hand_state_.set_all_references(hand_refs); + } + + std::vector get_all_references() const { + auto arm = arm_state_.get_all_references(); + auto hand = hand_state_.get_all_references(); + std::vector combined; + combined.reserve(arm.size() + hand.size()); + combined.insert(combined.end(), arm.begin(), arm.end()); + combined.insert(combined.end(), hand.begin(), hand.end()); + return combined; + } + + void set_all_responses(const std::vector& all_responses) { + const size_t arm_size = arm_state_.get_size(); + const size_t hand_size = hand_state_.get_size(); + + std::cout << "arm_size : " << arm_size << std::endl; + std::cout << "hand_size : " << hand_size << std::endl; + std::cout << "all_responses.size() : " << all_responses.size() << std::endl; + + if (all_responses.size() != arm_size + hand_size) { + throw std::runtime_error("set_all_responses: size mismatch."); + } + + std::vector arm_res(all_responses.begin(), all_responses.begin() + arm_size); + std::vector hand_res(all_responses.begin() + arm_size, all_responses.end()); + + arm_state_.set_all_responses(arm_res); + hand_state_.set_all_responses(hand_res); + + } + + size_t get_total_joint_count() const { + return arm_state_.get_size() + hand_state_.get_size(); + } + +private: + RobotState arm_state_; + RobotState hand_state_; +}; diff --git a/src/yamlloader.hpp b/src/yamlloader.hpp new file mode 100644 index 0000000..446d4aa --- /dev/null +++ b/src/yamlloader.hpp @@ -0,0 +1,60 @@ +// Copyright 2025 Enactic, Inc. +// +// 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. + + +#pragma once + +#include +#include +#include +#include + +class YamlLoader { +public: + explicit YamlLoader(const std::string& filepath) { + try { + root_ = YAML::LoadFile(filepath); + } catch (const std::exception& e) { + throw std::runtime_error("Failed to load YAML file: " + filepath + ", error: " + e.what()); + } + } + + // Get a scalar double value + double get_double(const std::string& node_name, const std::string& key) const { + return get_node(node_name, key).as(); + } + + // Get a vector of doubles + std::vector get_vector(const std::string& node_name, const std::string& key) const { + return get_node(node_name, key).as>(); + } + + // Check if key exists + bool has(const std::string& node_name, const std::string& key) const { + return root_[node_name] && root_[node_name][key]; + } + +private: + YAML::Node get_node(const std::string& node_name, const std::string& key) const { + if (!root_[node_name]) { + throw std::runtime_error("Node '" + node_name + "' not found."); + } + if (!root_[node_name][key]) { + throw std::runtime_error("Key '" + key + "' not found under node '" + node_name + "'."); + } + return root_[node_name][key]; + } + + YAML::Node root_; +};