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/.editorconfig b/.editorconfig new file mode 100644 index 0000000..fad9c10 --- /dev/null +++ b/.editorconfig @@ -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. + +# https://editorconfig.org/ + +root = true + +[*] +charset = utf-8 +insert_final_newline = true +spelling_language = en +trim_trailing_whitespace = true + +[*.sh] +indent_size = 4 +indent_style = space diff --git a/.github/workflows/test.yaml b/.github/workflows/test.yaml index e9d5e82..9937629 100644 --- a/.github/workflows/test.yaml +++ b/.github/workflows/test.yaml @@ -25,6 +25,25 @@ 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@v5 + 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 }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..228f175 --- /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/koalaman/shellcheck-precommit + rev: v0.10.0 + hooks: + - id: shellcheck + alias: shell + - repo: https://github.com/scop/pre-commit-shfmt + # v3.11.0-1 or later requires pre-commit 3.2.0 or later but Ubuntu + # 22.04 ships pre-commit 2.17.0. We can update this rev after + # Ubuntu 22.04 reached EOL (June 2027). + rev: v3.10.0-1 + hooks: + - id: shfmt + alias: shell + args: + # The default args is "--write --simplify" but we don't use + # "--simplify". Because it's conflicted will ShellCheck. + - "--write" diff --git a/.shellcheckrc b/.shellcheckrc new file mode 100644 index 0000000..0a68ce2 --- /dev/null +++ b/.shellcheckrc @@ -0,0 +1,16 @@ +# 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. + +external-sources=true +source-path=SCRIPTDIR diff --git a/CMakeLists.txt b/CMakeLists.txt index 03c241f..58fc69d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -40,28 +40,18 @@ endif() # ----------------------------- # Create static library # ----------------------------- -add_library(openarm_teleop_lib STATIC - src/controller/dynamics.cpp - src/controller/control.cpp - src/openarm_port/openarm_init.cpp - src/openarm_port/joint_mapper.cpp -) +add_library( + openarm_teleop_lib STATIC + src/controller/dynamics.cpp src/controller/control.cpp + src/openarm_port/openarm_init.cpp src/openarm_port/joint_mapper.cpp) -target_include_directories( - openarm_teleop_lib - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR}/src -) +target_include_directories(openarm_teleop_lib + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src) -target_link_libraries(openarm_teleop_lib - PUBLIC - OpenArmCAN::openarm_can - Eigen3::Eigen - ${orocos_kdl_LIBRARIES} - ${kdl_parser_LIBRARIES} - urdfdom::urdfdom_model - yaml-cpp::yaml-cpp -) +target_link_libraries( + openarm_teleop_lib + PUBLIC OpenArmCAN::openarm_can Eigen3::Eigen ${orocos_kdl_LIBRARIES} + ${kdl_parser_LIBRARIES} urdfdom::urdfdom_model yaml-cpp::yaml-cpp) # ----------------------------- # Executables diff --git a/control/gravity_compasation.cpp b/control/gravity_compasation.cpp index 387ab66..d77893a 100644 --- a/control/gravity_compasation.cpp +++ b/control/gravity_compasation.cpp @@ -14,16 +14,14 @@ #include #include +#include #include -#include -#include -#include -#include #include +#include #include #include -#include #include +#include std::atomic keep_running(true); @@ -41,10 +39,11 @@ int main(int argc, char** argv) { std::string arm_side = "right_arm"; std::string can_interface = "can0"; - if (argc < 4) { - std::cerr << "Usage: " << argv[0] << " " << std::endl; - std::cerr << "Example: " << argv[0] << " right_arm can0 /tmp/v10_bimanual.urdf" << std::endl; + std::cerr << "Usage: " << argv[0] << " " + << std::endl; + std::cerr << "Example: " << argv[0] << " right_arm can0 /tmp/v10_bimanual.urdf" + << std::endl; return 1; } @@ -53,7 +52,8 @@ int main(int argc, char** argv) { std::string urdf_path = argv[3]; if (arm_side != "left_arm" && arm_side != "right_arm") { - std::cerr << "[ERROR] Invalid arm_side: " << arm_side << ". Must be 'left_arm' or 'right_arm'." << std::endl; + std::cerr << "[ERROR] Invalid arm_side: " << arm_side + << ". Must be 'left_arm' or 'right_arm'." << std::endl; return 1; } @@ -68,13 +68,14 @@ int main(int argc, char** argv) { std::cout << "URDF path : " << urdf_path << std::endl; std::string root_link = "openarm_body_link0"; - std::string leaf_link = (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; + std::string leaf_link = + (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; Dynamics arm_dynamics(urdf_path, root_link, leaf_link); arm_dynamics.Init(); std::cout << "=== Initializing Leader OpenArm ===" << std::endl; - openarm::can::socket::OpenArm *openarm = + openarm::can::socket::OpenArm* openarm = openarm_init::OpenArmInitializer::initialize_openarm(can_interface, true); std::this_thread::sleep_for(std::chrono::milliseconds(100)); @@ -85,20 +86,25 @@ int main(int argc, char** argv) { std::vector arm_joint_positions(openarm->get_arm().get_motors().size(), 0.0); std::vector arm_joint_velocities(openarm->get_arm().get_motors().size(), 0.0); - std::vector gripper_joint_positions(openarm->get_gripper().get_motors().size(), 0.0); - std::vector gripper_joint_velocities(openarm->get_gripper().get_motors().size(), 0.0); + std::vector gripper_joint_positions(openarm->get_gripper().get_motors().size(), + 0.0); + std::vector gripper_joint_velocities(openarm->get_gripper().get_motors().size(), + 0.0); std::vector grav_torques(openarm->get_arm().get_motors().size(), 0.0); - while(keep_running){ - + while (keep_running) { frame_count++; auto current_time = std::chrono::high_resolution_clock::now(); // Calculate and display Hz every second - auto time_since_last_display = std::chrono::duration_cast(current_time - last_hz_display).count(); - if (time_since_last_display >= 1000) { // Every 1000ms (1 second) - auto total_time = std::chrono::duration_cast(current_time - start_time).count(); + auto time_since_last_display = std::chrono::duration_cast( + current_time - last_hz_display) + .count(); + if (time_since_last_display >= 1000) { // Every 1000ms (1 second) + auto total_time = + std::chrono::duration_cast(current_time - start_time) + .count(); double hz = (frame_count * 1000.0) / total_time; std::cout << "=== Loop Frequency: " << hz << " Hz ===" << std::endl; last_hz_display = current_time; @@ -112,7 +118,7 @@ int main(int argc, char** argv) { arm_dynamics.GetGravity(arm_joint_positions.data(), grav_torques.data()); - for(size_t i = 0; i < openarm->get_arm().get_motors().size(); ++i){ + for (size_t i = 0; i < openarm->get_arm().get_motors().size(); ++i) { // std::cout << "grav_torques[" << i << "] = " << grav_torques[i] << std::endl; } @@ -120,12 +126,11 @@ int main(int argc, char** argv) { cmds.reserve(grav_torques.size()); std::transform(grav_torques.begin(), grav_torques.end(), std::back_inserter(cmds), - [](double t) { return openarm::damiao_motor::MITParam{0, 0, 0, 0, t}; }); + [](double t) { return openarm::damiao_motor::MITParam{0, 0, 0, 0, t}; }); openarm->get_arm().mit_control_all(cmds); openarm->recv_all(); - } std::this_thread::sleep_for(std::chrono::milliseconds(100)); diff --git a/control/openarm_bilateral_control.cpp b/control/openarm_bilateral_control.cpp index d0e8463..a23f3df 100644 --- a/control/openarm_bilateral_control.cpp +++ b/control/openarm_bilateral_control.cpp @@ -12,28 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include #include +#include +#include #include -#include -#include -#include -#include #include - -#include -#include +#include #include #include #include -#include +#include +#include +#include #include -#include std::atomic keep_running(true); - void signal_handler(int signal) { if (signal == SIGINT) { std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; @@ -41,130 +36,115 @@ void signal_handler(int signal) { } } - class LeaderArmThread : public PeriodicTimerThread { - public: - LeaderArmThread(std::shared_ptr robot_state, Control *control_l, double hz = 500.0) - : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l){} +public: + LeaderArmThread(std::shared_ptr robot_state, Control *control_l, + double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {} - protected: +protected: + void before_start() override { std::cout << "leader start thread " << std::endl; } - void before_start() override { - std::cout << "leader start thread " << std::endl; - } + void after_stop() override { std::cout << "leader stop thread " << std::endl; } - void after_stop() override { - std::cout << "leader stop thread " << std::endl; - } + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); - void on_timer() override { - static auto prev_time = std::chrono::steady_clock::now(); + control_l_->bilateral_step(); - control_l_->bilateral_step(); + auto now = std::chrono::steady_clock::now(); - auto now = std::chrono::steady_clock::now(); + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; - auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; - - // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; - } - - private: - std::shared_ptr robot_state_; - Control *control_l_; - - }; + // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; + } +private: + std::shared_ptr robot_state_; + Control *control_l_; +}; class FollowerArmThread : public PeriodicTimerThread { - public: - FollowerArmThread(std::shared_ptr robot_state, Control *control_f, double hz = 500.0) - : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} +public: + FollowerArmThread(std::shared_ptr robot_state, Control *control_f, + double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} - protected: - void before_start() override { - std::cout << "follower start thread " << std::endl; - } +protected: + void before_start() override { std::cout << "follower start thread " << std::endl; } - void after_stop() override { - std::cout << "follower stop thread " << std::endl; - } + void after_stop() override { std::cout << "follower stop thread " << std::endl; } - void on_timer() override { - static auto prev_time = std::chrono::steady_clock::now(); + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); - control_f_->bilateral_step(); + control_f_->bilateral_step(); - auto now = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); - auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; - - // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; - } - - private: - std::shared_ptr robot_state_; - Control *control_f_; - - }; + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; + // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; + } +private: + std::shared_ptr robot_state_; + Control *control_f_; +}; class AdminThread : public PeriodicTimerThread { - public: - AdminThread(std::shared_ptr leader_state, - std::shared_ptr follower_state, - Control *control_l, - Control *control_f, - double hz = 500.0) - : PeriodicTimerThread(hz), leader_state_(leader_state), follower_state_(follower_state), control_l_(control_l), control_f_(control_f) {} +public: + AdminThread(std::shared_ptr leader_state, + std::shared_ptr follower_state, Control *control_l, + Control *control_f, double hz = 500.0) + : PeriodicTimerThread(hz), + leader_state_(leader_state), + follower_state_(follower_state), + control_l_(control_l), + control_f_(control_f) {} - protected: - void before_start() override { - std::cout << "admin start thread " << std::endl; - } +protected: + void before_start() override { std::cout << "admin start thread " << std::endl; } - void after_stop() override { - std::cout << "admin stop thread " << std::endl; - } + void after_stop() override { std::cout << "admin stop thread " << std::endl; } - void on_timer() override { + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); - static auto prev_time = std::chrono::steady_clock::now(); - auto now = std::chrono::steady_clock::now(); + // get response + auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); + auto follower_arm_resp = follower_state_->arm_state().get_all_responses(); - // get response - auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); - auto follower_arm_resp = follower_state_->arm_state().get_all_responses(); + auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); + auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); - auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); - auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); + // set referense + leader_state_->arm_state().set_all_references(follower_arm_resp); + leader_state_->hand_state().set_all_references(follower_hand_resp); - //set referense - leader_state_->arm_state().set_all_references(follower_arm_resp); - leader_state_->hand_state().set_all_references(follower_hand_resp); + follower_state_->arm_state().set_all_references(leader_arm_resp); + follower_state_->hand_state().set_all_references(leader_hand_resp); - follower_state_->arm_state().set_all_references(leader_arm_resp); - follower_state_->hand_state().set_all_references(leader_hand_resp); + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; - auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; + // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; + } - // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; - } +private: + std::shared_ptr leader_state_; + std::shared_ptr follower_state_; + Control *control_l_; + Control *control_f_; +}; - private: - std::shared_ptr leader_state_; - std::shared_ptr follower_state_; - Control *control_l_; - Control *control_f_; - - }; - - -int main(int argc, char** argv) { +int main(int argc, char **argv) { try { std::signal(SIGINT, signal_handler); @@ -175,7 +155,10 @@ int main(int argc, char** argv) { std::string follower_can_interface = "can2"; if (argc < 3) { - std::cerr << "Usage: " << argv[0] << " [arm_side] [leader_can] [follower_can]" << std::endl; + std::cerr + << "Usage: " << argv[0] + << " [arm_side] [leader_can] [follower_can]" + << std::endl; return 1; } @@ -187,7 +170,8 @@ int main(int argc, char** argv) { if (argc >= 4) { arm_side = argv[3]; if (arm_side != "left_arm" && arm_side != "right_arm") { - std::cerr << "[ERROR] Invalid arm_side: " << arm_side << ". Must be 'left_arm' or 'right_arm'." << std::endl; + std::cerr << "[ERROR] Invalid arm_side: " << arm_side + << ". Must be 'left_arm' or 'right_arm'." << std::endl; return 1; } } @@ -210,7 +194,8 @@ int main(int argc, char** argv) { // Setup dynamics std::string root_link = "openarm_body_link0"; - std::string leaf_link = (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; + std::string leaf_link = + (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; // Output confirmation std::cout << "=== OpenArm Bilateral Control ===" << std::endl; @@ -229,7 +214,7 @@ int main(int argc, char** argv) { std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); std::vector leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); std::vector leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); - std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); + std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); std::vector leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); std::vector leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); @@ -237,7 +222,7 @@ int main(int argc, char** argv) { std::vector follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); std::vector follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); std::vector follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc"); - std::vector follower_k = follower_loader.get_vector("FollowerArmParam", "k"); + std::vector follower_k = follower_loader.get_vector("FollowerArmParam", "k"); std::vector follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv"); std::vector follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo"); @@ -267,33 +252,37 @@ int main(int argc, char** argv) { // Declare robot_state (Joint and motor counts are assumed to be equal) std::shared_ptr leader_state = - std::make_shared(leader_arm_motor_num, leader_hand_motor_num); + std::make_shared(leader_arm_motor_num, leader_hand_motor_num); std::shared_ptr follower_state = std::make_shared(follower_arm_motor_num, follower_hand_motor_num); - Control* control_leader = new Control(leader_openarm,leader_arm_dynamics,follower_arm_dynamics, leader_state, 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); - Control* control_follower = new Control(follower_openarm,leader_arm_dynamics,follower_arm_dynamics, follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, follower_arm_motor_num, follower_hand_motor_num); + Control *control_leader = new Control( + leader_openarm, leader_arm_dynamics, follower_arm_dynamics, leader_state, + 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); + Control *control_follower = + new Control(follower_openarm, leader_arm_dynamics, follower_arm_dynamics, + follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, + follower_arm_motor_num, follower_hand_motor_num); - //set parameter - control_leader->SetParameter( - leader_kp, leader_kd, - leader_Fc, leader_k, leader_Fv, leader_Fo); + // set parameter + control_leader->SetParameter(leader_kp, leader_kd, leader_Fc, leader_k, leader_Fv, + leader_Fo); - control_follower->SetParameter( - follower_kp, follower_kd, - follower_Fc, follower_k, follower_Fv, follower_Fo); + control_follower->SetParameter(follower_kp, follower_kd, follower_Fc, follower_k, + follower_Fv, follower_Fo); - //set home postion + // set home postion std::thread thread_l(&Control::AdjustPosition, control_leader); std::thread thread_f(&Control::AdjustPosition, control_follower); thread_l.join(); thread_f.join(); // Start control process - LeaderArmThread leader_thread(leader_state ,control_leader, FREQUENCY); + LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY); FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); - AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, FREQUENCY); + AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, + FREQUENCY); // thread start in control leader_thread.start_thread(); @@ -311,9 +300,7 @@ int main(int argc, char** argv) { leader_openarm->disable_all(); follower_openarm->disable_all(); - } - catch(const std::exception& e) - { + } catch (const std::exception &e) { std::cerr << e.what() << '\n'; } diff --git a/control/openarm_communication_test.cpp b/control/openarm_communication_test.cpp index e7553c5..2909e0d 100644 --- a/control/openarm_communication_test.cpp +++ b/control/openarm_communication_test.cpp @@ -19,9 +19,6 @@ #include #include #include -#include -#include - int main(int argc, char** argv) { try { @@ -29,13 +26,12 @@ int main(int argc, char** argv) { std::cout << "This example demonstrates the OpenArm API functionality" << std::endl; std::string can_interface = "can0"; - if (argc > 1 ){ + if (argc > 1) { can_interface = argv[1]; } std::cout << "[INFO] Using CAN interface: " << can_interface << std::endl; - // Initialize OpenArm with CAN interface and enable CAN-FD std::cout << "Initializing OpenArm CAN..." << std::endl; openarm::can::socket::OpenArm openarm(can_interface, true); // Use CAN-FD on can0 interface @@ -45,8 +41,7 @@ int main(int argc, char** argv) { openarm::damiao_motor::MotorType::DM8009, openarm::damiao_motor::MotorType::DM8009, openarm::damiao_motor::MotorType::DM4340, openarm::damiao_motor::MotorType::DM4340, openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310, - openarm::damiao_motor::MotorType::DM4310 - }; + openarm::damiao_motor::MotorType::DM4310}; std::vector send_can_ids = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}; std::vector recv_can_ids = {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}; @@ -100,8 +95,7 @@ int main(int argc, char** argv) { openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}, - openarm::damiao_motor::MITParam{0, 0, 0, 0, 0} - }); + openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}}); openarm.get_gripper().mit_control_all({openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}}); diff --git a/control/openarm_unilateral_control.cpp b/control/openarm_unilateral_control.cpp index c8bac95..622c661 100644 --- a/control/openarm_unilateral_control.cpp +++ b/control/openarm_unilateral_control.cpp @@ -12,28 +12,23 @@ // See the License for the specific language governing permissions and // limitations under the License. - #include #include +#include +#include #include -#include -#include -#include -#include #include - -#include -#include +#include #include #include #include -#include +#include +#include +#include #include -#include std::atomic keep_running(true); - void signal_handler(int signal) { if (signal == SIGINT) { std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; @@ -41,129 +36,115 @@ void signal_handler(int signal) { } } - class LeaderArmThread : public PeriodicTimerThread { - public: - LeaderArmThread(std::shared_ptr robot_state, Control *control_l, double hz = 500.0) - : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l){} +public: + LeaderArmThread(std::shared_ptr robot_state, Control *control_l, + double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {} - protected: +protected: + void before_start() override { std::cout << "leader start thread " << std::endl; } - void before_start() override { - std::cout << "leader start thread " << std::endl; - } + void after_stop() override { std::cout << "leader stop thread " << std::endl; } - void after_stop() override { - std::cout << "leader stop thread " << std::endl; - } + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); - void on_timer() override { - static auto prev_time = std::chrono::steady_clock::now(); + control_l_->unilateral_step(); - control_l_->unilateral_step(); + auto now = std::chrono::steady_clock::now(); - auto now = std::chrono::steady_clock::now(); + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; - auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; - - // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; - } - - private: - std::shared_ptr robot_state_; - Control *control_l_; - - }; + // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; + } +private: + std::shared_ptr robot_state_; + Control *control_l_; +}; class FollowerArmThread : public PeriodicTimerThread { - public: - FollowerArmThread(std::shared_ptr robot_state, Control *control_f, double hz = 500.0) - : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} +public: + FollowerArmThread(std::shared_ptr robot_state, Control *control_f, + double hz = 500.0) + : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} - protected: - void before_start() override { - std::cout << "follower start thread " << std::endl; - } +protected: + void before_start() override { std::cout << "follower start thread " << std::endl; } - void after_stop() override { - std::cout << "follower stop thread " << std::endl; - } + void after_stop() override { std::cout << "follower stop thread " << std::endl; } - void on_timer() override { - static auto prev_time = std::chrono::steady_clock::now(); + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); - control_f_->unilateral_step(); + control_f_->unilateral_step(); - auto now = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); - auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; - // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; - } - - private: - std::shared_ptr robot_state_; - Control *control_f_; - - }; + // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; + } +private: + std::shared_ptr robot_state_; + Control *control_f_; +}; class AdminThread : public PeriodicTimerThread { - public: - AdminThread(std::shared_ptr leader_state, - std::shared_ptr follower_state, - Control *control_l, - Control *control_f, - double hz = 500.0) - : PeriodicTimerThread(hz), leader_state_(leader_state), follower_state_(follower_state), control_l_(control_l), control_f_(control_f) {} +public: + AdminThread(std::shared_ptr leader_state, + std::shared_ptr follower_state, Control *control_l, + Control *control_f, double hz = 500.0) + : PeriodicTimerThread(hz), + leader_state_(leader_state), + follower_state_(follower_state), + control_l_(control_l), + control_f_(control_f) {} - protected: - void before_start() override { - std::cout << "admin start thread " << std::endl; - } +protected: + void before_start() override { std::cout << "admin start thread " << std::endl; } - void after_stop() override { - std::cout << "admin stop thread " << std::endl; - } + void after_stop() override { std::cout << "admin stop thread " << std::endl; } - void on_timer() override { + void on_timer() override { + static auto prev_time = std::chrono::steady_clock::now(); + auto now = std::chrono::steady_clock::now(); - static auto prev_time = std::chrono::steady_clock::now(); - auto now = std::chrono::steady_clock::now(); + // get response + auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); + auto follower_arm_resp = follower_state_->arm_state().get_all_responses(); - // get response - auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); - auto follower_arm_resp = follower_state_->arm_state().get_all_responses(); + auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); + auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); - auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); - auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); + // set referense + leader_state_->arm_state().set_all_references(follower_arm_resp); + leader_state_->hand_state().set_all_references(follower_hand_resp); - //set referense - leader_state_->arm_state().set_all_references(follower_arm_resp); - leader_state_->hand_state().set_all_references(follower_hand_resp); + follower_state_->arm_state().set_all_references(leader_arm_resp); + follower_state_->hand_state().set_all_references(leader_hand_resp); - follower_state_->arm_state().set_all_references(leader_arm_resp); - follower_state_->hand_state().set_all_references(leader_hand_resp); + auto elapsed_us = + std::chrono::duration_cast(now - prev_time).count(); + prev_time = now; - auto elapsed_us = std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; + // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; + } - // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; - } +private: + std::shared_ptr leader_state_; + std::shared_ptr follower_state_; + Control *control_l_; + Control *control_f_; +}; - private: - std::shared_ptr leader_state_; - std::shared_ptr follower_state_; - Control *control_l_; - Control *control_f_; - - }; - - -int main(int argc, char** argv) { +int main(int argc, char **argv) { try { std::signal(SIGINT, signal_handler); @@ -175,7 +156,10 @@ int main(int argc, char** argv) { std::string follower_can_interface = "can2"; if (argc < 3) { - std::cerr << "Usage: " << argv[0] << " [arm_side] [leader_can] [follower_can]" << std::endl; + std::cerr + << "Usage: " << argv[0] + << " [arm_side] [leader_can] [follower_can]" + << std::endl; return 1; } @@ -187,7 +171,8 @@ int main(int argc, char** argv) { if (argc >= 4) { arm_side = argv[3]; if (arm_side != "left_arm" && arm_side != "right_arm") { - std::cerr << "[ERROR] Invalid arm_side: " << arm_side << ". Must be 'left_arm' or 'right_arm'." << std::endl; + std::cerr << "[ERROR] Invalid arm_side: " << arm_side + << ". Must be 'left_arm' or 'right_arm'." << std::endl; return 1; } } @@ -210,7 +195,8 @@ int main(int argc, char** argv) { // Setup dynamics std::string root_link = "openarm_body_link0"; - std::string leaf_link = (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; + std::string leaf_link = + (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand"; // Output confirmation std::cout << "=== OpenArm Unilateral Control ===" << std::endl; @@ -229,7 +215,7 @@ int main(int argc, char** argv) { std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); std::vector leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); std::vector leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); - std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); + std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); std::vector leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); std::vector leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); @@ -237,7 +223,7 @@ int main(int argc, char** argv) { std::vector follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); std::vector follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); std::vector follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc"); - std::vector follower_k = follower_loader.get_vector("FollowerArmParam", "k"); + std::vector follower_k = follower_loader.get_vector("FollowerArmParam", "k"); std::vector follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv"); std::vector follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo"); @@ -267,32 +253,36 @@ int main(int argc, char** argv) { // Declare robot_state (Joint and motor counts are assumed to be equal) std::shared_ptr leader_state = - std::make_shared(leader_arm_motor_num, leader_hand_motor_num); + std::make_shared(leader_arm_motor_num, leader_hand_motor_num); std::shared_ptr follower_state = std::make_shared(follower_arm_motor_num, follower_hand_motor_num); - Control* control_leader = new Control(leader_openarm,leader_arm_dynamics,follower_arm_dynamics, leader_state, 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); - Control* control_follower = new Control(follower_openarm,leader_arm_dynamics,follower_arm_dynamics, follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, follower_arm_motor_num, follower_hand_motor_num); + Control *control_leader = new Control( + leader_openarm, leader_arm_dynamics, follower_arm_dynamics, leader_state, + 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); + Control *control_follower = + new Control(follower_openarm, leader_arm_dynamics, follower_arm_dynamics, + follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, + follower_arm_motor_num, follower_hand_motor_num); - control_leader->SetParameter( - leader_kp, leader_kd , - leader_Fc, leader_k, leader_Fv, leader_Fo); + control_leader->SetParameter(leader_kp, leader_kd, leader_Fc, leader_k, leader_Fv, + leader_Fo); - control_follower->SetParameter( - follower_kp, follower_kd, - follower_Fc, follower_k, follower_Fv, follower_Fo); + control_follower->SetParameter(follower_kp, follower_kd, follower_Fc, follower_k, + follower_Fv, follower_Fo); - //set home postion + // set home postion std::thread thread_l(&Control::AdjustPosition, control_leader); std::thread thread_f(&Control::AdjustPosition, control_follower); thread_l.join(); thread_f.join(); // Start control process - LeaderArmThread leader_thread(leader_state ,control_leader, FREQUENCY); + LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY); FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); - AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, FREQUENCY); + AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, + FREQUENCY); leader_thread.start_thread(); follower_thread.start_thread(); @@ -309,9 +299,7 @@ int main(int argc, char** argv) { leader_openarm->disable_all(); follower_openarm->disable_all(); - } - catch(const std::exception& e) - { + } catch (const std::exception &e) { std::cerr << e.what() << '\n'; } diff --git a/script/launch_bilateral.sh b/script/launch_bilateral.sh index f7ffc41..a4151d4 100755 --- a/script/launch_bilateral.sh +++ b/script/launch_bilateral.sh @@ -15,34 +15,34 @@ # limitations under the License. # ========= Configuration ========= -ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm -LEADER_CAN_IF=$2 # Optional: leader CAN interface -FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface -ARM_TYPE="v10" # Fixed for now +ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm +LEADER_CAN_IF=$2 # Optional: leader CAN interface +FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface +ARM_TYPE="v10" # Fixed for now TMPDIR="/tmp/openarm_urdf_gen" # Validate arm side if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then - echo "[ERROR] Invalid arm_side: $ARM_SIDE" - echo "Usage: $0 [leader_can_if] [follower_can_if]" - exit 1 + echo "[ERROR] Invalid arm_side: $ARM_SIDE" + echo "Usage: $0 [leader_can_if] [follower_can_if]" + exit 1 fi # Set default CAN interfaces if not provided if [ -z "$LEADER_CAN_IF" ]; then - if [ "$ARM_SIDE" = "right_arm" ]; then - LEADER_CAN_IF="can0" - else - LEADER_CAN_IF="can1" - fi + if [ "$ARM_SIDE" = "right_arm" ]; then + LEADER_CAN_IF="can0" + else + LEADER_CAN_IF="can1" + fi fi if [ -z "$FOLLOWER_CAN_IF" ]; then - if [ "$ARM_SIDE" = "right_arm" ]; then - FOLLOWER_CAN_IF="can2" - else - FOLLOWER_CAN_IF="can3" - fi + if [ "$ARM_SIDE" = "right_arm" ]; then + FOLLOWER_CAN_IF="can2" + else + FOLLOWER_CAN_IF="can3" + fi fi # File paths @@ -56,45 +56,43 @@ echo $BIN_PATH # ================================ # Check workspace if [ ! -d "$WS_DIR" ]; then - echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 - echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 - echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 - exit 1 + echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 + echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 + echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 + exit 1 fi # Check openarm_description package if [ ! -d "$WS_DIR/src/openarm_description" ]; then - echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 - echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 - exit 1 + echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 + echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 + exit 1 fi # Check xacro if [ ! -f "$XACRO_PATH" ]; then - echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 - exit 1 + echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 + exit 1 fi - # Check binary if [ ! -f "$BIN_PATH" ]; then - echo "[ERROR] Compiled binary not found at: $BIN_PATH" - exit 1 + echo "[ERROR] Compiled binary not found at: $BIN_PATH" + exit 1 fi # Source ROS 2 +# shellcheck source=/dev/null source "$WS_DIR/install/setup.bash" # Generate URDFs echo "[INFO] Generating URDFs using xacro..." mkdir -p "$TMPDIR" -xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH" -cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" - -if [ $? -ne 0 ]; then +if ! xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH"; then echo "[ERROR] Failed to generate URDFs." exit 1 fi +cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" # Run binary echo "[INFO] Launching bilateral control..." diff --git a/script/launch_grav_comp.sh b/script/launch_grav_comp.sh index 3ca44cf..7c84ea5 100755 --- a/script/launch_grav_comp.sh +++ b/script/launch_grav_comp.sh @@ -24,44 +24,42 @@ XACRO_FILE="${ARM_TYPE}.urdf.xacro" WS_DIR=~/openarm_ros2_ws XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" URDF_OUT="$TMPDIR/$URDF_NAME" -BIN_PATH=~/openarm_teleop/build/gravity_comp # adjust if needed +BIN_PATH=~/openarm_teleop/build/gravity_comp # adjust if needed # =============================== # Check workspace if [ ! -d "$WS_DIR" ]; then - echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 - echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 - echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 - exit 1 + echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 + echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 + echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 + exit 1 fi # Check openarm_description package if [ ! -d "$WS_DIR/src/openarm_description" ]; then - echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 - echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 - exit 1 + echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 + echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 + exit 1 fi # Check xacro if [ ! -f "$XACRO_PATH" ]; then - echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 - exit 1 + echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 + exit 1 fi - # Check build binary if [ ! -f "$BIN_PATH" ]; then - echo "[ERROR] Compiled binary not found at: $BIN_PATH" - exit 1 + echo "[ERROR] Compiled binary not found at: $BIN_PATH" + exit 1 fi # Generate URDF echo "[INFO] Generating URDF using xacro..." +# shellcheck source=/dev/null source $WS_DIR/install/setup.bash mkdir -p "$TMPDIR" -xacro "$XACRO_PATH" bimanual:=true -o "$URDF_OUT" - -if [ $? -ne 0 ]; then +if ! xacro "$XACRO_PATH" bimanual:=true -o "$URDF_OUT"; then echo "[ERROR] Failed to generate URDF." exit 1 fi diff --git a/script/launch_unilateral.sh b/script/launch_unilateral.sh index 43c8b0a..3c35772 100755 --- a/script/launch_unilateral.sh +++ b/script/launch_unilateral.sh @@ -15,34 +15,34 @@ # limitations under the License. # ========= Configuration ========= -ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm -LEADER_CAN_IF=$2 # Optional: leader CAN interface -FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface -ARM_TYPE="v10" # Fixed for now +ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm +LEADER_CAN_IF=$2 # Optional: leader CAN interface +FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface +ARM_TYPE="v10" # Fixed for now TMPDIR="/tmp/openarm_urdf_gen" # Validate arm side if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then - echo "[ERROR] Invalid arm_side: $ARM_SIDE" - echo "Usage: $0 [leader_can_if] [follower_can_if]" - exit 1 + echo "[ERROR] Invalid arm_side: $ARM_SIDE" + echo "Usage: $0 [leader_can_if] [follower_can_if]" + exit 1 fi # Set default CAN interfaces if not provided if [ -z "$LEADER_CAN_IF" ]; then - if [ "$ARM_SIDE" = "right_arm" ]; then - LEADER_CAN_IF="can0" - else - LEADER_CAN_IF="can1" - fi + if [ "$ARM_SIDE" = "right_arm" ]; then + LEADER_CAN_IF="can0" + else + LEADER_CAN_IF="can1" + fi fi if [ -z "$FOLLOWER_CAN_IF" ]; then - if [ "$ARM_SIDE" = "right_arm" ]; then - FOLLOWER_CAN_IF="can2" - else - FOLLOWER_CAN_IF="can3" - fi + if [ "$ARM_SIDE" = "right_arm" ]; then + FOLLOWER_CAN_IF="can2" + else + FOLLOWER_CAN_IF="can3" + fi fi # File paths @@ -55,46 +55,45 @@ BIN_PATH=~/openarm_teleop/build/unilateral_control # Check workspace if [ ! -d "$WS_DIR" ]; then - echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 - echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 - echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 - exit 1 + echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 + echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 + echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 + exit 1 fi # Check openarm_description package if [ ! -d "$WS_DIR/src/openarm_description" ]; then - echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 - echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 - exit 1 + echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 + echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 + exit 1 fi # Check xacro if [ ! -f "$XACRO_PATH" ]; then - echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 - exit 1 + echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 + exit 1 fi # ================================ # Check binary if [ ! -f "$BIN_PATH" ]; then - echo "[ERROR] Compiled binary not found at: $BIN_PATH" - exit 1 + echo "[ERROR] Compiled binary not found at: $BIN_PATH" + exit 1 fi # Source ROS 2 +# shellcheck source=/dev/null source "$WS_DIR/install/setup.bash" # Generate URDFs echo "[INFO] Generating URDFs using xacro..." mkdir -p "$TMPDIR" -xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH" -cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" - -if [ $? -ne 0 ]; then +if ! xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH"; then echo "[ERROR] Failed to generate URDFs." exit 1 fi +cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" # Run binary echo "[INFO] Launching unilateral control..." diff --git a/src/controller/control.cpp b/src/controller/control.cpp index c6c33fe..7120018 100644 --- a/src/controller/control.cpp +++ b/src/controller/control.cpp @@ -12,38 +12,54 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include -#include #include -#include -#include +#include + #include -#include +#include #include #include +#include +#include -Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, size_t arm_motor_num, size_t hand_motor_num): - openarm_(arm), dynamics_l_(dynamics_l), dynamics_f_(dynamics_f), robot_state_(robot_state), Ts_(Ts), role_(role), arm_motor_num_(arm_motor_num), hand_motor_num_(hand_motor_num) -{ - differentiator_ = new Differentiator(Ts); - openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); - openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); +Control::Control(openarm::can::socket::OpenArm* arm, Dynamics* dynamics_l, Dynamics* dynamics_f, + std::shared_ptr robot_state, double Ts, int role, + size_t arm_motor_num, size_t hand_motor_num) + : openarm_(arm), + dynamics_l_(dynamics_l), + dynamics_f_(dynamics_f), + robot_state_(robot_state), + Ts_(Ts), + role_(role), + arm_motor_num_(arm_motor_num), + hand_motor_num_(hand_motor_num) { + differentiator_ = new Differentiator(Ts); + openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); + openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); } -Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, std::string arm_type, size_t arm_motor_num, size_t hand_motor_num): - openarm_(arm), dynamics_l_(dynamics_l), dynamics_f_(dynamics_f), robot_state_(robot_state), Ts_(Ts), role_(role), arm_motor_num_(arm_motor_num), hand_motor_num_(hand_motor_num) -{ - differentiator_ = new Differentiator(Ts); - openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); - openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); +Control::Control(openarm::can::socket::OpenArm* arm, Dynamics* dynamics_l, Dynamics* dynamics_f, + std::shared_ptr robot_state, double Ts, int role, + std::string arm_type, size_t arm_motor_num, size_t hand_motor_num) + : openarm_(arm), + dynamics_l_(dynamics_l), + dynamics_f_(dynamics_f), + robot_state_(robot_state), + Ts_(Ts), + role_(role), + arm_motor_num_(arm_motor_num), + hand_motor_num_(hand_motor_num) { + differentiator_ = new Differentiator(Ts); + openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); + openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); - arm_type_ = arm_type; + arm_type_ = arm_type; } Control::~Control() { - std::cout << "Control destructed " << std::endl; - delete openarmjointconverter_; - delete differentiator_; + std::cout << "Control destructed " << std::endl; + delete openarmjointconverter_; + delete differentiator_; } // bool Control::Setup(void) @@ -57,137 +73,237 @@ Control::~Control() { // return true; // } -void Control::Shutdown(void){ - std::cout << "control shutdown !!!" << std::endl; +void Control::Shutdown(void) { + std::cout << "control shutdown !!!" << std::endl; - openarm_->disable_all(); + openarm_->disable_all(); } -void Control::SetParameter( - const std::vector& Kp, - const std::vector& Kd, - const std::vector& Fc, - const std::vector& k, - const std::vector& Fv, - const std::vector& Fo) - { - Kp_ = Kp; - Kd_ = Kd; - Fc_ = Fc; - k_ = k; - Fv_ = Fv; - Fo_ = Fo; - } +void Control::SetParameter(const std::vector& Kp, const std::vector& Kd, + const std::vector& Fc, const std::vector& k, + const std::vector& Fv, const std::vector& Fo) { + Kp_ = Kp; + Kd_ = Kd; + Fc_ = Fc; + k_ = k; + Fv_ = Fv; + Fo_ = Fo; +} +bool Control::bilateral_step() { + // get motor status + std::vector arm_motor_states; + const auto& arm_motors = openarm_->get_arm().get_motors(); + for (size_t i = 0; i < arm_motors.size(); ++i) { + const auto& motor = arm_motors[i]; + arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0}); + } -bool Control::bilateral_step() -{ - // get motor status - std::vector arm_motor_states; - const auto& arm_motors = openarm_->get_arm().get_motors(); - for (size_t i = 0; i < arm_motors.size(); ++i) { - const auto& motor = arm_motors[i]; - arm_motor_states.push_back({ - motor.get_position(), - motor.get_velocity(), - 0 - }); - } + std::vector gripper_motor_states; + const auto& gripper_motors = openarm_->get_gripper().get_motors(); + for (size_t i = 0; i < gripper_motors.size(); ++i) { + const auto& motor = gripper_motors[i]; + gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0}); + } - std::vector gripper_motor_states; - const auto& gripper_motors = openarm_->get_gripper().get_motors(); - for (size_t i = 0; i < gripper_motors.size(); ++i) { - const auto& motor = gripper_motors[i]; - gripper_motor_states.push_back({ - motor.get_position(), - motor.get_velocity(), - 0 - }); - } + // convert joint to motor + std::vector joint_arm_states = + openarmjointconverter_->motor_to_joint(arm_motor_states); + std::vector joint_gripper_states = + openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); - // convert joint to motor - std::vector joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states); - std::vector joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); + // set reponse + robot_state_->arm_state().set_all_responses(joint_arm_states); + robot_state_->hand_state().set_all_responses(joint_gripper_states); - // set reponse - robot_state_->arm_state().set_all_responses(joint_arm_states); - robot_state_->hand_state().set_all_responses(joint_gripper_states); + size_t arm_dof = robot_state_->arm_state().get_size(); + size_t gripper_dof = robot_state_->hand_state().get_size(); - size_t arm_dof = robot_state_->arm_state().get_size(); - size_t gripper_dof = robot_state_->hand_state().get_size(); + std::vector joint_arm_positions(arm_dof, 0.0); + std::vector joint_arm_velocities(arm_dof, 0.0); + std::vector joint_arm_efforts(arm_dof, 0.0); - std::vector joint_arm_positions(arm_dof, 0.0); - std::vector joint_arm_velocities(arm_dof, 0.0); - std::vector joint_arm_efforts(arm_dof, 0.0); + std::vector joint_gripper_positions(gripper_dof, 0.0); + std::vector joint_gripper_velocities(gripper_dof, 0.0); + std::vector joint_gripper_efforts(gripper_dof, 0.0); - std::vector joint_gripper_positions(gripper_dof, 0.0); - std::vector joint_gripper_velocities(gripper_dof, 0.0); - std::vector joint_gripper_efforts(gripper_dof, 0.0); + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_positions[i] = joint_arm_states[i].position; + joint_arm_velocities[i] = joint_arm_states[i].velocity; + } - for (size_t i = 0; i < arm_dof; ++i) { - joint_arm_positions[i] = joint_arm_states[i].position; - joint_arm_velocities[i] = joint_arm_states[i].velocity; - } + for (size_t i = 0; i < gripper_dof; ++i) { + joint_gripper_positions[i] = joint_gripper_states[i].position; + joint_gripper_velocities[i] = joint_gripper_states[i].velocity; + } - for (size_t i = 0; i < gripper_dof; ++i) { - joint_gripper_positions[i] = joint_gripper_states[i].position; - joint_gripper_velocities[i] = joint_gripper_states[i].velocity; - } + std::vector gravity(arm_dof, 0.0); + std::vector coriolis(arm_dof, 0.0); + std::vector friction(arm_dof + gripper_dof, 0.0); - std::vector gravity(arm_dof, 0.0); - std::vector coriolis(arm_dof, 0.0); - std::vector friction(arm_dof + gripper_dof, 0.0); + std::vector joint_arm_states_ref = robot_state_->arm_state().get_all_references(); + std::vector joint_gripper_states_ref = + robot_state_->hand_state().get_all_references(); - std::vector joint_arm_states_ref = robot_state_->arm_state().get_all_references(); - std::vector joint_gripper_states_ref = robot_state_->hand_state().get_all_references(); + std::vector joint_arm_positions_ref(arm_dof); - std::vector joint_arm_positions_ref(arm_dof); + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_positions_ref[i] = joint_arm_states_ref[i].position; + } - for (size_t i = 0; i < arm_dof; ++i) { - joint_arm_positions_ref[i] = joint_arm_states_ref[i].position; - } + if (role_ == ROLE_LEADER) { + dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data()); + dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), + coriolis.data()); + } else if (role_ == ROLE_FOLLOWER) { + dynamics_f_->GetGravity(joint_arm_positions.data(), gravity.data()); + dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), + coriolis.data()); + } - if (role_ == ROLE_LEADER) { - dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data()); - dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); + // Friction (compute joint friction) + for (size_t i = 0; i < joint_arm_velocities.size(); ++i) + ComputeFriction(joint_arm_velocities.data(), friction.data(), i); + for (size_t i = 0; i < joint_gripper_velocities.size(); ++i) + ComputeFriction(joint_gripper_velocities.data(), friction.data(), + joint_arm_velocities.size() + i); - } else if (role_ == ROLE_FOLLOWER) { - dynamics_f_->GetGravity(joint_arm_positions.data(), gravity.data()); - dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); - } + // set gravity and friciton comp joint torque value + for (size_t i = 0; i < arm_dof; i++) { + joint_arm_states_ref[i].effort = gravity[i] + friction[i]; + } + for (size_t i = 0; i < gripper_dof; i++) { + joint_gripper_states_ref[i].effort = friction[i + arm_dof]; + } + + std::vector motor_arm_states = + openarmjointconverter_->joint_to_motor(joint_arm_states_ref); + std::vector motor_gripper_states = + openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref); + + // kp kd q dq tau + std::vector arm_cmds; + arm_cmds.reserve(arm_dof); + for (size_t i = 0; i < arm_dof; ++i) { + arm_cmds.emplace_back(openarm::damiao_motor::MITParam{ + Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity, + motor_arm_states[i].effort}); + } + + // gripper command mit param + std::vector gripper_cmds; + gripper_cmds.reserve(gripper_dof); + for (size_t i = 0; i < gripper_dof; ++i) { + gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{ + Kp_[i + arm_dof], Kd_[i + arm_dof], motor_gripper_states[i].position, + motor_gripper_states[i].velocity, motor_gripper_states[i].effort}); + } + + // send command to arm + openarm_->get_arm().mit_control_all(arm_cmds); + // send command to gripper + openarm_->get_gripper().mit_control_all(gripper_cmds); + + std::this_thread::sleep_for(std::chrono::microseconds(200)); + + openarm_->recv_all(220); + + return true; +} + +bool Control::unilateral_step() { + // get motor status + std::vector arm_motor_states; + for (const auto& motor : openarm_->get_arm().get_motors()) { + arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector gripper_motor_states; + for (const auto& motor : openarm_->get_gripper().get_motors()) { + gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + // convert joint to motor + std::vector joint_arm_states = + openarmjointconverter_->motor_to_joint(arm_motor_states); + std::vector joint_gripper_states = + openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); + + // set reponse + robot_state_->arm_state().set_all_responses(joint_arm_states); + robot_state_->hand_state().set_all_responses(joint_gripper_states); + + size_t arm_dof = robot_state_->arm_state().get_size(); + size_t gripper_dof = robot_state_->hand_state().get_size(); + + std::vector joint_arm_positions(arm_dof, 0.0); + std::vector joint_arm_velocities(arm_dof, 0.0); + std::vector joint_gripper_positions(gripper_dof, 0.0); + std::vector joint_gripper_velocities(gripper_dof, 0.0); + + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_positions[i] = joint_arm_states[i].position; + joint_arm_velocities[i] = joint_arm_states[i].velocity; + } + + for (size_t i = 0; i < gripper_dof; ++i) { + joint_gripper_positions[i] = joint_gripper_states[i].position; + joint_gripper_velocities[i] = joint_gripper_states[i].velocity; + } + + std::vector gravity(arm_dof, 0.0); + std::vector coriolis(arm_dof, 0.0); + std::vector friction(arm_dof + gripper_dof, 0.0); + + if (role_ == ROLE_LEADER) { + // calc dynamics + dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data()); + dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), + coriolis.data()); - // Friction (compute joint friction) for (size_t i = 0; i < joint_arm_velocities.size(); ++i) - ComputeFriction(joint_arm_velocities.data(), friction.data(), i); + ComputeFriction(joint_arm_velocities.data(), friction.data(), i); + for (size_t i = 0; i < joint_gripper_velocities.size(); ++i) - ComputeFriction(joint_gripper_velocities.data(), friction.data(), joint_arm_velocities.size() + i); + ComputeFriction(joint_gripper_velocities.data(), friction.data(), arm_dof + i); - // set gravity and friciton comp joint torque value - for(size_t i = 0 ; i < arm_dof; i++){ - joint_arm_states_ref[i].effort = gravity[i] + friction[i]; + // arm joint state + std::vector joint_arm_state_torque(arm_dof); + for (size_t i = 0; i < arm_dof; ++i) { + joint_arm_state_torque[i].position = joint_arm_positions[i]; + joint_arm_state_torque[i].velocity = joint_arm_velocities[i]; + joint_arm_state_torque[i].effort = gravity[i] + friction[i] * 0.3 + coriolis[i] * 0.1; } - for(size_t i = 0 ; i < gripper_dof; i++){ - joint_gripper_states_ref[i].effort = friction[i + arm_dof]; + // gripper joint state + std::vector joint_gripper_state_torque(gripper_dof); + for (size_t i = 0; i < gripper_dof; ++i) { + joint_gripper_state_torque[i].position = joint_gripper_positions[i]; + joint_gripper_state_torque[i].velocity = joint_gripper_velocities[i]; + joint_gripper_state_torque[i].effort = friction[arm_dof + i] * 0.3; } - std::vector motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_states_ref); - std::vector motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref); + std::vector motor_arm_states = + openarmjointconverter_->joint_to_motor(joint_arm_state_torque); + std::vector motor_gripper_states = + openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque); - // kp kd q dq tau + // arm command mit param std::vector arm_cmds; arm_cmds.reserve(arm_dof); for (size_t i = 0; i < arm_dof; ++i) { - arm_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity, motor_arm_states[i].effort}); + arm_cmds.emplace_back( + openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort}); } // gripper command mit param std::vector gripper_cmds; gripper_cmds.reserve(gripper_dof); for (size_t i = 0; i < gripper_dof; ++i) { - gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i + arm_dof],Kd_[i + arm_dof], motor_gripper_states[i].position, motor_gripper_states[i].velocity, motor_gripper_states[i].effort}); + gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{ + 0.0, 0.0, 0.0, 0.0, motor_gripper_states[i].effort}); } // send command to arm @@ -195,316 +311,200 @@ bool Control::bilateral_step() // send command to gripper openarm_->get_gripper().mit_control_all(gripper_cmds); - std::this_thread::sleep_for(std::chrono::microseconds(200)); - - openarm_->recv_all(220); + openarm_->recv_all(200); return true; + } + + else if (role_ == ROLE_FOLLOWER) { + std::vector joint_arm_states_ref = + robot_state_->arm_state().get_all_references(); + std::vector joint_hand_states_ref = + robot_state_->hand_state().get_all_references(); + + // Joint → Motor + std::vector arm_motor_refs = + openarmjointconverter_->joint_to_motor(joint_arm_states_ref); + std::vector hand_motor_refs = + openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref); + + std::vector arm_cmds; + arm_cmds.reserve(arm_motor_refs.size()); + for (size_t i = 0; i < arm_motor_refs.size(); ++i) { + arm_cmds.emplace_back(openarm::damiao_motor::MITParam{ + Kp_[i], Kd_[i], arm_motor_refs[i].position, arm_motor_refs[i].velocity, 0.0}); } - - bool Control::unilateral_step(){ - - // get motor status - std::vector arm_motor_states; - for (const auto& motor : openarm_->get_arm().get_motors()) { - arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); - } - - std::vector gripper_motor_states; - for (const auto& motor : openarm_->get_gripper().get_motors()) { - gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); - } - - // convert joint to motor - std::vector joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states); - std::vector joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); - - // set reponse - robot_state_->arm_state().set_all_responses(joint_arm_states); - robot_state_->hand_state().set_all_responses(joint_gripper_states); - - size_t arm_dof = robot_state_->arm_state().get_size(); - size_t gripper_dof = robot_state_->hand_state().get_size(); - - std::vector joint_arm_positions(arm_dof, 0.0); - std::vector joint_arm_velocities(arm_dof, 0.0); - std::vector joint_gripper_positions(gripper_dof, 0.0); - std::vector joint_gripper_velocities(gripper_dof, 0.0); - - for (size_t i = 0; i < arm_dof; ++i) { - joint_arm_positions[i] = joint_arm_states[i].position; - joint_arm_velocities[i] = joint_arm_states[i].velocity; - } - - for (size_t i = 0; i < gripper_dof; ++i) { - joint_gripper_positions[i] = joint_gripper_states[i].position; - joint_gripper_velocities[i] = joint_gripper_states[i].velocity; - } - - std::vector gravity(arm_dof, 0.0); - std::vector coriolis(arm_dof, 0.0); - std::vector friction(arm_dof + gripper_dof, 0.0); - - if (role_ == ROLE_LEADER) { - - // calc dynamics - dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data()); - dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); - - for (size_t i = 0; i < joint_arm_velocities.size(); ++i) - ComputeFriction(joint_arm_velocities.data(), friction.data(), i); - - for (size_t i = 0; i < joint_gripper_velocities.size(); ++i) - ComputeFriction(joint_gripper_velocities.data(), friction.data(), arm_dof + i); - - - // arm joint state - std::vector joint_arm_state_torque(arm_dof); - for (size_t i = 0; i < arm_dof; ++i) { - joint_arm_state_torque[i].position = joint_arm_positions[i]; - joint_arm_state_torque[i].velocity = joint_arm_velocities[i]; - joint_arm_state_torque[i].effort = gravity[i] + friction[i]*0.3 + coriolis[i]*0.1; - } - - // gripper joint state - std::vector joint_gripper_state_torque(gripper_dof); - for (size_t i = 0; i < gripper_dof; ++i) { - joint_gripper_state_torque[i].position = joint_gripper_positions[i]; - joint_gripper_state_torque[i].velocity = joint_gripper_velocities[i]; - joint_gripper_state_torque[i].effort = friction[arm_dof + i]*0.3 ; - } - - std::vector motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_state_torque); - std::vector motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque); - - // arm command mit param - std::vector arm_cmds; - arm_cmds.reserve(arm_dof); - for (size_t i = 0; i < arm_dof; ++i) { - arm_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort}); - } - - // gripper command mit param - std::vector gripper_cmds; - gripper_cmds.reserve(gripper_dof); - for (size_t i = 0; i < gripper_dof; ++i) { - gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_gripper_states[i].effort}); - } - - // send command to arm - openarm_->get_arm().mit_control_all(arm_cmds); - // send command to gripper - openarm_->get_gripper().mit_control_all(gripper_cmds); - - openarm_->recv_all(200); - - return true; - - } - - else if (role_ == ROLE_FOLLOWER) { - - std::vector joint_arm_states_ref = robot_state_->arm_state().get_all_references(); - std::vector joint_hand_states_ref = robot_state_->hand_state().get_all_references(); - - // Joint → Motor - std::vector arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_states_ref); - std::vector hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref); - - std::vector arm_cmds; - arm_cmds.reserve(arm_motor_refs.size()); - for (size_t i = 0; i < arm_motor_refs.size(); ++i) { - arm_cmds.emplace_back(openarm::damiao_motor::MITParam{ - Kp_[i], - Kd_[i], - arm_motor_refs[i].position, - arm_motor_refs[i].velocity, - 0.0 - }); - } - - std::vector hand_cmds; - hand_cmds.reserve(hand_motor_refs.size()); - for (size_t i = 0; i < hand_motor_refs.size(); ++i) { - hand_cmds.emplace_back(openarm::damiao_motor::MITParam{ - Kp_[i + arm_dof], - Kd_[i + arm_dof], - hand_motor_refs[i].position, - hand_motor_refs[i].velocity, - 0.0 - }); - } - - openarm_->get_arm().mit_control_all(arm_cmds); - openarm_->get_gripper().mit_control_all(hand_cmds); - - openarm_->recv_all(200); - - return true; - } - - return true; - + std::vector hand_cmds; + hand_cmds.reserve(hand_motor_refs.size()); + for (size_t i = 0; i < hand_motor_refs.size(); ++i) { + hand_cmds.emplace_back(openarm::damiao_motor::MITParam{ + Kp_[i + arm_dof], Kd_[i + arm_dof], hand_motor_refs[i].position, + hand_motor_refs[i].velocity, 0.0}); } - void Control::ComputeFriction(const double* velocity, double* friction, size_t index) - { - if (TANHFRIC) { - const double amp_tmp = 1.0; - const double coef_tmp = 0.1; + openarm_->get_arm().mit_control_all(arm_cmds); + openarm_->get_gripper().mit_control_all(hand_cmds); - const double v = velocity[index]; - const double Fc = Fc_.at(index); - const double k = k_.at(index); - const double Fv = Fv_.at(index); - const double Fo = Fo_.at(index); + openarm_->recv_all(200); - friction[index] = amp_tmp * Fc * std::tanh(coef_tmp * k * v) + Fv * v + Fo; - } else { - friction[index] = velocity[index] * Dn_.at(index); - } + return true; + } + + return true; +} + +void Control::ComputeFriction(const double* velocity, double* friction, size_t index) { + if (TANHFRIC) { + const double amp_tmp = 1.0; + const double coef_tmp = 0.1; + + const double v = velocity[index]; + const double Fc = Fc_.at(index); + const double k = k_.at(index); + const double Fv = Fv_.at(index); + const double Fo = Fo_.at(index); + + friction[index] = amp_tmp * Fc * std::tanh(coef_tmp * k * v) + Fv * v + Fo; + } else { + friction[index] = velocity[index] * Dn_.at(index); + } +} + +bool Control::AdjustPosition(void) { + int nstep = 220; + double alpha; + + std::vector arm_motor_states; + for (const auto& motor : openarm_->get_arm().get_motors()) { + arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector gripper_motor_states; + for (const auto& motor : openarm_->get_gripper().get_motors()) { + gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } + + std::vector joint_arm_now = + openarmjointconverter_->motor_to_joint(arm_motor_states); + std::vector joint_hand_now = + openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); + + std::vector joint_arm_goal(NMOTORS - 1); + for (size_t i = 0; i < NMOTORS - 1; ++i) { + joint_arm_goal[i].position = INITIAL_POSITION[i]; + joint_arm_goal[i].velocity = 0.0; + joint_arm_goal[i].effort = 0.0; + } + + std::vector joint_hand_goal(joint_hand_now.size()); + for (size_t i = 0; i < joint_hand_goal.size(); ++i) { + joint_hand_goal[i].position = 0.0; + joint_hand_goal[i].velocity = 0.0; + joint_hand_goal[i].effort = 0.0; + } + + std::vector kp_arm_temp = {50, 50.0, 50.0, 50.0, 10.0, 10.0, 10.0}; + std::vector kd_arm_temp = {1.2, 1.2, 1.2, 1.2, 0.3, 0.2, 0.3}; + + std::vector kp_hand_temp = {10.0}; + std::vector kd_hand_temp = {0.5}; + + for (int step = 0; step < nstep; ++step) { + alpha = static_cast(step + 1) / nstep; + + std::vector joint_arm_interp(NMOTORS - 1); + for (size_t i = 0; i < NMOTORS - 1; ++i) { + joint_arm_interp[i].position = + joint_arm_goal[i].position * alpha + joint_arm_now[i].position * (1.0 - alpha); + joint_arm_interp[i].velocity = 0.0; } - bool Control::AdjustPosition(void) - { - int nstep = 220; - double alpha; - - std::vector arm_motor_states; - for (const auto& motor : openarm_->get_arm().get_motors()) { - arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); - } - - std::vector gripper_motor_states; - for (const auto& motor : openarm_->get_gripper().get_motors()) { - gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0}); - } - - std::vector joint_arm_now = openarmjointconverter_->motor_to_joint(arm_motor_states); - std::vector joint_hand_now = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); - - std::vector joint_arm_goal(NMOTORS-1); - for (size_t i = 0; i < NMOTORS-1; ++i) { - joint_arm_goal[i].position = INITIAL_POSITION[i]; - joint_arm_goal[i].velocity = 0.0; - joint_arm_goal[i].effort = 0.0; - } - - std::vector joint_hand_goal(joint_hand_now.size()); - for (size_t i = 0; i < joint_hand_goal.size(); ++i) { - joint_hand_goal[i].position = 0.0; - joint_hand_goal[i].velocity = 0.0; - joint_hand_goal[i].effort = 0.0; - } - - std::vector kp_arm_temp = {50, 50.0, 50.0, 50.0, 10.0, 10.0, 10.0}; - std::vector kd_arm_temp = {1.2, 1.2, 1.2, 1.2, 0.3, 0.2, 0.3}; - - std::vector kp_hand_temp = {10.0}; - std::vector kd_hand_temp = {0.5}; - - for (int step = 0; step < nstep; ++step) { - alpha = static_cast(step + 1) / nstep; - - std::vector joint_arm_interp(NMOTORS-1); - for (size_t i = 0; i < NMOTORS-1; ++i) { - joint_arm_interp[i].position = joint_arm_goal[i].position * alpha + joint_arm_now[i].position * (1.0 - alpha); - joint_arm_interp[i].velocity = 0.0; - } - - std::vector joint_hand_interp(joint_hand_goal.size()); - for (size_t i = 0; i < joint_hand_interp.size(); ++i) { - joint_hand_interp[i].position = joint_hand_goal[i].position * alpha + joint_hand_now[i].position * (1.0 - alpha); - joint_hand_interp[i].velocity = 0.0; - } - - std::vector arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_interp); - std::vector hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_interp); - - std::vector arm_cmds; - arm_cmds.reserve(arm_motor_refs.size()); - for (size_t i = 0; i < arm_motor_refs.size(); ++i) { - arm_cmds.emplace_back(openarm::damiao_motor::MITParam{ - kp_arm_temp[i], - kd_arm_temp[i], - arm_motor_refs[i].position, - arm_motor_refs[i].velocity, - 0.0 - }); - } - - std::vector hand_cmds; - hand_cmds.reserve(hand_motor_refs.size()); - for (size_t i = 0; i < hand_motor_refs.size(); ++i) { - hand_cmds.emplace_back(openarm::damiao_motor::MITParam{ - kp_hand_temp[i], - kd_hand_temp[i], - hand_motor_refs[i].position, - hand_motor_refs[i].velocity, - 0.0 - }); - } - - openarm_->get_arm().mit_control_all(arm_cmds); - openarm_->get_gripper().mit_control_all(hand_cmds); - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - openarm_->recv_all(); - } - - std::vector arm_motor_states_final; - for (const auto& motor : openarm_->get_arm().get_motors()) { - arm_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0}); - } - - std::vector gripper_motor_states_final; - for (const auto& motor : openarm_->get_gripper().get_motors()) { - gripper_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0}); - } - - std::vector joint_arm_final = openarmjointconverter_->motor_to_joint(arm_motor_states_final); - std::vector joint_hand_final = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final); - - robot_state_->arm_state().set_all_references(joint_arm_final); - robot_state_->hand_state().set_all_references(joint_hand_final); - - return true; + std::vector joint_hand_interp(joint_hand_goal.size()); + for (size_t i = 0; i < joint_hand_interp.size(); ++i) { + joint_hand_interp[i].position = + joint_hand_goal[i].position * alpha + joint_hand_now[i].position * (1.0 - alpha); + joint_hand_interp[i].velocity = 0.0; } - bool Control::DetectVibration(const double* velocity, bool* what_axis) - { - bool vibration_detected = false; + std::vector arm_motor_refs = + openarmjointconverter_->joint_to_motor(joint_arm_interp); + std::vector hand_motor_refs = + openarmgripperjointconverter_->joint_to_motor(joint_hand_interp); - for (int i = 0; i < NJOINTS; ++i) { - what_axis[i] = false; + std::vector arm_cmds; + arm_cmds.reserve(arm_motor_refs.size()); + for (size_t i = 0; i < arm_motor_refs.size(); ++i) { + arm_cmds.emplace_back(openarm::damiao_motor::MITParam{kp_arm_temp[i], kd_arm_temp[i], + arm_motor_refs[i].position, + arm_motor_refs[i].velocity, 0.0}); + } - velocity_buffer_[i].push_back(velocity[i]); - if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE) - velocity_buffer_[i].pop_front(); + std::vector hand_cmds; + hand_cmds.reserve(hand_motor_refs.size()); + for (size_t i = 0; i < hand_motor_refs.size(); ++i) { + hand_cmds.emplace_back(openarm::damiao_motor::MITParam{ + kp_hand_temp[i], kd_hand_temp[i], hand_motor_refs[i].position, + hand_motor_refs[i].velocity, 0.0}); + } - if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE) - continue; + openarm_->get_arm().mit_control_all(arm_cmds); + openarm_->get_gripper().mit_control_all(hand_cmds); - double mean = std::accumulate( - velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0 - ) / velocity_buffer_[i].size(); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); - double var = 0.0; - for (double v : velocity_buffer_[i]) { - var += (v - mean) * (v - mean); - } + openarm_->recv_all(); + } - double stddev = std::sqrt(var / velocity_buffer_[i].size()); + std::vector arm_motor_states_final; + for (const auto& motor : openarm_->get_arm().get_motors()) { + arm_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } - if (stddev > VIB_THRESHOLD) { - what_axis[i] = true; - vibration_detected = true; - std::cout << "[VIBRATION] Joint " << i << " stddev: " << stddev << std::endl; - } - } + std::vector gripper_motor_states_final; + for (const auto& motor : openarm_->get_gripper().get_motors()) { + gripper_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0}); + } - return vibration_detected; - } \ No newline at end of file + std::vector joint_arm_final = + openarmjointconverter_->motor_to_joint(arm_motor_states_final); + std::vector joint_hand_final = + openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final); + + robot_state_->arm_state().set_all_references(joint_arm_final); + robot_state_->hand_state().set_all_references(joint_hand_final); + + return true; +} + +bool Control::DetectVibration(const double* velocity, bool* what_axis) { + bool vibration_detected = false; + + for (int i = 0; i < NJOINTS; ++i) { + what_axis[i] = false; + + velocity_buffer_[i].push_back(velocity[i]); + if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE) velocity_buffer_[i].pop_front(); + + if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE) continue; + + double mean = std::accumulate(velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0) / + velocity_buffer_[i].size(); + + double var = 0.0; + for (double v : velocity_buffer_[i]) { + var += (v - mean) * (v - mean); + } + + double stddev = std::sqrt(var / velocity_buffer_[i].size()); + + if (stddev > VIB_THRESHOLD) { + what_axis[i] = true; + vibration_detected = true; + std::cout << "[VIBRATION] Joint " << i << " stddev: " << stddev << std::endl; + } + } + + return vibration_detected; +} \ No newline at end of file diff --git a/src/controller/control.hpp b/src/controller/control.hpp index ed283c8..79bdb02 100644 --- a/src/controller/control.hpp +++ b/src/controller/control.hpp @@ -15,90 +15,87 @@ #pragma once // #include -#include -#include -#include -#include -#include #include #include -#include +#include +#include #include -#include -#include +#include +#include #include #include +#include +#include +#include +class Control { + openarm::can::socket::OpenArm *openarm_; -class Control -{ - openarm::can::socket::OpenArm* openarm_; + double Ts_; + int role_; - double Ts_; - int role_; + size_t arm_motor_num_; + size_t hand_motor_num_; - size_t arm_motor_num_; - size_t hand_motor_num_; + Differentiator *differentiator_; + OpenArmJointConverter *openarmjointconverter_; + OpenArmJGripperJointConverter *openarmgripperjointconverter_; - Differentiator *differentiator_; - OpenArmJointConverter *openarmjointconverter_; - OpenArmJGripperJointConverter *openarmgripperjointconverter_; + std::shared_ptr robot_state_; - std::shared_ptr robot_state_; + std::string arm_type_; - std::string arm_type_; + Dynamics *dynamics_f_; + Dynamics *dynamics_l_; - Dynamics *dynamics_f_; - Dynamics *dynamics_l_; + double oblique_coordinates_force; + double oblique_coordinates_position; - double oblique_coordinates_force; - double oblique_coordinates_position; + // for easy logging + // std::vector> velocity_log_; // (differ_velocity, motor_velocity) + // std::string log_file_path_ = "../data/velocity_comparison.csv"; + static constexpr int VEL_WINDOW_SIZE = 10; + static constexpr double VIB_THRESHOLD = 0.7; // [rad/s] + std::deque velocity_buffer_[NJOINTS]; - // for easy logging - // std::vector> velocity_log_; // (differ_velocity, motor_velocity) - // std::string log_file_path_ = "../data/velocity_comparison.csv"; - static constexpr int VEL_WINDOW_SIZE = 10; - static constexpr double VIB_THRESHOLD = 0.7; // [rad/s] - std::deque velocity_buffer_[NJOINTS]; +public: + Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, + std::shared_ptr robot_state, double Ts, int role, + size_t arm_joint_num, size_t hand_motor_num); + Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, + std::shared_ptr robot_state, double Ts, int role, + std::string arm_type, size_t arm_joint_num, size_t hand_motor_num); + ~Control(); - public: - Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, size_t arm_joint_num, size_t hand_motor_num); - Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr robot_state, double Ts, int role, std::string arm_type, size_t arm_joint_num, size_t hand_motor_num); - ~Control(); + std::shared_ptr response_; + std::shared_ptr reference_; - std::shared_ptr response_; - std::shared_ptr reference_; + std::vector Dn_, Kp_, Kd_, Fc_, k_, Fv_, Fo_; - std::vector Dn_, Kp_, Kd_,Fc_, k_, Fv_, Fo_; + // bool Setup(void); + void Setstate(int state); + void Shutdown(void); - // bool Setup(void); - void Setstate(int state); - void Shutdown(void); + void SetParameter(const std::vector &Kp, const std::vector &Kd, + const std::vector &Fc, const std::vector &k, + const std::vector &Fv, const std::vector &Fo); - void SetParameter( - const std::vector& Kp, - const std::vector& Kd, - const std::vector& Fc, - const std::vector& k, - const std::vector& Fv, - const std::vector& Fo); + bool AdjustPosition(void); - bool AdjustPosition(void); + // Compute torque based on bilateral + bool bilateral_step(); + bool unilateral_step(); - // Compute torque based on bilateral - bool bilateral_step(); - bool unilateral_step(); + // NOTE! Control() class operates on "joints", while the underlying + // classes operates on "actuators". The following functions map + // joints to actuators. - // NOTE! Control() class operates on "joints", while the underlying - // classes operates on "actuators". The following functions map - // joints to actuators. + void ComputeJointPosition(const double *motor_position, double *joint_position); + void ComputeJointVelocity(const double *motor_velocity, double *joint_velocity); + void ComputeMotorTorque(const double *joint_torque, double *motor_torque); - void ComputeJointPosition(const double *motor_position, double *joint_position); - void ComputeJointVelocity(const double *motor_velocity, double *joint_velocity); - void ComputeMotorTorque(const double *joint_torque, double *motor_torque); - - // void ComputeFriction(const double *velocity, double *friction); - void ComputeFriction(const double* velocity, double* friction, size_t index); - void ComputeGravity(const double *position, double *gravity); - bool DetectVibration(const double* velocity, bool *what_axis); + // void ComputeFriction(const double *velocity, double *friction); + void ComputeFriction(const double *velocity, double *friction, size_t index); + void ComputeGravity(const double *position, double *gravity); + bool DetectVibration(const double *velocity, bool *what_axis); }; diff --git a/src/controller/diff.hpp b/src/controller/diff.hpp index 19f8550..b9b6f25 100644 --- a/src/controller/diff.hpp +++ b/src/controller/diff.hpp @@ -18,57 +18,52 @@ #include -class Differentiator -{ - private: - double Ts_; // Sampling time - double velocity_z1_[NMOTORS] = {0.0}; // Velocity (1 step before) - double position_z1_[NMOTORS] = {0.0}; // Position (1 step before) - double acc_z1_[NMOTORS] = {0.0}; - double acc_[NMOTORS] = {0.0}; - public: - Differentiator(double Ts) : Ts_(Ts) {} +class Differentiator { +private: + double Ts_; // Sampling time + double velocity_z1_[NMOTORS] = {0.0}; // Velocity (1 step before) + double position_z1_[NMOTORS] = {0.0}; // Position (1 step before) + double acc_z1_[NMOTORS] = {0.0}; + double acc_[NMOTORS] = {0.0}; - /* - * Compute the motor speed by taking the derivative of - * the motion. - */ - void Differentiate(const double *position, double *velocity) - { - double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY); - double b = a * CUTOFF_FREQUENCY; +public: + Differentiator(double Ts) : Ts_(Ts) {} - for (int i = 0; i < NMOTORS; i++) { - if (position_z1_[i] == 0.0) { - position_z1_[i] = position[i]; - } + /* + * Compute the motor speed by taking the derivative of + * the motion. + */ + void Differentiate(const double *position, double *velocity) { + double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY); + double b = a * CUTOFF_FREQUENCY; - velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]); - position_z1_[i] = position[i]; - velocity_z1_[i] = velocity[i]; - } + for (int i = 0; i < NMOTORS; i++) { + if (position_z1_[i] == 0.0) { + position_z1_[i] = position[i]; + } - } + velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]); + position_z1_[i] = position[i]; + velocity_z1_[i] = velocity[i]; + } + } - void Differentiate_w_obs(const double *position, double *velocity, double *mass, double *input_torque) - { + void Differentiate_w_obs(const double *position, double *velocity, double *mass, + double *input_torque) { + double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY); + double b = a * CUTOFF_FREQUENCY; - double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY); - double b = a * CUTOFF_FREQUENCY; + for (int i = 0; i < NMOTORS; i++) { + if (position_z1_[i] == 0.0000000) { + position_z1_[i] = position[i]; + acc_z1_[i] = acc_[i]; + } - for (int i = 0; i < NMOTORS; i++) { - if (position_z1_[i] == 0.0000000) { - position_z1_[i] = position[i]; - acc_z1_[i] = acc_[i]; - } - - acc_[i] = acc_z1_[i] * a + b * (input_torque[i] / (mass[i])); - velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]) + acc_[i]; - position_z1_[i] = position[i]; - velocity_z1_[i] = velocity[i]; - acc_z1_[i] = acc_[i]; - - } - - } + acc_[i] = acc_z1_[i] * a + b * (input_torque[i] / (mass[i])); + velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]) + acc_[i]; + position_z1_[i] = position[i]; + velocity_z1_[i] = velocity[i]; + acc_z1_[i] = acc_[i]; + } + } }; diff --git a/src/controller/dynamics.cpp b/src/controller/dynamics.cpp index 45530b2..283f43c 100644 --- a/src/controller/dynamics.cpp +++ b/src/controller/dynamics.cpp @@ -14,18 +14,15 @@ #include -Dynamics::Dynamics(std::string urdf_path, std::string start_link, std::string end_link) -{ - this->urdf_path = urdf_path; - this->start_link = start_link; - this->end_link = end_link; +Dynamics::Dynamics(std::string urdf_path, std::string start_link, std::string end_link) { + this->urdf_path = urdf_path; + this->start_link = start_link; + this->end_link = end_link; } -Dynamics::~Dynamics(){} +Dynamics::~Dynamics() {} - -bool Dynamics::Init() -{ +bool Dynamics::Init() { std::ifstream file(urdf_path); if (!file.is_open()) { fprintf(stderr, "Failed to open URDF file: %s\n", urdf_path.c_str()); @@ -52,192 +49,181 @@ bool Dynamics::Init() return false; } - std::cout << "[GetGravity] kdl_chain.getNrOfJoints() = " << kdl_chain.getNrOfJoints() << std::endl; + std::cout << "[GetGravity] kdl_chain.getNrOfJoints() = " << kdl_chain.getNrOfJoints() + << std::endl; coriolis_forces.resize(kdl_chain.getNrOfJoints()); gravity_forces.resize(kdl_chain.getNrOfJoints()); inertia_matrix.resize(kdl_chain.getNrOfJoints()); - coriolis_forces.data.setZero(); gravity_forces.data.setZero(); inertia_matrix.data.setZero(); - solver = std::make_unique( - kdl_chain, KDL::Vector(0, 0.0, -9.81)); + solver = std::make_unique(kdl_chain, KDL::Vector(0, 0.0, -9.81)); return true; } -void Dynamics::GetGravity(const double *motor_position, double *gravity) -{ +void Dynamics::GetGravity(const double *motor_position, double *gravity) { + const auto njoints = kdl_chain.getNrOfJoints(); - const auto njoints = kdl_chain.getNrOfJoints(); + KDL::JntArray q_(kdl_chain.getNrOfJoints()); - KDL::JntArray q_(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + q_(i) = motor_position[i]; + } - for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { - q_(i) = motor_position[i]; - } - - solver->JntToGravity(q_, gravity_forces); - for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { - gravity[i] = gravity_forces(i); - } + solver->JntToGravity(q_, gravity_forces); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + gravity[i] = gravity_forces(i); + } } -void Dynamics::GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis) { - KDL::JntArray q_(kdl_chain.getNrOfJoints()); - KDL::JntArray q_dot(kdl_chain.getNrOfJoints()); +void Dynamics::GetCoriolis(const double *motor_position, const double *motor_velocity, + double *coriolis) { + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + KDL::JntArray q_dot(kdl_chain.getNrOfJoints()); - for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { - q_(i) = motor_position[i]; - q_dot(i) = motor_velocity[i]; - } + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + q_(i) = motor_position[i]; + q_dot(i) = motor_velocity[i]; + } - solver->JntToCoriolis(q_, q_dot, coriolis_forces); + solver->JntToCoriolis(q_, q_dot, coriolis_forces); - for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { - coriolis[i] = coriolis_forces(i); - } + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + coriolis[i] = coriolis_forces(i); + } } -void Dynamics::GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag) -{ - KDL::JntArray q_(kdl_chain.getNrOfJoints()); - KDL::JntSpaceInertiaMatrix inertia_matrix(kdl_chain.getNrOfJoints()); - for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { - q_(i) = motor_position[i]; - } +void Dynamics::GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag) { + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + KDL::JntSpaceInertiaMatrix inertia_matrix(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + q_(i) = motor_position[i]; + } - solver->JntToMass(q_, inertia_matrix); - - for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { - inertia_diag[i] = inertia_matrix(i, i); - } + solver->JntToMass(q_, inertia_matrix); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { + inertia_diag[i] = inertia_matrix(i, i); + } } -void Dynamics::GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian) -{ - KDL::JntArray q_(kdl_chain.getNrOfJoints()); - for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { - q_(i) = motor_position[i]; +void Dynamics::GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian) { + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { + q_(i) = motor_position[i]; + } + + KDL::Jacobian kdl_jac(kdl_chain.getNrOfJoints()); + KDL::ChainJntToJacSolver jac_solver(kdl_chain); + jac_solver.JntToJac(q_, kdl_jac); + + jacobian = Eigen::MatrixXd(6, kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < 6; ++i) { + for (size_t j = 0; j < kdl_chain.getNrOfJoints(); ++j) { + jacobian(i, j) = kdl_jac(i, j); } - - KDL::Jacobian kdl_jac(kdl_chain.getNrOfJoints()); - KDL::ChainJntToJacSolver jac_solver(kdl_chain); - jac_solver.JntToJac(q_, kdl_jac); - - jacobian = Eigen::MatrixXd(6, kdl_chain.getNrOfJoints()); - for (size_t i = 0; i < 6; ++i) { - for (size_t j = 0; j < kdl_chain.getNrOfJoints(); ++j) { - jacobian(i, j) = kdl_jac(i, j); - } - } - + } } -void Dynamics::GetNullSpace(const double* motor_position, Eigen::MatrixXd& nullspace) { - const size_t dof = kdl_chain.getNrOfJoints(); +void Dynamics::GetNullSpace(const double *motor_position, Eigen::MatrixXd &nullspace) { + const size_t dof = kdl_chain.getNrOfJoints(); - bool use_stable_svd = false; + bool use_stable_svd = false; - Eigen::MatrixXd J; - GetJacobian(motor_position, J); + Eigen::MatrixXd J; + GetJacobian(motor_position, J); - Eigen::MatrixXd J_pinv; + Eigen::MatrixXd J_pinv; - if (use_stable_svd) { - Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); - double tol = 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff(); - Eigen::VectorXd singularValuesInv = svd.singularValues(); - for (int i = 0; i < singularValuesInv.size(); ++i) { - singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0; - } - J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); - } else { - J_pinv = J.transpose() * (J * J.transpose()).inverse(); + if (use_stable_svd) { + Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); + double tol = + 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff(); + Eigen::VectorXd singularValuesInv = svd.singularValues(); + for (int i = 0; i < singularValuesInv.size(); ++i) { + singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0; } + J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); + } else { + J_pinv = J.transpose() * (J * J.transpose()).inverse(); + } - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); - nullspace = I - J_pinv * J; + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); + nullspace = I - J_pinv * J; - // std::cout << "[INFO] Null space projector computed.\n"; + // std::cout << "[INFO] Null space projector computed.\n"; } +void Dynamics::GetNullSpaceTauSpace(const double *motor_position, Eigen::MatrixXd &nullspace_T) { + const size_t dof = kdl_chain.getNrOfJoints(); + bool use_stable_svd = false; -void Dynamics::GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixXd& nullspace_T) -{ - const size_t dof = kdl_chain.getNrOfJoints(); - bool use_stable_svd = false; + Eigen::MatrixXd J; + GetJacobian(motor_position, J); - Eigen::MatrixXd J; - GetJacobian(motor_position, J); + Eigen::MatrixXd J_pinv; - Eigen::MatrixXd J_pinv; - - if (use_stable_svd) { - Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); - double tol = 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff(); - Eigen::VectorXd singularValuesInv = svd.singularValues(); - for (int i = 0; i < singularValuesInv.size(); ++i) { - singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0; - } - J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); - } else { - J_pinv = J.transpose() * (J * J.transpose()).inverse(); + if (use_stable_svd) { + Eigen::JacobiSVD svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); + double tol = + 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff(); + Eigen::VectorXd singularValuesInv = svd.singularValues(); + for (int i = 0; i < singularValuesInv.size(); ++i) { + singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0; } + J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose(); + } else { + J_pinv = J.transpose() * (J * J.transpose()).inverse(); + } - Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); - Eigen::MatrixXd N = I - J_pinv * J; + Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); + Eigen::MatrixXd N = I - J_pinv * J; - nullspace_T = N.transpose(); + nullspace_T = N.transpose(); } -void Dynamics::GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p) -{ - KDL::JntArray q_(kdl_chain.getNrOfJoints()); - for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { - q_(i) = motor_position[i]; - } +void Dynamics::GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, + Eigen::Vector3d &p) { + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { + q_(i) = motor_position[i]; + } - KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); - KDL::Frame kdl_frame; + KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); + KDL::Frame kdl_frame; - if (fk_solver.JntToCart(q_, kdl_frame) < 0) { - // std::cerr << "[KDL] FK failed in GetEECordinate!" << std::endl; - return; - } + if (fk_solver.JntToCart(q_, kdl_frame) < 0) { + // std::cerr << "[KDL] FK failed in GetEECordinate!" << std::endl; + return; + } - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - R(i, j) = kdl_frame.M(i, j); + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) R(i, j) = kdl_frame.M(i, j); - p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2]; + p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2]; } -void Dynamics::GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p) -{ - KDL::JntArray q_(kdl_chain.getNrOfJoints()); - for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { - q_(i) = motor_position[i]; - } +void Dynamics::GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, + Eigen::Vector3d &p) { + KDL::JntArray q_(kdl_chain.getNrOfJoints()); + for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { + q_(i) = motor_position[i]; + } - KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); - KDL::Frame kdl_frame; - - if (fk_solver.JntToCart(q_, kdl_frame, kdl_chain.getNrOfSegments() - 1) < 0) { - // std::cerr << "[KDL] FK failed in GetPreEECordinate!" << std::endl; - return; - } - - for (int i = 0; i < 3; ++i) - for (int j = 0; j < 3; ++j) - R(i, j) = kdl_frame.M(i, j); - - p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2]; + KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); + KDL::Frame kdl_frame; + if (fk_solver.JntToCart(q_, kdl_frame, kdl_chain.getNrOfSegments() - 1) < 0) { + // std::cerr << "[KDL] FK failed in GetPreEECordinate!" << std::endl; + return; + } + for (int i = 0; i < 3; ++i) + for (int j = 0; j < 3; ++j) R(i, j) = kdl_frame.M(i, j); + p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2]; } diff --git a/src/controller/dynamics.hpp b/src/controller/dynamics.hpp index 2f3779e..a9ac36d 100644 --- a/src/controller/dynamics.hpp +++ b/src/controller/dynamics.hpp @@ -13,60 +13,60 @@ // limitations under the License. #pragma once -#include #include +#include +#include + +#include +#include +#include #include #include -#include #include #include -#include -#include -#include -#include -#include +#include #include +#include /* * Compute gravity and inertia compensation using Orocos * Kinematics and Dynamics Library (KDL). */ -class Dynamics -{ - private: - std::shared_ptr urdf_model_interface; +class Dynamics { +private: + std::shared_ptr urdf_model_interface; - std::string urdf_path; - std::string start_link; - std::string end_link; + std::string urdf_path; + std::string start_link; + std::string end_link; - KDL::JntSpaceInertiaMatrix inertia_matrix; - KDL::JntArray q; - KDL::JntArray q_d; - KDL::JntArray coriolis_forces; - KDL::JntArray gravity_forces; + KDL::JntSpaceInertiaMatrix inertia_matrix; + KDL::JntArray q; + KDL::JntArray q_d; + KDL::JntArray coriolis_forces; + KDL::JntArray gravity_forces; - KDL::JntArray biasangle; + KDL::JntArray biasangle; - KDL::Tree kdl_tree; - KDL::Chain kdl_chain; - std::unique_ptr solver; + KDL::Tree kdl_tree; + KDL::Chain kdl_chain; + std::unique_ptr solver; - public: - Dynamics(std::string urdf_path, std::string start_link, std::string end_link); - ~Dynamics(); +public: + Dynamics(std::string urdf_path, std::string start_link, std::string end_link); + ~Dynamics(); - bool Init(); - void GetGravity(const double *motor_position, double *gravity); - void GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis); - void GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag); + bool Init(); + void GetGravity(const double *motor_position, double *gravity); + void GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis); + void GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag); - void GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian); + void GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian); - void GetNullSpace(const double *motor_positon, Eigen::MatrixXd &nullspace); + void GetNullSpace(const double *motor_positon, Eigen::MatrixXd &nullspace); - void GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixXd& nullspace_T); + void GetNullSpaceTauSpace(const double *motor_position, Eigen::MatrixXd &nullspace_T); - void GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); + void GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); - void GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); + void GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); }; diff --git a/src/joint_state_converter.hpp b/src/joint_state_converter.hpp index cce0cd0..1d5a01c 100644 --- a/src/joint_state_converter.hpp +++ b/src/joint_state_converter.hpp @@ -14,8 +14,8 @@ #pragma once -#include #include +#include // Represents the state of a single joint // struct JointState { @@ -28,7 +28,7 @@ struct MotorState { double position = 0.0; double velocity = 0.0; - double effort = 0.0; + double effort = 0.0; }; // Abstract base class for converting between motor and joint states @@ -37,71 +37,71 @@ public: virtual ~MotorJointConverter() = default; // MotorState vector → JointState vector - virtual std::vector motor_to_joint(const std::vector& motor_states) const = 0; + virtual std::vector motor_to_joint( + const std::vector& motor_states) const = 0; // JointState vector → MotorState vector - virtual std::vector joint_to_motor(const std::vector& joint_states) const = 0; + virtual std::vector joint_to_motor( + const std::vector& joint_states) const = 0; virtual size_t get_joint_count() const = 0; }; // assume motor num equals to joint num class OpenArmJointConverter : public MotorJointConverter { - public: - explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) { - std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl; - } - - std::vector motor_to_joint(const std::vector& m) const override { - // std::cout << "joint num conv : " << m.size() << std::endl; +public: + explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) { + std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl; + } - std::vector j(m.size()); - for (size_t i = 0; i < m.size(); ++i){ - j[i] = {m[i].position, m[i].velocity, m[i].effort}; + std::vector motor_to_joint(const std::vector& m) const override { + // std::cout << "joint num conv : " << m.size() << std::endl; - } - - return j; + std::vector j(m.size()); + for (size_t i = 0; i < m.size(); ++i) { + j[i] = {m[i].position, m[i].velocity, m[i].effort}; } - - std::vector joint_to_motor(const std::vector& j) const override { - std::vector m(j.size()); - for (size_t i = 0; i < j.size(); ++i) - m[i] = {j[i].position, j[i].velocity, j[i].effort}; - return m; + + return j; + } + + std::vector joint_to_motor(const std::vector& j) const override { + std::vector m(j.size()); + for (size_t i = 0; i < j.size(); ++i) m[i] = {j[i].position, j[i].velocity, j[i].effort}; + return m; + } + + size_t get_joint_count() const override { return joint_count_; } + +private: + size_t joint_count_; +}; + +// assume motor num equals to joint num +class OpenArmJGripperJointConverter : public MotorJointConverter { +public: + explicit OpenArmJGripperJointConverter(size_t joint_count) : joint_count_(joint_count) { + std::cout << "Gripper joint converter joint_count is : " << joint_count << std::endl; + } + + std::vector motor_to_joint(const std::vector& m) const override { + std::vector j(m.size()); + for (size_t i = 0; i < m.size(); ++i) { + j[i] = {m[i].position, m[i].velocity, m[i].effort}; } - - size_t get_joint_count() const override { return joint_count_; } - - private: - size_t joint_count_; - }; - - // assume motor num equals to joint num - class OpenArmJGripperJointConverter : public MotorJointConverter { - public: - explicit OpenArmJGripperJointConverter(size_t joint_count) : joint_count_(joint_count) { - std::cout << "Gripper joint converter joint_count is : " << joint_count << std::endl; - } - - std::vector motor_to_joint(const std::vector& m) const override { - std::vector j(m.size()); - for (size_t i = 0; i < m.size(); ++i) { - j[i] = {m[i].position, m[i].velocity, m[i].effort}; - } - return j; - } - - std::vector joint_to_motor(const std::vector& j) const override { - std::vector m(j.size()); - for (size_t i = 0; i < j.size(); ++i) { - m[i] = {j[i].position, j[i].velocity, j[i].effort}; - } - return m; - } - - size_t get_joint_count() const override { return joint_count_; } - - private: - size_t joint_count_; - }; \ No newline at end of file + return j; + } + + std::vector joint_to_motor(const std::vector& j) const override { + std::vector m(j.size()); + for (size_t i = 0; i < j.size(); ++i) { + m[i] = {j[i].position, j[i].velocity, j[i].effort}; + } + return m; + } + + size_t get_joint_count() const override { return joint_count_; } + +private: + size_t joint_count_; +}; \ No newline at end of file diff --git a/src/openarm_constants.hpp b/src/openarm_constants.hpp index f9b3cf6..5140436 100644 --- a/src/openarm_constants.hpp +++ b/src/openarm_constants.hpp @@ -14,11 +14,12 @@ #pragma once -#include #include +#include + #include +#include #include -#include constexpr double PI = 3.14159265358979323846; @@ -43,53 +44,51 @@ constexpr double PI = 3.14159265358979323846; #define ELBOWLIMIT 0.0 -static const double INITIAL_POSITION[NMOTORS] = { - 0, 0, 0, PI/5.0, 0, 0, 0, 0 -}; +static const double INITIAL_POSITION[NMOTORS] = {0, 0, 0, PI / 5.0, 0, 0, 0, 0}; // safety limit position -static const double position_limit_max_L[] = { (2.0/3.0)*PI, PI, PI/2.0, PI, PI/2.0, PI/2.0, PI/2.0, PI }; -static const double position_limit_min_L[] = { -(2.0/3.0)*PI, -PI/2.0, -PI/2.0, ELBOWLIMIT, -PI/2.0, -PI/2.0, -PI/2.0, -PI }; -static const double position_limit_max_F[] = { (2.0/3.0)*PI, PI, PI/2.0, PI, PI/2.0, PI/2.0, PI/2.0, PI }; -static const double position_limit_min_F[] = { -(2.0/3.0)*PI, -PI/2.0, -PI/2.0, ELBOWLIMIT, -PI/2.0, -PI/2.0, -PI/2.0, -PI }; +static const double position_limit_max_L[] = {(2.0 / 3.0) * PI, PI, PI / 2.0, PI, + PI / 2.0, PI / 2.0, PI / 2.0, PI}; +static const double position_limit_min_L[] = {-(2.0 / 3.0) * PI, -PI / 2.0, -PI / 2.0, ELBOWLIMIT, + -PI / 2.0, -PI / 2.0, -PI / 2.0, -PI}; +static const double position_limit_max_F[] = {(2.0 / 3.0) * PI, PI, PI / 2.0, PI, + PI / 2.0, PI / 2.0, PI / 2.0, PI}; +static const double position_limit_min_F[] = {-(2.0 / 3.0) * PI, -PI / 2.0, -PI / 2.0, ELBOWLIMIT, + -PI / 2.0, -PI / 2.0, -PI / 2.0, -PI}; // sefaty limit velocity -static const double velocity_limit_L[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0}; -static const double velocity_limit_F[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0}; +static const double velocity_limit_L[] = {8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0}; +static const double velocity_limit_F[] = {8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0}; // sefaty limit effort -static const double effort_limit_L[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0}; -static const double effort_limit_F[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0}; - +static const double effort_limit_L[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}; +static const double effort_limit_F[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}; // Motor configuration structure struct MotorConfig { - std::vector arm_motor_types; - std::vector arm_send_can_ids; - std::vector arm_recv_can_ids; - openarm::damiao_motor::MotorType gripper_motor_type; - uint32_t gripper_send_can_id; - uint32_t gripper_recv_can_id; + std::vector arm_motor_types; + std::vector arm_send_can_ids; + std::vector arm_recv_can_ids; + openarm::damiao_motor::MotorType gripper_motor_type; + uint32_t gripper_send_can_id; + uint32_t gripper_recv_can_id; }; // Global default motor configuration static const MotorConfig DEFAULT_MOTOR_CONFIG = { - // Standard 7-DOF arm motor configuration - {openarm::damiao_motor::MotorType::DM8009, - openarm::damiao_motor::MotorType::DM8009, - openarm::damiao_motor::MotorType::DM4340, - openarm::damiao_motor::MotorType::DM4340, - openarm::damiao_motor::MotorType::DM4310, - openarm::damiao_motor::MotorType::DM4310, - openarm::damiao_motor::MotorType::DM4310}, + // Standard 7-DOF arm motor configuration + {openarm::damiao_motor::MotorType::DM8009, openarm::damiao_motor::MotorType::DM8009, + openarm::damiao_motor::MotorType::DM4340, openarm::damiao_motor::MotorType::DM4340, + openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310, + openarm::damiao_motor::MotorType::DM4310}, - // Standard CAN IDs for arm motors - {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}, - {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}, + // Standard CAN IDs for arm motors + {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}, + {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}, - // Standard gripper configuration - openarm::damiao_motor::MotorType::DM4310, - 0x08, - 0x18}; + // Standard gripper configuration + openarm::damiao_motor::MotorType::DM4310, + 0x08, + 0x18}; // opening function inline void printOpenArmBanner() { diff --git a/src/openarm_port/joint_mapper.cpp b/src/openarm_port/joint_mapper.cpp index d81651e..deac891 100644 --- a/src/openarm_port/joint_mapper.cpp +++ b/src/openarm_port/joint_mapper.cpp @@ -13,6 +13,7 @@ // limitations under the License. #include "joint_mapper.hpp" + #include JointMapper::JointMapper() {} @@ -20,38 +21,35 @@ JointMapper::JointMapper() {} JointMapper::~JointMapper() {} // Only copying for now -void JointMapper::motor_to_joint_position(const double *motor_position, - double *joint_position) { - joint_position[0] = motor_position[0]; - joint_position[1] = motor_position[1]; - joint_position[2] = motor_position[2]; - joint_position[3] = motor_position[3]; - joint_position[4] = motor_position[4]; - joint_position[5] = motor_position[5]; - joint_position[6] = motor_position[6]; - joint_position[7] = motor_position[7]; +void JointMapper::motor_to_joint_position(const double *motor_position, double *joint_position) { + joint_position[0] = motor_position[0]; + joint_position[1] = motor_position[1]; + joint_position[2] = motor_position[2]; + joint_position[3] = motor_position[3]; + joint_position[4] = motor_position[4]; + joint_position[5] = motor_position[5]; + joint_position[6] = motor_position[6]; + joint_position[7] = motor_position[7]; } -void JointMapper::motor_to_joint_velocity(const double *motor_velocity, - double *joint_velocity) { - joint_velocity[0] = motor_velocity[0]; - joint_velocity[1] = motor_velocity[1]; - joint_velocity[2] = motor_velocity[2]; - joint_velocity[3] = motor_velocity[3]; - joint_velocity[4] = motor_velocity[4]; - joint_velocity[5] = motor_velocity[5]; - joint_velocity[6] = motor_velocity[6]; - joint_velocity[7] = motor_velocity[7]; +void JointMapper::motor_to_joint_velocity(const double *motor_velocity, double *joint_velocity) { + joint_velocity[0] = motor_velocity[0]; + joint_velocity[1] = motor_velocity[1]; + joint_velocity[2] = motor_velocity[2]; + joint_velocity[3] = motor_velocity[3]; + joint_velocity[4] = motor_velocity[4]; + joint_velocity[5] = motor_velocity[5]; + joint_velocity[6] = motor_velocity[6]; + joint_velocity[7] = motor_velocity[7]; } -void JointMapper::joint_to_motor_torque(const double *joint_torque, - double *motor_torque) { - motor_torque[0] = joint_torque[0]; - motor_torque[1] = joint_torque[1]; - motor_torque[2] = joint_torque[2]; - motor_torque[3] = joint_torque[3]; - motor_torque[4] = joint_torque[4]; - motor_torque[5] = joint_torque[5]; - motor_torque[6] = joint_torque[6]; - motor_torque[7] = joint_torque[7]; +void JointMapper::joint_to_motor_torque(const double *joint_torque, double *motor_torque) { + motor_torque[0] = joint_torque[0]; + motor_torque[1] = joint_torque[1]; + motor_torque[2] = joint_torque[2]; + motor_torque[3] = joint_torque[3]; + motor_torque[4] = joint_torque[4]; + motor_torque[5] = joint_torque[5]; + motor_torque[6] = joint_torque[6]; + motor_torque[7] = joint_torque[7]; } diff --git a/src/openarm_port/joint_mapper.hpp b/src/openarm_port/joint_mapper.hpp index d27fb95..14a8041 100644 --- a/src/openarm_port/joint_mapper.hpp +++ b/src/openarm_port/joint_mapper.hpp @@ -18,12 +18,10 @@ class JointMapper { public: - JointMapper(); - ~JointMapper(); + JointMapper(); + ~JointMapper(); - void motor_to_joint_position(const double *motor_position, - double *joint_position); - void motor_to_joint_velocity(const double *motor_velocity, - double *joint_velocity); - void joint_to_motor_torque(const double *joint_torque, double *motor_torque); + void motor_to_joint_position(const double *motor_position, double *joint_position); + void motor_to_joint_velocity(const double *motor_velocity, double *joint_velocity); + void joint_to_motor_torque(const double *joint_torque, double *motor_torque); }; diff --git a/src/openarm_port/openarm_init.cpp b/src/openarm_port/openarm_init.cpp index 70876e4..7212894 100644 --- a/src/openarm_port/openarm_init.cpp +++ b/src/openarm_port/openarm_init.cpp @@ -13,75 +13,69 @@ // limitations under the License. #include "openarm_init.hpp" + #include "../openarm_constants.hpp" namespace openarm_init { -openarm::can::socket::OpenArm * -OpenArmInitializer::initialize_openarm(const std::string &can_device, - bool enable_debug) { - - MotorConfig config = DEFAULT_MOTOR_CONFIG; - return initialize_openarm(can_device, config, enable_debug); +openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std::string &can_device, + bool enable_debug) { + MotorConfig config = DEFAULT_MOTOR_CONFIG; + return initialize_openarm(can_device, config, enable_debug); } -openarm::can::socket::OpenArm * -OpenArmInitializer::initialize_openarm(const std::string &can_device, - const MotorConfig &config, - bool enable_debug) { +openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std::string &can_device, + const MotorConfig &config, + bool enable_debug) { + // Create OpenArm instance + openarm::can::socket::OpenArm *openarm = + new openarm::can::socket::OpenArm(can_device, enable_debug); - // Create OpenArm instance - openarm::can::socket::OpenArm *openarm = - new openarm::can::socket::OpenArm(can_device, enable_debug); + // Perform common initialization + initialize_(openarm, config, enable_debug); - // Perform common initialization - initialize_(openarm, config, enable_debug); - - return openarm; + return openarm; } void OpenArmInitializer::initialize_(openarm::can::socket::OpenArm *openarm, - const MotorConfig &config, - bool enable_debug) { + const MotorConfig &config, bool enable_debug) { + if (enable_debug) { + std::cout << "Initializing arm motors..." << std::endl; + } - if (enable_debug) { - std::cout << "Initializing arm motors..." << std::endl; - } + // Initialize arm motors + openarm->init_arm_motors(config.arm_motor_types, config.arm_send_can_ids, + config.arm_recv_can_ids); - // Initialize arm motors - openarm->init_arm_motors(config.arm_motor_types, config.arm_send_can_ids, - config.arm_recv_can_ids); + if (enable_debug) { + std::cout << "Initializing gripper motor..." << std::endl; + } - if (enable_debug) { - std::cout << "Initializing gripper motor..." << std::endl; - } + // Initialize gripper motor + openarm->init_gripper_motor(config.gripper_motor_type, config.gripper_send_can_id, + config.gripper_recv_can_id); - // Initialize gripper motor - openarm->init_gripper_motor(config.gripper_motor_type, - config.gripper_send_can_id, - config.gripper_recv_can_id); + // Set callback mode for all motors + openarm->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); - // Set callback mode for all motors - openarm->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); + if (enable_debug) { + std::cout << "Enabling motors..." << std::endl; + } - if (enable_debug) { - std::cout << "Enabling motors..." << std::endl; - } + // Enable all motors with appropriate delays + openarm->enable_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + openarm->recv_all(); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); - // Enable all motors with appropriate delays - openarm->enable_all(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - openarm->recv_all(); - std::this_thread::sleep_for(std::chrono::milliseconds(100)); + // Print motor counts for verification + if (enable_debug) { + size_t arm_motor_num = openarm->get_arm().get_motors().size(); + size_t gripper_motor_num = openarm->get_gripper().get_motors().size(); - // Print motor counts for verification - if (enable_debug) { - size_t arm_motor_num = openarm->get_arm().get_motors().size(); - size_t gripper_motor_num = openarm->get_gripper().get_motors().size(); - - std::cout << "Arm motor count: " << arm_motor_num << std::endl; - std::cout << "Gripper motor count: " << gripper_motor_num << std::endl; - } + std::cout << "Arm motor count: " << arm_motor_num << std::endl; + std::cout << "Gripper motor count: " << gripper_motor_num << std::endl; + } } -} // namespace openarm_init +} // namespace openarm_init diff --git a/src/openarm_port/openarm_init.hpp b/src/openarm_port/openarm_init.hpp index 08a652a..718815e 100644 --- a/src/openarm_port/openarm_init.hpp +++ b/src/openarm_port/openarm_init.hpp @@ -14,43 +14,44 @@ #pragma once -#include "../openarm_constants.hpp" #include #include #include #include #include +#include "../openarm_constants.hpp" + namespace openarm_init { class OpenArmInitializer { public: - /** - * @brief Initialize OpenArm with default configuration - * @param can_device CAN device name (e.g., "can0", "can1") - * @param enable_debug Enable debug output - * @return Initialized OpenArm pointer (caller owns memory) - */ - static openarm::can::socket::OpenArm * - initialize_openarm(const std::string &can_device, bool enable_debug = true); + /** + * @brief Initialize OpenArm with default configuration + * @param can_device CAN device name (e.g., "can0", "can1") + * @param enable_debug Enable debug output + * @return Initialized OpenArm pointer (caller owns memory) + */ + static openarm::can::socket::OpenArm *initialize_openarm(const std::string &can_device, + bool enable_debug = true); - /** - * @brief Initialize OpenArm with custom motor configuration - * @param can_device CAN device name - * @param config Custom motor configuration - * @param enable_debug Enable debug output - * @return Initialized OpenArm pointer (caller owns memory) - */ - static openarm::can::socket::OpenArm * - initialize_openarm(const std::string &can_device, const MotorConfig &config, - bool enable_debug = true); + /** + * @brief Initialize OpenArm with custom motor configuration + * @param can_device CAN device name + * @param config Custom motor configuration + * @param enable_debug Enable debug output + * @return Initialized OpenArm pointer (caller owns memory) + */ + static openarm::can::socket::OpenArm *initialize_openarm(const std::string &can_device, + const MotorConfig &config, + bool enable_debug = true); private: - /** - * @brief Common initialization steps for OpenArm - */ - static void initialize_(openarm::can::socket::OpenArm *openarm, - const MotorConfig &config, bool enable_debug); + /** + * @brief Common initialization steps for OpenArm + */ + static void initialize_(openarm::can::socket::OpenArm *openarm, const MotorConfig &config, + bool enable_debug); }; -} // namespace openarm_init +} // namespace openarm_init diff --git a/src/periodic_timer_thread.hpp b/src/periodic_timer_thread.hpp index 3544ed3..e9d9a37 100644 --- a/src/periodic_timer_thread.hpp +++ b/src/periodic_timer_thread.hpp @@ -14,55 +14,42 @@ #pragma once #include +#include +#include + #include #include -#include #include #include -#include -#include - +#include class PeriodicTimerThread { public: - explicit PeriodicTimerThread(double hz = 1000.0) - : is_running_(false) - { + explicit PeriodicTimerThread(double hz = 1000.0) : is_running_(false) { if (hz <= 0.0) { throw std::invalid_argument("Hz must be positive"); } period_us_.store(static_cast(1e6 / hz)); } - virtual ~PeriodicTimerThread() { - stop_thread(); - } + virtual ~PeriodicTimerThread() { stop_thread(); } - virtual void start_thread() { - start_thread_base(); - } + virtual void start_thread() { start_thread_base(); } - virtual void stop_thread() { - stop_thread_base(); - } + virtual void stop_thread() { stop_thread_base(); } - int64_t get_elapsed_time_us() const { - return last_elapsed_us_.load(); - } + int64_t get_elapsed_time_us() const { return last_elapsed_us_.load(); } void set_period(double hz) { if (hz > 0.0) { period_us_.store(static_cast(1e6 / hz)); } } - + protected: virtual void on_timer() = 0; - virtual void before_start() { - - set_thread_priority(50); - } + virtual void before_start() { set_thread_priority(50); } virtual void after_stop() {} @@ -82,7 +69,6 @@ protected: } private: - void start_thread_base() { before_start(); is_running_ = true; @@ -106,7 +92,7 @@ private: void timer_thread() { struct timespec next_time; clock_gettime(CLOCK_MONOTONIC, &next_time); - + while (is_running_) { auto start = std::chrono::steady_clock::now(); @@ -117,9 +103,9 @@ private: } auto end = std::chrono::steady_clock::now(); - last_elapsed_us_.store(std::chrono::duration_cast(end - start).count()); - - + last_elapsed_us_.store( + std::chrono::duration_cast(end - start).count()); + int period_us = period_us_.load(); next_time.tv_nsec += period_us * 1000; while (next_time.tv_nsec >= 1000000000) { @@ -129,7 +115,6 @@ private: clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, nullptr); } } - pthread_t thread_{}; std::atomic is_running_; diff --git a/src/robot_state.hpp b/src/robot_state.hpp index 628ae7e..e47b209 100644 --- a/src/robot_state.hpp +++ b/src/robot_state.hpp @@ -14,8 +14,8 @@ #pragma once -#include #include +#include // Represents the state of a single joint: position, velocity, and effort. struct JointState { @@ -26,74 +26,72 @@ struct JointState { // Manages reference and response states for a robot component (e.g., arm, hand). class RobotState { - public: - RobotState() = default; - - explicit RobotState(size_t num_joints) - : response_(num_joints), reference_(num_joints) {} - - // --- Set/Get reference (target) joint states --- - void set_reference(size_t index, const JointState& state) { - std::lock_guard lock(mutex_); - if (index < reference_.size()) { - reference_[index] = state; - } +public: + RobotState() = default; + + explicit RobotState(size_t num_joints) : response_(num_joints), reference_(num_joints) {} + + // --- Set/Get reference (target) joint states --- + void set_reference(size_t index, const JointState& state) { + std::lock_guard lock(mutex_); + if (index < reference_.size()) { + reference_[index] = state; } - - void set_all_references(const std::vector& states) { - std::lock_guard lock(mutex_); - reference_ = states; + } + + void set_all_references(const std::vector& states) { + std::lock_guard lock(mutex_); + reference_ = states; + } + + JointState get_reference(size_t index) const { + std::lock_guard lock(mutex_); + return index < reference_.size() ? reference_[index] : JointState{}; + } + + std::vector get_all_references() const { + std::lock_guard lock(mutex_); + return reference_; + } + + void set_response(size_t index, const JointState& state) { + std::lock_guard lock(mutex_); + if (index < response_.size()) { + response_[index] = state; } - - JointState get_reference(size_t index) const { - std::lock_guard lock(mutex_); - return index < reference_.size() ? reference_[index] : JointState{}; - } - - std::vector get_all_references() const { - std::lock_guard lock(mutex_); - return reference_; - } - - void set_response(size_t index, const JointState& state) { - std::lock_guard lock(mutex_); - if (index < response_.size()) { - response_[index] = state; - } - } - - void set_all_responses(const std::vector& states) { - std::lock_guard lock(mutex_); - response_ = states; - } - - JointState get_response(size_t index) const { - std::lock_guard lock(mutex_); - return index < response_.size() ? response_[index] : JointState{}; - } - - std::vector get_all_responses() const { - std::lock_guard lock(mutex_); - return response_; - } - - void resize(size_t new_size) { - std::lock_guard lock(mutex_); - reference_.resize(new_size); - response_.resize(new_size); - } - - size_t get_size() const { - std::lock_guard lock(mutex_); - return response_.size(); // assume same size for both - } - - private: - mutable std::mutex mutex_; - std::vector response_; - std::vector reference_; - }; - + } + + void set_all_responses(const std::vector& states) { + std::lock_guard lock(mutex_); + response_ = states; + } + + JointState get_response(size_t index) const { + std::lock_guard lock(mutex_); + return index < response_.size() ? response_[index] : JointState{}; + } + + std::vector get_all_responses() const { + std::lock_guard lock(mutex_); + return response_; + } + + void resize(size_t new_size) { + std::lock_guard lock(mutex_); + reference_.resize(new_size); + response_.resize(new_size); + } + + size_t get_size() const { + std::lock_guard lock(mutex_); + return response_.size(); // assume same size for both + } + +private: + mutable std::mutex mutex_; + std::vector response_; + std::vector reference_; +}; // Manages the joint states of robot components (arm, hand). class RobotSystemState { @@ -120,14 +118,14 @@ public: void set_all_references(const std::vector& all_refs) { const size_t arm_size = arm_state_.get_size(); const size_t hand_size = hand_state_.get_size(); - + if (all_refs.size() != arm_size + hand_size) { throw std::runtime_error("set_all_references: size mismatch."); } - + std::vector arm_refs(all_refs.begin(), all_refs.begin() + arm_size); std::vector hand_refs(all_refs.begin() + arm_size, all_refs.end()); - + arm_state_.set_all_references(arm_refs); hand_state_.set_all_references(hand_refs); } @@ -141,7 +139,7 @@ public: combined.insert(combined.end(), hand.begin(), hand.end()); return combined; } - + void set_all_responses(const std::vector& all_responses) { const size_t arm_size = arm_state_.get_size(); const size_t hand_size = hand_state_.get_size(); @@ -153,18 +151,15 @@ public: if (all_responses.size() != arm_size + hand_size) { throw std::runtime_error("set_all_responses: size mismatch."); } - + std::vector arm_res(all_responses.begin(), all_responses.begin() + arm_size); std::vector hand_res(all_responses.begin() + arm_size, all_responses.end()); - + arm_state_.set_all_responses(arm_res); hand_state_.set_all_responses(hand_res); - } - size_t get_total_joint_count() const { - return arm_state_.get_size() + hand_state_.get_size(); - } + size_t get_total_joint_count() const { return arm_state_.get_size() + hand_state_.get_size(); } private: RobotState arm_state_; diff --git a/src/yamlloader.hpp b/src/yamlloader.hpp index 446d4aa..3575ba8 100644 --- a/src/yamlloader.hpp +++ b/src/yamlloader.hpp @@ -12,13 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. - #pragma once #include + +#include #include #include -#include class YamlLoader { public: @@ -26,7 +26,8 @@ public: try { root_ = YAML::LoadFile(filepath); } catch (const std::exception& e) { - throw std::runtime_error("Failed to load YAML file: " + filepath + ", error: " + e.what()); + throw std::runtime_error("Failed to load YAML file: " + filepath + + ", error: " + e.what()); } }