Add formatter (#5)

.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

.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

.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

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

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

.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

CMakeLists.txt

@@ -40,28 +40,18 @@ endif()
 # -----------------------------
 # Create static library
 # -----------------------------
-add_library(openarm_teleop_lib STATIC
+add_library(
-    src/controller/dynamics.cpp
+  openarm_teleop_lib STATIC
-    src/controller/control.cpp
+  src/controller/dynamics.cpp src/controller/control.cpp
-    src/openarm_port/openarm_init.cpp
+  src/openarm_port/openarm_init.cpp src/openarm_port/joint_mapper.cpp)
-    src/openarm_port/joint_mapper.cpp
-)
 
-target_include_directories(
+target_include_directories(openarm_teleop_lib
+                           PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
+
+target_link_libraries(
   openarm_teleop_lib
-    PUBLIC
+  PUBLIC OpenArmCAN::openarm_can Eigen3::Eigen ${orocos_kdl_LIBRARIES}
-    ${CMAKE_CURRENT_SOURCE_DIR}/src
+         ${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

control/gravity_compasation.cpp

@@ -14,16 +14,14 @@
 
 #include <atomic>
 #include <chrono>
+#include <controller/dynamics.hpp>
 #include <csignal>
-#include <iostream>
-#include <thread>
-#include <csignal>
-#include <atomic>
 #include <filesystem>
+#include <iostream>
 #include <openarm/can/socket/openarm.hpp>
 #include <openarm/damiao_motor/dm_motor_constants.hpp>
-#include <controller/dynamics.hpp>
 #include <openarm_port/openarm_init.hpp>
+#include <thread>
 
 std::atomic<bool> 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] << " <arm_side> <can_interface> <urdf_path>" << std::endl;
+            std::cerr << "Usage: " << argv[0] << " <arm_side> <can_interface> <urdf_path>"
-            std::cerr << "Example: " << argv[0] << " right_arm can0 /tmp/v10_bimanual.urdf" << std::endl;
+                      << 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,7 +68,8 @@ 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();
@@ -85,20 +86,25 @@ int main(int argc, char** argv) {
         std::vector<double> arm_joint_positions(openarm->get_arm().get_motors().size(), 0.0);
         std::vector<double> arm_joint_velocities(openarm->get_arm().get_motors().size(), 0.0);
 
-        std::vector<double> gripper_joint_positions(openarm->get_gripper().get_motors().size(), 0.0);
+        std::vector<double> gripper_joint_positions(openarm->get_gripper().get_motors().size(),
-        std::vector<double> gripper_joint_velocities(openarm->get_gripper().get_motors().size(), 0.0);
+                                                    0.0);
+        std::vector<double> gripper_joint_velocities(openarm->get_gripper().get_motors().size(),
+                                                     0.0);
 
         std::vector<double> grav_torques(openarm->get_arm().get_motors().size(), 0.0);
 
         while (keep_running) {
-
             frame_count++;
             auto current_time = std::chrono::high_resolution_clock::now();
 
             // Calculate and display Hz every second
-            auto time_since_last_display = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_hz_display).count();
+            auto time_since_last_display = std::chrono::duration_cast<std::chrono::milliseconds>(
+                                               current_time - last_hz_display)
+                                               .count();
             if (time_since_last_display >= 1000) {  // Every 1000ms (1 second)
-                auto total_time = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - start_time).count();
+                auto total_time =
+                    std::chrono::duration_cast<std::chrono::milliseconds>(current_time - start_time)
+                        .count();
                 double hz = (frame_count * 1000.0) / total_time;
                 std::cout << "=== Loop Frequency: " << hz << " Hz ===" << std::endl;
                 last_hz_display = current_time;
@@ -125,7 +131,6 @@ int main(int argc, char** argv) {
             openarm->get_arm().mit_control_all(cmds);
 
             openarm->recv_all();
-
         }
 
         std::this_thread::sleep_for(std::chrono::milliseconds(100));

control/openarm_bilateral_control.cpp

@@ -12,28 +12,23 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-
 #include <atomic>
 #include <chrono>
+#include <controller/control.hpp>
+#include <controller/dynamics.hpp>
 #include <csignal>
-#include <iostream>
-#include <thread>
-#include <csignal>
-#include <atomic>
 #include <filesystem>
-
+#include <iostream>
-#include <periodic_timer_thread.hpp>
-#include <robot_state.hpp>
 #include <openarm/can/socket/openarm.hpp>
 #include <openarm/damiao_motor/dm_motor_constants.hpp>
 #include <openarm_port/openarm_init.hpp>
-#include <controller/dynamics.hpp>
+#include <periodic_timer_thread.hpp>
+#include <robot_state.hpp>
+#include <thread>
 #include <yamlloader.hpp>
-#include <controller/control.hpp>
 
 std::atomic<bool> keep_running(true);
 
-
 void signal_handler(int signal) {
     if (signal == SIGINT) {
         std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl;
@@ -41,21 +36,16 @@ void signal_handler(int signal) {
     }
 }
 
-
 class LeaderArmThread : public PeriodicTimerThread {
 public:
-        LeaderArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_l, double hz = 500.0)
+    LeaderArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_l,
+                    double hz = 500.0)
         : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {}
 
 protected:
+    void before_start() override { std::cout << "leader start thread " << std::endl; }
 
-        void before_start() override {
+    void after_stop() override { std::cout << "leader stop thread " << std::endl; }
-            std::cout << "leader start thread " << std::endl;
-        }
-
-        void after_stop() override {
-            std::cout << "leader stop thread " << std::endl;
-        }
 
     void on_timer() override {
         static auto prev_time = std::chrono::steady_clock::now();
@@ -64,7 +54,8 @@ class LeaderArmThread : public PeriodicTimerThread {
 
         auto now = std::chrono::steady_clock::now();
 
-            auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
+        auto elapsed_us =
+            std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
         prev_time = now;
 
         // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
@@ -73,23 +64,18 @@ class LeaderArmThread : public PeriodicTimerThread {
 private:
     std::shared_ptr<RobotSystemState> robot_state_;
     Control *control_l_;
-
 };
 
-
 class FollowerArmThread : public PeriodicTimerThread {
 public:
-        FollowerArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_f, double hz = 500.0)
+    FollowerArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_f,
+                      double hz = 500.0)
         : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {}
 
 protected:
-        void before_start() override {
+    void before_start() override { std::cout << "follower start thread " << std::endl; }
-            std::cout << "follower start thread " << std::endl;
-        }
 
-        void after_stop() override {
+    void after_stop() override { std::cout << "follower stop thread " << std::endl; }
-            std::cout << "follower stop thread " << std::endl;
-        }
 
     void on_timer() override {
         static auto prev_time = std::chrono::steady_clock::now();
@@ -98,7 +84,8 @@ class FollowerArmThread : public PeriodicTimerThread {
 
         auto now = std::chrono::steady_clock::now();
 
-            auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
+        auto elapsed_us =
+            std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
         prev_time = now;
 
         // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
@@ -107,31 +94,25 @@ class FollowerArmThread : public PeriodicTimerThread {
 private:
     std::shared_ptr<RobotSystemState> robot_state_;
     Control *control_f_;
-
 };
 
-
-
 class AdminThread : public PeriodicTimerThread {
 public:
     AdminThread(std::shared_ptr<RobotSystemState> leader_state,
-                    std::shared_ptr<RobotSystemState> follower_state,
+                std::shared_ptr<RobotSystemState> follower_state, Control *control_l,
-                    Control *control_l,
+                Control *control_f, double hz = 500.0)
-                    Control *control_f,
+        : PeriodicTimerThread(hz),
-                    double hz = 500.0)
+          leader_state_(leader_state),
-            : PeriodicTimerThread(hz), leader_state_(leader_state), follower_state_(follower_state), control_l_(control_l), control_f_(control_f)  {}
+          follower_state_(follower_state),
+          control_l_(control_l),
+          control_f_(control_f) {}
 
 protected:
-        void before_start() override {
+    void before_start() override { std::cout << "admin start thread " << std::endl; }
-            std::cout << "admin start thread " << std::endl;
-        }
 
-        void after_stop() override {
+    void after_stop() override { std::cout << "admin stop thread " << std::endl; }
-            std::cout << "admin stop thread " << std::endl;
-        }
 
     void on_timer() override {
-
         static auto prev_time = std::chrono::steady_clock::now();
         auto now = std::chrono::steady_clock::now();
 
@@ -149,7 +130,8 @@ class AdminThread : public PeriodicTimerThread {
         follower_state_->arm_state().set_all_references(leader_arm_resp);
         follower_state_->hand_state().set_all_references(leader_hand_resp);
 
-            auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
+        auto elapsed_us =
+            std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
         prev_time = now;
 
         // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
@@ -160,10 +142,8 @@ class AdminThread : public PeriodicTimerThread {
     std::shared_ptr<RobotSystemState> follower_state_;
     Control *control_l_;
     Control *control_f_;
-
 };
 
-
 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] << " <leader_urdf_path> <follower_urdf_path> [arm_side] [leader_can] [follower_can]" << std::endl;
+            std::cerr
+                << "Usage: " << argv[0]
+                << " <leader_urdf_path> <follower_urdf_path> [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;
@@ -272,17 +257,20 @@ int main(int argc, char** argv) {
         std::shared_ptr<RobotSystemState> follower_state =
             std::make_shared<RobotSystemState>(follower_arm_motor_num, follower_hand_motor_num);
 
-        Control* control_leader = new Control(leader_openarm,leader_arm_dynamics,follower_arm_dynamics, leader_state, 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num);
+        Control *control_leader = new Control(
-        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);
+            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(
+        control_leader->SetParameter(leader_kp, leader_kd, leader_Fc, leader_k, leader_Fv,
-                                     leader_kp, leader_kd,
+                                     leader_Fo);
-                                     leader_Fc, leader_k, leader_Fv, leader_Fo);
 
-        control_follower->SetParameter(
+        control_follower->SetParameter(follower_kp, follower_kd, follower_Fc, follower_k,
-                                       follower_kp, follower_kd,
+                                       follower_Fv, follower_Fo);
-                                       follower_Fc, follower_k, follower_Fv, follower_Fo);
 
         // set home postion
         std::thread thread_l(&Control::AdjustPosition, control_leader);
@@ -293,7 +281,8 @@ int main(int argc, char** argv) {
         // Start control process
         LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY);
         FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY);
-        AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, FREQUENCY);
+        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';
     }
 

control/openarm_communication_test.cpp

@@ -19,9 +19,6 @@
 #include <openarm/can/socket/openarm.hpp>
 #include <openarm/damiao_motor/dm_motor_constants.hpp>
 #include <thread>
-#include <csignal>
-#include <atomic>
-
 
 int main(int argc, char** argv) {
     try {
@@ -35,7 +32,6 @@ int main(int argc, char** argv) {
 
         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<uint32_t> send_can_ids = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
         std::vector<uint32_t> recv_can_ids = {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17};
@@ -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}});
 

control/openarm_unilateral_control.cpp

@@ -12,28 +12,23 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-
 #include <atomic>
 #include <chrono>
+#include <controller/control.hpp>
+#include <controller/dynamics.hpp>
 #include <csignal>
-#include <iostream>
-#include <thread>
-#include <csignal>
-#include <atomic>
 #include <filesystem>
-
+#include <iostream>
-#include <periodic_timer_thread.hpp>
-#include <robot_state.hpp>
 #include <openarm/can/socket/openarm.hpp>
 #include <openarm/damiao_motor/dm_motor_constants.hpp>
 #include <openarm_port/openarm_init.hpp>
-#include <controller/dynamics.hpp>
+#include <periodic_timer_thread.hpp>
+#include <robot_state.hpp>
+#include <thread>
 #include <yamlloader.hpp>
-#include <controller/control.hpp>
 
 std::atomic<bool> keep_running(true);
 
-
 void signal_handler(int signal) {
     if (signal == SIGINT) {
         std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl;
@@ -41,21 +36,16 @@ void signal_handler(int signal) {
     }
 }
 
-
 class LeaderArmThread : public PeriodicTimerThread {
 public:
-        LeaderArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_l, double hz = 500.0)
+    LeaderArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_l,
+                    double hz = 500.0)
         : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {}
 
 protected:
+    void before_start() override { std::cout << "leader start thread " << std::endl; }
 
-        void before_start() override {
+    void after_stop() override { std::cout << "leader stop thread " << std::endl; }
-            std::cout << "leader start thread " << std::endl;
-        }
-
-        void after_stop() override {
-            std::cout << "leader stop thread " << std::endl;
-        }
 
     void on_timer() override {
         static auto prev_time = std::chrono::steady_clock::now();
@@ -64,7 +54,8 @@ class LeaderArmThread : public PeriodicTimerThread {
 
         auto now = std::chrono::steady_clock::now();
 
-            auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
+        auto elapsed_us =
+            std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
         prev_time = now;
 
         // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
@@ -73,23 +64,18 @@ class LeaderArmThread : public PeriodicTimerThread {
 private:
     std::shared_ptr<RobotSystemState> robot_state_;
     Control *control_l_;
-
 };
 
-
 class FollowerArmThread : public PeriodicTimerThread {
 public:
-        FollowerArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_f, double hz = 500.0)
+    FollowerArmThread(std::shared_ptr<RobotSystemState> robot_state, Control *control_f,
+                      double hz = 500.0)
         : PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {}
 
 protected:
-        void before_start() override {
+    void before_start() override { std::cout << "follower start thread " << std::endl; }
-            std::cout << "follower start thread " << std::endl;
-        }
 
-        void after_stop() override {
+    void after_stop() override { std::cout << "follower stop thread " << std::endl; }
-            std::cout << "follower stop thread " << std::endl;
-        }
 
     void on_timer() override {
         static auto prev_time = std::chrono::steady_clock::now();
@@ -98,7 +84,8 @@ class FollowerArmThread : public PeriodicTimerThread {
 
         auto now = std::chrono::steady_clock::now();
 
-            auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
+        auto elapsed_us =
+            std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
         prev_time = now;
 
         // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
@@ -107,30 +94,25 @@ class FollowerArmThread : public PeriodicTimerThread {
 private:
     std::shared_ptr<RobotSystemState> robot_state_;
     Control *control_f_;
-
 };
 
-
 class AdminThread : public PeriodicTimerThread {
 public:
     AdminThread(std::shared_ptr<RobotSystemState> leader_state,
-                    std::shared_ptr<RobotSystemState> follower_state,
+                std::shared_ptr<RobotSystemState> follower_state, Control *control_l,
-                    Control *control_l,
+                Control *control_f, double hz = 500.0)
-                    Control *control_f,
+        : PeriodicTimerThread(hz),
-                    double hz = 500.0)
+          leader_state_(leader_state),
-            : PeriodicTimerThread(hz), leader_state_(leader_state), follower_state_(follower_state), control_l_(control_l), control_f_(control_f)  {}
+          follower_state_(follower_state),
+          control_l_(control_l),
+          control_f_(control_f) {}
 
 protected:
-        void before_start() override {
+    void before_start() override { std::cout << "admin start thread " << std::endl; }
-            std::cout << "admin start thread " << std::endl;
-        }
 
-        void after_stop() override {
+    void after_stop() override { std::cout << "admin stop thread " << std::endl; }
-            std::cout << "admin stop thread " << std::endl;
-        }
 
     void on_timer() override {
-
         static auto prev_time = std::chrono::steady_clock::now();
         auto now = std::chrono::steady_clock::now();
 
@@ -148,7 +130,8 @@ class AdminThread : public PeriodicTimerThread {
         follower_state_->arm_state().set_all_references(leader_arm_resp);
         follower_state_->hand_state().set_all_references(leader_hand_resp);
 
-            auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
+        auto elapsed_us =
+            std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
         prev_time = now;
 
         // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
@@ -159,10 +142,8 @@ class AdminThread : public PeriodicTimerThread {
     std::shared_ptr<RobotSystemState> follower_state_;
     Control *control_l_;
     Control *control_f_;
-
 };
 
-
 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] << " <leader_urdf_path> <follower_urdf_path> [arm_side] [leader_can] [follower_can]" << std::endl;
+            std::cerr
+                << "Usage: " << argv[0]
+                << " <leader_urdf_path> <follower_urdf_path> [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;
@@ -272,16 +258,19 @@ int main(int argc, char** argv) {
         std::shared_ptr<RobotSystemState> follower_state =
             std::make_shared<RobotSystemState>(follower_arm_motor_num, follower_hand_motor_num);
 
-        Control* control_leader = new Control(leader_openarm,leader_arm_dynamics,follower_arm_dynamics, leader_state, 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num);
+        Control *control_leader = new Control(
-        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);
+            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(
+        control_leader->SetParameter(leader_kp, leader_kd, leader_Fc, leader_k, leader_Fv,
-            leader_kp, leader_kd ,
+                                     leader_Fo);
-            leader_Fc, leader_k, leader_Fv, leader_Fo);
 
-        control_follower->SetParameter(
+        control_follower->SetParameter(follower_kp, follower_kd, follower_Fc, follower_k,
-            follower_kp, follower_kd,
+                                       follower_Fv, follower_Fo);
-            follower_Fc, follower_k, follower_Fv, follower_Fo);
 
         // set home postion
         std::thread thread_l(&Control::AdjustPosition, control_leader);
@@ -292,7 +281,8 @@ int main(int argc, char** argv) {
         // Start control process
         LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY);
         FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY);
-        AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, FREQUENCY);
+        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';
     }
 

script/launch_bilateral.sh

@@ -75,7 +75,6 @@ if [ ! -f "$XACRO_PATH" ]; then
     exit 1
 fi
 
-
 # Check binary
 if [ ! -f "$BIN_PATH" ]; then
     echo "[ERROR] Compiled binary not found at: $BIN_PATH"
@@ -83,18 +82,17 @@ if [ ! -f "$BIN_PATH" ]; then
 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"
+if ! xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH"; then
-cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH"
-
-if [ $? -ne 0 ]; then
     echo "[ERROR] Failed to generate URDFs."
     exit 1
 fi
+cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH"
 
 # Run binary
 echo "[INFO] Launching bilateral control..."

script/launch_grav_comp.sh

@@ -47,7 +47,6 @@ if [ ! -f "$XACRO_PATH" ]; then
     exit 1
 fi
 
-
 # Check build binary
 if [ ! -f "$BIN_PATH" ]; then
     echo "[ERROR] Compiled binary not found at: $BIN_PATH"
@@ -56,12 +55,11 @@ 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 ! xacro "$XACRO_PATH" bimanual:=true -o "$URDF_OUT"; then
-
-if [ $? -ne 0 ]; then
     echo "[ERROR] Failed to generate URDF."
     exit 1
 fi

script/launch_unilateral.sh

@@ -83,18 +83,17 @@ if [ ! -f "$BIN_PATH" ]; then
 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"
+if ! xacro "$XACRO_PATH" bimanual:=true -o "$LEADER_URDF_PATH"; then
-cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH"
-
-if [ $? -ne 0 ]; then
     echo "[ERROR] Failed to generate URDFs."
     exit 1
 fi
+cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH"
 
 # Run binary
 echo "[INFO] Launching unilateral control..."

src/controller/control.cpp

@@ -12,27 +12,43 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <cmath>
-#include <unistd.h>
 #include <string.h>
-#include <thread>
+#include <unistd.h>
-#include <cmath>
+
 #include <algorithm>
-#include <iomanip>
+#include <cmath>
 #include <controller/control.hpp>
 #include <controller/dynamics.hpp>
+#include <iomanip>
+#include <thread>
 
-Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, size_t arm_motor_num, size_t hand_motor_num):
+Control::Control(openarm::can::socket::OpenArm* arm, Dynamics* dynamics_l, Dynamics* dynamics_f,
-        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)
+                 std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
-{
+                 size_t arm_motor_num, size_t hand_motor_num)
+    : openarm_(arm),
+      dynamics_l_(dynamics_l),
+      dynamics_f_(dynamics_f),
+      robot_state_(robot_state),
+      Ts_(Ts),
+      role_(role),
+      arm_motor_num_(arm_motor_num),
+      hand_motor_num_(hand_motor_num) {
     differentiator_ = new Differentiator(Ts);
     openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
     openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
 }
 
-Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, std::string arm_type, size_t arm_motor_num, size_t hand_motor_num):
+Control::Control(openarm::can::socket::OpenArm* arm, Dynamics* dynamics_l, Dynamics* dynamics_f,
-        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)
+                 std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
-{
+                 std::string arm_type, size_t arm_motor_num, size_t hand_motor_num)
+    : openarm_(arm),
+      dynamics_l_(dynamics_l),
+      dynamics_f_(dynamics_f),
+      robot_state_(robot_state),
+      Ts_(Ts),
+      role_(role),
+      arm_motor_num_(arm_motor_num),
+      hand_motor_num_(hand_motor_num) {
     differentiator_ = new Differentiator(Ts);
     openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
     openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
@@ -63,14 +79,9 @@ void Control::Shutdown(void){
     openarm_->disable_all();
 }
 
-void Control::SetParameter(
+void Control::SetParameter(const std::vector<double>& Kp, const std::vector<double>& Kd,
-        const std::vector<double>& Kp,
+                           const std::vector<double>& Fc, const std::vector<double>& k,
-        const std::vector<double>& Kd,
+                           const std::vector<double>& Fv, const std::vector<double>& Fo) {
-        const std::vector<double>& Fc,
-        const std::vector<double>& k,
-        const std::vector<double>& Fv,
-        const std::vector<double>& Fo)
-        {
     Kp_ = Kp;
     Kd_ = Kd;
     Fc_ = Fc;
@@ -79,35 +90,27 @@ void Control::SetParameter(
     Fo_ = Fo;
 }
 
-
+bool Control::bilateral_step() {
-bool Control::bilateral_step()
-{
     // get motor status
     std::vector<MotorState> arm_motor_states;
     const auto& arm_motors = openarm_->get_arm().get_motors();
     for (size_t i = 0; i < arm_motors.size(); ++i) {
         const auto& motor = arm_motors[i];
-            arm_motor_states.push_back({
+        arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0});
-                motor.get_position(),
-                motor.get_velocity(),
-                0
-            });
     }
 
     std::vector<MotorState> gripper_motor_states;
     const auto& gripper_motors = openarm_->get_gripper().get_motors();
     for (size_t i = 0; i < gripper_motors.size(); ++i) {
         const auto& motor = gripper_motors[i];
-            gripper_motor_states.push_back({
+        gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0});
-                motor.get_position(),
-                motor.get_velocity(),
-                0
-            });
     }
 
     // convert joint to motor
-        std::vector<JointState> joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states);
+    std::vector<JointState> joint_arm_states =
-        std::vector<JointState> joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
+        openarmjointconverter_->motor_to_joint(arm_motor_states);
+    std::vector<JointState> joint_gripper_states =
+        openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
 
     // set reponse
     robot_state_->arm_state().set_all_responses(joint_arm_states);
@@ -139,7 +142,8 @@ bool Control::bilateral_step()
     std::vector<double> friction(arm_dof + gripper_dof, 0.0);
 
     std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
-        std::vector<JointState> joint_gripper_states_ref = robot_state_->hand_state().get_all_references();
+    std::vector<JointState> joint_gripper_states_ref =
+        robot_state_->hand_state().get_all_references();
 
     std::vector<double> joint_arm_positions_ref(arm_dof);
 
@@ -147,22 +151,23 @@ bool Control::bilateral_step()
         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());
+        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());
+        dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(),
+                                 coriolis.data());
     }
 
-
     // Friction (compute joint friction)
     for (size_t i = 0; i < joint_arm_velocities.size(); ++i)
         ComputeFriction(joint_arm_velocities.data(), friction.data(), i);
     for (size_t i = 0; i < joint_gripper_velocities.size(); ++i)
-                ComputeFriction(joint_gripper_velocities.data(), friction.data(), joint_arm_velocities.size() + i);
+        ComputeFriction(joint_gripper_velocities.data(), friction.data(),
+                        joint_arm_velocities.size() + i);
 
     // set gravity and friciton comp joint torque value
     for (size_t i = 0; i < arm_dof; i++) {
@@ -173,21 +178,27 @@ bool Control::bilateral_step()
         joint_gripper_states_ref[i].effort = friction[i + arm_dof];
     }
 
-        std::vector<MotorState> motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
+    std::vector<MotorState> motor_arm_states =
-        std::vector<MotorState> motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref);
+        openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
+    std::vector<MotorState> motor_gripper_states =
+        openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref);
 
     // kp kd q dq tau
     std::vector<openarm::damiao_motor::MITParam> arm_cmds;
     arm_cmds.reserve(arm_dof);
     for (size_t i = 0; i < arm_dof; ++i) {
-             arm_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity, motor_arm_states[i].effort});
+        arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
+            Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity,
+            motor_arm_states[i].effort});
     }
 
     // gripper command mit param
     std::vector<openarm::damiao_motor::MITParam> gripper_cmds;
     gripper_cmds.reserve(gripper_dof);
     for (size_t i = 0; i < gripper_dof; ++i) {
-            gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i + arm_dof],Kd_[i + arm_dof], motor_gripper_states[i].position, motor_gripper_states[i].velocity, motor_gripper_states[i].effort});
+        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
@@ -200,12 +211,9 @@ bool Control::bilateral_step()
     openarm_->recv_all(220);
 
     return true;
