openarm/openarm_teleop
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add formatter (#5)

Browse Source
This commit is contained in:
Sutou Kouhei 2025-10-01 16:59:19 +09:00 committed by GitHub
parent df87fade0e
commit 7cc98e533f
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
28 changed files with 1431 additions and 1381 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
.clang-format Normal file
View File

@ -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

19
.cmake-format.py Normal file
View File

@ -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

27
.editorconfig Normal file
View File

@ -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

19
.github/workflows/test.yaml vendored
View File

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

42
.pre-commit-config.yaml Normal file
View File

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

16
.shellcheckrc Normal file
View File

@ -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

30
CMakeLists.txt
View File

@ -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(
target_include_directories(openarm_teleop_lib
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
target_link_libraries(
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
)
PUBLIC OpenArmCAN::openarm_can Eigen3::Eigen ${orocos_kdl_LIBRARIES}
${kdl_parser_LIBRARIES} urdfdom::urdfdom_model yaml-cpp::yaml-cpp)
# -----------------------------
# Executables

37
control/gravity_compasation.cpp
View File

@ -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 << "Example: " << argv[0] << " right_arm can0 /tmp/v10_bimanual.urdf" << std::endl;
std::cerr << "Usage: " << argv[0] << " <arm_side> <can_interface> <urdf_path>"
<< std::endl;
std::cerr << "Example: " << argv[0] << " right_arm can0 /tmp/v10_bimanual.urdf"
<< std::endl;
return 1;
}
@ -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_velocities(openarm->get_gripper().get_motors().size(), 0.0);
std::vector<double> gripper_joint_positions(openarm->get_gripper().get_motors().size(),
0.0);
std::vector<double> gripper_joint_velocities(openarm->get_gripper().get_motors().size(),
0.0);
std::vector<double> grav_torques(openarm->get_arm().get_motors().size(), 0.0);
while (keep_running) {
frame_count++;
auto current_time = std::chrono::high_resolution_clock::now();
// Calculate and display Hz every second
auto time_since_last_display = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - last_hz_display).count();
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));

115
control/openarm_bilateral_control.cpp
View File

@ -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 <periodic_timer_thread.hpp>
#include <robot_state.hpp>
#include <iostream>
#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 {
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();
@ -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 {
std::cout << "follower start thread " << std::endl;
}
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();
@ -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,
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) {}
std::shared_ptr<RobotSystemState> follower_state, Control *control_l,
Control *control_f, double hz = 500.0)
: PeriodicTimerThread(hz),
leader_state_(leader_state),
follower_state_(follower_state),
control_l_(control_l),
control_f_(control_f) {}
protected:
void before_start() override {
std::cout << "admin start thread " << std::endl;
}
void 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 {
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_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);
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
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';
}

10
control/openarm_communication_test.cpp
View File

@ -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}});

114
control/openarm_unilateral_control.cpp
View File

@ -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 <periodic_timer_thread.hpp>
#include <robot_state.hpp>
#include <iostream>
#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 {
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();
@ -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 {
std::cout << "follower start thread " << std::endl;
}
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();
@ -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,
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) {}
std::shared_ptr<RobotSystemState> follower_state, Control *control_l,
Control *control_f, double hz = 500.0)
: PeriodicTimerThread(hz),
leader_state_(leader_state),
follower_state_(follower_state),
control_l_(control_l),
control_f_(control_f) {}
protected:
void before_start() override {
std::cout << "admin start thread " << std::endl;
}
void 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 {
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_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
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';
}

8
script/launch_bilateral.sh
View File

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

6
script/launch_grav_comp.sh
View File

@ -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 [ $? -ne 0 ]; then
if ! xacro "$XACRO_PATH" bimanual:=true -o "$URDF_OUT"; then
echo "[ERROR] Failed to generate URDF."
exit 1
fi

7
script/launch_unilateral.sh
View File

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

214
src/controller/control.cpp
View File

