Import
This commit is contained in:
Sutou Kouhei
commit
556ddba8d6
25
.github/ISSUE_TEMPLATE/1-bug-report.yml
vendored
Normal file
25
.github/ISSUE_TEMPLATE/1-bug-report.yml
vendored
Normal file
@ -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
|
||||||
25
.github/ISSUE_TEMPLATE/2-feature-request.yml
vendored
Normal file
25
.github/ISSUE_TEMPLATE/2-feature-request.yml
vendored
Normal file
@ -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
|
||||||
22
.github/ISSUE_TEMPLATE/config.yml
vendored
Normal file
22
.github/ISSUE_TEMPLATE/config.yml
vendored
Normal file
@ -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.
|
||||||
21
.gitignore
vendored
Normal file
21
.gitignore
vendored
Normal file
@ -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/
|
||||||
77
CMakeLists.txt
Normal file
77
CMakeLists.txt
Normal file
@ -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})
|
||||||
133
CODE_OF_CONDUCT.md
Normal file
133
CODE_OF_CONDUCT.md
Normal file
@ -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
|
||||||
19
CONTRIBUTING.md
Normal file
19
CONTRIBUTING.md
Normal file
@ -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)!
|
||||||
201
LICENSE.txt
Normal file
201
LICENSE.txt
Normal file
@ -0,0 +1,201 @@
|
|||||||
|
Apache License
|
||||||
|
Version 2.0, January 2004
|
||||||
|
http://www.apache.org/licenses/
|
||||||
|
|
||||||
|
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||||
|
|
||||||
|
1. Definitions.
|
||||||
|
|
||||||
|
"License" shall mean the terms and conditions for use, reproduction,
|
||||||
|
and distribution as defined by Sections 1 through 9 of this document.
|
||||||
|
|
||||||
|
"Licensor" shall mean the copyright owner or entity authorized by
|
||||||
|
the copyright owner that is granting the License.
|
||||||
|
|
||||||
|
"Legal Entity" shall mean the union of the acting entity and all
|
||||||
|
other entities that control, are controlled by, or are under common
|
||||||
|
control with that entity. For the purposes of this definition,
|
||||||
|
"control" means (i) the power, direct or indirect, to cause the
|
||||||
|
direction or management of such entity, whether by contract or
|
||||||
|
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||||
|
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||||
|
|
||||||
|
"You" (or "Your") shall mean an individual or Legal Entity
|
||||||
|
exercising permissions granted by this License.
|
||||||
|
|
||||||
|
"Source" form shall mean the preferred form for making modifications,
|
||||||
|
including but not limited to software source code, documentation
|
||||||
|
source, and configuration files.
|
||||||
|
|
||||||
|
"Object" form shall mean any form resulting from mechanical
|
||||||
|
transformation or translation of a Source form, including but
|
||||||
|
not limited to compiled object code, generated documentation,
|
||||||
|
and conversions to other media types.
|
||||||
|
|
||||||
|
"Work" shall mean the work of authorship, whether in Source or
|
||||||
|
Object form, made available under the License, as indicated by a
|
||||||
|
copyright notice that is included in or attached to the work
|
||||||
|
(an example is provided in the Appendix below).
|
||||||
|
|
||||||
|
"Derivative Works" shall mean any work, whether in Source or Object
|
||||||
|
form, that is based on (or derived from) the Work and for which the
|
||||||
|
editorial revisions, annotations, elaborations, or other modifications
|
||||||
|
represent, as a whole, an original work of authorship. For the purposes
|
||||||
|
of this License, Derivative Works shall not include works that remain
|
||||||
|
separable from, or merely link (or bind by name) to the interfaces of,
|
||||||
|
the Work and Derivative Works thereof.
|
||||||
|
|
||||||
|
"Contribution" shall mean any work of authorship, including
|
||||||
|
the original version of the Work and any modifications or additions
|
||||||
|
to that Work or Derivative Works thereof, that is intentionally
|
||||||
|
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||||
|
or by an individual or Legal Entity authorized to submit on behalf of
|
||||||
|
the copyright owner. For the purposes of this definition, "submitted"
|
||||||
|
means any form of electronic, verbal, or written communication sent
|
||||||
|
to the Licensor or its representatives, including but not limited to
|
||||||
|
communication on electronic mailing lists, source code control systems,
|
||||||
|
and issue tracking systems that are managed by, or on behalf of, the
|
||||||
|
Licensor for the purpose of discussing and improving the Work, but
|
||||||
|
excluding communication that is conspicuously marked or otherwise
|
||||||
|
designated in writing by the copyright owner as "Not a Contribution."
|
||||||
|
|
||||||
|
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||||
|
on behalf of whom a Contribution has been received by Licensor and
|
||||||
|
subsequently incorporated within the Work.
|
||||||
|
|
||||||
|
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||||
|
this License, each Contributor hereby grants to You a perpetual,
|
||||||
|
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||||
|
copyright license to reproduce, prepare Derivative Works of,
|
||||||
|
publicly display, publicly perform, sublicense, and distribute the
|
||||||
|
Work and such Derivative Works in Source or Object form.
|
||||||
|
|
||||||
|
3. Grant of Patent License. Subject to the terms and conditions of
|
||||||
|
this License, each Contributor hereby grants to You a perpetual,
|
||||||
|
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||||
|
(except as stated in this section) patent license to make, have made,
|
||||||
|
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||||
|
where such license applies only to those patent claims licensable
|
||||||
|
by such Contributor that are necessarily infringed by their
|
||||||
|
Contribution(s) alone or by combination of their Contribution(s)
|
||||||
|
with the Work to which such Contribution(s) was submitted. If You
|
||||||
|
institute patent litigation against any entity (including a
|
||||||
|
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||||
|
or a Contribution incorporated within the Work constitutes direct
|
||||||
|
or contributory patent infringement, then any patent licenses
|
||||||
|
granted to You under this License for that Work shall terminate
|
||||||
|
as of the date such litigation is filed.
|
||||||
|
|
||||||
|
4. Redistribution. You may reproduce and distribute copies of the
|
||||||
|
Work or Derivative Works thereof in any medium, with or without
|
||||||
|
modifications, and in Source or Object form, provided that You
|
||||||
|
meet the following conditions:
|
||||||
|
|
||||||
|
(a) You must give any other recipients of the Work or
|
||||||
|
Derivative Works a copy of this License; and
|
||||||
|
|
||||||
|
(b) You must cause any modified files to carry prominent notices
|
||||||
|
stating that You changed the files; and
|
||||||
|
|
||||||
|
(c) You must retain, in the Source form of any Derivative Works
|
||||||
|
that You distribute, all copyright, patent, trademark, and
|
||||||
|
attribution notices from the Source form of the Work,
|
||||||
|
excluding those notices that do not pertain to any part of
|
||||||
|
the Derivative Works; and
|
||||||
|
|
||||||
|
(d) If the Work includes a "NOTICE" text file as part of its
|
||||||
|
distribution, then any Derivative Works that You distribute must
|
||||||
|
include a readable copy of the attribution notices contained
|
||||||
|
within such NOTICE file, excluding those notices that do not
|
||||||
|
pertain to any part of the Derivative Works, in at least one
|
||||||
|
of the following places: within a NOTICE text file distributed
|
||||||
|
as part of the Derivative Works; within the Source form or
|
||||||
|
documentation, if provided along with the Derivative Works; or,
|
||||||
|
within a display generated by the Derivative Works, if and
|
||||||
|
wherever such third-party notices normally appear. The contents
|
||||||
|
of the NOTICE file are for informational purposes only and
|
||||||
|
do not modify the License. You may add Your own attribution
|
||||||
|
notices within Derivative Works that You distribute, alongside
|
||||||
|
or as an addendum to the NOTICE text from the Work, provided
|
||||||
|
that such additional attribution notices cannot be construed
|
||||||
|
as modifying the License.
|
||||||
|
|
||||||
|
You may add Your own copyright statement to Your modifications and
|
||||||
|
may provide additional or different license terms and conditions
|
||||||
|
for use, reproduction, or distribution of Your modifications, or
|
||||||
|
for any such Derivative Works as a whole, provided Your use,
|
||||||
|
reproduction, and distribution of the Work otherwise complies with
|
||||||
|
the conditions stated in this License.
|
||||||
|
|
||||||
|
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||||
|
any Contribution intentionally submitted for inclusion in the Work
|
||||||
|
by You to the Licensor shall be under the terms and conditions of
|
||||||
|
this License, without any additional terms or conditions.
|
||||||
|
Notwithstanding the above, nothing herein shall supersede or modify
|
||||||
|
the terms of any separate license agreement you may have executed
|
||||||
|
with Licensor regarding such Contributions.
|
||||||
|
|
||||||
|
6. Trademarks. This License does not grant permission to use the trade
|
||||||
|
names, trademarks, service marks, or product names of the Licensor,
|
||||||
|
except as required for reasonable and customary use in describing the
|
||||||
|
origin of the Work and reproducing the content of the NOTICE file.
|
||||||
|
|
||||||
|
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||||
|
agreed to in writing, Licensor provides the Work (and each
|
||||||
|
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||||
|
implied, including, without limitation, any warranties or conditions
|
||||||
|
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||||
|
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||||
|
appropriateness of using or redistributing the Work and assume any
|
||||||
|
risks associated with Your exercise of permissions under this License.
|
||||||
|
|
||||||
|
8. Limitation of Liability. In no event and under no legal theory,
|
||||||
|
whether in tort (including negligence), contract, or otherwise,
|
||||||
|
unless required by applicable law (such as deliberate and grossly
|
||||||
|
negligent acts) or agreed to in writing, shall any Contributor be
|
||||||
|
liable to You for damages, including any direct, indirect, special,
|
||||||
|
incidental, or consequential damages of any character arising as a
|
||||||
|
result of this License or out of the use or inability to use the
|
||||||
|
Work (including but not limited to damages for loss of goodwill,
|
||||||
|
work stoppage, computer failure or malfunction, or any and all
|
||||||
|
other commercial damages or losses), even if such Contributor
|
||||||
|
has been advised of the possibility of such damages.
|
||||||
|
|
||||||
|
9. Accepting Warranty or Additional Liability. While redistributing
|
||||||
|
the Work or Derivative Works thereof, You may choose to offer,
|
||||||
|
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||||
|
or other liability obligations and/or rights consistent with this
|
||||||
|
License. However, in accepting such obligations, You may act only
|
||||||
|
on Your own behalf and on Your sole responsibility, not on behalf
|
||||||
|
of any other Contributor, and only if You agree to indemnify,
|
||||||
|
defend, and hold each Contributor harmless for any liability
|
||||||
|
incurred by, or claims asserted against, such Contributor by reason
|
||||||
|
of your accepting any such warranty or additional liability.
|
||||||
|
|
||||||
|
END OF TERMS AND CONDITIONS
|
||||||
|
|
||||||
|
APPENDIX: How to apply the Apache License to your work.
|
||||||
|
|
||||||
|
To apply the Apache License to your work, attach the following
|
||||||
|
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||||
|
replaced with your own identifying information. (Don't include
|
||||||
|
the brackets!) The text should be enclosed in the appropriate
|
||||||
|
comment syntax for the file format. We also recommend that a
|
||||||
|
file or class name and description of purpose be included on the
|
||||||
|
same "printed page" as the copyright notice for easier
|
||||||
|
identification within third-party archives.
|
||||||
|
|
||||||
|
Copyright [yyyy] [name of copyright owner]
|
||||||
|
|
||||||
|
Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
you may not use this file except in compliance with the License.
|
||||||
|
You may obtain a copy of the License at
|
||||||
|
|
||||||
|
http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
|
||||||
|
Unless required by applicable law or agreed to in writing, software
|
||||||
|
distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
See the License for the specific language governing permissions and
|
||||||
|
limitations under the License.
|
||||||
19
README.md
Normal file
19
README.md
Normal file
@ -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 <openarm@enactic.ai>
|
||||||
|
|
||||||
|
## 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).
|
||||||
34
config/follower.yaml
Normal file
34
config/follower.yaml
Normal file
@ -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]
|
||||||
34
config/leader.yaml
Normal file
34
config/leader.yaml
Normal file
@ -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]
|
||||||
142
control/gravity_compasation.cpp
Normal file
142
control/gravity_compasation.cpp
Normal file
@ -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 <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <csignal>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
|
#include <csignal>
|
||||||
|
#include <atomic>
|
||||||
|
#include <filesystem>
|
||||||
|
#include <openarm/can/socket/openarm.hpp>
|
||||||
|
#include <openarm/damiao_motor/dm_motor_constants.hpp>
|
||||||
|
#include <controller/dynamics.hpp>
|
||||||
|
#include <openarm_port/openarm_init.hpp>
|
||||||
|
|
||||||
|
std::atomic<bool> 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] << " <arm_side> <can_interface> <urdf_path>" << 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<double> arm_joint_positions(openarm->get_arm().get_motors().size(), 0.0);
|
||||||
|
std::vector<double> arm_joint_velocities(openarm->get_arm().get_motors().size(), 0.0);
|
||||||
|
|
||||||
|
std::vector<double> gripper_joint_positions(openarm->get_gripper().get_motors().size(), 0.0);
|
||||||
|
std::vector<double> gripper_joint_velocities(openarm->get_gripper().get_motors().size(), 0.0);
|
||||||
|
|
||||||
|
std::vector<double> 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<std::chrono::milliseconds>(current_time - last_hz_display).count();
|
||||||
|
if (time_since_last_display >= 1000) { // Every 1000ms (1 second)
|
||||||
|
auto total_time = std::chrono::duration_cast<std::chrono::milliseconds>(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<openarm::damiao_motor::MITParam> 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;
|
||||||
|
}
|
||||||
321
control/openarm_bilateral_control.cpp
Normal file
321
control/openarm_bilateral_control.cpp
Normal file
@ -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 <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <csignal>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
|
#include <csignal>
|
||||||
|
#include <atomic>
|
||||||
|
#include <filesystem>
|
||||||
|
|
||||||
|
#include <periodic_timer_thread.hpp>
|
||||||
|
#include <robot_state.hpp>
|
||||||
|
#include <openarm/can/socket/openarm.hpp>
|
||||||
|
#include <openarm/damiao_motor/dm_motor_constants.hpp>
|
||||||
|
#include <openarm_port/openarm_init.hpp>
|
||||||
|
#include <controller/dynamics.hpp>
|
||||||
|
#include <yamlloader.hpp>
|
||||||
|
#include <controller/control.hpp>
|
||||||
|
|
||||||
|
std::atomic<bool> 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<RobotSystemState> 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<std::chrono::microseconds>(now - prev_time).count();
|
||||||
|
prev_time = now;
|
||||||
|
|
||||||
|
// std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<RobotSystemState> robot_state_;
|
||||||
|
Control *control_l_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class FollowerArmThread : public PeriodicTimerThread {
|
||||||
|
public:
|
||||||
|
FollowerArmThread(std::shared_ptr<RobotSystemState> 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<std::chrono::microseconds>(now - prev_time).count();
|
||||||
|
prev_time = now;
|
||||||
|
|
||||||
|
// std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<RobotSystemState> robot_state_;
|
||||||
|
Control *control_f_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
class AdminThread : public PeriodicTimerThread {
|
||||||
|
public:
|
||||||
|
AdminThread(std::shared_ptr<RobotSystemState> leader_state,
|
||||||
|
std::shared_ptr<RobotSystemState> 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<std::chrono::microseconds>(now - prev_time).count();
|
||||||
|
prev_time = now;
|
||||||
|
|
||||||
|
// std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<RobotSystemState> leader_state_;
|
||||||
|
std::shared_ptr<RobotSystemState> 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] << " <leader_urdf_path> <follower_urdf_path> [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<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp");
|
||||||
|
std::vector<double> leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd");
|
||||||
|
std::vector<double> leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc");
|
||||||
|
std::vector<double> leader_k = leader_loader.get_vector("LeaderArmParam", "k");
|
||||||
|
std::vector<double> leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv");
|
||||||
|
std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo");
|
||||||
|
|
||||||
|
// Follower parameters
|
||||||
|
std::vector<double> follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp");
|
||||||
|
std::vector<double> follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd");
|
||||||
|
std::vector<double> follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc");
|
||||||
|
std::vector<double> follower_k = follower_loader.get_vector("FollowerArmParam", "k");
|
||||||
|
std::vector<double> follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv");
|
||||||
|
std::vector<double> 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<RobotSystemState> leader_state =
|
||||||
|
std::make_shared<RobotSystemState>(leader_arm_motor_num, leader_hand_motor_num);
|
||||||
|
|
||||||
|
std::shared_ptr<RobotSystemState> follower_state =
|
||||||
|
std::make_shared<RobotSystemState>(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;
|
||||||
|
}
|
||||||
145
control/openarm_communication_test.cpp
Normal file
145
control/openarm_communication_test.cpp
Normal file
@ -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 <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <csignal>
|
||||||
|
#include <iostream>
|
||||||
|
#include <openarm/can/socket/openarm.hpp>
|
||||||
|
#include <openarm/damiao_motor/dm_motor_constants.hpp>
|
||||||
|
#include <thread>
|
||||||
|
#include <csignal>
|
||||||
|
#include <atomic>
|
||||||
|
|
||||||
|
|
||||||
|
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<openarm::damiao_motor::MotorType> 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<uint32_t> send_can_ids = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
|
||||||
|
std::vector<uint32_t> 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<int>(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<int>(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<int>(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;
|
||||||
|
}
|
||||||
319
control/openarm_unilateral_control.cpp
Normal file
319
control/openarm_unilateral_control.cpp
Normal file
@ -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 <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <csignal>
|
||||||
|
#include <iostream>
|
||||||
|
#include <thread>
|
||||||
|
#include <csignal>
|
||||||
|
#include <atomic>
|
||||||
|
#include <filesystem>
|
||||||
|
|
||||||
|
#include <periodic_timer_thread.hpp>
|
||||||
|
#include <robot_state.hpp>
|
||||||
|
#include <openarm/can/socket/openarm.hpp>
|
||||||
|
#include <openarm/damiao_motor/dm_motor_constants.hpp>
|
||||||
|
#include <openarm_port/openarm_init.hpp>
|
||||||
|
#include <controller/dynamics.hpp>
|
||||||
|
#include <yamlloader.hpp>
|
||||||
|
#include <controller/control.hpp>
|
||||||
|
|
||||||
|
std::atomic<bool> 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<RobotSystemState> 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<std::chrono::microseconds>(now - prev_time).count();
|
||||||
|
prev_time = now;
|
||||||
|
|
||||||
|
// std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<RobotSystemState> robot_state_;
|
||||||
|
Control *control_l_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class FollowerArmThread : public PeriodicTimerThread {
|
||||||
|
public:
|
||||||
|
FollowerArmThread(std::shared_ptr<RobotSystemState> 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<std::chrono::microseconds>(now - prev_time).count();
|
||||||
|
prev_time = now;
|
||||||
|
|
||||||
|
// std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<RobotSystemState> robot_state_;
|
||||||
|
Control *control_f_;
|
||||||
|
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
class AdminThread : public PeriodicTimerThread {
|
||||||
|
public:
|
||||||
|
AdminThread(std::shared_ptr<RobotSystemState> leader_state,
|
||||||
|
std::shared_ptr<RobotSystemState> 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<std::chrono::microseconds>(now - prev_time).count();
|
||||||
|
prev_time = now;
|
||||||
|
|
||||||
|
// std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<RobotSystemState> leader_state_;
|
||||||
|
std::shared_ptr<RobotSystemState> 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] << " <leader_urdf_path> <follower_urdf_path> [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<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp");
|
||||||
|
std::vector<double> leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd");
|
||||||
|
std::vector<double> leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc");
|
||||||
|
std::vector<double> leader_k = leader_loader.get_vector("LeaderArmParam", "k");
|
||||||
|
std::vector<double> leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv");
|
||||||
|
std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo");
|
||||||
|
|
||||||
|
// Follower parameters
|
||||||
|
std::vector<double> follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp");
|
||||||
|
std::vector<double> follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd");
|
||||||
|
std::vector<double> follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc");
|
||||||
|
std::vector<double> follower_k = follower_loader.get_vector("FollowerArmParam", "k");
|
||||||
|
std::vector<double> follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv");
|
||||||
|
std::vector<double> 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<RobotSystemState> leader_state =
|
||||||
|
std::make_shared<RobotSystemState>(leader_arm_motor_num, leader_hand_motor_num);
|
||||||
|
|
||||||
|
std::shared_ptr<RobotSystemState> follower_state =
|
||||||
|
std::make_shared<RobotSystemState>(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;
|
||||||
|
}
|
||||||
105
script/launch_bilateral.sh
Executable file
105
script/launch_bilateral.sh
Executable file
@ -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 <arm_side: right_arm|left_arm> [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"
|
||||||
75
script/launch_grav_comp.sh
Executable file
75
script/launch_grav_comp.sh
Executable file
@ -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"
|
||||||
105
script/launch_unilateral.sh
Executable file
105
script/launch_unilateral.sh
Executable file
@ -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 <arm_side: right_arm|left_arm> [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"
|
||||||
510
src/controller/control.cpp
Normal file
510
src/controller/control.cpp
Normal file
@ -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 <cmath>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <thread>
|
||||||
|
#include <cmath>
|
||||||
|
#include <algorithm>
|
||||||
|
#include <iomanip>
|
||||||
|
#include <controller/control.hpp>
|
||||||
|
#include <controller/dynamics.hpp>
|
||||||
|
|
||||||
|
Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> 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<RobotSystemState> 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<double>& Kp,
|
||||||
|
const std::vector<double>& Kd,
|
||||||
|
const std::vector<double>& Fc,
|
||||||
|
const std::vector<double>& k,
|
||||||
|
const std::vector<double>& Fv,
|
||||||
|
const std::vector<double>& Fo)
|
||||||
|
{
|
||||||
|
Kp_ = Kp;
|
||||||
|
Kd_ = Kd;
|
||||||
|
Fc_ = Fc;
|
||||||
|
k_ = k;
|
||||||
|
Fv_ = Fv;
|
||||||
|
Fo_ = Fo;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
bool Control::bilateral_step()
|
||||||
|
{
|
||||||
|
// get motor status
|
||||||
|
std::vector<MotorState> 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<MotorState> 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<JointState> joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states);
|
||||||
|
std::vector<JointState> 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<double> joint_arm_positions(arm_dof, 0.0);
|
||||||
|
std::vector<double> joint_arm_velocities(arm_dof, 0.0);
|
||||||
|
std::vector<double> joint_arm_efforts(arm_dof, 0.0);
|
||||||
|
|
||||||
|
std::vector<double> joint_gripper_positions(gripper_dof, 0.0);
|
||||||
|
std::vector<double> joint_gripper_velocities(gripper_dof, 0.0);
|
||||||
|
std::vector<double> 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<double> gravity(arm_dof, 0.0);
|
||||||
|
std::vector<double> coriolis(arm_dof, 0.0);
|
||||||
|
std::vector<double> friction(arm_dof + gripper_dof, 0.0);
|
||||||
|
|
||||||
|
std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
|
||||||
|
std::vector<JointState> joint_gripper_states_ref = robot_state_->hand_state().get_all_references();
|
||||||
|
|
||||||
|
std::vector<double> 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<MotorState> motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
|
||||||
|
std::vector<MotorState> motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref);
|
||||||
|
|
||||||
|
// kp kd q dq tau
|
||||||
|
std::vector<openarm::damiao_motor::MITParam> 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<openarm::damiao_motor::MITParam> 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<MotorState> 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<MotorState> 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<JointState> joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states);
|
||||||
|
std::vector<JointState> 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<double> joint_arm_positions(arm_dof, 0.0);
|
||||||
|
std::vector<double> joint_arm_velocities(arm_dof, 0.0);
|
||||||
|
std::vector<double> joint_gripper_positions(gripper_dof, 0.0);
|
||||||
|
std::vector<double> 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<double> gravity(arm_dof, 0.0);
|
||||||
|
std::vector<double> coriolis(arm_dof, 0.0);
|
||||||
|
std::vector<double> 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<JointState> 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<JointState> 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<MotorState> motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_state_torque);
|
||||||
|
std::vector<MotorState> motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque);
|
||||||
|
|
||||||
|
// arm command mit param
|
||||||
|
std::vector<openarm::damiao_motor::MITParam> 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<openarm::damiao_motor::MITParam> 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<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
|
||||||
|
std::vector<JointState> joint_hand_states_ref = robot_state_->hand_state().get_all_references();
|
||||||
|
|
||||||
|
// Joint → Motor
|
||||||
|
std::vector<MotorState> arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
|
||||||
|
std::vector<MotorState> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref);
|
||||||
|
|
||||||
|
std::vector<openarm::damiao_motor::MITParam> 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<openarm::damiao_motor::MITParam> 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<MotorState> 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<MotorState> 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<JointState> joint_arm_now = openarmjointconverter_->motor_to_joint(arm_motor_states);
|
||||||
|
std::vector<JointState> joint_hand_now = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
|
||||||
|
|
||||||
|
std::vector<JointState> 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<JointState> 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<double> kp_arm_temp = {50, 50.0, 50.0, 50.0, 10.0, 10.0, 10.0};
|
||||||
|
std::vector<double> kd_arm_temp = {1.2, 1.2, 1.2, 1.2, 0.3, 0.2, 0.3};
|
||||||
|
|
||||||
|
std::vector<double> kp_hand_temp = {10.0};
|
||||||
|
std::vector<double> kd_hand_temp = {0.5};
|
||||||
|
|
||||||
|
for (int step = 0; step < nstep; ++step) {
|
||||||
|
alpha = static_cast<double>(step + 1) / nstep;
|
||||||
|
|
||||||
|
std::vector<JointState> 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<JointState> 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<MotorState> arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_interp);
|
||||||
|
std::vector<MotorState> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_interp);
|
||||||
|
|
||||||
|
std::vector<openarm::damiao_motor::MITParam> 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<openarm::damiao_motor::MITParam> 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<MotorState> 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<MotorState> 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<JointState> joint_arm_final = openarmjointconverter_->motor_to_joint(arm_motor_states_final);
|
||||||
|
std::vector<JointState> 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;
|
||||||
|
}
|
||||||
104
src/controller/control.hpp
Normal file
104
src/controller/control.hpp
Normal file
@ -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 <sensor_msgs/msg/joint_state.hpp>
|
||||||
|
#include <utility>
|
||||||
|
#include <fstream>
|
||||||
|
#include <deque>
|
||||||
|
#include <numeric>
|
||||||
|
#include <memory>
|
||||||
|
#include <controller/diff.hpp>
|
||||||
|
#include <controller/dynamics.hpp>
|
||||||
|
#include <robot_state.hpp>
|
||||||
|
#include <joint_state_converter.hpp>
|
||||||
|
#include <openarm_constants.hpp>
|
||||||
|
#include <robot_state.hpp>
|
||||||
|
#include <openarm/can/socket/openarm.hpp>
|
||||||
|
#include <openarm/damiao_motor/dm_motor_constants.hpp>
|
||||||
|
|
||||||
|
|
||||||
|
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<RobotSystemState> 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<std::pair<double, double>> 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<double> velocity_buffer_[NJOINTS];
|
||||||
|
|
||||||
|
public:
|
||||||
|
Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> 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<RobotSystemState> robot_state, double Ts, int role, std::string arm_type, size_t arm_joint_num, size_t hand_motor_num);
|
||||||
|
~Control();
|
||||||
|
|
||||||
|
std::shared_ptr<RobotSystemState> response_;
|
||||||
|
std::shared_ptr<RobotSystemState> reference_;
|
||||||
|
|
||||||
|
std::vector<double> Dn_, Kp_, Kd_,Fc_, k_, Fv_, Fo_;
|
||||||
|
|
||||||
|
// bool Setup(void);
|
||||||
|
void Setstate(int state);
|
||||||
|
void Shutdown(void);
|
||||||
|
|
||||||
|
void SetParameter(
|
||||||
|
const std::vector<double>& Kp,
|
||||||
|
const std::vector<double>& Kd,
|
||||||
|
const std::vector<double>& Fc,
|
||||||
|
const std::vector<double>& k,
|
||||||
|
const std::vector<double>& Fv,
|
||||||
|
const std::vector<double>& 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);
|
||||||
|
};
|
||||||
74
src/controller/diff.hpp
Normal file
74
src/controller/diff.hpp
Normal file
@ -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 <global.hpp>
|
||||||
|
|
||||||
|
#include <openarm_constants.hpp>
|
||||||
|
|
||||||
|
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];
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
};
|
||||||
243
src/controller/dynamics.cpp
Normal file
243
src/controller/dynamics.cpp
Normal file
@ -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 <controller/dynamics.hpp>
|
||||||
|
|
||||||
|
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::ChainDynParam>(
|
||||||
|
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<Eigen::MatrixXd> 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<Eigen::MatrixXd> 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];
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
72
src/controller/dynamics.hpp
Normal file
72
src/controller/dynamics.hpp
Normal file
@ -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 <unistd.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <kdl/chain.hpp>
|
||||||
|
#include <kdl/chaindynparam.hpp>
|
||||||
|
#include <kdl_parser/kdl_parser.hpp>
|
||||||
|
#include <kdl/chainfksolverpos_recursive.hpp>
|
||||||
|
#include <kdl/chainjnttojacsolver.hpp>
|
||||||
|
#include <urdf_parser/urdf_parser.h>
|
||||||
|
#include <Eigen/Dense>
|
||||||
|
#include <vector>
|
||||||
|
#include <iostream>
|
||||||
|
#include <fstream>
|
||||||
|
#include <sstream>
|
||||||
|
/*
|
||||||
|
* Compute gravity and inertia compensation using Orocos
|
||||||
|
* Kinematics and Dynamics Library (KDL).
|
||||||
|
*/
|
||||||
|
class Dynamics
|
||||||
|
{
|
||||||
|
private:
|
||||||
|
std::shared_ptr<urdf::ModelInterface> 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<KDL::ChainDynParam> 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);
|
||||||
|
};
|
||||||
107
src/joint_state_converter.hpp
Normal file
107
src/joint_state_converter.hpp
Normal file
@ -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 <vector>
|
||||||
|
#include <robot_state.hpp>
|
||||||
|
|
||||||
|
// 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<JointState> motor_to_joint(const std::vector<MotorState>& motor_states) const = 0;
|
||||||
|
|
||||||
|
// JointState vector → MotorState vector
|
||||||
|
virtual std::vector<MotorState> joint_to_motor(const std::vector<JointState>& 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<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
|
||||||
|
// std::cout << "joint num conv : " << m.size() << std::endl;
|
||||||
|
|
||||||
|
std::vector<JointState> 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<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
|
||||||
|
std::vector<MotorState> 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<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
|
||||||
|
std::vector<JointState> 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<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
|
||||||
|
std::vector<MotorState> 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_;
|
||||||
|
};
|
||||||
113
src/openarm_constants.hpp
Normal file
113
src/openarm_constants.hpp
Normal file
@ -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 <unistd.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <iostream>
|
||||||
|
#include <vector>
|
||||||
|
#include <openarm/damiao_motor//dm_motor_constants.hpp>
|
||||||
|
|
||||||
|
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<openarm::damiao_motor::MotorType> arm_motor_types;
|
||||||
|
std::vector<uint32_t> arm_send_can_ids;
|
||||||
|
std::vector<uint32_t> 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;
|
||||||
|
}
|
||||||
57
src/openarm_port/joint_mapper.cpp
Normal file
57
src/openarm_port/joint_mapper.cpp
Normal file
@ -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 <cmath>
|
||||||
|
|
||||||
|
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];
|
||||||
|
}
|
||||||
29
src/openarm_port/joint_mapper.hpp
Normal file
29
src/openarm_port/joint_mapper.hpp
Normal file
@ -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);
|
||||||
|
};
|
||||||
87
src/openarm_port/openarm_init.cpp
Normal file
87
src/openarm_port/openarm_init.cpp
Normal file
@ -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
|
||||||
56
src/openarm_port/openarm_init.hpp
Normal file
56
src/openarm_port/openarm_init.hpp
Normal file
@ -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 <chrono>
|
||||||
|
#include <iostream>
|
||||||
|
#include <openarm/can/socket/openarm.hpp>
|
||||||
|
#include <string>
|
||||||
|
#include <thread>
|
||||||
|
|
||||||
|
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
|
||||||
138
src/periodic_timer_thread.hpp
Normal file
138
src/periodic_timer_thread.hpp
Normal file
@ -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 <pthread.h>
|
||||||
|
#include <atomic>
|
||||||
|
#include <chrono>
|
||||||
|
#include <thread>
|
||||||
|
#include <iostream>
|
||||||
|
#include <stdexcept>
|
||||||
|
#include <unistd.h>
|
||||||
|
#include <time.h>
|
||||||
|
|
||||||
|
|
||||||
|
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<int>(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<int>(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<PeriodicTimerThread*>(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<std::chrono::microseconds>(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<bool> is_running_;
|
||||||
|
std::atomic<int> period_us_;
|
||||||
|
std::atomic<int64_t> last_elapsed_us_{0};
|
||||||
|
};
|
||||||
172
src/robot_state.hpp
Normal file
172
src/robot_state.hpp
Normal file
@ -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 <vector>
|
||||||
|
#include <mutex>
|
||||||
|
|
||||||
|
// 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<std::mutex> lock(mutex_);
|
||||||
|
if (index < reference_.size()) {
|
||||||
|
reference_[index] = state;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_all_references(const std::vector<JointState>& states) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
reference_ = states;
|
||||||
|
}
|
||||||
|
|
||||||
|
JointState get_reference(size_t index) const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return index < reference_.size() ? reference_[index] : JointState{};
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<JointState> get_all_references() const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return reference_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_response(size_t index, const JointState& state) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
if (index < response_.size()) {
|
||||||
|
response_[index] = state;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void set_all_responses(const std::vector<JointState>& states) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
response_ = states;
|
||||||
|
}
|
||||||
|
|
||||||
|
JointState get_response(size_t index) const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return index < response_.size() ? response_[index] : JointState{};
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<JointState> get_all_responses() const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return response_;
|
||||||
|
}
|
||||||
|
|
||||||
|
void resize(size_t new_size) {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
reference_.resize(new_size);
|
||||||
|
response_.resize(new_size);
|
||||||
|
}
|
||||||
|
|
||||||
|
size_t get_size() const {
|
||||||
|
std::lock_guard<std::mutex> lock(mutex_);
|
||||||
|
return response_.size(); // assume same size for both
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
mutable std::mutex mutex_;
|
||||||
|
std::vector<JointState> response_;
|
||||||
|
std::vector<JointState> 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<JointState> get_all_responses() const {
|
||||||
|
auto arm = arm_state_.get_all_responses();
|
||||||
|
auto hand = hand_state_.get_all_responses();
|
||||||
|
std::vector<JointState> 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<JointState>& 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<JointState> arm_refs(all_refs.begin(), all_refs.begin() + arm_size);
|
||||||
|
std::vector<JointState> 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<JointState> get_all_references() const {
|
||||||
|
auto arm = arm_state_.get_all_references();
|
||||||
|
auto hand = hand_state_.get_all_references();
|
||||||
|
std::vector<JointState> 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<JointState>& 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<JointState> arm_res(all_responses.begin(), all_responses.begin() + arm_size);
|
||||||
|
std::vector<JointState> 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_;
|
||||||
|
};
|
||||||
60
src/yamlloader.hpp
Normal file
60
src/yamlloader.hpp
Normal file
@ -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 <yaml-cpp/yaml.h>
|
||||||
|
#include <string>
|
||||||
|
#include <vector>
|
||||||
|
#include <stdexcept>
|
||||||
|
|
||||||
|
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<double>();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get a vector of doubles
|
||||||
|
std::vector<double> get_vector(const std::string& node_name, const std::string& key) const {
|
||||||
|
return get_node(node_name, key).as<std::vector<double>>();
|
||||||
|
}
|
||||||
|
|
||||||
|
// 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_;
|
||||||
|
};
|
||||||
Loading…
Reference in New Issue
Block a user