-
 }
 
-
 bool Control::unilateral_step() {
-
     // get motor status
     std::vector<MotorState> arm_motor_states;
     for (const auto& motor : openarm_->get_arm().get_motors()) {
@@ -218,8 +226,10 @@ bool Control::bilateral_step()
     }
 
     // convert joint to motor
-                std::vector<JointState> joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states);
+    std::vector<JointState> joint_arm_states =
-                std::vector<JointState> joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
+        openarmjointconverter_->motor_to_joint(arm_motor_states);
+    std::vector<JointState> joint_gripper_states =
+        openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
 
     // set reponse
     robot_state_->arm_state().set_all_responses(joint_arm_states);
@@ -248,10 +258,10 @@ bool Control::bilateral_step()
     std::vector<double> friction(arm_dof + gripper_dof, 0.0);
 
     if (role_ == ROLE_LEADER) {
-
         // calc dynamics
         dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data());
-                        dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data());
+        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);
@@ -259,7 +269,6 @@ bool Control::bilateral_step()
         for (size_t i = 0; i < joint_gripper_velocities.size(); ++i)
             ComputeFriction(joint_gripper_velocities.data(), friction.data(), arm_dof + i);
 
-
         // arm joint state
         std::vector<JointState> joint_arm_state_torque(arm_dof);
         for (size_t i = 0; i < arm_dof; ++i) {
@@ -276,21 +285,25 @@ bool Control::bilateral_step()
             joint_gripper_state_torque[i].effort = friction[arm_dof + i] * 0.3;
         }
 