@ -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 <cmath>
#include <unistd.h>
#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):
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)
{
Control::Control(openarm::can::socket::OpenArm* arm, Dynamics* dynamics_l, Dynamics* dynamics_f,
std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
size_t arm_motor_num, size_t hand_motor_num)
: openarm_(arm),
dynamics_l_(dynamics_l),
dynamics_f_(dynamics_f),
robot_state_(robot_state),
Ts_(Ts),
role_(role),
arm_motor_num_(arm_motor_num),
hand_motor_num_(hand_motor_num) {
differentiator_ = new Differentiator(Ts);
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
}
Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, std::string arm_type, size_t arm_motor_num, size_t hand_motor_num):
openarm_(arm), dynamics_l_(dynamics_l), dynamics_f_(dynamics_f), robot_state_(robot_state), Ts_(Ts), role_(role), arm_motor_num_(arm_motor_num), hand_motor_num_(hand_motor_num)
{
Control::Control(openarm::can::socket::OpenArm* arm, Dynamics* dynamics_l, Dynamics* dynamics_f,
std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
std::string arm_type, size_t arm_motor_num, size_t hand_motor_num)
: openarm_(arm),
dynamics_l_(dynamics_l),
dynamics_f_(dynamics_f),
robot_state_(robot_state),
Ts_(Ts),
role_(role),
arm_motor_num_(arm_motor_num),
hand_motor_num_(hand_motor_num) {
differentiator_ = new Differentiator(Ts);
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
@ -63,14 +79,9 @@ void Control::Shutdown(void){
openarm_->disable_all();
}
void Control::SetParameter(
const std::vector<double>& Kp,
const std::vector<double>& Kd,
const std::vector<double>& Fc,
const std::vector<double>& k,
const std::vector<double>& Fv,
const std::vector<double>& Fo)
{
void Control::SetParameter(const std::vector<double>& Kp, const std::vector<double>& Kd,
const std::vector<double>& Fc, const std::vector<double>& k,
const std::vector<double>& Fv, const std::vector<double>& Fo) {
Kp_ = Kp;
Kd_ = Kd;
Fc_ = Fc;
@ -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({
motor.get_position(),
motor.get_velocity(),
0
});
arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0});
}
std::vector<MotorState> gripper_motor_states;
const auto& gripper_motors = openarm_->get_gripper().get_motors();
for (size_t i = 0; i < gripper_motors.size(); ++i) {
const auto& motor = gripper_motors[i];
gripper_motor_states.push_back({
motor.get_position(),
motor.get_velocity(),
0
});
gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0});
}
// convert joint to motor
std::vector<JointState> joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
std::vector<JointState> joint_arm_states =
openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_gripper_states =
openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
// set reponse
robot_state_->arm_state().set_all_responses(joint_arm_states);
@ -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_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref);
std::vector<MotorState> motor_arm_states =
openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
std::vector<MotorState> motor_gripper_states =
openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref);
// kp kd q dq tau
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_dof);
for (size_t i = 0; i < arm_dof; ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity, motor_arm_states[i].effort});
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_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
std::vector<JointState> joint_arm_states =
openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_gripper_states =
openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
// set reponse
robot_state_->arm_state().set_all_responses(joint_arm_states);
@ -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_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque);
std::vector<MotorState> motor_arm_states =
openarmjointconverter_->joint_to_motor(joint_arm_state_torque);
std::vector<MotorState> motor_gripper_states =
openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque);
// arm command mit param
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_dof);
for (size_t i = 0; i < arm_dof; ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort});
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 = 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_arm_states_ref =
robot_state_->arm_state().get_all_references();
std::vector<JointState> joint_hand_states_ref =
robot_state_->hand_state().get_all_references();
// Joint → Motor
std::vector<MotorState> arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
std::vector<MotorState> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref);
std::vector<MotorState> arm_motor_refs =
openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
std::vector<MotorState> hand_motor_refs =
openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref);
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_motor_refs.size());
for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
Kp_[i],
Kd_[i],
arm_motor_refs[i].position,
arm_motor_refs[i].velocity,
0.0
});
Kp_[i], Kd_[i], arm_motor_refs[i].position, arm_motor_refs[i].velocity, 0.0});
}
std::vector<openarm::damiao_motor::MITParam> hand_cmds;
hand_cmds.reserve(hand_motor_refs.size());
for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
Kp_[i + arm_dof],
Kd_[i + arm_dof],
hand_motor_refs[i].position,
hand_motor_refs[i].velocity,
0.0
});
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);
@ -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_hand_now = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
std::vector<JointState> joint_arm_now =
openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_hand_now =
openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
std::vector<JointState> joint_arm_goal(NMOTORS - 1);
for (size_t i = 0; i < NMOTORS - 1; ++i) {
@ -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> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_interp);
std::vector<MotorState> arm_motor_refs =
openarmjointconverter_->joint_to_motor(joint_arm_interp);
std::vector<MotorState> hand_motor_refs =
openarmgripperjointconverter_->joint_to_motor(joint_hand_interp);
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_motor_refs.size());
for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
kp_arm_temp[i],
kd_arm_temp[i],
arm_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
});
arm_motor_refs[i].velocity, 0.0});
}
std::vector<openarm::damiao_motor::MITParam> hand_cmds;
hand_cmds.reserve(hand_motor_refs.size());
for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
kp_hand_temp[i],
kd_hand_temp[i],
hand_motor_refs[i].position,
hand_motor_refs[i].velocity,
0.0
});
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);
@ -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_hand_final = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final);
std::vector<JointState> joint_arm_final =
openarmjointconverter_->motor_to_joint(arm_motor_states_final);
std::vector<JointState> joint_hand_final =
openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final);
robot_state_->arm_state().set_all_references(joint_arm_final);
robot_state_->hand_state().set_all_references(joint_hand_final);
@ -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)
velocity_buffer_[i].pop_front();
if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE) velocity_buffer_[i].pop_front();
if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE)
continue;
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 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]) {

37
src/controller/control.hpp
View File

@ -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 <robot_state.hpp>
#include <memory>
#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, 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(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
size_t arm_joint_num, size_t hand_motor_num);
Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
std::string arm_type, size_t arm_joint_num, size_t hand_motor_num);
~Control();
std::shared_ptr<RobotSystemState> response_;
@ -75,13 +76,9 @@ class Control
void Setstate(int state);
void Shutdown(void);
void SetParameter(
const std::vector<double>& Kp,
const std::vector<double>& Kd,
const std::vector<double>& Fc,
const std::vector<double>& k,
const std::vector<double>& Fv,
const std::vector<double>& Fo);
void SetParameter(const std::vector<double> &Kp, const std::vector<double> &Kd,
const std::vector<double> &Fc, const std::vector<double> &k,
const std::vector<double> &Fv, const std::vector<double> &Fo);
bool AdjustPosition(void);

