From 584ca1533859da11d8da936e426609c98cfbe525 Mon Sep 17 00:00:00 2001 From: Sutou Kouhei Date: Tue, 22 Jul 2025 15:15:21 +0900 Subject: [PATCH] Import --- .clang-format | 19 + .cmake-format.py | 19 + .github/ISSUE_TEMPLATE/1-bug-report.yml | 25 ++ .github/ISSUE_TEMPLATE/2-feature-request.yml | 25 ++ .github/ISSUE_TEMPLATE/config.yml | 22 + .github/workflows/test.yaml | 96 +++++ .pre-commit-config.yaml | 42 ++ CMakeLists.txt | 127 ++++++ CODE_OF_CONDUCT.md | 133 ++++++ CONTRIBUTING.md | 19 + LICENSE.txt | 202 +++++++++ OpenArmCANConfig.cmake.in | 19 + README.md | 96 +++++ examples/demo.cpp | 118 ++++++ include/openarm/can/socket/arm_component.hpp | 38 ++ .../openarm/can/socket/gripper_component.hpp | 70 ++++ include/openarm/can/socket/openarm.hpp | 68 +++ include/openarm/canbus/can_device.hpp | 50 +++ .../openarm/canbus/can_device_collection.hpp | 43 ++ include/openarm/canbus/can_socket.hpp | 75 ++++ include/openarm/damiao_motor/dm_motor.hpp | 77 ++++ .../damiao_motor/dm_motor_constants.hpp | 111 +++++ .../openarm/damiao_motor/dm_motor_control.hpp | 91 ++++ .../openarm/damiao_motor/dm_motor_device.hpp | 50 +++ .../dm_motor_device_collection.hpp | 64 +++ openarm-can.pc.in | 23 ++ python/README.md | 3 + python/build.sh | 55 +++ python/examples/example.py | 56 +++ python/meson.build | 41 ++ python/openarm/can/__init__.py | 53 +++ python/openarm/can/core.py | 53 +++ python/pyproject.toml | 36 ++ python/requirements.txt | 13 + python/src/openarm_can.cpp | 390 ++++++++++++++++++ python/subprojects/.gitignore | 17 + python/subprojects/nanobind.wrap | 27 ++ python/subprojects/robin-map.wrap | 27 ++ setup/change_baudrate.py | 261 ++++++++++++ setup/configure_socketcan.sh | 114 +++++ setup/motor_check.cpp | 186 +++++++++ setup/set_zero.sh | 160 +++++++ src/openarm/can/socket/arm_component.cpp | 43 ++ src/openarm/can/socket/gripper_component.cpp | 48 +++ src/openarm/can/socket/openarm.cpp | 110 +++++ src/openarm/canbus/can_device_collection.cpp | 62 +++ src/openarm/canbus/can_socket.cpp | 140 +++++++ src/openarm/damiao_motor/dm_motor.cpp | 70 ++++ src/openarm/damiao_motor/dm_motor_control.cpp | 177 ++++++++ src/openarm/damiao_motor/dm_motor_device.cpp | 116 ++++++ .../dm_motor_device_collection.cpp | 128 ++++++ 51 files changed, 4108 insertions(+) create mode 100644 .clang-format create mode 100644 .cmake-format.py create mode 100644 .github/ISSUE_TEMPLATE/1-bug-report.yml create mode 100644 .github/ISSUE_TEMPLATE/2-feature-request.yml create mode 100644 .github/ISSUE_TEMPLATE/config.yml create mode 100644 .github/workflows/test.yaml create mode 100644 .pre-commit-config.yaml create mode 100755 CMakeLists.txt create mode 100644 CODE_OF_CONDUCT.md create mode 100644 CONTRIBUTING.md create mode 100644 LICENSE.txt create mode 100644 OpenArmCANConfig.cmake.in create mode 100644 README.md create mode 100644 examples/demo.cpp create mode 100644 include/openarm/can/socket/arm_component.hpp create mode 100644 include/openarm/can/socket/gripper_component.hpp create mode 100644 include/openarm/can/socket/openarm.hpp create mode 100644 include/openarm/canbus/can_device.hpp create mode 100644 include/openarm/canbus/can_device_collection.hpp create mode 100644 include/openarm/canbus/can_socket.hpp create mode 100644 include/openarm/damiao_motor/dm_motor.hpp create mode 100644 include/openarm/damiao_motor/dm_motor_constants.hpp create mode 100644 include/openarm/damiao_motor/dm_motor_control.hpp create mode 100644 include/openarm/damiao_motor/dm_motor_device.hpp create mode 100644 include/openarm/damiao_motor/dm_motor_device_collection.hpp create mode 100644 openarm-can.pc.in create mode 100644 python/README.md create mode 100755 python/build.sh create mode 100644 python/examples/example.py create mode 100644 python/meson.build create mode 100644 python/openarm/can/__init__.py create mode 100644 python/openarm/can/core.py create mode 100644 python/pyproject.toml create mode 100755 python/requirements.txt create mode 100644 python/src/openarm_can.cpp create mode 100644 python/subprojects/.gitignore create mode 100644 python/subprojects/nanobind.wrap create mode 100644 python/subprojects/robin-map.wrap create mode 100755 setup/change_baudrate.py create mode 100755 setup/configure_socketcan.sh create mode 100644 setup/motor_check.cpp create mode 100755 setup/set_zero.sh create mode 100644 src/openarm/can/socket/arm_component.cpp create mode 100644 src/openarm/can/socket/gripper_component.cpp create mode 100644 src/openarm/can/socket/openarm.cpp create mode 100644 src/openarm/canbus/can_device_collection.cpp create mode 100644 src/openarm/canbus/can_socket.cpp create mode 100644 src/openarm/damiao_motor/dm_motor.cpp create mode 100644 src/openarm/damiao_motor/dm_motor_control.cpp create mode 100644 src/openarm/damiao_motor/dm_motor_device.cpp create mode 100644 src/openarm/damiao_motor/dm_motor_device_collection.cpp diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..50af43a --- /dev/null +++ b/.clang-format @@ -0,0 +1,19 @@ +# 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. + +--- +AccessModifierOffset: -4 +BasedOnStyle: Google +ColumnLimit: 100 +IndentWidth: 4 diff --git a/.cmake-format.py b/.cmake-format.py new file mode 100644 index 0000000..3a7c5f5 --- /dev/null +++ b/.cmake-format.py @@ -0,0 +1,19 @@ +# 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. + +with section("format"): + command_case = "lower" + +with section("markup"): + enable_markup = False diff --git a/.github/ISSUE_TEMPLATE/1-bug-report.yml b/.github/ISSUE_TEMPLATE/1-bug-report.yml new file mode 100644 index 0000000..cd35d2a --- /dev/null +++ b/.github/ISSUE_TEMPLATE/1-bug-report.yml @@ -0,0 +1,25 @@ +# Copyright 2025 Enactic, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +name: Bug Report +description: File a bug report. +type: Bug +body: + - type: textarea + id: description + attributes: + label: Describe the problem you got + description: It's better that you provide information as much as possible. + validations: + required: true diff --git a/.github/ISSUE_TEMPLATE/2-feature-request.yml b/.github/ISSUE_TEMPLATE/2-feature-request.yml new file mode 100644 index 0000000..5c17335 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/2-feature-request.yml @@ -0,0 +1,25 @@ +# Copyright 2025 Enactic, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +name: Feature Request +description: Request a feature. +type: Feature +body: + - type: textarea + id: description + attributes: + label: Describe the feature you want + description: It's better that you also provide your use case. + validations: + required: true diff --git a/.github/ISSUE_TEMPLATE/config.yml b/.github/ISSUE_TEMPLATE/config.yml new file mode 100644 index 0000000..ca3a4e1 --- /dev/null +++ b/.github/ISSUE_TEMPLATE/config.yml @@ -0,0 +1,22 @@ +# Copyright 2025 Enactic, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +blank_issues_enabled: false +contact_links: + - name: Discord + url: https://discord.gg/FsZaZ4z3We + about: Please ask and answer questions here. + - name: GitHub Discussions + url: https://github.com/enactic/openarm_can/discussions + about: If you prefer GitHub Discussions to Discord, you can use GitHub Discussions too. diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml new file mode 100644 index 0000000..bf40299 --- /dev/null +++ b/.github/workflows/test.yaml @@ -0,0 +1,96 @@ +# 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: Test +on: + push: + branches: + - '**' + - '!dependabot/**' + tags: + - '**' + pull_request: +concurrency: + group: ${{ github.head_ref || github.sha }}-${{ github.workflow }} + cancel-in-progress: true +jobs: + lint: + name: Lint + runs-on: ubuntu-latest + timeout-minutes: 5 + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - name: Install pre-commit + run: | + python -m pip install pre-commit + - uses: actions/cache@v4 + with: + path: ~/.cache/pre-commit + key: pre-commit-${{ hashFiles('.pre-commit-config.yaml') }} + - name: Run pre-commit + run: | + pre-commit run --show-diff-on-failure --color=always --all-files + + build: + name: Build + runs-on: ${{ matrix.runs-on }} + timeout-minutes: 5 + strategy: + fail-fast: false + matrix: + runs-on: + - ubuntu-22.04 + - ubuntu-24.04 + steps: + - uses: actions/checkout@v4 + with: + submodules: recursive + - name: "C++: Install dependencies" + run: | + sudo apt update + sudo apt install -y -V \ + cmake \ + ninja-build + - name: "C++: CMake" + run: | + cmake \ + -B ../build \ + -S . \ + -GNinja \ + -DCMAKE_INSTALL_PREFIX=$PWD/install \ + -DCMAKE_BUILD_TYPE=Debug + - name: "C++: Build" + run: | + ninja -C ../build + - name: Install + run: | + ninja -C ../build install + - uses: actions/setup-python@v5 + with: + python-version: 3 + - name: "Python: Install dependencies" + run: | + python3 -m pip install -r python/requirements.txt + - name: "Python: Build: pkg-config" + run: | + python3 -m pip install \ + -Csetup-args=--build.pkg-config-path=$PWD/install/lib/pkgconfig \ + ./python + - name: "Python: Build: CMake" + run: | + python3 -m pip install \ + -Csetup-args=--build.cmake-prefix-path=$PWD/install \ + ./python diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..84f6438 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,42 @@ +# 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. + +repos: + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v20.1.7 + hooks: + - id: clang-format + alias: cpp + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.13 + hooks: + - id: cmake-format + alias: cpp + - repo: https://github.com/hhatto/autopep8 + rev: v2.3.2 + hooks: + - id: autopep8 + alias: python + name: Python Format + args: + - "--global-config" + - "python/pyproject.toml" + - "--ignore-local-config" + - "--in-place" + - repo: https://github.com/trim21/pre-commit-mirror-meson + rev: v1.8.2 + hooks: + - id: meson-fmt + alias: python + args: ['--inplace'] diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100755 index 0000000..63e1afd --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,127 @@ +# 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_can VERSION 1.0.0) + +# Set C++ standard +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() + +# Testing support +include(CTest) + +include(GNUInstallDirs) + +# Create the main library +add_library( + openarm_can + src/openarm/can/socket/arm_component.cpp + src/openarm/can/socket/gripper_component.cpp + src/openarm/can/socket/openarm.cpp + src/openarm/canbus/can_device_collection.cpp + src/openarm/canbus/can_socket.cpp + src/openarm/damiao_motor/dm_motor.cpp + src/openarm/damiao_motor/dm_motor_control.cpp + src/openarm/damiao_motor/dm_motor_device.cpp + src/openarm/damiao_motor/dm_motor_device_collection.cpp) +set_target_properties(openarm_can PROPERTIES POSITION_INDEPENDENT_CODE ON) +set(USE_FILE_SET_HEADERS FALSE) +# Meson doesn't support FILE_SET TYPE HEADERS... +# if(CMAKE_VERSION VERSION_GREATER_EQUAL 3.23) +# set(USE_FILE_SET_HEADERS TRUE) +# endif() +if(USE_FILE_SET_HEADERS) + target_sources( + openarm_can + PUBLIC FILE_SET + HEADERS + TYPE + HEADERS + BASE_DIRS + include + FILES + include/openarm/can/socket/arm_component.hpp + include/openarm/can/socket/gripper_component.hpp + include/openarm/can/socket/openarm.hpp + include/openarm/canbus/can_device.hpp + include/openarm/canbus/can_device_collection.hpp + include/openarm/canbus/can_socket.hpp + include/openarm/damiao_motor/dm_motor.hpp + include/openarm/damiao_motor/dm_motor_constants.hpp + include/openarm/damiao_motor/dm_motor_control.hpp + include/openarm/damiao_motor/dm_motor_device.hpp + include/openarm/damiao_motor/dm_motor_device_collection.hpp) + install( + TARGETS openarm_can + EXPORT openarm_can_export + FILE_SET HEADERS) +else() + target_include_directories( + openarm_can PUBLIC $ + $) + install(TARGETS openarm_can EXPORT openarm_can_export) + install(DIRECTORY include/openarm TYPE INCLUDE) +endif() + +# CMake package +set(INSTALL_CMAKE_DIR ${CMAKE_INSTALL_LIBDIR}/cmake/OpenArmCAN) +install( + EXPORT openarm_can_export + DESTINATION ${INSTALL_CMAKE_DIR} + NAMESPACE OpenArmCAN:: + FILE OpenArmCANTargets.cmake) +include(CMakePackageConfigHelpers) +configure_package_config_file( + OpenArmCANConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/OpenArmCANConfig.cmake + INSTALL_DESTINATION ${INSTALL_CMAKE_DIR}) +write_basic_package_version_file( + ${CMAKE_CURRENT_BINARY_DIR}/OpenArmCANVersion.cmake + VERSION ${PROJECT_VERSION} + COMPATIBILITY SameMajorVersion) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/OpenArmCANConfig.cmake + ${CMAKE_CURRENT_BINARY_DIR}/OpenArmCANVersion.cmake + DESTINATION ${INSTALL_CMAKE_DIR}) + +# pkg-config package +if(IS_ABSOLUTE "${CMAKE_INSTALL_INCLUDEDIR}") + set(PKG_CONFIG_INCLUDEDIR "${CMAKE_INSTALL_INCLUDEDIR}") +else() + set(PKG_CONFIG_INCLUDEDIR "\${prefix}/${CMAKE_INSTALL_INCLUDEDIR}") +endif() +if(IS_ABSOLUTE "${CMAKE_INSTALL_LIBDIR}") + set(PKG_CONFIG_LIBDIR "${CMAKE_INSTALL_LIBDIR}") +else() + set(PKG_CONFIG_LIBDIR "\${prefix}/${CMAKE_INSTALL_LIBDIR}") +endif() +configure_file(openarm-can.pc.in ${CMAKE_CURRENT_BINARY_DIR}/openarm-can.pc + @ONLY) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/openarm-can.pc + DESTINATION ${CMAKE_INSTALL_LIBDIR}/pkgconfig) + +add_executable(motor-check setup/motor_check.cpp) +target_link_libraries(motor-check openarm_can) + +# Add motor control example executable +add_executable(openarm-demo examples/demo.cpp) +target_link_libraries(openarm-demo openarm_can) + +# Add tests +if(BUILD_TESTING) + # add_subdirectory(test) +endif() diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 0000000..bee83e9 --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,133 @@ + +# Contributor Covenant Code of Conduct + +## Our Pledge + +We as members, contributors, and leaders pledge to make participation in our +community a harassment-free experience for everyone, regardless of age, body +size, visible or invisible disability, ethnicity, sex characteristics, gender +identity and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, caste, color, religion, or sexual +identity and orientation. + +We pledge to act and interact in ways that contribute to an open, welcoming, +diverse, inclusive, and healthy community. + +## Our Standards + +Examples of behavior that contributes to a positive environment for our +community include: + +* Demonstrating empathy and kindness toward other people +* Being respectful of differing opinions, viewpoints, and experiences +* Giving and gracefully accepting constructive feedback +* Accepting responsibility and apologizing to those affected by our mistakes, + and learning from the experience +* Focusing on what is best not just for us as individuals, but for the overall + community + +Examples of unacceptable behavior include: + +* The use of sexualized language or imagery, and sexual attention or advances of + any kind +* Trolling, insulting or derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or email address, + without their explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Enforcement Responsibilities + +Community leaders are responsible for clarifying and enforcing our standards of +acceptable behavior and will take appropriate and fair corrective action in +response to any behavior that they deem inappropriate, threatening, offensive, +or harmful. + +Community leaders have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, and will communicate reasons for moderation +decisions when appropriate. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official email address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported to the community leaders responsible for enforcement at +hi_public@reazon.jp. +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Enforcement Guidelines + +Community leaders will follow these Community Impact Guidelines in determining +the consequences for any action they deem in violation of this Code of Conduct: + +### 1. Correction + +**Community Impact**: Use of inappropriate language or other behavior deemed +unprofessional or unwelcome in the community. + +**Consequence**: A private, written warning from community leaders, providing +clarity around the nature of the violation and an explanation of why the +behavior was inappropriate. A public apology may be requested. + +### 2. Warning + +**Community Impact**: A violation through a single incident or series of +actions. + +**Consequence**: A warning with consequences for continued behavior. No +interaction with the people involved, including unsolicited interaction with +those enforcing the Code of Conduct, for a specified period of time. This +includes avoiding interactions in community spaces as well as external channels +like social media. Violating these terms may lead to a temporary or permanent +ban. + +### 3. Temporary Ban + +**Community Impact**: A serious violation of community standards, including +sustained inappropriate behavior. + +**Consequence**: A temporary ban from any sort of interaction or public +communication with the community for a specified period of time. No public or +private interaction with the people involved, including unsolicited interaction +with those enforcing the Code of Conduct, is allowed during this period. +Violating these terms may lead to a permanent ban. + +### 4. Permanent Ban + +**Community Impact**: Demonstrating a pattern of violation of community +standards, including sustained inappropriate behavior, harassment of an +individual, or aggression toward or disparagement of classes of individuals. + +**Consequence**: A permanent ban from any sort of public interaction within the +community. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 2.1, available at +[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. + +Community Impact Guidelines were inspired by +[Mozilla's code of conduct enforcement ladder][Mozilla CoC]. + +For answers to common questions about this code of conduct, see the FAQ at +[https://www.contributor-covenant.org/faq][FAQ]. Translations are available at +[https://www.contributor-covenant.org/translations][translations]. + +[homepage]: https://www.contributor-covenant.org +[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html +[Mozilla CoC]: https://github.com/mozilla/diversity +[FAQ]: https://www.contributor-covenant.org/faq +[translations]: https://www.contributor-covenant.org/translations diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..7106f6d --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,19 @@ +# How to contribute + +## Did you find a bug? + +Please report it to [GitHub Issues](https://github.com/enactic/openarm_can/issues/new?template=1-bug-report.yml)! + +## Did you have a feature request? + +Please share it to [GitHub Issues](https://github.com/enactic/openarm_can/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_can/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_can/discussions)! diff --git a/LICENSE.txt b/LICENSE.txt new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/LICENSE.txt @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/OpenArmCANConfig.cmake.in b/OpenArmCANConfig.cmake.in new file mode 100644 index 0000000..a91785b --- /dev/null +++ b/OpenArmCANConfig.cmake.in @@ -0,0 +1,19 @@ +# 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. + +@PACKAGE_INIT@ + +include("${CMAKE_CURRENT_LIST_DIR}/OpenArmCANTargets.cmake") + +check_required_components(OpenArmCAN) diff --git a/README.md b/README.md new file mode 100644 index 0000000..db1b0e2 --- /dev/null +++ b/README.md @@ -0,0 +1,96 @@ +# OpenArm CAN Library + +A C++ library for CAN communication with OpenArm robotic hardware, supporting Damiao motors over CAN/CAN-FD interfaces. +This library is a part of [OpenArm SDK](https://github.com/enactic/openarm_sdk). + +## Quick Start + +### Prerequisites + +- Linux with SocketCAN support +- CAN interface hardware +- CMake 3.22+ +- C++17 compiler + +### 1. Setup CAN Interface + +Configure your CAN interface using the provided script: + +```bash +# CAN 2.0 (default) +setup/configure_socketcan.sh can0 + +# CAN-FD with 5Mbps data rate +setup/configure_socketcan.sh can0 -fd +``` + +### 2. C++ Library + +**Build & Install:** + +```bash +cd openarm_can +cmake -S . -B build -DCMAKE_BUILD_TYPE=Release +cmake --build build +sudo cmake --install build +``` + +**Usage:** + +```cpp +#include + +openarm::can::socket::OpenArm arm("can0", true); // CAN-FD enabled +arm.init_arm_motors({MotorType::DM4310}, {0x01}, {0x11}); +arm.enable_all(); +``` + +### 3. Python (šŸš§ EXPERIMENTAL - TEMPORARY šŸš§) + +> āš ļø **WARNING: UNSTABLE API** āš ļø +> Python bindings are currently a direct low level **temporary port**, and will change **DRASTICALLY**. +> The interface is may break between versions.Use at your own risk! Discussions on the interface are welcomed. + +**Build & Install:** + +```bash +cd python + +# Create and activate virtual environment (recommended) +python -m venv venv +source venv/bin/activate + +./build.sh +``` + +**Usage:** + +```python +# WARNING: This API is unstable and will change! +import openarm_can as oa + +arm = oa.OpenArm("can0", True) # CAN-FD enabled +arm.init_arm_motors([oa.MotorType.DM4310], [0x01], [0x11]) +arm.enable_all() +``` + +### Examples + +- **C++**: `examples/demo.cpp` - Complete arm control demo +- **Python**: `python/examples/example.py` - Basic Python usage + +## Related links + +- šŸ“š Read the [documentation](https://docs.openarm.dev/software/sdk/openarm-can/) +- šŸ’¬ Join the community on [Discord](https://discord.gg/FsZaZ4z3We) +- šŸ“¬ Contact us through + +## License + +Licensed under the Apache License 2.0. See `LICENSE.txt` for details. + +Copyright 2025 Enactic, Inc. + +## Code of Conduct + +All participation in the OpenArm project is governed by our [Code of Conduct](CODE_OF_CONDUCT.md). diff --git a/examples/demo.cpp b/examples/demo.cpp new file mode 100644 index 0000000..8cce261 --- /dev/null +++ b/examples/demo.cpp @@ -0,0 +1,118 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include + +int main() { + try { + std::cout << "=== OpenArm CAN Example ===" << std::endl; + std::cout << "This example demonstrates the OpenArm API functionality" << std::endl; + + // Initialize OpenArm with CAN interface and enable CAN-FD + std::cout << "Initializing OpenArm CAN..." << std::endl; + openarm::can::socket::OpenArm openarm("can0", true); // Use CAN-FD on can0 interface + + // Initialize arm motors + std::vector motor_types = { + openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310}; + std::vector send_can_ids = {0x01, 0x02}; + std::vector recv_can_ids = {0x11, 0x12}; + 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 enable all motors + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::IGNORE); + + // Enable all motors + std::cout << "\n=== Enabling Motors ===" << std::endl; + openarm.enable_all(); + // Allow time (2ms) for the motors to respond for slow operations like enabling + openarm.recv_all(2000); + + // Set device mode to param and query motor id + std::cout << "\n=== Querying Motor Recv IDs ===" << std::endl; + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::PARAM); + openarm.query_param_all(static_cast(openarm::damiao_motor::RID::MST_ID)); + // Allow time (2ms) for the motors to respond for slow operations like querying + // parameter from register + openarm.recv_all(2000); + + // Access motors through components + for (const auto& motor : openarm.get_arm().get_motors()) { + std::cout << "Arm Motor: " << motor.get_send_can_id() << " ID: " + << motor.get_param(static_cast(openarm::damiao_motor::RID::MST_ID)) + << std::endl; + } + for (const auto& motor : openarm.get_gripper().get_motors()) { + std::cout << "Gripper Motor: " << motor.get_send_can_id() << " ID: " + << motor.get_param(static_cast(openarm::damiao_motor::RID::MST_ID)) + << std::endl; + } + + // Set device mode to state and control motor + std::cout << "\n=== Controlling Motors ===" << std::endl; + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); + + // Control arm motors with position control + openarm.get_arm().mit_control_all({openarm::damiao_motor::MITParam{2, 1, 0, 0, 0}, + openarm::damiao_motor::MITParam{2, 1, 0, 0, 0}}); + openarm.recv_all(500); + + // Control arm motors with torque control + openarm.get_arm().mit_control_all({openarm::damiao_motor::MITParam{0, 0, 0, 0, 0.1}, + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0.1}}); + openarm.recv_all(500); + + // Control gripper + std::cout << "Closing gripper..." << std::endl; + openarm.get_gripper().close(); + openarm.recv_all(1000); + + for (int i = 0; i < 10; i++) { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + openarm.refresh_all(); + openarm.recv_all(300); + + // 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; + } + } + + openarm.disable_all(); + openarm.recv_all(1000); + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return -1; + } + + return 0; +} diff --git a/include/openarm/can/socket/arm_component.hpp b/include/openarm/can/socket/arm_component.hpp new file mode 100644 index 0000000..34688fd --- /dev/null +++ b/include/openarm/can/socket/arm_component.hpp @@ -0,0 +1,38 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include "../../canbus/can_socket.hpp" +#include "../../damiao_motor/dm_motor.hpp" +#include "../../damiao_motor/dm_motor_device_collection.hpp" + +namespace openarm::can::socket { + +class ArmComponent : public damiao_motor::DMDeviceCollection { +public: + ArmComponent(canbus::CANSocket& can_socket); + ~ArmComponent() = default; + + void init_motor_devices(std::vector motor_types, + std::vector send_can_ids, std::vector recv_can_ids, + bool use_fd); + +private: + std::vector motors_; +}; + +} // namespace openarm::can::socket diff --git a/include/openarm/can/socket/gripper_component.hpp b/include/openarm/can/socket/gripper_component.hpp new file mode 100644 index 0000000..7a9036c --- /dev/null +++ b/include/openarm/can/socket/gripper_component.hpp @@ -0,0 +1,70 @@ +// 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 + +#define _USE_MATH_DEFINES +#include +#include + +#include "../../damiao_motor/dm_motor.hpp" +#include "../../damiao_motor/dm_motor_device_collection.hpp" + +namespace openarm::can::socket { + +class GripperComponent : public damiao_motor::DMDeviceCollection { +public: + GripperComponent(canbus::CANSocket& can_socket); + ~GripperComponent() = default; + + void init_motor_device(damiao_motor::MotorType motor_type, uint32_t send_can_id, + uint32_t recv_can_id, bool use_fd); + + // Gripper-specific controls + void open(double kp = 50.0, double kd = 1.0); + void close(double kp = 50.0, double kd = 1.0); + damiao_motor::Motor* get_motor() const { return motor_.get(); } + +private: + std::unique_ptr motor_; + std::shared_ptr motor_device_; + + void set_position(double position, double kp = 50.0, double kd = 1.0); + + // The actual physical gripper uses a slider cranker-like mechanism, this mapping is an + // approximation. + double gripper_to_motor_position(double gripper_position) { + // Map gripper position (0.0=closed, 1.0=open) to motor position + return (gripper_position - gripper_open_position_) / + (gripper_closed_position_ - gripper_open_position_) * + (motor_closed_position_ - motor_open_position_) + + motor_open_position_; + } + + double motor_to_gripper_position(double motor_position) { + // Map motor position back to gripper position (0.0=closed, 1.0=open) + return (motor_position - motor_open_position_) / + (motor_closed_position_ - motor_open_position_) * + (gripper_closed_position_ - gripper_open_position_) + + gripper_open_position_; + } + + // Gripper configuration + double gripper_open_position_ = 1.0; + double gripper_closed_position_ = 0.0; + double motor_open_position_ = -1.0472; // 60 degrees + double motor_closed_position_ = 0.0; +}; + +} // namespace openarm::can::socket diff --git a/include/openarm/can/socket/openarm.hpp b/include/openarm/can/socket/openarm.hpp new file mode 100644 index 0000000..b61f974 --- /dev/null +++ b/include/openarm/can/socket/openarm.hpp @@ -0,0 +1,68 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include "../../canbus/can_device_collection.hpp" +#include "../../canbus/can_socket.hpp" +#include "arm_component.hpp" +#include "gripper_component.hpp" + +namespace openarm::can::socket { +class OpenArm { +public: + OpenArm(const std::string& can_interface, bool enable_fd = false); + ~OpenArm() = default; + + // Component initialization + void init_arm_motors(const std::vector& motor_types, + const std::vector& send_can_ids, + const std::vector& recv_can_ids); + + void init_gripper_motor(damiao_motor::MotorType motor_type, uint32_t send_can_id, + uint32_t recv_can_id); + + // Component access + ArmComponent& get_arm() { return *arm_; } + GripperComponent& get_gripper() { return *gripper_; } + canbus::CANDeviceCollection& get_master_can_device_collection() { + return *master_can_device_collection_; + } + + // Damiao Motor operations (works only on sub_dm_device_collections_) + void enable_all(); + void disable_all(); + void set_zero_all(); + void refresh_all(); + // The timeout for reading from socket, set to timeout_us. + // Tuning this value may improve the performance but should be done with caution. + void recv_all(int timeout_us = 500); + void set_callback_mode_all(damiao_motor::CallbackMode callback_mode); + void query_param_all(int RID); + +private: + std::string can_interface_; + bool enable_fd_; + std::unique_ptr can_socket_; + std::unique_ptr arm_; + std::unique_ptr gripper_; + std::unique_ptr master_can_device_collection_; + std::vector sub_dm_device_collections_; + void register_dm_device_collection(damiao_motor::DMDeviceCollection& device_collection); +}; + +} // namespace openarm::can::socket diff --git a/include/openarm/canbus/can_device.hpp b/include/openarm/canbus/can_device.hpp new file mode 100644 index 0000000..b81dfec --- /dev/null +++ b/include/openarm/canbus/can_device.hpp @@ -0,0 +1,50 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include +#include + +namespace openarm::canbus { +// Abstract base class for CAN devices +class CANDevice { +public: + explicit CANDevice(canid_t send_can_id, canid_t recv_can_id, canid_t recv_can_mask, + bool is_fd_enabled = false) + : send_can_id_(send_can_id), + recv_can_id_(recv_can_id), + recv_can_mask_(recv_can_mask), + is_fd_enabled_(is_fd_enabled) {} + virtual ~CANDevice() = default; + + virtual void callback(const can_frame& frame) = 0; + virtual void callback(const canfd_frame& frame) = 0; + + canid_t get_send_can_id() const { return send_can_id_; } + canid_t get_recv_can_id() const { return recv_can_id_; } + canid_t get_recv_can_mask() const { return recv_can_mask_; } + bool is_fd_enabled() const { return is_fd_enabled_; } + +protected: + canid_t send_can_id_; + canid_t recv_can_id_; + // mask for receiving + canid_t recv_can_mask_ = CAN_SFF_MASK; + bool is_fd_enabled_ = false; +}; +} // namespace openarm::canbus diff --git a/include/openarm/canbus/can_device_collection.hpp b/include/openarm/canbus/can_device_collection.hpp new file mode 100644 index 0000000..11e5436 --- /dev/null +++ b/include/openarm/canbus/can_device_collection.hpp @@ -0,0 +1,43 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include + +#include "can_device.hpp" +#include "can_socket.hpp" + +namespace openarm::canbus { +class CANDeviceCollection { +public: + CANDeviceCollection(canbus::CANSocket& can_socket); + ~CANDeviceCollection(); + + void add_device(const std::shared_ptr& device); + void remove_device(const std::shared_ptr& device); + void dispatch_frame_callback(can_frame& frame); + void dispatch_frame_callback(canfd_frame& frame); + const std::map>& get_devices() const { return devices_; } + canbus::CANSocket& get_can_socket() const { return can_socket_; } + int get_socket_fd() const { return can_socket_.get_socket_fd(); } + +private: + canbus::CANSocket& can_socket_; + std::map> devices_; +}; +} // namespace openarm::canbus diff --git a/include/openarm/canbus/can_socket.hpp b/include/openarm/canbus/can_socket.hpp new file mode 100644 index 0000000..0669c7b --- /dev/null +++ b/include/openarm/canbus/can_socket.hpp @@ -0,0 +1,75 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include +#include + +namespace openarm::canbus { + +// Exception classes for socket operations +class CANSocketException : public std::runtime_error { +public: + explicit CANSocketException(const std::string& message) + : std::runtime_error("Socket error: " + message) {} +}; + +// Base socket management class +class CANSocket { +public: + explicit CANSocket(const std::string& interface, bool enable_fd = false); + ~CANSocket(); + + // Disable copy, enable move + CANSocket(const CANSocket&) = delete; + CANSocket& operator=(const CANSocket&) = delete; + CANSocket(CANSocket&&) = default; + CANSocket& operator=(CANSocket&&) = default; + + // File descriptor access for Python bindings + int get_socket_fd() const { return socket_fd_; } + const std::string& get_interface() const { return interface_; } + bool is_canfd_enabled() const { return fd_enabled_; } + bool is_initialized() const { return initialized_; } + + // Direct frame operations for Python bindings + ssize_t read_raw_frame(void* buffer, size_t buffer_size); + ssize_t write_raw_frame(const void* buffer, size_t frame_size); + + // write can_frame or canfd_frame + bool write_can_frame(const can_frame& frame); + bool write_canfd_frame(const canfd_frame& frame); + + // read can_frame or canfd_frame + bool read_can_frame(can_frame& frame); + bool read_canfd_frame(canfd_frame& frame); + + // check if data is available for reading (non-blocking) + bool is_data_available(int timeout_us = 100); + +protected: + bool initialize_socket(const std::string& interface); + void cleanup(); + + int socket_fd_; + std::string interface_; + bool fd_enabled_; + bool initialized_; +}; + +} // namespace openarm::canbus diff --git a/include/openarm/damiao_motor/dm_motor.hpp b/include/openarm/damiao_motor/dm_motor.hpp new file mode 100644 index 0000000..93149e7 --- /dev/null +++ b/include/openarm/damiao_motor/dm_motor.hpp @@ -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. + +#pragma once + +#include +#include +#include + +#include "dm_motor_constants.hpp" + +namespace openarm::damiao_motor { +class Motor { + friend class DMCANDevice; // Allow MotorDeviceCan to access protected + // members + friend class DMControl; + +public: + // Constructor + Motor(MotorType motor_type, uint32_t send_can_id, uint32_t recv_can_id); + + // State getters + double get_position() const { return state_q_; } + double get_velocity() const { return state_dq_; } + double get_torque() const { return state_tau_; } + int get_state_tmos() const { return state_tmos_; } + int get_state_trotor() const { return state_trotor_; } + + // Motor property getters + uint32_t get_send_can_id() const { return send_can_id_; } + uint32_t get_recv_can_id() const { return recv_can_id_; } + MotorType get_motor_type() const { return motor_type_; } + + // Enable status getters + bool is_enabled() const { return enabled_; } + + // Parameter methods + double get_param(int RID) const; + + // Static methods for motor properties + static LimitParam get_limit_param(MotorType motor_type); + +protected: + // State update methods + void update_state(double q, double dq, double tau, int tmos, int trotor); + void set_state_tmos(int tmos); + void set_state_trotor(int trotor); + void set_enabled(bool enabled); + void set_temp_param(int RID, int val); + + // Motor identifiers + uint32_t send_can_id_; + uint32_t recv_can_id_; + MotorType motor_type_; + + // Enable status + bool enabled_; + + // Current state + double state_q_, state_dq_, state_tau_; + int state_tmos_, state_trotor_; + + // Parameter storage + std::map temp_param_dict_; +}; +} // namespace openarm::damiao_motor diff --git a/include/openarm/damiao_motor/dm_motor_constants.hpp b/include/openarm/damiao_motor/dm_motor_constants.hpp new file mode 100644 index 0000000..c4ce5f5 --- /dev/null +++ b/include/openarm/damiao_motor/dm_motor_constants.hpp @@ -0,0 +1,111 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +namespace openarm::damiao_motor { +enum class MotorType : uint8_t { + DM3507 = 0, + DM4310 = 1, + DM4310_48V = 2, + DM4340 = 3, + DM4340_48V = 4, + DM6006 = 5, + DM8006 = 6, + DM8009 = 7, + DM10010L = 8, + DM10010 = 9, + DMH3510 = 10, + DMH6215 = 11, + DMG6220 = 12, + COUNT = 13 +}; + +enum class RID : uint8_t { + UV_Value = 0, + KT_Value = 1, + OT_Value = 2, + OC_Value = 3, + ACC = 4, + DEC = 5, + MAX_SPD = 6, + MST_ID = 7, + ESC_ID = 8, + TIMEOUT = 9, + CTRL_MODE = 10, + Damp = 11, + Inertia = 12, + hw_ver = 13, + sw_ver = 14, + SN = 15, + NPP = 16, + Rs = 17, + LS = 18, + Flux = 19, + Gr = 20, + PMAX = 21, + VMAX = 22, + TMAX = 23, + I_BW = 24, + KP_ASR = 25, + KI_ASR = 26, + KP_APR = 27, + KI_APR = 28, + OV_Value = 29, + GREF = 30, + Deta = 31, + V_BW = 32, + IQ_c1 = 33, + VL_c1 = 34, + can_br = 35, + sub_ver = 36, + u_off = 50, + v_off = 51, + k1 = 52, + k2 = 53, + m_off = 54, + dir = 55, + p_m = 80, + xout = 81, + COUNT = 82 +}; + +// Limit parameters structure for different motor types +struct LimitParam { + double pMax; // Position limit (rad) + double vMax; // Velocity limit (rad/s) + double tMax; // Torque limit (Nm) +}; +// Limit parameters for each motor type [pMax, vMax, tMax] +inline constexpr std::array(MotorType::COUNT)> + MOTOR_LIMIT_PARAMS = {{ + {12.5, 50, 5}, // DM3507 + {12.5, 30, 10}, // DM4310 + {12.5, 50, 10}, // DM4310_48V + {12.5, 8, 28}, // DM4340 + {12.5, 10, 28}, // DM4340_48V + {12.5, 45, 20}, // DM6006 + {12.5, 45, 40}, // DM8006 + {12.5, 45, 54}, // DM8009 + {12.5, 25, 200}, // DM10010L + {12.5, 20, 200}, // DM10010 + {12.5, 280, 1}, // DMH3510 + {12.5, 45, 10}, // DMH6215 + {12.5, 45, 10} // DMG6220 + }}; +} // namespace openarm::damiao_motor diff --git a/include/openarm/damiao_motor/dm_motor_control.hpp b/include/openarm/damiao_motor/dm_motor_control.hpp new file mode 100644 index 0000000..ffdca2b --- /dev/null +++ b/include/openarm/damiao_motor/dm_motor_control.hpp @@ -0,0 +1,91 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include +#include // for memcpy +#include +#include +#include + +#include "dm_motor.hpp" +#include "dm_motor_constants.hpp" + +namespace openarm::damiao_motor { +// Forward declarations +class Motor; + +struct ParamResult { + int rid; + double value; + bool valid; +}; + +struct StateResult { + double position; + double velocity; + double torque; + int t_mos; + int t_rotor; + bool valid; +}; + +struct CANPacket { + uint32_t send_can_id; + std::vector data; +}; + +struct MITParam { + double kp; + double kd; + double q; + double dq; + double tau; +}; + +class CanPacketEncoder { +public: + static CANPacket create_enable_command(const Motor& motor); + static CANPacket create_disable_command(const Motor& motor); + static CANPacket create_set_zero_command(const Motor& motor); + static CANPacket create_mit_control_command(const Motor& motor, const MITParam& mit_param); + static CANPacket create_query_param_command(const Motor& motor, int RID); + static CANPacket create_refresh_command(const Motor& motor); + +private: + static std::vector pack_mit_control_data(MotorType motor_type, + const MITParam& mit_param); + static std::vector pack_query_param_data(uint32_t send_can_id, int RID); + static std::vector pack_command_data(uint8_t cmd); + + static double limit_min_max(double x, double min, double max); + static uint16_t double_to_uint(double x, double x_min, double x_max, int bits); +}; + +class CanPacketDecoder { +public: + static StateResult parse_motor_state_data(const Motor& motor, const std::vector& data); + static ParamResult parse_motor_param_data(const std::vector& data); + +private: + static double uint_to_double(uint16_t x, double min, double max, int bits); + static float uint8s_to_float(const std::array& bytes); + static uint32_t uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, uint8_t byte4); + static bool is_in_ranges(int number); +}; + +} // namespace openarm::damiao_motor diff --git a/include/openarm/damiao_motor/dm_motor_device.hpp b/include/openarm/damiao_motor/dm_motor_device.hpp new file mode 100644 index 0000000..c3c17a3 --- /dev/null +++ b/include/openarm/damiao_motor/dm_motor_device.hpp @@ -0,0 +1,50 @@ +// 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 "../canbus/can_device.hpp" +#include "../canbus/can_socket.hpp" +#include "dm_motor.hpp" +#include "dm_motor_control.hpp" + +namespace openarm::damiao_motor { +enum CallbackMode { + STATE, + PARAM, + // discard + IGNORE +}; + +class DMCANDevice : public canbus::CANDevice { +public: + explicit DMCANDevice(Motor& motor, canid_t recv_can_mask, bool use_fd); + void callback(const can_frame& frame); + void callback(const canfd_frame& frame); + + // Create frame from data array + can_frame create_can_frame(canid_t send_can_id, std::vector data); + canfd_frame create_canfd_frame(canid_t send_can_id, std::vector data); + // Getter method to access motor state + Motor& get_motor() { return motor_; } + void set_callback_mode(CallbackMode callback_mode) { callback_mode_ = callback_mode; } + +private: + std::vector get_data_from_frame(const can_frame& frame); + std::vector get_data_from_frame(const canfd_frame& frame); + Motor& motor_; + CallbackMode callback_mode_; + bool use_fd_; // Track if using CAN-FD +}; +} // namespace openarm::damiao_motor diff --git a/include/openarm/damiao_motor/dm_motor_device_collection.hpp b/include/openarm/damiao_motor/dm_motor_device_collection.hpp new file mode 100644 index 0000000..dde4407 --- /dev/null +++ b/include/openarm/damiao_motor/dm_motor_device_collection.hpp @@ -0,0 +1,64 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include "../canbus/can_device_collection.hpp" +#include "dm_motor_constants.hpp" +#include "dm_motor_control.hpp" +#include "dm_motor_device.hpp" + +namespace openarm::damiao_motor { + +class DMDeviceCollection { +public: + DMDeviceCollection(canbus::CANSocket& can_socket); + virtual ~DMDeviceCollection() = default; + + // Common motor operations + void enable_all(); + void disable_all(); + void set_zero_all(); + void set_callback_mode_all(CallbackMode callback_mode); + + // Refresh operations (for individual motors) + void refresh_one(int i); + void refresh_all(); + + // Query parameter operations + void query_param_one(int i, int RID); + void query_param_all(int RID); + + // MIT control operations + void mit_control_one(int i, const MITParam& mit_param); + void mit_control_all(const std::vector& mit_params); + + // Device collection access + std::vector get_motors() const; + canbus::CANDeviceCollection& get_device_collection() { return *device_collection_; } + +protected: + canbus::CANSocket& can_socket_; + std::unique_ptr can_packet_encoder_; + std::unique_ptr can_packet_decoder_; + std::unique_ptr device_collection_; + + // Helper methods for subclasses + void send_command_to_device(std::shared_ptr dm_device, const CANPacket& packet); + std::vector> get_dm_devices() const; +}; +} // namespace openarm::damiao_motor diff --git a/openarm-can.pc.in b/openarm-can.pc.in new file mode 100644 index 0000000..428a8db --- /dev/null +++ b/openarm-can.pc.in @@ -0,0 +1,23 @@ +# 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. + +prefix=@CMAKE_INSTALL_PREFIX@ +includedir=@PKG_CONFIG_INCLUDEDIR@ +libdir=@PKG_CONFIG_LIBDIR@ + +Name: OpenArm CAN +Description: OpenArm CAN library +Version: @PROJECT_VERSION@ +Libs: -L${libdir} -lopenarm_can +Cflags: -I${includedir} diff --git a/python/README.md b/python/README.md new file mode 100644 index 0000000..8f3513d --- /dev/null +++ b/python/README.md @@ -0,0 +1,3 @@ +# OpenArm + +...TODO... diff --git a/python/build.sh b/python/build.sh new file mode 100755 index 0000000..018fffe --- /dev/null +++ b/python/build.sh @@ -0,0 +1,55 @@ +#!/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. +# +# Build script for OpenArm Python bindings + +set -e + +# Get script directory +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +PROJECT_ROOT="$(dirname "$SCRIPT_DIR")" + +echo "Building OpenArm Python bindings..." +echo "Script directory: $SCRIPT_DIR" +echo "Project root: $PROJECT_ROOT" + +# Check if we're in a virtual environment +if [[ "$VIRTUAL_ENV" != "" ]]; then + echo "Using virtual environment: $VIRTUAL_ENV" +else + echo "Warning: Not in a virtual environment. Consider using 'python -m venv venv && source venv/bin/activate'" +fi + +# Build the C++ library first if needed +if [ ! -d "$PROJECT_ROOT/build" ]; then + echo "Building C++ library..." + mkdir -p "$PROJECT_ROOT/build" + cd "$PROJECT_ROOT/build" + cmake .. + make -j$(nproc) + cd "$SCRIPT_DIR" +fi + +# Install in development mode +echo "Installing in development mode..." +pip install . + +echo "Build completed successfully!" +echo "" +echo "To test the installation, run:" +echo " python -c 'import openarm; print(openarm.__version__)'" +echo "" +echo "See examples/ directory for usage examples." diff --git a/python/examples/example.py b/python/examples/example.py new file mode 100644 index 0000000..06ed05e --- /dev/null +++ b/python/examples/example.py @@ -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. + +import openarm_can as oa +import time +# Create OpenArm instance + +arm = oa.OpenArm("can0", True) + +# Initialize arm motors +motor_types = [oa.MotorType.DM4310, oa.MotorType.DM4310] +send_ids = [0x01, 0x02] +recv_ids = [0x11, 0x12] +arm.init_arm_motors(motor_types, send_ids, recv_ids) + +# Initialize gripper +arm.init_gripper_motor(oa.MotorType.DM4310, 0x7, 0x17) +arm.set_callback_mode_all(oa.CallbackMode.IGNORE) +# Use high-level operations +arm.enable_all() +arm.recv_all() + + +# return to zero position +arm.set_callback_mode_all(oa.CallbackMode.STATE) +arm.get_arm().mit_control_all([oa.MITParam(2, 0.5, 0, 0, 0), + oa.MITParam(2, 0.5, 0, 0, 0)]) + +arm.recv_all() + +# torque control test + +arm.get_gripper().mit_control_all([oa.MITParam(0, 0, 0, 0, 0.15)]) +arm.get_arm().mit_control_all( + [oa.MITParam(0, 0, 0, 0, 0.15), oa.MITParam(0, 0, 0, 0, 0.15)]) +arm.recv_all() + +# read motor position +while True: + arm.refresh_all() + arm.recv_all() + for motor in arm.get_arm().get_motors(): + print(motor.get_position()) + for motor in arm.get_gripper().get_motors(): + print(motor.get_position()) diff --git a/python/meson.build b/python/meson.build new file mode 100644 index 0000000..a6c62db --- /dev/null +++ b/python/meson.build @@ -0,0 +1,41 @@ +# 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. + +project( + 'openarm_can', + 'cpp', + version: '1.0.0', + default_options: ['cpp_std=c++17'], +) + +py = import('python').find_installation(pure: false) +nanobind_dep = dependency('nanobind') + +openarm_cpp_dep = dependency( + 'openarm-can', + 'OpenArmCAN', + modules: ['OpenArmCAN::openarm_can'], +) + +py.install_sources( + 'openarm/can/__init__.py', + 'openarm/can/core.py', + subdir: 'openarm/can', +) +py.extension_module( + 'openarm_can', + 'src/openarm_can.cpp', + install: true, + dependencies: [openarm_cpp_dep, nanobind_dep], +) diff --git a/python/openarm/can/__init__.py b/python/openarm/can/__init__.py new file mode 100644 index 0000000..d6affd8 --- /dev/null +++ b/python/openarm/can/__init__.py @@ -0,0 +1,53 @@ +# 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. + +""" +OpenArm CAN Python bindings for motor control via SocketCAN. + +This package provides Python bindings for the OpenArm motor control system, +allowing you to control DAMIAO motors through SocketCAN. +""" + +# Import all C++ bindings directly - 1:1 mapping with C++ API +from .core import * + +__version__ = "1.0.0" +__author__ = "Enactic, Inc." + +# Direct export of C++ classes - no wrappers +__all__ = [ + # Enums + "MotorType", + "MotorVariable", + "CallbackMode", + + # Data structures + "LimitParam", + "ParamResult", + "MotorStateResult", + "CanFrame", + "CanFdFrame", + "MITParam", + + # Main C++ classes (1:1 mapping) + "Motor", + "MotorControl", + "CANSocket", # Low-level socket with file descriptor access + "CANDevice", # Base CAN device class + "MotorDeviceCan", # Motor device management + "CANDeviceCollection", # Device collection management + + # Exceptions + "CANSocketException", +] diff --git a/python/openarm/can/core.py b/python/openarm/can/core.py new file mode 100644 index 0000000..3bd57ce --- /dev/null +++ b/python/openarm/can/core.py @@ -0,0 +1,53 @@ +# 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. + +""" +Core bindings module that imports the C++ extension. +""" + +try: + from openarm_can import * +except ImportError as e: + raise ImportError( + "Failed to import openarm_can C++ extension. " + "Make sure the package is properly installed. " + f"Original error: {e}" + ) from e + +# Re-export main classes with better names if needed +__all__ = [ + # Enums + "MotorType", + "MotorVariable", + "CallbackMode", + + # Data structures + "LimitParam", + "ParamResult", + "MotorStateResult", + "CanFrame", + "CanFdFrame", + "MITParam", + + # Main classes (1:1 C++ mapping) + "Motor", + "MotorControl", + "CANSocket", + "CANDevice", + "MotorDeviceCan", + "CANDeviceCollection", + + # Exceptions + "CANSocketException", +] diff --git a/python/pyproject.toml b/python/pyproject.toml new file mode 100644 index 0000000..c1a11fd --- /dev/null +++ b/python/pyproject.toml @@ -0,0 +1,36 @@ +# 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-system] +build-backend = "mesonpy" +requires = ["meson-python"] + +[project] +authors = [{name = "Enactic, Inc."}] +license = "Apache-2.0" +# license-files = ["../LICENSE.txt"] +maintainers = [{name = "Enactic, Inc."}] +name = "openarm_can" +readme = "README.md" +requires-python = ">= 3.10" +version = "1.0.0" + +[project.urls] +# TODO: changelog = "https://github.com/reazon-research/openarm_socketcan/releases" +# TODO: documentation = "https://..." +# TODO: homepage = "https://open-arm.org/" +# TODO: issues = "https://github.com/reazon-research/openarm_socketcan/issues" +# TODO: repository = "https://github.com/reazon-research/openarm_socketcan.git" + +[tool.autopep8] diff --git a/python/requirements.txt b/python/requirements.txt new file mode 100755 index 0000000..97ca7d3 --- /dev/null +++ b/python/requirements.txt @@ -0,0 +1,13 @@ +# 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. diff --git a/python/src/openarm_can.cpp b/python/src/openarm_can.cpp new file mode 100644 index 0000000..421ab57 --- /dev/null +++ b/python/src/openarm_can.cpp @@ -0,0 +1,390 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +// Include the C++ headers +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace openarm::canbus; +using namespace openarm::damiao_motor; +using namespace openarm::can::socket; + +namespace nb = nanobind; + +NB_MODULE(openarm_can, m) { + m.doc() = "OpenArm CAN Python bindings for motor control via SocketCAN"; + + // ============================================================================ + // DAMIAO MOTOR NAMESPACE - ENUMS AND CONSTANTS + // ============================================================================ + + // Motor Type enum + nb::enum_(m, "MotorType") + .value("DM3507", MotorType::DM3507) + .value("DM4310", MotorType::DM4310) + .value("DM4310_48V", MotorType::DM4310_48V) + .value("DM4340", MotorType::DM4340) + .value("DM4340_48V", MotorType::DM4340_48V) + .value("DM6006", MotorType::DM6006) + .value("DM8006", MotorType::DM8006) + .value("DM8009", MotorType::DM8009) + .value("DM10010L", MotorType::DM10010L) + .value("DM10010", MotorType::DM10010) + .value("DMH3510", MotorType::DMH3510) + .value("DMH6215", MotorType::DMH6215) + .value("DMG6220", MotorType::DMG6220) + .value("COUNT", MotorType::COUNT) + .export_values(); + + // Motor Variable enum + nb::enum_(m, "MotorVariable") + .value("UV_Value", RID::UV_Value) + .value("KT_Value", RID::KT_Value) + .value("OT_Value", RID::OT_Value) + .value("OC_Value", RID::OC_Value) + .value("ACC", RID::ACC) + .value("DEC", RID::DEC) + .value("MAX_SPD", RID::MAX_SPD) + .value("MST_ID", RID::MST_ID) + .value("ESC_ID", RID::ESC_ID) + .value("TIMEOUT", RID::TIMEOUT) + .value("CTRL_MODE", RID::CTRL_MODE) + .value("Damp", RID::Damp) + .value("Inertia", RID::Inertia) + .value("hw_ver", RID::hw_ver) + .value("sw_ver", RID::sw_ver) + .value("SN", RID::SN) + .value("NPP", RID::NPP) + .value("Rs", RID::Rs) + .value("LS", RID::LS) + .value("Flux", RID::Flux) + .value("Gr", RID::Gr) + .value("PMAX", RID::PMAX) + .value("VMAX", RID::VMAX) + .value("TMAX", RID::TMAX) + .value("I_BW", RID::I_BW) + .value("KP_ASR", RID::KP_ASR) + .value("KI_ASR", RID::KI_ASR) + .value("KP_APR", RID::KP_APR) + .value("KI_APR", RID::KI_APR) + .value("OV_Value", RID::OV_Value) + .value("GREF", RID::GREF) + .value("Deta", RID::Deta) + .value("V_BW", RID::V_BW) + .value("IQ_c1", RID::IQ_c1) + .value("VL_c1", RID::VL_c1) + .value("can_br", RID::can_br) + .value("sub_ver", RID::sub_ver) + .value("u_off", RID::u_off) + .value("v_off", RID::v_off) + .value("k1", RID::k1) + .value("k2", RID::k2) + .value("m_off", RID::m_off) + .value("dir", RID::dir) + .value("p_m", RID::p_m) + .value("xout", RID::xout) + .value("COUNT", RID::COUNT) + .export_values(); + + // Callback Mode enum + nb::enum_(m, "CallbackMode") + .value("STATE", CallbackMode::STATE) + .value("PARAM", CallbackMode::PARAM) + .value("IGNORE", CallbackMode::IGNORE) + .export_values(); + + // ============================================================================ + // DAMIAO MOTOR NAMESPACE - STRUCTS + // ============================================================================ + + // LimitParam struct + nb::class_(m, "LimitParam") + .def(nb::init<>()) + .def_rw("pMax", &LimitParam::pMax) + .def_rw("vMax", &LimitParam::vMax) + .def_rw("tMax", &LimitParam::tMax); + + // ParamResult struct + nb::class_(m, "ParamResult") + .def(nb::init<>()) + .def_rw("rid", &ParamResult::rid) + .def_rw("value", &ParamResult::value) + .def_rw("valid", &ParamResult::valid); + + // MotorStateResult struct + nb::class_(m, "MotorStateResult") + .def(nb::init<>()) + .def_rw("position", &StateResult::position) + .def_rw("velocity", &StateResult::velocity) + .def_rw("torque", &StateResult::torque) + .def_rw("t_mos", &StateResult::t_mos) + .def_rw("t_rotor", &StateResult::t_rotor) + .def_rw("valid", &StateResult::valid); + + // CANPacket struct + nb::class_(m, "CANPacket") + .def(nb::init<>()) + .def_rw("send_can_id", &CANPacket::send_can_id) + .def_rw("data", &CANPacket::data); + + // MITParam struct + nb::class_(m, "MITParam") + .def(nb::init<>()) + .def( + "__init__", + [](MITParam* param, double kp, double kd, double q, double dq, double tau) { + new (param) MITParam(MITParam{kp, kd, q, dq, tau}); + }, + nb::arg("kp"), nb::arg("kd"), nb::arg("q"), nb::arg("dq"), nb::arg("tau")) + .def_rw("kp", &MITParam::kp) + .def_rw("kd", &MITParam::kd) + .def_rw("q", &MITParam::q) + .def_rw("dq", &MITParam::dq) + .def_rw("tau", &MITParam::tau); + // ============================================================================ + // DAMIAO MOTOR NAMESPACE - MAIN CLASSES + // ============================================================================ + + // Motor class + nb::class_(m, "Motor") + .def(nb::init(), nb::arg("motor_type"), + nb::arg("send_can_id"), nb::arg("recv_can_id")) + .def("get_position", &Motor::get_position) + .def("get_velocity", &Motor::get_velocity) + .def("get_torque", &Motor::get_torque) + .def("get_state_tmos", &Motor::get_state_tmos) + .def("get_state_trotor", &Motor::get_state_trotor) + .def("get_send_can_id", &Motor::get_send_can_id) + .def("get_recv_can_id", &Motor::get_recv_can_id) + .def("get_motor_type", &Motor::get_motor_type) + .def("is_enabled", &Motor::is_enabled) + .def("get_param", &Motor::get_param, nb::arg("rid")) + .def_static("get_limit_param", &Motor::get_limit_param, nb::arg("motor_type")); + + // MotorControl class + nb::class_(m, "CanPacketEncoder") + .def_static("create_refresh_command", &CanPacketEncoder::create_refresh_command, + nb::arg("motor")) + .def_static("create_enable_command", &CanPacketEncoder::create_enable_command, + nb::arg("motor")) + .def_static("create_disable_command", &CanPacketEncoder::create_disable_command, + nb::arg("motor")) + .def_static("create_set_zero_command", &CanPacketEncoder::create_set_zero_command, + nb::arg("motor")) + .def_static("create_mit_control_command", &CanPacketEncoder::create_mit_control_command, + nb::arg("motor"), nb::arg("mit_param")) + .def_static("create_query_param_command", &CanPacketEncoder::create_query_param_command, + nb::arg("motor"), nb::arg("rid")); + + nb::class_(m, "CanPacketDecoder") + .def_static("parse_motor_state_data", &CanPacketDecoder::parse_motor_state_data, + nb::arg("motor"), nb::arg("data")) + .def_static("parse_motor_param_data", &CanPacketDecoder::parse_motor_param_data, + nb::arg("data")); + + // ============================================================================ + // CANBUS NAMESPACE - EXCEPTIONS AND BASE CLASSES + // ============================================================================ + + // CAN Socket Exception + nb::exception(m, "CANSocketException"); + + // CANDevice base class (MUST be defined before derived classes) + nb::class_(m, "CANDevice") + .def("get_send_can_id", &CANDevice::get_send_can_id) + .def("get_recv_can_id", &CANDevice::get_recv_can_id) + .def("get_recv_can_mask", &CANDevice::get_recv_can_mask) + .def("is_fd_enabled", &CANDevice::is_fd_enabled); + + // MotorDeviceCan class (NOW can inherit from CANDevice) + nb::class_(m, "MotorDeviceCan") + .def(nb::init(), nb::arg("motor"), nb::arg("recv_can_mask"), + nb::arg("use_fd")) + .def("get_motor", &DMCANDevice::get_motor, nb::rv_policy::reference) + .def("callback", + static_cast(&DMCANDevice::callback), + nb::arg("frame")) + .def("callback", + static_cast(&DMCANDevice::callback), + nb::arg("frame")) + .def("create_can_frame", &DMCANDevice::create_can_frame, nb::arg("send_can_id"), + nb::arg("data")) + .def("create_canfd_frame", &DMCANDevice::create_canfd_frame, nb::arg("send_can_id"), + nb::arg("data")) + .def("set_callback_mode", &DMCANDevice::set_callback_mode, nb::arg("callback_mode")); + + // CANDeviceCollection class + nb::class_(m, "CANDeviceCollection") + .def(nb::init(), nb::arg("can_socket")) + .def( + "add_device", + [](CANDeviceCollection& self, std::shared_ptr device) { + self.add_device(device); + }, + nb::arg("device")) + .def( + "remove_device", + [](CANDeviceCollection& self, std::shared_ptr device) { + self.remove_device(device); + }, + nb::arg("device")) + .def("dispatch_frame_callback", + static_cast( + &CANDeviceCollection::dispatch_frame_callback), + nb::arg("frame")) + .def("dispatch_frame_callback", + static_cast( + &CANDeviceCollection::dispatch_frame_callback), + nb::arg("frame")) + .def("get_devices", &CANDeviceCollection::get_devices); + + // CAN Socket class + nb::class_(m, "CANSocket") + .def(nb::init(), nb::arg("interface"), + nb::arg("enable_fd") = false) + .def("get_socket_fd", &CANSocket::get_socket_fd) + .def("get_interface", &CANSocket::get_interface) + .def("is_canfd_enabled", &CANSocket::is_canfd_enabled) + .def("is_initialized", &CANSocket::is_initialized) + .def( + "read_raw_frame", + [](CANSocket& self, size_t buffer_size) { + std::vector buffer(buffer_size); + ssize_t bytes_read = self.read_raw_frame(buffer.data(), buffer_size); + if (bytes_read > 0) { + buffer.resize(bytes_read); + return nb::bytes(reinterpret_cast(buffer.data()), bytes_read); + } + return nb::bytes(); + }, + nb::arg("buffer_size")) + .def( + "write_raw_frame", + [](CANSocket& self, nb::bytes data) { + return self.write_raw_frame(data.data(), data.size()); + }, + nb::arg("data")) + .def("write_can_frame", &CANSocket::write_can_frame, nb::arg("frame")) + .def("read_can_frame", &CANSocket::read_can_frame, nb::arg("frame")) + .def("write_canfd_frame", &CANSocket::write_canfd_frame, nb::arg("frame")) + .def("read_canfd_frame", &CANSocket::read_canfd_frame, nb::arg("frame")); + + // ============================================================================ + // LINUX CAN FRAME STRUCTURES + // ============================================================================ + + // CAN frame structures + nb::class_(m, "CanFrame") + .def(nb::init<>()) + .def_rw("can_id", &can_frame::can_id) + .def_rw("can_dlc", &can_frame::can_dlc) + .def_prop_rw( + "data", + [](const can_frame& frame) { + return nb::bytes(reinterpret_cast(frame.data), frame.can_dlc); + }, + [](can_frame& frame, nb::bytes data) { + size_t len = std::min(data.size(), sizeof(frame.data)); + frame.can_dlc = len; + std::memcpy(frame.data, data.data(), len); + }); + + nb::class_(m, "CanFdFrame") + .def(nb::init<>()) + .def_rw("can_id", &canfd_frame::can_id) + .def_rw("len", &canfd_frame::len) + .def_rw("flags", &canfd_frame::flags) + .def_prop_rw( + "data", + [](const canfd_frame& frame) { + return nb::bytes(reinterpret_cast(frame.data), frame.len); + }, + [](canfd_frame& frame, nb::bytes data) { + size_t len = std::min(data.size(), sizeof(frame.data)); + frame.len = len; + std::memcpy(frame.data, data.data(), len); + }); + + // ============================================================================ + // TOP-LEVEL COMPONENT CLASSES + // ============================================================================ + + // DMDeviceCollection class (base class for ArmComponent and + // GripperComponent) + nb::class_(m, "DMDeviceCollection") + .def(nb::init(), nb::arg("can_socket")) + .def("enable_all", &DMDeviceCollection::enable_all) + .def("disable_all", &DMDeviceCollection::disable_all) + .def("set_zero_all", &DMDeviceCollection::set_zero_all) + .def("refresh_all", &DMDeviceCollection::refresh_all) + .def("set_callback_mode_all", &DMDeviceCollection::set_callback_mode_all, + nb::arg("callback_mode")) + .def("query_param_all", &DMDeviceCollection::query_param_all, nb::arg("rid")) + .def("mit_control_one", &DMDeviceCollection::mit_control_one, nb::arg("index"), + nb::arg("mit_param")) + .def("mit_control_all", &DMDeviceCollection::mit_control_all, nb::arg("mit_params")) + .def("get_motors", &DMDeviceCollection::get_motors) + .def("get_device_collection", &DMDeviceCollection::get_device_collection, + nb::rv_policy::reference); + + // ArmComponent class + nb::class_(m, "ArmComponent") + .def(nb::init(), nb::arg("can_socket")) + .def("init_motor_devices", &ArmComponent::init_motor_devices, nb::arg("motor_types"), + nb::arg("send_can_ids"), nb::arg("recv_can_ids"), nb::arg("use_fd")); + + // GripperComponent class + nb::class_(m, "GripperComponent") + .def(nb::init(), nb::arg("can_socket")) + .def("init_motor_device", &GripperComponent::init_motor_device, nb::arg("motor_type"), + nb::arg("send_can_id"), nb::arg("recv_can_id"), nb::arg("use_fd")) + .def("open", &GripperComponent::open, nb::arg("kp") = 50.0, nb::arg("kd") = 1.0) + .def("close", &GripperComponent::close, nb::arg("kp") = 50.0, nb::arg("kd") = 1.0) + .def("get_motor", &GripperComponent::get_motor, nb::rv_policy::reference_internal); + + // OpenArm class (main high-level interface) + nb::class_(m, "OpenArm") + .def(nb::init(), nb::arg("can_interface"), + nb::arg("enable_fd") = false) + .def("init_arm_motors", &OpenArm::init_arm_motors, nb::arg("motor_types"), + nb::arg("send_can_ids"), nb::arg("recv_can_ids")) + .def("init_gripper_motor", &OpenArm::init_gripper_motor, nb::arg("motor_type"), + nb::arg("send_can_id"), nb::arg("recv_can_id")) + .def("get_arm", &OpenArm::get_arm, nb::rv_policy::reference) + .def("get_gripper", &OpenArm::get_gripper, nb::rv_policy::reference) + .def("get_master_can_device_collection", &OpenArm::get_master_can_device_collection, + nb::rv_policy::reference) + .def("enable_all", &OpenArm::enable_all) + .def("disable_all", &OpenArm::disable_all) + .def("set_zero_all", &OpenArm::set_zero_all) + .def("refresh_all", &OpenArm::refresh_all) + .def("recv_all", &OpenArm::recv_all, nb::arg("timeout_us") = 500) + .def("set_callback_mode_all", &OpenArm::set_callback_mode_all, nb::arg("callback_mode")) + .def("query_param_all", &OpenArm::query_param_all, nb::arg("rid")); +} diff --git a/python/subprojects/.gitignore b/python/subprojects/.gitignore new file mode 100644 index 0000000..fbb105f --- /dev/null +++ b/python/subprojects/.gitignore @@ -0,0 +1,17 @@ +# 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. + +/nanobind-* +/packagecache/ +/robin-map-* diff --git a/python/subprojects/nanobind.wrap b/python/subprojects/nanobind.wrap new file mode 100644 index 0000000..2292bed --- /dev/null +++ b/python/subprojects/nanobind.wrap @@ -0,0 +1,27 @@ +# 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. + +[wrap-file] +directory = nanobind-2.7.0 +source_url = https://github.com/wjakob/nanobind/archive/refs/tags/v2.7.0.tar.gz +source_filename = nanobind-2.7.0.tar.gz +source_hash = 6c8c6bf0435b9d8da9312801686affcf34b6dbba142db60feec8d8e220830499 +patch_filename = nanobind_2.7.0-4_patch.zip +patch_url = https://wrapdb.mesonbuild.com/v2/nanobind_2.7.0-4/get_patch +patch_hash = 53e1f5ef483fb4a7cf45a0fdf1825a27d1ca45c41cc331ac3e80763560d8a2cb +source_fallback_url = https://github.com/mesonbuild/wrapdb/releases/download/nanobind_2.7.0-4/nanobind-2.7.0.tar.gz +wrapdb_version = 2.7.0-4 + +[provide] +nanobind = nanobind_dep diff --git a/python/subprojects/robin-map.wrap b/python/subprojects/robin-map.wrap new file mode 100644 index 0000000..c4dc295 --- /dev/null +++ b/python/subprojects/robin-map.wrap @@ -0,0 +1,27 @@ +# 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. + +[wrap-file] +directory = robin-map-1.4.0 +source_url = https://github.com/Tessil/robin-map/archive/refs/tags/v1.4.0.tar.gz +source_filename = robin-map-1.4.0.tar.gz +source_hash = 7930dbf9634acfc02686d87f615c0f4f33135948130b8922331c16d90a03250c +patch_filename = robin-map_1.4.0-1_patch.zip +patch_url = https://wrapdb.mesonbuild.com/v2/robin-map_1.4.0-1/get_patch +patch_hash = feb14b6752b7d439fb2f3ee968e595a9a3de00ef8cb029488af2ceb4f504b95d +source_fallback_url = https://github.com/mesonbuild/wrapdb/releases/download/robin-map_1.4.0-1/robin-map-1.4.0.tar.gz +wrapdb_version = 1.4.0-1 + +[provide] +robin-map = robin_map_dep diff --git a/setup/change_baudrate.py b/setup/change_baudrate.py new file mode 100755 index 0000000..83f5cb5 --- /dev/null +++ b/setup/change_baudrate.py @@ -0,0 +1,261 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# +# 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. + +""" +DM Motor ID and Baudrate Writer script for writing baudrate and ID parameters to DM motors +Work with python-can library +Original author: @necobit (Co-Authored-By: Claude) +""" + +import argparse +import sys +import can +import time +from typing import Optional + + +class DMMotorWriter: + """DM Motor parameter writer""" + + # Supported baudrates and their codes + BAUDRATE_MAP = { + 125000: 0, # 125K + 200000: 1, # 200K + 250000: 2, # 250K + 500000: 3, # 500K + 1000000: 4, # 1M + 2000000: 5, # 2M + 2500000: 6, # 2.5M + 3200000: 7, # 3.2M + 4000000: 8, # 4M + 5000000: 9 # 5M + } + + def __init__(self, socketcan_port: str = "can0"): + self.socketcan_port = socketcan_port + self.can_bus: Optional[can.BusABC] = None + + def connect(self) -> bool: + """Connect to CAN bus""" + try: + print(f"Connecting to CAN interface: {self.socketcan_port}") + self.can_bus = can.interface.Bus( + channel=self.socketcan_port, + interface='socketcan' + ) + print(f"āœ“ Connected to {self.socketcan_port}") + return True + except Exception as e: + print(f"āœ— Connection failed: {e}") + return False + + def disconnect(self): + """Disconnect from CAN bus""" + if self.can_bus: + self.can_bus.shutdown() + self.can_bus = None + print("Disconnected from CAN bus") + + def validate_baudrate(self, baudrate: int) -> bool: + """Validate if baudrate is supported""" + return baudrate in self.BAUDRATE_MAP + + def write_baudrate(self, motor_id: int, baudrate: int) -> bool: + """Write baudrate to motor""" + if not self.can_bus: + print("āœ— Not connected to CAN bus") + return False + + if not self.validate_baudrate(baudrate): + print(f"āœ— Unsupported baudrate: {baudrate}") + print(f"Supported baudrates: {list(self.BAUDRATE_MAP.keys())}") + return False + + try: + baudrate_code = self.BAUDRATE_MAP[baudrate] + + print( + f"Writing baudrate {baudrate} (code: {baudrate_code}) to motor ID {motor_id}") + + # Set Baudrate (can_br) - RID 35 (0x23) + baudrate_data = [ + motor_id & 0xFF, + (motor_id >> 8) & 0xFF, + 0x55, + 0x23, + baudrate_code, + 0x00, + 0x00, + 0x00 + ] + + baudrate_msg = can.Message( + arbitration_id=0x7FF, + data=baudrate_data, + is_extended_id=False + ) + + self.can_bus.send(baudrate_msg) + time.sleep(0.1) # Wait for processing + + print(f"āœ“ Baudrate {baudrate} written to motor ID {motor_id}") + return True + + except Exception as e: + print(f"āœ— Failed to write baudrate: {e}") + return False + + def save_to_flash(self, motor_id: int) -> bool: + """Save parameters to flash memory""" + if not self.can_bus: + print("āœ— Not connected to CAN bus") + return False + + try: + print(f"Saving parameters to flash for motor ID {motor_id}") + print("āš ļø Motor has a hard limit of writing to flash of 10000 times") + print("āš ļø Motor will be disabled during save operation") + + # Step 1: Disable motor first (required for save operation) + disable_data = [0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFD] + disable_msg = can.Message( + arbitration_id=motor_id, + data=disable_data, + is_extended_id=False + ) + + self.can_bus.send(disable_msg) + time.sleep(0.5) # Wait for disable to take effect + + # Step 2: Send save command + save_data = [ + motor_id & 0xFF, + (motor_id >> 8) & 0xFF, + 0xAA, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00 + ] + + save_msg = can.Message( + arbitration_id=0x7FF, + data=save_data, + is_extended_id=False + ) + + self.can_bus.send(save_msg) + time.sleep(0.5) # Wait for save operation (up to 30ms) + + print(f"āœ“ Parameters saved to flash for motor ID {motor_id}") + print("āš ļø Motor is now in DISABLE mode") + return True + + except Exception as e: + print(f"āœ— Failed to save to flash: {e}") + return False + + +def main(): + parser = argparse.ArgumentParser( + description="DM Motor ID and Baudrate Writer", + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +Examples: + # Write baudrate 1000000 to motor ID 2 on can0 + python change_baudrate.py --baudrate 1000000 --canid 2 --socketcan can0 + + # Write baudrate and save to flash + python change_baudrate.py --baudrate 500000 --canid 1 --socketcan can0 --flash + + # Write to motor on different CAN port + python change_baudrate.py --baudrate 2000000 --canid 3 --socketcan can1 + +Supported baudrates: + 125000, 200000, 250000, 500000, 1000000, 2000000, 2500000, 3200000, 4000000, 5000000 + """ + ) + + parser.add_argument( + '-b', '--baudrate', + type=int, + required=True, + help='Baudrate to write (125000, 200000, 250000, 500000, 1000000, 2000000, 2500000, 3200000, 4000000, 5000000)' + ) + + parser.add_argument( + '-c', '--canid', + type=int, + required=True, + help='Motor CAN ID (slave ID) to write to (0-255)' + ) + + parser.add_argument( + '-s', '--socketcan', + type=str, + default='can0', + help='SocketCAN port (default: can0)' + ) + + parser.add_argument( + '-f', '--flash', + action='store_true', + help='Save parameters to flash memory after writing' + ) + + args = parser.parse_args() + + # Validate inputs + if args.canid < 0 or args.canid > 255: + print("āœ— Error: CAN ID must be between 0 and 255") + sys.exit(1) + + # Create writer instance + writer = DMMotorWriter(args.socketcan) + + try: + # Connect to CAN bus + if not writer.connect(): + print("āœ— Failed to connect to CAN bus") + sys.exit(1) + + # Write baudrate + if not writer.write_baudrate(args.canid, args.baudrate): + print("āœ— Failed to write baudrate") + sys.exit(1) + + # Save to flash if requested + if args.flash: + if not writer.save_to_flash(args.canid): + print("āœ— Failed to save to flash") + sys.exit(1) + + print("āœ“ Operation completed successfully") + + except KeyboardInterrupt: + print("\nāš ļø Operation cancelled by user") + sys.exit(1) + except Exception as e: + print(f"āœ— Unexpected error: {e}") + sys.exit(1) + finally: + writer.disconnect() + + +if __name__ == "__main__": + main() diff --git a/setup/configure_socketcan.sh b/setup/configure_socketcan.sh new file mode 100755 index 0000000..ab22770 --- /dev/null +++ b/setup/configure_socketcan.sh @@ -0,0 +1,114 @@ +#!/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. + +# Simple CAN Interface Setup Script +# Usage: script/configure_socketcan.sh [options] + +# Default values +BITRATE=1000000 +DBITRATE=5000000 +FD_MODE=false + +# Show usage +usage() { + echo "Usage: $0 [options]" + echo "" + echo "Options:" + echo " -fd Enable CAN FD mode (default: CAN 2.0)" + echo " -b Set bitrate (default: 1000000)" + echo " -d Set CAN FD data bitrate (default: 5000000)" + echo " -h Show help" + echo "" + echo "Examples:" + echo " $0 can0 # CAN 2.0 at 1Mbps" + echo " $0 can0 -fd # CAN FD at 1Mbps/5Mbps" + echo " $0 can0 -b 500000 # CAN 2.0 at 500kbps" + echo " $0 can0 -fd -d 8000000 # CAN FD with 8Mbps data rate" +} + +# Check if interface provided +if [ -z "$1" ]; then + echo "Error: Specify CAN interface (e.g., can0)" + usage + exit 1 +fi + +CAN_IF=$1 +shift + +# Parse options +while [[ $# -gt 0 ]]; do + case $1 in + -fd) + FD_MODE=true + shift + ;; + -b) + BITRATE="$2" + shift 2 + ;; + -d) + DBITRATE="$2" + shift 2 + ;; + -h) + usage + exit 0 + ;; + *) + echo "Error: Unknown option '$1'" + usage + exit 1 + ;; + esac +done + +# Check if interface exists +if ! ip link show $CAN_IF &>/dev/null; then + echo "Error: CAN interface '$CAN_IF' not found" + echo "Available interfaces:" + ip link show | grep -E "can[0-9]" | cut -d: -f2 | tr -d ' ' || echo " No CAN interfaces found" + exit 1 +fi + +# Configure CAN interface +echo "Configuring $CAN_IF..." + +if ! sudo ip link set $CAN_IF down; then + echo "Error: Failed to bring down $CAN_IF" + exit 1 +fi + +if [ "$FD_MODE" = true ]; then + if ! sudo ip link set $CAN_IF type can bitrate $BITRATE dbitrate $DBITRATE fd on; then + echo "Error: Failed to configure CAN FD mode" + exit 1 + fi + echo "$CAN_IF is now set to CAN FD mode (${BITRATE} bps / ${DBITRATE} bps)" +else + if ! sudo ip link set $CAN_IF type can bitrate $BITRATE; then + echo "Error: Failed to configure CAN 2.0 mode" + exit 1 + fi + echo "$CAN_IF is now set to CAN 2.0 mode (${BITRATE} bps)" +fi + +if ! sudo ip link set $CAN_IF up; then + echo "Error: Failed to bring up $CAN_IF" + exit 1 +fi + +echo "āœ“ $CAN_IF is active" diff --git a/setup/motor_check.cpp b/setup/motor_check.cpp new file mode 100644 index 0000000..2dc86d6 --- /dev/null +++ b/setup/motor_check.cpp @@ -0,0 +1,186 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +namespace { +void print_usage(const char* program_name) { + std::cout << "Usage: " << program_name << " [can_interface] [-fd]" + << std::endl; + std::cout << " send_can_id: The send CAN ID of the motor (e.g., 1, 2, 7)" << std::endl; + std::cout << " recv_can_id: The receive CAN ID of the motor (e.g., 17, 18, 23)" << std::endl; + std::cout << " can_interface: CAN interface name (default: can0)" << std::endl; + std::cout << " -fd: Enable CAN-FD (default: disabled)" << std::endl; + std::cout << std::endl; + std::cout << "Example: " << program_name << " 1 17" << std::endl; + std::cout << "Example: " << program_name << " 1 17 can1" << std::endl; + std::cout << "Example: " << program_name << " 1 17 can1 -fd" << std::endl; +} + +void print_motor_status(const openarm::damiao_motor::Motor& motor) { + std::cout << "Motor ID: " << motor.get_send_can_id() << std::endl; + std::cout << " Position: " << motor.get_position() << " rad" << std::endl; + std::cout << " Velocity: " << motor.get_velocity() << " rad/s" << std::endl; + std::cout << " Torque: " << motor.get_torque() << " Nm" << std::endl; + std::cout << " Temperature (MOS): " << motor.get_state_tmos() << " Ā°C" << std::endl; + std::cout << " Temperature (Rotor): " << motor.get_state_trotor() << " Ā°C" << std::endl; +} +} // namespace + +int main(int argc, char* argv[]) { + if (argc < 3 || argc > 5) { + print_usage(argv[0]); + return 1; + } + + // Parse send and receive CAN IDs from command line + uint32_t send_can_id, recv_can_id; + try { + send_can_id = std::stoul(argv[1]); + recv_can_id = std::stoul(argv[2]); + } catch (const std::exception& e) { + std::cerr << "Error: Invalid CAN ID format" << std::endl; + print_usage(argv[0]); + return 1; + } + + // Parse optional CAN interface and FD flag + std::string can_interface = "can0"; // default + bool use_fd = false; // default: disabled + + if (argc >= 4) { + std::string arg4 = argv[3]; + if (arg4 == "-fd") { + use_fd = true; + } else { + can_interface = arg4; + } + } + + if (argc >= 5) { + std::string arg5 = argv[4]; + if (arg5 == "-fd") { + use_fd = true; + } else { + std::cerr << "Error: Unknown argument '" << arg5 << "'. Use -fd to enable CAN-FD" + << std::endl; + print_usage(argv[0]); + return 1; + } + } + + try { + std::cout << "=== OpenArm Motor Control Script ===" << std::endl; + std::cout << "Send CAN ID: " << send_can_id << std::endl; + std::cout << "Receive CAN ID: " << recv_can_id << std::endl; + std::cout << "CAN Interface: " << can_interface << std::endl; + std::cout << "CAN-FD Enabled: " << (use_fd ? "Yes" : "No") << std::endl; + std::cout << std::endl; + + // Initialize OpenArm with CAN interface + std::cout << "Initializing OpenArm CAN..." << std::endl; + openarm::can::socket::OpenArm openarm(can_interface, + use_fd); // Use specified interface and FD setting + + // Initialize single motor + std::cout << "Initializing motor..." << std::endl; + openarm.init_arm_motors({openarm::damiao_motor::MotorType::DM4310}, {send_can_id}, + {recv_can_id}); + + // Set callback mode to param for initial parameter reading + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::PARAM); + + // Query motor parameters (Master ID and Baudrate) + std::cout << "Reading motor parameters..." << std::endl; + openarm.query_param_all(static_cast(openarm::damiao_motor::RID::MST_ID)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm.recv_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + openarm.query_param_all(static_cast(openarm::damiao_motor::RID::can_br)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm.recv_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + // Get motor and verify parameters + const auto& motors = openarm.get_arm().get_motors(); + if (!motors.empty()) { + const auto& motor = motors[0]; + double queried_mst_id = + motor.get_param(static_cast(openarm::damiao_motor::RID::MST_ID)); + double queried_baudrate = + motor.get_param(static_cast(openarm::damiao_motor::RID::can_br)); + + std::cout << "\n=== Motor Parameters ===" << std::endl; + std::cout << "Send CAN ID: " << motor.get_send_can_id() << std::endl; + std::cout << "Queried Master ID: " << queried_mst_id << std::endl; + std::cout << "Queried Baudrate (1-9): " << queried_baudrate << std::endl; + + // Verify recv_can_id matches queried master ID + if (static_cast(queried_mst_id) != recv_can_id) { + std::cerr << "Error: Queried Master ID (" << queried_mst_id + << ") does not match provided recv_can_id (" << recv_can_id << ")" + << std::endl; + return 1; + } + std::cout << "āœ“ Master ID verification passed" << std::endl; + } + + // Switch to state callback mode for motor status updates + openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); + + // Enable the motor + std::cout << "\n=== Enabling Motor ===" << std::endl; + openarm.enable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm.recv_all(); + + // Refresh 10 times at 10Hz (100ms intervals) + std::cout << "\n=== Refreshing Motor Status (10Hz for 1 second) ===" << std::endl; + for (int i = 1; i <= 10; i++) { + openarm.refresh_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm.recv_all(); + + for (const auto& motor : openarm.get_arm().get_motors()) { + std::cout << "\n--- Refresh " << i << "/10 ---" << std::endl; + print_motor_status(motor); + } + } + + // Disable the motor + std::cout << "\n=== Disabling Motor ===" << std::endl; + openarm.disable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm.recv_all(); + + // Print final status + if (!motors.empty()) { + std::cout << "\n=== Final Motor Status ===" << std::endl; + print_motor_status(motors[0]); + } + + std::cout << "\n=== Script Completed Successfully ===" << std::endl; + + } catch (const std::exception& e) { + std::cerr << "Error: " << e.what() << std::endl; + return -1; + } + + return 0; +} diff --git a/setup/set_zero.sh b/setup/set_zero.sh new file mode 100755 index 0000000..f35f5df --- /dev/null +++ b/setup/set_zero.sh @@ -0,0 +1,160 @@ +#!/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. + + +# CAN Interface Script +# Usage: scripts/set_zero.sh [CAN_ID] [-all] + +# Function to display usage +usage() { + echo "Usage: $0 [CAN_ID] [-all]" + echo " CAN_IF: CAN interface name (e.g., can0)" + echo " CAN_ID: CAN ID in hex format (e.g., 00x) - not needed with -all" + echo " -all: Send to all IDs from 001 to 008" + echo "" + echo "Examples:" + echo " $0 can0 001" + echo " $0 can0 -all" + exit 1 +} + +# Function to check if CAN interface is up and get baudrate +check_can_interface() { + local interface=$1 + + # Check if interface exists and is up + if ! ip link show "$interface" &>/dev/null; then + echo "Error: CAN interface $interface does not exist" + return 1 + fi + + # Check if interface is up + local state=$(ip link show "$interface" | grep -o "state [A-Z]*" | cut -d' ' -f2) + if [ "$state" != "UP" ]; then + echo "Error: CAN interface $interface is not UP (current state: $state)" + return 1 + fi + + echo "CAN interface $interface is UP" + + # Try to get baudrate information + if command -v ethtool &>/dev/null; then + local baudrate=$(ethtool "$interface" 2>/dev/null | grep -i speed | cut -d: -f2 | tr -d ' ') + if [ -n "$baudrate" ]; then + echo "Baudrate: $baudrate" + fi + fi + + # Alternative method using ip command + local bitrate=$(ip -details link show "$interface" 2>/dev/null | grep -o "bitrate [0-9]*" | cut -d' ' -f2) + if [ -n "$bitrate" ]; then + echo "Bitrate: ${bitrate} bps" + fi + + return 0 +} + +# Function to send CAN messages for a single ID +send_can_messages() { + local CAN_ID=$1 + local CAN_IF=$2 + + echo "Sending CAN messages for ID: $CAN_ID on interface: $CAN_IF" + + # Send first disablemessage + echo "Sending: cansend $CAN_IF ${CAN_ID}#FFFFFFFFFFFFFFFD" + cansend "$CAN_IF" "${CAN_ID}#FFFFFFFFFFFFFFFD" + + sleep 0.1 + + # Send second set zero message + echo "Sending: cansend $CAN_IF ${CAN_ID}#FFFFFFFFFFFFFFFE" + cansend "$CAN_IF" "${CAN_ID}#FFFFFFFFFFFFFFFE" + + sleep 0.1 + + # Send third disable message + echo "Sending: cansend $CAN_IF ${CAN_ID}#FFFFFFFFFFFFFFFD" + cansend "$CAN_IF" "${CAN_ID}#FFFFFFFFFFFFFFFD" + + sleep 0.1 + + echo "Messages sent for ID: $CAN_ID" + echo "" +} + +# Main script logic +main() { + # Check for minimum arguments + if [ $# -lt 1 ]; then + usage + fi + + local CAN_IF=$1 + local CAN_ID="" + local all_flag=false + + # Check for -all flag + if [ "$2" = "-all" ]; then + all_flag=true + else + CAN_ID=$2 + if [ "$3" = "-all" ]; then + all_flag=true + fi + fi + + # Validate CAN_IF + if [ -z "$CAN_IF" ]; then + usage + fi + + # Validate CAN_ID only if -all flag is not set + if [ "$all_flag" = false ] && [ -z "$CAN_ID" ]; then + echo "Error: CAN_ID is required when -all flag is not used" + usage + fi + + # Check if cansend command is available + if ! command -v cansend &>/dev/null; then + echo "Error: cansend command not found. Please install can-utils package." + exit 1 + fi + + # Check CAN interface status + if ! check_can_interface "$CAN_IF"; then + exit 1 + fi + + echo "" + + # Execute based on flags + if [ "$all_flag" = true ]; then + echo "Sending set zero messages to all motor with CAN IDs from 001 to 008" + echo "==========================================" + for i in {1..8}; do + local padded_id=$(printf "%03d" $i) + send_can_messages "$padded_id" "$CAN_IF" + done + else + send_can_messages "$CAN_ID" "$CAN_IF" + fi + + echo "Set zero completed." +} + +# Run main function with all arguments +main "$@" \ No newline at end of file diff --git a/src/openarm/can/socket/arm_component.cpp b/src/openarm/can/socket/arm_component.cpp new file mode 100644 index 0000000..1a2471b --- /dev/null +++ b/src/openarm/can/socket/arm_component.cpp @@ -0,0 +1,43 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +namespace openarm::can::socket { + +ArmComponent::ArmComponent(canbus::CANSocket& can_socket) + : damiao_motor::DMDeviceCollection(can_socket) {} + +void ArmComponent::init_motor_devices(std::vector motor_types, + std::vector send_can_ids, + std::vector recv_can_ids, bool use_fd) { + // Reserve space to prevent vector reallocation that would invalidate motor + // references + motors_.reserve(motor_types.size()); + + for (size_t i = 0; i < motor_types.size(); i++) { + // First, create and store the motor in the vector + motors_.emplace_back(motor_types[i], send_can_ids[i], recv_can_ids[i]); + // Then create the device with a reference to the stored motor + auto motor_device = + std::make_shared(motors_.back(), CAN_SFF_MASK, use_fd); + get_device_collection().add_device(motor_device); + } +} + +} // namespace openarm::can::socket diff --git a/src/openarm/can/socket/gripper_component.cpp b/src/openarm/can/socket/gripper_component.cpp new file mode 100644 index 0000000..076e4e9 --- /dev/null +++ b/src/openarm/can/socket/gripper_component.cpp @@ -0,0 +1,48 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +namespace openarm::can::socket { + +GripperComponent::GripperComponent(canbus::CANSocket& can_socket) + : DMDeviceCollection(can_socket) {} + +void GripperComponent::init_motor_device(damiao_motor::MotorType motor_type, uint32_t send_can_id, + uint32_t recv_can_id, bool use_fd) { + // Create the motor + motor_ = std::make_unique(motor_type, send_can_id, recv_can_id); + // Create the device with a reference to the motor + motor_device_ = std::make_shared(*motor_, CAN_SFF_MASK, use_fd); + get_device_collection().add_device(motor_device_); +} + +void GripperComponent::open(double kp, double kd) { set_position(gripper_open_position_, kp, kd); } + +void GripperComponent::close(double kp, double kd) { + set_position(gripper_closed_position_, kp, kd); +} + +void GripperComponent::set_position(double gripper_position, double kp, double kd) { + if (!motor_device_) return; + + // MIT control to desired position (zero velocity and torque) + mit_control_one( + 0, damiao_motor::MITParam{kp, kd, gripper_to_motor_position(gripper_position), 0.0, 0.0}); +} +} // namespace openarm::can::socket diff --git a/src/openarm/can/socket/openarm.cpp b/src/openarm/can/socket/openarm.cpp new file mode 100644 index 0000000..ac3cb5b --- /dev/null +++ b/src/openarm/can/socket/openarm.cpp @@ -0,0 +1,110 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +namespace openarm::can::socket { + +OpenArm::OpenArm(const std::string& can_interface, bool enable_fd) + : can_interface_(can_interface), enable_fd_(enable_fd) { + can_socket_ = std::make_unique(can_interface_, enable_fd_); + master_can_device_collection_ = std::make_unique(*can_socket_); + arm_ = std::make_unique(*can_socket_); + gripper_ = std::make_unique(*can_socket_); +} + +void OpenArm::init_arm_motors(const std::vector& motor_types, + const std::vector& send_can_ids, + const std::vector& recv_can_ids) { + arm_->init_motor_devices(motor_types, send_can_ids, recv_can_ids, enable_fd_); + register_dm_device_collection(*arm_); +} + +void OpenArm::init_gripper_motor(damiao_motor::MotorType motor_type, uint32_t send_can_id, + uint32_t recv_can_id) { + gripper_->init_motor_device(motor_type, send_can_id, recv_can_id, enable_fd_); + register_dm_device_collection(*gripper_); +} + +void OpenArm::register_dm_device_collection(damiao_motor::DMDeviceCollection& device_collection) { + for (const auto& [id, device] : device_collection.get_device_collection().get_devices()) { + master_can_device_collection_->add_device(device); + } + sub_dm_device_collections_.push_back(&device_collection); +} + +void OpenArm::enable_all() { + for (damiao_motor::DMDeviceCollection* device_collection : sub_dm_device_collections_) { + device_collection->enable_all(); + } +} + +void OpenArm::set_zero_all() { + for (damiao_motor::DMDeviceCollection* device_collection : sub_dm_device_collections_) { + device_collection->set_zero_all(); + } +} + +void OpenArm::refresh_all() { + for (damiao_motor::DMDeviceCollection* device_collection : sub_dm_device_collections_) { + device_collection->refresh_all(); + } +} + +void OpenArm::disable_all() { + for (damiao_motor::DMDeviceCollection* device_collection : sub_dm_device_collections_) { + device_collection->disable_all(); + } +} + +void OpenArm::recv_all(int timeout_us) { + // The timeout for select() is set to timeout_us (default: 500 us). + // Tuning this value may improve the performance but should be done with caution. + + // CAN FD + if (enable_fd_) { + canfd_frame response_frame; + while (can_socket_->is_data_available(timeout_us) && + can_socket_->read_canfd_frame(response_frame)) { + master_can_device_collection_->dispatch_frame_callback(response_frame); + } + } + // CAN 2.0 + else { + can_frame response_frame; + while (can_socket_->is_data_available(timeout_us) && + can_socket_->read_can_frame(response_frame)) { + master_can_device_collection_->dispatch_frame_callback(response_frame); + } + } + // } +} + +void OpenArm::query_param_all(int RID) { + for (damiao_motor::DMDeviceCollection* device_collection : sub_dm_device_collections_) { + device_collection->query_param_all(RID); + } +} + +void OpenArm::set_callback_mode_all(damiao_motor::CallbackMode callback_mode) { + for (damiao_motor::DMDeviceCollection* device_collection : sub_dm_device_collections_) { + device_collection->set_callback_mode_all(callback_mode); + } +} + +} // namespace openarm::can::socket diff --git a/src/openarm/canbus/can_device_collection.cpp b/src/openarm/canbus/can_device_collection.cpp new file mode 100644 index 0000000..f4b0ab6 --- /dev/null +++ b/src/openarm/canbus/can_device_collection.cpp @@ -0,0 +1,62 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include + +namespace openarm::canbus { + +CANDeviceCollection::CANDeviceCollection(CANSocket& can_socket) : can_socket_(can_socket) {} + +CANDeviceCollection::~CANDeviceCollection() {} + +void CANDeviceCollection::add_device(const std::shared_ptr& device) { + if (!device) return; + + // Add device to our collection + canid_t device_id = device->get_recv_can_id(); + devices_[device_id] = device; +} + +void CANDeviceCollection::remove_device(const std::shared_ptr& device) { + if (!device) return; + + canid_t device_id = device->get_recv_can_id(); + auto it = devices_.find(device_id); + if (it != devices_.end()) { + // Remove from our collection + devices_.erase(it); + } +} + +void CANDeviceCollection::dispatch_frame_callback(can_frame& frame) { + auto it = devices_.find(frame.can_id); + if (it != devices_.end()) { + it->second->callback(frame); + } + // Note: Silently ignore frames for unknown devices (this is normal in CAN + // networks) +} + +void CANDeviceCollection::dispatch_frame_callback(canfd_frame& frame) { + auto it = devices_.find(frame.can_id); + if (it != devices_.end()) { + it->second->callback(frame); + } + // Note: Silently ignore frames for unknown devices (this is normal in CAN + // networks) +} + +} // namespace openarm::canbus diff --git a/src/openarm/canbus/can_socket.cpp b/src/openarm/canbus/can_socket.cpp new file mode 100644 index 0000000..1cb27c3 --- /dev/null +++ b/src/openarm/canbus/can_socket.cpp @@ -0,0 +1,140 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +namespace openarm::canbus { + +CANSocket::CANSocket(const std::string& interface, bool enable_fd) + : socket_fd_(-1), interface_(interface), fd_enabled_(enable_fd), initialized_(false) { + if (!initialize_socket(interface)) { + throw CANSocketException("Failed to initialize socket for interface: " + interface); + } + initialized_ = true; +} + +CANSocket::~CANSocket() { cleanup(); } + +bool CANSocket::initialize_socket(const std::string& interface) { + // Create socket + socket_fd_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (socket_fd_ < 0) { + return false; + } + + struct ifreq ifr; + struct sockaddr_can addr; + + strncpy(ifr.ifr_name, interface.c_str(), IFNAMSIZ - 1); + ifr.ifr_name[IFNAMSIZ - 1] = '\0'; + + if (ioctl(socket_fd_, SIOCGIFINDEX, &ifr) < 0) { + close(socket_fd_); + return false; + } + + memset(&addr, 0, sizeof(addr)); + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + if (fd_enabled_) { + int enable_canfd = 1; + if (setsockopt(socket_fd_, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd, + sizeof(enable_canfd)) < 0) { + close(socket_fd_); + return false; + } + } + + if (bind(socket_fd_, reinterpret_cast(&addr), sizeof(addr)) < 0) { + close(socket_fd_); + return false; + } + + struct timeval timeout; + timeout.tv_sec = 0; + timeout.tv_usec = 100; + if (setsockopt(socket_fd_, SOL_SOCKET, SO_RCVTIMEO, &timeout, sizeof(timeout)) < 0) { + close(socket_fd_); + return false; + } + + return true; +} + +void CANSocket::cleanup() { + if (socket_fd_ > 0) { + close(socket_fd_); + socket_fd_ = -1; + } +} + +ssize_t CANSocket::read_raw_frame(void* buffer, size_t buffer_size) { + if (!initialized_) return -1; + return read(socket_fd_, buffer, buffer_size); +} + +ssize_t CANSocket::write_raw_frame(const void* buffer, size_t frame_size) { + if (!initialized_) return -1; + return write(socket_fd_, buffer, frame_size); +} + +bool CANSocket::write_can_frame(const can_frame& frame) { + return write(socket_fd_, &frame, sizeof(frame)) == sizeof(frame); +} + +bool CANSocket::write_canfd_frame(const canfd_frame& frame) { + return write(socket_fd_, &frame, sizeof(frame)) == sizeof(frame); +} + +bool CANSocket::read_can_frame(can_frame& frame) { + if (!initialized_) return false; + ssize_t bytes_read = read(socket_fd_, &frame, sizeof(frame)); + return bytes_read == sizeof(frame); +} + +bool CANSocket::read_canfd_frame(canfd_frame& frame) { + if (!initialized_) return false; + ssize_t bytes_read = read(socket_fd_, &frame, sizeof(frame)); + return bytes_read == sizeof(frame); +} + +bool CANSocket::is_data_available(int timeout_us) { + if (!initialized_) return false; + + fd_set read_fds; + struct timeval timeout; + + FD_ZERO(&read_fds); + FD_SET(socket_fd_, &read_fds); + + timeout.tv_sec = timeout_us / 1000000; + timeout.tv_usec = (timeout_us % 1000000); + + int result = select(socket_fd_ + 1, &read_fds, nullptr, nullptr, &timeout); + + return (result > 0 && FD_ISSET(socket_fd_, &read_fds)); +} + +} // namespace openarm::canbus diff --git a/src/openarm/damiao_motor/dm_motor.cpp b/src/openarm/damiao_motor/dm_motor.cpp new file mode 100644 index 0000000..17bcb29 --- /dev/null +++ b/src/openarm/damiao_motor/dm_motor.cpp @@ -0,0 +1,70 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +namespace openarm::damiao_motor { + +// Constructor +Motor::Motor(MotorType motor_type, uint32_t send_can_id, uint32_t recv_can_id) + : send_can_id_(send_can_id), + recv_can_id_(recv_can_id), + motor_type_(motor_type), + enabled_(false), + state_q_(0.0), + state_dq_(0.0), + state_tau_(0.0), + state_tmos_(0), + state_trotor_(0) {} + +// Enable methods +void Motor::set_enabled(bool enable) { this->enabled_ = enable; } + +// Parameter methods +// TODO: storing temp params in motor object might not be a good idea +// also -1 is not a good default value, consider using a different value +double Motor::get_param(int RID) const { + auto it = temp_param_dict_.find(RID); + return (it != temp_param_dict_.end()) ? it->second : -1; +} + +void Motor::set_temp_param(int RID, int val) { temp_param_dict_[RID] = val; } + +// State update methods +void Motor::update_state(double q, double dq, double tau, int tmos, int trotor) { + state_q_ = q; + state_dq_ = dq; + state_tau_ = tau; + state_tmos_ = tmos; + state_trotor_ = trotor; +} + +void Motor::set_state_tmos(int tmos) { state_tmos_ = tmos; } + +void Motor::set_state_trotor(int trotor) { state_trotor_ = trotor; } + +// Static methods +LimitParam Motor::get_limit_param(MotorType motor_type) { + size_t index = static_cast(motor_type); + if (index >= MOTOR_LIMIT_PARAMS.size()) { + throw std::invalid_argument("Invalid motor type: " + + std::to_string(static_cast(motor_type))); + } + return MOTOR_LIMIT_PARAMS[index]; +} + +} // namespace openarm::damiao_motor diff --git a/src/openarm/damiao_motor/dm_motor_control.cpp b/src/openarm/damiao_motor/dm_motor_control.cpp new file mode 100644 index 0000000..948724a --- /dev/null +++ b/src/openarm/damiao_motor/dm_motor_control.cpp @@ -0,0 +1,177 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace openarm::damiao_motor { + +// Command creation methods (return data array, can_id handled externally) +CANPacket CanPacketEncoder::create_enable_command(const Motor& motor) { + return {motor.get_send_can_id(), pack_command_data(0xFC)}; +} + +CANPacket CanPacketEncoder::create_disable_command(const Motor& motor) { + return {motor.get_send_can_id(), pack_command_data(0xFD)}; +} + +CANPacket CanPacketEncoder::create_set_zero_command(const Motor& motor) { + return {motor.get_send_can_id(), pack_command_data(0xFE)}; +} + +CANPacket CanPacketEncoder::create_mit_control_command(const Motor& motor, + const MITParam& mit_param) { + return {motor.get_send_can_id(), pack_mit_control_data(motor.get_motor_type(), mit_param)}; +} + +CANPacket CanPacketEncoder::create_query_param_command(const Motor& motor, int RID) { + return {0x7FF, pack_query_param_data(motor.get_send_can_id(), RID)}; +} + +CANPacket CanPacketEncoder::create_refresh_command(const Motor& motor) { + uint8_t send_can_id = motor.get_send_can_id(); + std::vector data = {static_cast(send_can_id & 0xFF), + static_cast((send_can_id >> 8) & 0xFF), + 0xCC, + 0x00, + 0x00, + 0x00, + 0x00, + 0x00}; + return {0x7FF, data}; +} + +// Data interpretation methods (use recv_can_id for received data) +StateResult CanPacketDecoder::parse_motor_state_data(const Motor& motor, + const std::vector& data) { + if (data.size() < 8) { + std::cerr << "Warning: Skipping motor state data less than 8 bytes" << std::endl; + return {0, 0, 0, 0, 0, false}; + } + + // Parse state data + uint16_t q_uint = (static_cast(data[1]) << 8) | data[2]; + uint16_t dq_uint = + (static_cast(data[3]) << 4) | (static_cast(data[4]) >> 4); + uint16_t tau_uint = (static_cast(data[4] & 0xf) << 8) | data[5]; + int t_mos = static_cast(data[6]); + int t_rotor = static_cast(data[7]); + + // Convert to physical values + LimitParam limits = MOTOR_LIMIT_PARAMS[static_cast(motor.get_motor_type())]; + double recv_q = CanPacketDecoder::uint_to_double(q_uint, -limits.pMax, limits.pMax, 16); + double recv_dq = CanPacketDecoder::uint_to_double(dq_uint, -limits.vMax, limits.vMax, 12); + double recv_tau = CanPacketDecoder::uint_to_double(tau_uint, -limits.tMax, limits.tMax, 12); + + return {recv_q, recv_dq, recv_tau, t_mos, t_rotor, true}; +} + +ParamResult CanPacketDecoder::parse_motor_param_data(const std::vector& data) { + if (data.size() < 8) return {0, NAN, false}; + + if ((data[2] == 0x33 || data[2] == 0x55)) { + uint8_t RID = data[3]; + double num; + if (CanPacketDecoder::is_in_ranges(RID)) { + num = CanPacketDecoder::uint8s_to_uint32(data[4], data[5], data[6], data[7]); + } else { + std::array float_bytes = {data[4], data[5], data[6], data[7]}; + num = CanPacketDecoder::uint8s_to_float(float_bytes); + } + return {RID, num, true}; + } else { + std::cerr << "WARNING: INVALID PARAM DATA" << std::endl; + return {0, NAN, false}; + } +} + +// Data packing utility methods +std::vector CanPacketEncoder::pack_mit_control_data(MotorType motor_type, + const MITParam& mit_param) { + uint16_t kp_uint = double_to_uint(mit_param.kp, 0, 500, 12); + uint16_t kd_uint = double_to_uint(mit_param.kd, 0, 5, 12); + + // Get motor limits based on type + LimitParam limits = MOTOR_LIMIT_PARAMS[static_cast(motor_type)]; + uint16_t q_uint = double_to_uint(mit_param.q, -(double)limits.pMax, (double)limits.pMax, 16); + uint16_t dq_uint = double_to_uint(mit_param.dq, -(double)limits.vMax, (double)limits.vMax, 12); + uint16_t tau_uint = + double_to_uint(mit_param.tau, -(double)limits.tMax, (double)limits.tMax, 12); + + return {static_cast((q_uint >> 8) & 0xFF), + static_cast(q_uint & 0xFF), + static_cast(dq_uint >> 4), + static_cast(((dq_uint & 0xF) << 4) | ((kp_uint >> 8) & 0xF)), + static_cast(kp_uint & 0xFF), + static_cast(kd_uint >> 4), + static_cast(((kd_uint & 0xF) << 4) | ((tau_uint >> 8) & 0xF)), + static_cast(tau_uint & 0xFF)}; +} + +std::vector CanPacketEncoder::pack_query_param_data(uint32_t send_can_id, int RID) { + return {static_cast(send_can_id & 0xFF), + static_cast((send_can_id >> 8) & 0xFF), + 0x33, + static_cast(RID), + 0x00, + 0x00, + 0x00, + 0x00}; +} + +std::vector CanPacketEncoder::pack_command_data(uint8_t cmd) { + return {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, cmd}; +} + +// Utility function implementations +double CanPacketEncoder::limit_min_max(double x, double min, double max) { + return std::max(min, std::min(x, max)); +} + +uint16_t CanPacketEncoder::double_to_uint(double x, double x_min, double x_max, int bits) { + x = limit_min_max(x, x_min, x_max); + double span = x_max - x_min; + double data_norm = (x - x_min) / span; + return static_cast(data_norm * ((1 << bits) - 1)); +} + +double CanPacketDecoder::uint_to_double(uint16_t x, double min, double max, int bits) { + double span = max - min; + double data_norm = static_cast(x) / ((1 << bits) - 1); + return data_norm * span + min; +} + +float CanPacketDecoder::uint8s_to_float(const std::array& bytes) { + float value; + std::memcpy(&value, bytes.data(), sizeof(float)); + return value; +} + +uint32_t CanPacketDecoder::uint8s_to_uint32(uint8_t byte1, uint8_t byte2, uint8_t byte3, + uint8_t byte4) { + uint32_t value; + uint8_t bytes[4] = {byte1, byte2, byte3, byte4}; + std::memcpy(&value, bytes, sizeof(uint32_t)); + return value; +} + +bool CanPacketDecoder::is_in_ranges(int number) { + return (7 <= number && number <= 10) || (13 <= number && number <= 16) || + (35 <= number && number <= 36); +} +} // namespace openarm::damiao_motor diff --git a/src/openarm/damiao_motor/dm_motor_device.cpp b/src/openarm/damiao_motor/dm_motor_device.cpp new file mode 100644 index 0000000..f106689 --- /dev/null +++ b/src/openarm/damiao_motor/dm_motor_device.cpp @@ -0,0 +1,116 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +namespace openarm::damiao_motor { + +DMCANDevice::DMCANDevice(Motor& motor, canid_t recv_can_mask, bool use_fd) + : canbus::CANDevice(motor.get_send_can_id(), motor.get_recv_can_id(), recv_can_mask, use_fd), + motor_(motor), + callback_mode_(CallbackMode::STATE), + use_fd_(use_fd) {} + +std::vector DMCANDevice::get_data_from_frame(const can_frame& frame) { + return std::vector(frame.data, frame.data + frame.can_dlc); +} + +std::vector DMCANDevice::get_data_from_frame(const canfd_frame& frame) { + return std::vector(frame.data, frame.data + frame.len); +} +void DMCANDevice::callback(const can_frame& frame) { + if (use_fd_) { + std::cerr << "WARNING: WRONG CALLBACK FUNCTION" << std::endl; + return; + } + + std::vector data = get_data_from_frame(frame); + + switch (callback_mode_) { + case STATE: + if (frame.can_dlc >= 8) { + // Convert frame data to vector and let Motor handle parsing + StateResult result = CanPacketDecoder::parse_motor_state_data(motor_, data); + if (frame.can_id == motor_.get_recv_can_id() && result.valid) { + motor_.update_state(result.position, result.velocity, result.torque, + result.t_mos, result.t_rotor); + } + } + break; + case PARAM: { + ParamResult result = CanPacketDecoder::parse_motor_param_data(data); + if (result.valid) { + motor_.set_temp_param(result.rid, result.value); + } + break; + } + case IGNORE: + return; + default: + break; + } +} + +void DMCANDevice::callback(const canfd_frame& frame) { + if (not use_fd_) { + std::cerr << "WARNING: CANFD MODE NOT ENABLED" << std::endl; + return; + } + + if (frame.can_id != motor_.get_recv_can_id()) { + std::cerr << "WARNING: CANFD FRAME ID DOES NOT MATCH MOTOR ID" << std::endl; + return; + } + + std::vector data = get_data_from_frame(frame); + if (callback_mode_ == STATE) { + StateResult result = CanPacketDecoder::parse_motor_state_data(motor_, data); + if (result.valid) { + motor_.update_state(result.position, result.velocity, result.torque, result.t_mos, + result.t_rotor); + } + } else if (callback_mode_ == PARAM) { + ParamResult result = CanPacketDecoder::parse_motor_param_data(data); + if (result.valid) { + motor_.set_temp_param(result.rid, result.value); + } + } else if (callback_mode_ == IGNORE) { + return; + } +} + +can_frame DMCANDevice::create_can_frame(canid_t send_can_id, std::vector data) { + can_frame frame; + std::memset(&frame, 0, sizeof(frame)); + frame.can_id = send_can_id; + frame.can_dlc = data.size(); + std::copy(data.begin(), data.end(), frame.data); + return frame; +} + +canfd_frame DMCANDevice::create_canfd_frame(canid_t send_can_id, std::vector data) { + canfd_frame frame; + frame.can_id = send_can_id; + frame.len = data.size(); + frame.flags = CANFD_BRS; + std::copy(data.begin(), data.end(), frame.data); + return frame; +} + +} // namespace openarm::damiao_motor diff --git a/src/openarm/damiao_motor/dm_motor_device_collection.cpp b/src/openarm/damiao_motor/dm_motor_device_collection.cpp new file mode 100644 index 0000000..f8c6206 --- /dev/null +++ b/src/openarm/damiao_motor/dm_motor_device_collection.cpp @@ -0,0 +1,128 @@ +// Copyright 2025 Enactic, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include + +#include +#include + +namespace openarm::damiao_motor { + +DMDeviceCollection::DMDeviceCollection(canbus::CANSocket& can_socket) + : can_socket_(can_socket), + can_packet_encoder_(std::make_unique()), + can_packet_decoder_(std::make_unique()), + device_collection_(std::make_unique(can_socket_)) {} + +void DMDeviceCollection::enable_all() { + for (auto dm_device : get_dm_devices()) { + auto& motor = dm_device->get_motor(); + CANPacket enable_packet = CanPacketEncoder::create_enable_command(motor); + send_command_to_device(dm_device, enable_packet); + } +} + +void DMDeviceCollection::disable_all() { + for (auto dm_device : get_dm_devices()) { + CANPacket disable_packet = CanPacketEncoder::create_disable_command(dm_device->get_motor()); + send_command_to_device(dm_device, disable_packet); + } +} + +void DMDeviceCollection::set_zero_all() { + for (auto dm_device : get_dm_devices()) { + CANPacket zero_packet = CanPacketEncoder::create_set_zero_command(dm_device->get_motor()); + send_command_to_device(dm_device, zero_packet); + } +} + +void DMDeviceCollection::refresh_one(int i) { + auto dm_device = get_dm_devices()[i]; + auto& motor = dm_device->get_motor(); + CANPacket refresh_packet = CanPacketEncoder::create_refresh_command(motor); + send_command_to_device(dm_device, refresh_packet); +} + +void DMDeviceCollection::refresh_all() { + for (auto dm_device : get_dm_devices()) { + auto& motor = dm_device->get_motor(); + CANPacket refresh_packet = CanPacketEncoder::create_refresh_command(motor); + send_command_to_device(dm_device, refresh_packet); + } +} + +void DMDeviceCollection::set_callback_mode_all(CallbackMode callback_mode) { + for (auto dm_device : get_dm_devices()) { + dm_device->set_callback_mode(callback_mode); + } +} + +void DMDeviceCollection::query_param_one(int i, int RID) { + CANPacket param_query = + CanPacketEncoder::create_query_param_command(get_dm_devices()[i]->get_motor(), RID); + send_command_to_device(get_dm_devices()[i], param_query); +} + +void DMDeviceCollection::query_param_all(int RID) { + for (auto dm_device : get_dm_devices()) { + CANPacket param_query = + CanPacketEncoder::create_query_param_command(dm_device->get_motor(), RID); + send_command_to_device(dm_device, param_query); + } +} + +void DMDeviceCollection::send_command_to_device(std::shared_ptr dm_device, + const CANPacket& packet) { + if (can_socket_.is_canfd_enabled()) { + canfd_frame frame = dm_device->create_canfd_frame(packet.send_can_id, packet.data); + can_socket_.write_canfd_frame(frame); + } else { + can_frame frame = dm_device->create_can_frame(packet.send_can_id, packet.data); + can_socket_.write_can_frame(frame); + } +} + +void DMDeviceCollection::mit_control_one(int i, const MITParam& mit_param) { + CANPacket mit_cmd = + CanPacketEncoder::create_mit_control_command(get_dm_devices()[i]->get_motor(), mit_param); + send_command_to_device(get_dm_devices()[i], mit_cmd); +} + +void DMDeviceCollection::mit_control_all(const std::vector& mit_params) { + for (size_t i = 0; i < mit_params.size(); i++) { + mit_control_one(i, mit_params[i]); + } +} + +std::vector DMDeviceCollection::get_motors() const { + std::vector motors; + for (auto dm_device : get_dm_devices()) { + motors.push_back(dm_device->get_motor()); + } + return motors; +} + +std::vector> DMDeviceCollection::get_dm_devices() const { + std::vector> dm_devices; + for (const auto& [id, device] : device_collection_->get_devices()) { + auto dm_device = std::dynamic_pointer_cast(device); + if (dm_device) { + dm_devices.push_back(dm_device); + } + } + return dm_devices; +} + +} // namespace openarm::damiao_motor