-                        std::vector<MotorState> motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_state_torque);
+        std::vector<MotorState> motor_arm_states =
-                        std::vector<MotorState> motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque);
+            openarmjointconverter_->joint_to_motor(joint_arm_state_torque);
+        std::vector<MotorState> motor_gripper_states =
+            openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque);
 
         // arm command mit param
         std::vector<openarm::damiao_motor::MITParam> arm_cmds;
         arm_cmds.reserve(arm_dof);
         for (size_t i = 0; i < arm_dof; ++i) {
-                            arm_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort});
+            arm_cmds.emplace_back(
+                openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort});
         }
 
         // gripper command mit param
         std::vector<openarm::damiao_motor::MITParam> gripper_cmds;
         gripper_cmds.reserve(gripper_dof);
         for (size_t i = 0; i < gripper_dof; ++i) {
-                            gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_gripper_states[i].effort});
+            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
@@ -305,36 +318,30 @@ bool Control::bilateral_step()
     }
 
     else if (role_ == ROLE_FOLLOWER) {
-
+        std::vector<JointState> joint_arm_states_ref =
-                        std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
+            robot_state_->arm_state().get_all_references();
-                        std::vector<JointState> joint_hand_states_ref = robot_state_->hand_state().get_all_references();
+        std::vector<JointState> joint_hand_states_ref =
+            robot_state_->hand_state().get_all_references();
 
         // Joint → Motor
-                        std::vector<MotorState> arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
+        std::vector<MotorState> arm_motor_refs =
-                        std::vector<MotorState> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref);
+            openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
+        std::vector<MotorState> hand_motor_refs =
+            openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref);
 
         std::vector<openarm::damiao_motor::MITParam> arm_cmds;
         arm_cmds.reserve(arm_motor_refs.size());
         for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
             arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