15
src/controller/diff.hpp
View File

@ -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];
}
}
};

56
src/controller/dynamics.cpp
View File

@ -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>(
kdl_chain, KDL::Vector(0, 0.0, -9.81));
solver = std::make_unique<KDL::ChainDynParam>(kdl_chain, KDL::Vector(0, 0.0, -9.81));
return true;
}
void Dynamics::GetGravity(const double *motor_position, double *gravity)
{
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)
R(i, j) = kdl_frame.M(i, j);
for (int j = 0; j < 3; ++j) R(i, j) = kdl_frame.M(i, j);
p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2];
}
void Dynamics::GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p)
{
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)
R(i, j) = kdl_frame.M(i, j);
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];
}

18
src/controller/dynamics.hpp
View File

@ -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 <Eigen/Dense>
#include <vector>
#include <iostream>
#include <fstream>
#include <kdl_parser/kdl_parser.hpp>
#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;

12
src/joint_state_converter.hpp
View File

@ -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)
m[i] = {j[i].position, j[i].velocity, j[i].effort};
for (size_t i = 0; i < j.size(); ++i) m[i] = {j[i].position, j[i].velocity, j[i].effort};
return m;
}

31
src/openarm_constants.hpp
View File

@ -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] = {
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};
@ -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::DM4340,
openarm::damiao_motor::MotorType::DM4340,
openarm::damiao_motor::MotorType::DM4310,
openarm::damiao_motor::MotorType::DM4310,
{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

10
src/openarm_port/joint_mapper.cpp
View File

@ -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,
double *joint_position) {
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];
@ -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,
double *joint_velocity) {
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];
@ -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,
double *motor_torque) {
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];

6
src/openarm_port/joint_mapper.hpp
View File

@ -21,9 +21,7 @@ public:
JointMapper();
~JointMapper();
void motor_to_joint_position(const double *motor_position,
double *joint_position);
void motor_to_joint_velocity(const double *motor_velocity,
double *joint_velocity);
void 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);
};

16
src/openarm_port/openarm_init.cpp
View File

@ -13,23 +13,20 @@
// 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,
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,
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);
@ -41,9 +38,7 @@ OpenArmInitializer::initialize_openarm(const std::string &can_device,
}
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;
}
@ -57,8 +52,7 @@ void OpenArmInitializer::initialize_(openarm::can::socket::OpenArm *openarm,
}
// Initialize gripper motor
openarm->init_gripper_motor(config.gripper_motor_type,
config.gripper_send_can_id,
openarm->init_gripper_motor(config.gripper_motor_type, config.gripper_send_can_id,
config.gripper_recv_can_id);
// Set callback mode for all motors

15
src/openarm_port/openarm_init.hpp
View File

@ -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 *
initialize_openarm(const std::string &can_device, bool enable_debug = true);
static openarm::can::socket::OpenArm *initialize_openarm(const std::string &can_device,
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 *
initialize_openarm(const std::string &can_device, const MotorConfig &config,
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);
static void initialize_(openarm::can::socket::OpenArm *openarm, const MotorConfig &config,
bool enable_debug);
};
} // namespace openarm_init

39
src/periodic_timer_thread.hpp
View File

@ -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 <time.h>
#include <thread>
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<int>(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) {
@ -59,10 +49,7 @@ public:
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;
@ -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_;

11
src/robot_state.hpp
View File

@ -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)
: response_(num_joints), reference_(num_joints) {}
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) {
@ -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 {
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_;

7
src/yamlloader.hpp
View File

@ -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());
}
}