-                                    Kp_[i],
+                Kp_[i], Kd_[i], arm_motor_refs[i].position, arm_motor_refs[i].velocity, 0.0});
-                                    Kd_[i],
-                                    arm_motor_refs[i].position,
-                                    arm_motor_refs[i].velocity,
-                                    0.0
-                                });
         }
 
         std::vector<openarm::damiao_motor::MITParam> hand_cmds;
         hand_cmds.reserve(hand_motor_refs.size());
         for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
             hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
-                                    Kp_[i + arm_dof],
+                Kp_[i + arm_dof], Kd_[i + arm_dof], hand_motor_refs[i].position,
-                                    Kd_[i + arm_dof],
+                hand_motor_refs[i].velocity, 0.0});
-                                    hand_motor_refs[i].position,
-                                    hand_motor_refs[i].velocity,
-                                    0.0
-                                });
         }
 
         openarm_->get_arm().mit_control_all(arm_cmds);
@@ -346,11 +353,9 @@ bool Control::bilateral_step()
     }
 
     return true;
-
 }
 
-        void Control::ComputeFriction(const double* velocity, double* friction, size_t index)
+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;
@@ -367,8 +372,7 @@ bool Control::bilateral_step()
     }
 }
 
-        bool Control::AdjustPosition(void)
+bool Control::AdjustPosition(void) {
-        {
     int nstep = 220;
     double alpha;
 
@@ -382,8 +386,10 @@ bool Control::bilateral_step()
         gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
     }
 
-            std::vector<JointState> joint_arm_now = openarmjointconverter_->motor_to_joint(arm_motor_states);
+    std::vector<JointState> joint_arm_now =
-            std::vector<JointState> joint_hand_now = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
+        openarmjointconverter_->motor_to_joint(arm_motor_states);
+    std::vector<JointState> joint_hand_now =
+        openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
 
     std::vector<JointState> joint_arm_goal(NMOTORS - 1);
     for (size_t i = 0; i < NMOTORS - 1; ++i) {
@@ -410,41 +416,37 @@ bool Control::bilateral_step()
 
         std::vector<JointState> joint_arm_interp(NMOTORS - 1);
         for (size_t i = 0; i < NMOTORS - 1; ++i) {
-                    joint_arm_interp[i].position = joint_arm_goal[i].position * alpha + joint_arm_now[i].position * (1.0 - alpha);
+            joint_arm_interp[i].position =
+                joint_arm_goal[i].position * alpha + joint_arm_now[i].position * (1.0 - alpha);
             joint_arm_interp[i].velocity = 0.0;
         }
 
         std::vector<JointState> joint_hand_interp(joint_hand_goal.size());
         for (size_t i = 0; i < joint_hand_interp.size(); ++i) {
-                    joint_hand_interp[i].position = joint_hand_goal[i].position * alpha + joint_hand_now[i].position * (1.0 - alpha);
+            joint_hand_interp[i].position =
+                joint_hand_goal[i].position * alpha + joint_hand_now[i].position * (1.0 - alpha);
             joint_hand_interp[i].velocity = 0.0;
         }
 
-                std::vector<MotorState> arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_interp);
+        std::vector<MotorState> arm_motor_refs =
-                std::vector<MotorState> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_interp);
+            openarmjointconverter_->joint_to_motor(joint_arm_interp);
+        std::vector<MotorState> hand_motor_refs =
+            openarmgripperjointconverter_->joint_to_motor(joint_hand_interp);
 
         std::vector<openarm::damiao_motor::MITParam> arm_cmds;
         arm_cmds.reserve(arm_motor_refs.size());
         for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
-                    arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
+            arm_cmds.emplace_back(openarm::damiao_motor::MITParam{kp_arm_temp[i], kd_arm_temp[i],
-                        kp_arm_temp[i],
-                        kd_arm_temp[i],
                                                                   arm_motor_refs[i].position,
-                        arm_motor_refs[i].velocity,
+                                                                  arm_motor_refs[i].velocity, 0.0});
-                        0.0
-                    });
         }
 
         std::vector<openarm::damiao_motor::MITParam> hand_cmds;
         hand_cmds.reserve(hand_motor_refs.size());
         for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
             hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
-                        kp_hand_temp[i],
+                kp_hand_temp[i], kd_hand_temp[i], hand_motor_refs[i].position,
-                        kd_hand_temp[i],
+                hand_motor_refs[i].velocity, 0.0});
-                        hand_motor_refs[i].position,
-                        hand_motor_refs[i].velocity,
-                        0.0
-                    });
         }
 
         openarm_->get_arm().mit_control_all(arm_cmds);
@@ -465,8 +467,10 @@ bool Control::bilateral_step()
         gripper_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0});
     }
 
-                std::vector<JointState> joint_arm_final = openarmjointconverter_->motor_to_joint(arm_motor_states_final);
+    std::vector<JointState> joint_arm_final =
-                std::vector<JointState> joint_hand_final = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final);
+        openarmjointconverter_->motor_to_joint(arm_motor_states_final);
+    std::vector<JointState> joint_hand_final =
+        openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final);
 
     robot_state_->arm_state().set_all_references(joint_arm_final);
     robot_state_->hand_state().set_all_references(joint_hand_final);
@@ -474,23 +478,19 @@ bool Control::bilateral_step()
     return true;
 }
 
-        bool Control::DetectVibration(const double* velocity, bool* what_axis)
+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)
+        if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE) velocity_buffer_[i].pop_front();
-                                velocity_buffer_[i].pop_front();
 
-                        if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE)
+        if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE) continue;
-                                continue;
 
-                        double mean = std::accumulate(
+        double mean = std::accumulate(velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0) /
-                                        velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0
+                      velocity_buffer_[i].size();
-                                        ) / velocity_buffer_[i].size();
 
         double var = 0.0;
         for (double v : velocity_buffer_[i]) {

src/controller/control.hpp

@@ -15,23 +15,20 @@
 #pragma once
 
 // #include <sensor_msgs/msg/joint_state.hpp>
-#include <utility>
-#include <fstream>
-#include <deque>
-#include <numeric>
-#include <memory>
 #include <controller/diff.hpp>
 #include <controller/dynamics.hpp>
-#include <robot_state.hpp>
+#include <deque>
+#include <fstream>
 #include <joint_state_converter.hpp>
-#include <openarm_constants.hpp>
+#include <memory>
-#include <robot_state.hpp>
+#include <numeric>
 #include <openarm/can/socket/openarm.hpp>
 #include <openarm/damiao_motor/dm_motor_constants.hpp>
+#include <openarm_constants.hpp>
+#include <robot_state.hpp>
+#include <utility>
 
-
+class Control {
-class Control
-{
     openarm::can::socket::OpenArm *openarm_;
 
     double Ts_;
@@ -62,8 +59,12 @@ class Control
     std::deque<double> velocity_buffer_[NJOINTS];
 
 public:
-        Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, size_t arm_joint_num, size_t hand_motor_num);
+    Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
-        Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, std::string arm_type, size_t arm_joint_num, size_t hand_motor_num);
+            std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
+            size_t arm_joint_num, size_t hand_motor_num);
+    Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
+            std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
+            std::string arm_type, size_t arm_joint_num, size_t hand_motor_num);
     ~Control();
 
     std::shared_ptr<RobotSystemState> response_;
@@ -75,13 +76,9 @@ class Control
     void Setstate(int state);
     void Shutdown(void);
 
-        void SetParameter(
+    void SetParameter(const std::vector<double> &Kp, const std::vector<double> &Kd,
-                const std::vector<double>& Kp,
+                      const std::vector<double> &Fc, const std::vector<double> &k,
-                const std::vector<double>& Kd,
+                      const std::vector<double> &Fv, const std::vector<double> &Fo);
-                const std::vector<double>& Fc,
-                const std::vector<double>& k,
-                const std::vector<double>& Fv,
-                const std::vector<double>& Fo);
 
     bool AdjustPosition(void);
 

src/controller/diff.hpp

@@ -18,14 +18,14 @@
 
 #include <openarm_constants.hpp>
 
-class Differentiator
+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) {}
 
@@ -33,8 +33,7 @@ class Differentiator
      * Compute the motor speed by taking the derivative of
      * the motion.
      */
-                void Differentiate(const double *position, double *velocity)
+    void Differentiate(const double *position, double *velocity) {
-                {
         double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY);
         double b = a * CUTOFF_FREQUENCY;
 
@@ -47,12 +46,10 @@ class Differentiator
             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;
 
@@ -67,8 +64,6 @@ class Differentiator
             position_z1_[i] = position[i];
             velocity_z1_[i] = velocity[i];
             acc_z1_[i] = acc_[i];
-
         }
-
     }
 };

src/controller/dynamics.cpp

@@ -14,8 +14,7 @@
 
 #include <controller/dynamics.hpp>
 
-Dynamics::Dynamics(std::string urdf_path, std::string start_link, std::string 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;
@@ -23,9 +22,7 @@ Dynamics::Dynamics(std::string urdf_path, std::string start_link, std::string en
 
 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,26 +49,23 @@ 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::ChainDynParam>(
+    solver = std::make_unique<KDL::ChainDynParam>(kdl_chain, KDL::Vector(0, 0.0, -9.81));
-        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();
 
     KDL::JntArray q_(kdl_chain.getNrOfJoints());
@@ -86,7 +80,8 @@ void Dynamics::GetGravity(const double *motor_position, double *gravity)
     }
 }
 
-void Dynamics::GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis) {
+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());
 
@@ -102,8 +97,7 @@ void Dynamics::GetCoriolis(const double *motor_position, const double *motor_vel
     }
 }
 
-void Dynamics::GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag)
+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++) {
@@ -115,11 +109,9 @@ void Dynamics::GetMassMatrixDiagonal(const double *motor_position, double *inert
     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)
+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];
@@ -135,7 +127,6 @@ void Dynamics::GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobi
             jacobian(i, j) = kdl_jac(i, j);
         }
     }
-
 }
 
 void Dynamics::GetNullSpace(const double *motor_position, Eigen::MatrixXd &nullspace) {
@@ -150,7 +141,8 @@ void Dynamics::GetNullSpace(const double* motor_position, Eigen::MatrixXd& nulls
 
     if (use_stable_svd) {
         Eigen::JacobiSVD<Eigen::MatrixXd> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
-                double tol = 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff();
+        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;
@@ -166,9 +158,7 @@ void Dynamics::GetNullSpace(const double* motor_position, Eigen::MatrixXd& nulls
     //        std::cout << "[INFO] Null space projector computed.\n";
 }
 
-
+void Dynamics::GetNullSpaceTauSpace(const double *motor_position, Eigen::MatrixXd &nullspace_T) {
-void Dynamics::GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixXd& nullspace_T)
-{
     const size_t dof = kdl_chain.getNrOfJoints();
     bool use_stable_svd = false;
 
@@ -179,7 +169,8 @@ void Dynamics::GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixX
 
     if (use_stable_svd) {
         Eigen::JacobiSVD<Eigen::MatrixXd> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
-                double tol = 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff();
+        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;
@@ -195,8 +186,8 @@ void Dynamics::GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixX
     nullspace_T = N.transpose();
 }
 
-void Dynamics::GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p)
+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];
@@ -211,14 +202,13 @@ void Dynamics::GetEECordinate(const double *motor_position, Eigen::Matrix3d &R,
     }
 
     for (int i = 0; i < 3; ++i)
-                for (int j = 0; j < 3; ++j)
+        for (int j = 0; j < 3; ++j) R(i, j) = kdl_frame.M(i, j);
-                        R(i, j) = kdl_frame.M(i, j);
 
     p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2];
 }
 
-void Dynamics::GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p)
+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];
@@ -233,11 +223,7 @@ void Dynamics::GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &
     }
 
     for (int i = 0; i < 3; ++i)
-                for (int j = 0; j < 3; ++j)
+        for (int j = 0; j < 3; ++j) R(i, j) = kdl_frame.M(i, j);
-                        R(i, j) = kdl_frame.M(i, j);
 
     p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2];
-
-
-
 }

src/controller/dynamics.hpp

@@ -13,25 +13,25 @@
 // limitations under the License.
 
 #pragma once
-#include <unistd.h>
 #include <string.h>
+#include <unistd.h>
+#include <urdf_parser/urdf_parser.h>
+
+#include <Eigen/Dense>
+#include <fstream>
+#include <iostream>
 #include <kdl/chain.hpp>
 #include <kdl/chaindynparam.hpp>
-#include <kdl_parser/kdl_parser.hpp>
 #include <kdl/chainfksolverpos_recursive.hpp>
 #include <kdl/chainjnttojacsolver.hpp>
-#include <urdf_parser/urdf_parser.h>
+#include <kdl_parser/kdl_parser.hpp>
-#include <Eigen/Dense>
-#include <vector>
-#include <iostream>
-#include <fstream>
 #include <sstream>
+#include <vector>
 /*
  * Compute gravity and inertia compensation using Orocos
  * Kinematics and Dynamics Library (KDL).
  */
-class Dynamics
+class Dynamics {
-{
 private:
     std::shared_ptr<urdf::ModelInterface> urdf_model_interface;
 

src/joint_state_converter.hpp

@@ -14,8 +14,8 @@
 
 #pragma once
 
-#include <vector>
 #include <robot_state.hpp>
+#include <vector>
 
 // Represents the state of a single joint
 // struct JointState {
@@ -37,10 +37,12 @@ public:
     virtual ~MotorJointConverter() = default;
 
     // MotorState vector → JointState vector
-    virtual std::vector<JointState> motor_to_joint(const std::vector<MotorState>& motor_states) const = 0;
+    virtual std::vector<JointState> motor_to_joint(
+        const std::vector<MotorState>& motor_states) const = 0;
 
     // JointState vector → MotorState vector
-    virtual std::vector<MotorState> joint_to_motor(const std::vector<JointState>& joint_states) const = 0;
+    virtual std::vector<MotorState> joint_to_motor(
+        const std::vector<JointState>& joint_states) const = 0;
 
     virtual size_t get_joint_count() const = 0;
 };
@@ -58,7 +60,6 @@ class OpenArmJointConverter : public MotorJointConverter {
         std::vector<JointState> j(m.size());
         for (size_t i = 0; i < m.size(); ++i) {
             j[i] = {m[i].position, m[i].velocity, m[i].effort};
-
         }
 
         return j;
@@ -66,8 +67,7 @@ class OpenArmJointConverter : public MotorJointConverter {
 
     std::vector<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
         std::vector<MotorState> m(j.size());
-            for (size_t i = 0; i < j.size(); ++i)
+        for (size_t i = 0; i < j.size(); ++i) m[i] = {j[i].position, j[i].velocity, j[i].effort};
-                m[i] = {j[i].position, j[i].velocity, j[i].effort};
         return m;
     }
 

src/openarm_constants.hpp

@@ -14,11 +14,12 @@
 
 #pragma once
 
-#include <unistd.h>
 #include <time.h>
+#include <unistd.h>
+
 #include <iostream>
+#include <openarm/damiao_motor/dm_motor_constants.hpp>
 #include <vector>
-#include <openarm/damiao_motor//dm_motor_constants.hpp>
 
 constexpr double PI = 3.14159265358979323846;
 
@@ -43,15 +44,17 @@ constexpr double PI = 3.14159265358979323846;
 
 #define ELBOWLIMIT 0.0
 
-static const double INITIAL_POSITION[NMOTORS] = {
+static const double INITIAL_POSITION[NMOTORS] = {0, 0, 0, PI / 5.0, 0, 0, 0, 0};
-  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_max_L[] = {(2.0 / 3.0) * PI, PI,       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 };
+                                              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_L[] = {-(2.0 / 3.0) * PI, -PI / 2.0, -PI / 2.0, ELBOWLIMIT,
-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 };
+                                              -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};
@@ -60,7 +63,6 @@ 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 effort_limit_L[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0};
 static const double effort_limit_F[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0};
 
-
 // Motor configuration structure
 struct MotorConfig {
     std::vector<openarm::damiao_motor::MotorType> arm_motor_types;
@@ -74,12 +76,9 @@ struct MotorConfig {
 // 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::DM8009,
-   openarm::damiao_motor::MotorType::DM8009,
+     openarm::damiao_motor::MotorType::DM4340, openarm::damiao_motor::MotorType::DM4340,
-   openarm::damiao_motor::MotorType::DM4340,
+     openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310,
-   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

src/openarm_port/joint_mapper.cpp

@@ -13,6 +13,7 @@
 // limitations under the License.
 
 #include "joint_mapper.hpp"
+
 #include <cmath>
 
 JointMapper::JointMapper() {}
@@ -20,8 +21,7 @@ JointMapper::JointMapper() {}
 JointMapper::~JointMapper() {}
 
 // Only copying for now
-void JointMapper::motor_to_joint_position(const double *motor_position,
+void JointMapper::motor_to_joint_position(const double *motor_position, double *joint_position) {
-                                          double *joint_position) {
     joint_position[0] = motor_position[0];
     joint_position[1] = motor_position[1];
     joint_position[2] = motor_position[2];
@@ -32,8 +32,7 @@ void JointMapper::motor_to_joint_position(const double *motor_position,
     joint_position[7] = motor_position[7];
 }
 
-void JointMapper::motor_to_joint_velocity(const double *motor_velocity,
+void JointMapper::motor_to_joint_velocity(const double *motor_velocity, double *joint_velocity) {
-                                          double *joint_velocity) {
     joint_velocity[0] = motor_velocity[0];
     joint_velocity[1] = motor_velocity[1];
     joint_velocity[2] = motor_velocity[2];
@@ -44,8 +43,7 @@ void JointMapper::motor_to_joint_velocity(const double *motor_velocity,
     joint_velocity[7] = motor_velocity[7];
 }
 
-void JointMapper::joint_to_motor_torque(const double *joint_torque,
+void JointMapper::joint_to_motor_torque(const double *joint_torque, double *motor_torque) {
-                                        double *motor_torque) {
     motor_torque[0] = joint_torque[0];
     motor_torque[1] = joint_torque[1];
     motor_torque[2] = joint_torque[2];

src/openarm_port/joint_mapper.hpp

@@ -21,9 +21,7 @@ public:
     JointMapper();
     ~JointMapper();
 
-  void motor_to_joint_position(const double *motor_position,
+    void motor_to_joint_position(const double *motor_position, double *joint_position);
-                               double *joint_position);
+    void motor_to_joint_velocity(const double *motor_velocity, double *joint_velocity);
-  void motor_to_joint_velocity(const double *motor_velocity,
-                               double *joint_velocity);
     void joint_to_motor_torque(const double *joint_torque, double *motor_torque);
 };

src/openarm_port/openarm_init.cpp

@@ -13,23 +13,20 @@
 // limitations under the License.
 
 #include "openarm_init.hpp"
+
 #include "../openarm_constants.hpp"
 
 namespace openarm_init {
 
-openarm::can::socket::OpenArm *
+openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std::string &can_device,
-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 *
+openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std::string &can_device,
-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);
@@ -41,9 +38,7 @@ OpenArmInitializer::initialize_openarm(const std::string &can_device,
 }
 
 void OpenArmInitializer::initialize_(openarm::can::socket::OpenArm *openarm,
-                                     const MotorConfig &config,
+                                     const MotorConfig &config, bool enable_debug) {
-                                     bool enable_debug) {
-
     if (enable_debug) {
         std::cout << "Initializing arm motors..." << std::endl;
     }
@@ -57,8 +52,7 @@ void OpenArmInitializer::initialize_(openarm::can::socket::OpenArm *openarm,
     }
 
     // Initialize gripper motor
-  openarm->init_gripper_motor(config.gripper_motor_type,
+    openarm->init_gripper_motor(config.gripper_motor_type, config.gripper_send_can_id,
-                              config.gripper_send_can_id,
                                 config.gripper_recv_can_id);
 
     // Set callback mode for all motors

src/openarm_port/openarm_init.hpp

@@ -14,13 +14,14 @@
 
 #pragma once
 
-#include "../openarm_constants.hpp"
 #include <chrono>
 #include <iostream>
 #include <openarm/can/socket/openarm.hpp>
 #include <string>
 #include <thread>
 
+#include "../openarm_constants.hpp"
+
 namespace openarm_init {
 
 class OpenArmInitializer {
@@ -31,8 +32,8 @@ public:
      * @param enable_debug Enable debug output
      * @return Initialized OpenArm pointer (caller owns memory)
      */
-  static openarm::can::socket::OpenArm *
+    static openarm::can::socket::OpenArm *initialize_openarm(const std::string &can_device,
-  initialize_openarm(const std::string &can_device, bool enable_debug = true);
+                                                             bool enable_debug = true);
 
     /**
      * @brief Initialize OpenArm with custom motor configuration
@@ -41,16 +42,16 @@ public:
      * @param enable_debug Enable debug output
      * @return Initialized OpenArm pointer (caller owns memory)
      */
-  static openarm::can::socket::OpenArm *
+    static openarm::can::socket::OpenArm *initialize_openarm(const std::string &can_device,
-  initialize_openarm(const std::string &can_device, const MotorConfig &config,
+                                                             const MotorConfig &config,
                                                              bool enable_debug = true);
 
 private:
     /**
      * @brief Common initialization steps for OpenArm
      */
-  static void initialize_(openarm::can::socket::OpenArm *openarm,
+    static void initialize_(openarm::can::socket::OpenArm *openarm, const MotorConfig &config,
-                          const MotorConfig &config, bool enable_debug);
+                            bool enable_debug);
 };
 
 }  // namespace openarm_init

src/periodic_timer_thread.hpp

@@ -14,41 +14,31 @@
 
 #pragma once
 #include <pthread.h>
+#include <time.h>
+#include <unistd.h>
+
 #include <atomic>
 #include <chrono>
-#include <thread>
 #include <iostream>
 #include <stdexcept>
-#include <unistd.h>
+#include <thread>
-#include <time.h>
-
 
 class PeriodicTimerThread {
 public:
-    explicit PeriodicTimerThread(double hz = 1000.0)
+    explicit PeriodicTimerThread(double hz = 1000.0) : is_running_(false) {
-        : is_running_(false)
-    {
         if (hz <= 0.0) {
             throw std::invalid_argument("Hz must be positive");
         }
         period_us_.store(static_cast<int>(1e6 / hz));
     }
 
-    virtual ~PeriodicTimerThread() {
+    virtual ~PeriodicTimerThread() { stop_thread(); }
-        stop_thread();
-    }
 
-    virtual void start_thread() {
+    virtual void start_thread() { start_thread_base(); }
-        start_thread_base();
-    }
 
-    virtual void stop_thread() {
+    virtual void stop_thread() { stop_thread_base(); }
-        stop_thread_base();
-    }
 
-    int64_t get_elapsed_time_us() const {
+    int64_t get_elapsed_time_us() const { return last_elapsed_us_.load(); }
-        return last_elapsed_us_.load();
-    }
 
     void set_period(double hz) {
         if (hz > 0.0) {
@@ -59,10 +49,7 @@ public:
 protected:
     virtual void on_timer() = 0;
 
-    virtual void before_start() {
+    virtual void before_start() { set_thread_priority(50); }
-        
-        set_thread_priority(50);
-    }
 
     virtual void after_stop() {}
 
@@ -82,7 +69,6 @@ protected:
     }
 
 private:
-
     void start_thread_base() {
         before_start();
         is_running_ = true;
@@ -117,8 +103,8 @@ private:
             }
 
             auto end = std::chrono::steady_clock::now();
-            last_elapsed_us_.store(std::chrono::duration_cast<std::chrono::microseconds>(end - start).count());
+            last_elapsed_us_.store(
-    
+                std::chrono::duration_cast<std::chrono::microseconds>(end - start).count());
 
             int period_us = period_us_.load();
             next_time.tv_nsec += period_us * 1000;
@@ -130,7 +116,6 @@ private:
         }
     }
 
-
     pthread_t thread_{};
     std::atomic<bool> is_running_;
     std::atomic<int> period_us_;

src/robot_state.hpp

@@ -14,8 +14,8 @@
 
 #pragma once
 
-#include <vector>
 #include <mutex>
+#include <vector>
 
 // Represents the state of a single joint: position, velocity, and effort.
 struct JointState {
@@ -29,8 +29,7 @@ class RobotState {
 public:
     RobotState() = default;
 
-        explicit RobotState(size_t num_joints)
+    explicit RobotState(size_t num_joints) : response_(num_joints), reference_(num_joints) {}
-            : response_(num_joints), reference_(num_joints) {}
 
     // --- Set/Get reference (target) joint states ---
     void set_reference(size_t index, const JointState& state) {
@@ -94,7 +93,6 @@ class RobotState {
     std::vector<JointState> reference_;
 };
 
-
 // Manages the joint states of robot components (arm, hand).
 class RobotSystemState {
 public:
@@ -159,12 +157,9 @@ public:
 
         arm_state_.set_all_responses(arm_res);
         hand_state_.set_all_responses(hand_res);
-                
     }
 
-    size_t get_total_joint_count() const {
+    size_t get_total_joint_count() const { return arm_state_.get_size() + hand_state_.get_size(); }
-        return arm_state_.get_size() + hand_state_.get_size();
-    }
 
 private:
     RobotState arm_state_;

src/yamlloader.hpp

@@ -12,13 +12,13 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-
 #pragma once
 
 #include <yaml-cpp/yaml.h>
+
+#include <stdexcept>
 #include <string>
 #include <vector>
-#include <stdexcept>
 
 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());
         }
     }