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 }} group: ${{ github.head_ref || github.sha }}-${{ github.workflow }}
cancel-in-progress: true cancel-in-progress: true
jobs: 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: build:
name: Build name: Build
runs-on: ${{ matrix.runs-on }} 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 # 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
openarm_teleop_lib PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/src
)
target_link_libraries(openarm_teleop_lib target_link_libraries(
PUBLIC openarm_teleop_lib
OpenArmCAN::openarm_can PUBLIC OpenArmCAN::openarm_can Eigen3::Eigen ${orocos_kdl_LIBRARIES}
Eigen3::Eigen ${kdl_parser_LIBRARIES} urdfdom::urdfdom_model yaml-cpp::yaml-cpp)
${orocos_kdl_LIBRARIES}
${kdl_parser_LIBRARIES}
urdfdom::urdfdom_model
yaml-cpp::yaml-cpp
)
# ----------------------------- # -----------------------------
# Executables # Executables

47
control/gravity_compasation.cpp
View File

@ -14,16 +14,14 @@
#include <atomic> #include <atomic>
#include <chrono> #include <chrono>
#include <controller/dynamics.hpp>
#include <csignal> #include <csignal>
#include <iostream>
#include <thread>
#include <csignal>
#include <atomic>
#include <filesystem> #include <filesystem>
#include <iostream>
#include <openarm/can/socket/openarm.hpp> #include <openarm/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp> #include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <controller/dynamics.hpp>
#include <openarm_port/openarm_init.hpp> #include <openarm_port/openarm_init.hpp>
#include <thread>
std::atomic<bool> keep_running(true); std::atomic<bool> keep_running(true);
@ -41,10 +39,11 @@ int main(int argc, char** argv) {
std::string arm_side = "right_arm"; std::string arm_side = "right_arm";
std::string can_interface = "can0"; std::string can_interface = "can0";
if (argc < 4) { 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; return 1;
} }
@ -53,7 +52,8 @@ int main(int argc, char** argv) {
std::string urdf_path = argv[3]; std::string urdf_path = argv[3];
if (arm_side != "left_arm" && arm_side != "right_arm") { 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; return 1;
} }
@ -68,13 +68,14 @@ int main(int argc, char** argv) {
std::cout << "URDF path : " << urdf_path << std::endl; std::cout << "URDF path : " << urdf_path << std::endl;
std::string root_link = "openarm_body_link0"; 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); Dynamics arm_dynamics(urdf_path, root_link, leaf_link);
arm_dynamics.Init(); arm_dynamics.Init();
std::cout << "=== Initializing Leader OpenArm ===" << std::endl; std::cout << "=== Initializing Leader OpenArm ===" << std::endl;
openarm::can::socket::OpenArm *openarm = openarm::can::socket::OpenArm* openarm =
openarm_init::OpenArmInitializer::initialize_openarm(can_interface, true); openarm_init::OpenArmInitializer::initialize_openarm(can_interface, true);
std::this_thread::sleep_for(std::chrono::milliseconds(100)); std::this_thread::sleep_for(std::chrono::milliseconds(100));
@ -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_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> 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); std::vector<double> grav_torques(openarm->get_arm().get_motors().size(), 0.0);
while(keep_running){ while (keep_running) {
frame_count++; frame_count++;
auto current_time = std::chrono::high_resolution_clock::now(); auto current_time = std::chrono::high_resolution_clock::now();
// Calculate and display Hz every second // 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>(
if (time_since_last_display >= 1000) { // Every 1000ms (1 second) current_time - last_hz_display)
auto total_time = std::chrono::duration_cast<std::chrono::milliseconds>(current_time - start_time).count(); .count();
if (time_since_last_display >= 1000) { // Every 1000ms (1 second)
auto total_time =
std::chrono::duration_cast<std::chrono::milliseconds>(current_time - start_time)
.count();
double hz = (frame_count * 1000.0) / total_time; double hz = (frame_count * 1000.0) / total_time;
std::cout << "=== Loop Frequency: " << hz << " Hz ===" << std::endl; std::cout << "=== Loop Frequency: " << hz << " Hz ===" << std::endl;
last_hz_display = current_time; last_hz_display = current_time;
@ -112,7 +118,7 @@ int main(int argc, char** argv) {
arm_dynamics.GetGravity(arm_joint_positions.data(), grav_torques.data()); arm_dynamics.GetGravity(arm_joint_positions.data(), grav_torques.data());
for(size_t i = 0; i < openarm->get_arm().get_motors().size(); ++i){ for (size_t i = 0; i < openarm->get_arm().get_motors().size(); ++i) {
// std::cout << "grav_torques[" << i << "] = " << grav_torques[i] << std::endl; // std::cout << "grav_torques[" << i << "] = " << grav_torques[i] << std::endl;
} }
@ -120,12 +126,11 @@ int main(int argc, char** argv) {
cmds.reserve(grav_torques.size()); cmds.reserve(grav_torques.size());
std::transform(grav_torques.begin(), grav_torques.end(), std::back_inserter(cmds), std::transform(grav_torques.begin(), grav_torques.end(), std::back_inserter(cmds),
[](double t) { return openarm::damiao_motor::MITParam{0, 0, 0, 0, t}; }); [](double t) { return openarm::damiao_motor::MITParam{0, 0, 0, 0, t}; });
openarm->get_arm().mit_control_all(cmds); openarm->get_arm().mit_control_all(cmds);
openarm->recv_all(); openarm->recv_all();
} }
std::this_thread::sleep_for(std::chrono::milliseconds(100)); std::this_thread::sleep_for(std::chrono::milliseconds(100));

235
control/openarm_bilateral_control.cpp
View File

@ -12,28 +12,23 @@
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#include <atomic> #include <atomic>
#include <chrono> #include <chrono>
#include <controller/control.hpp>
#include <controller/dynamics.hpp>
#include <csignal> #include <csignal>
#include <iostream>
#include <thread>
#include <csignal>
#include <atomic>
#include <filesystem> #include <filesystem>
#include <iostream>
#include <periodic_timer_thread.hpp>
#include <robot_state.hpp>
#include <openarm/can/socket/openarm.hpp> #include <openarm/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp> #include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <openarm_port/openarm_init.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 <yamlloader.hpp>
#include <controller/control.hpp>
std::atomic<bool> keep_running(true); std::atomic<bool> keep_running(true);
void signal_handler(int signal) { void signal_handler(int signal) {
if (signal == SIGINT) { if (signal == SIGINT) {
std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl;
@ -41,130 +36,115 @@ void signal_handler(int signal) {
} }
} }
class LeaderArmThread : public PeriodicTimerThread { class LeaderArmThread : public PeriodicTimerThread {
public: 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,
: PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l){} double hz = 500.0)
: PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {}
protected: protected:
void before_start() override { std::cout << "leader start thread " << std::endl; }
void before_start() override { void after_stop() override { std::cout << "leader stop thread " << std::endl; }
std::cout << "leader start thread " << std::endl;
}
void after_stop() override { void on_timer() override {
std::cout << "leader stop thread " << std::endl; static auto prev_time = std::chrono::steady_clock::now();
}
void on_timer() override { control_l_->bilateral_step();
static auto prev_time = std::chrono::steady_clock::now();
control_l_->bilateral_step(); auto now = std::chrono::steady_clock::now();
auto now = std::chrono::steady_clock::now(); auto elapsed_us =
std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
prev_time = now;
auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count(); // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
prev_time = now; }
// std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
}
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_l_;
};
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_l_;
};
class FollowerArmThread : public PeriodicTimerThread { class FollowerArmThread : public PeriodicTimerThread {
public: 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,
: PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} double hz = 500.0)
: PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {}
protected: 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 { void on_timer() override {
static auto prev_time = std::chrono::steady_clock::now(); static auto prev_time = std::chrono::steady_clock::now();
control_f_->bilateral_step(); control_f_->bilateral_step();
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count(); auto elapsed_us =
prev_time = now; std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
prev_time = now;
// std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
}
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_f_;
};
// std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
}
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_f_;
};
class AdminThread : public PeriodicTimerThread { class AdminThread : public PeriodicTimerThread {
public: public:
AdminThread(std::shared_ptr<RobotSystemState> leader_state, 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: 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 { void on_timer() override {
static auto prev_time = std::chrono::steady_clock::now();
auto now = std::chrono::steady_clock::now();
static auto prev_time = std::chrono::steady_clock::now(); // get response
auto now = std::chrono::steady_clock::now(); auto leader_arm_resp = leader_state_->arm_state().get_all_responses();
auto follower_arm_resp = follower_state_->arm_state().get_all_responses();
// get response auto leader_hand_resp = leader_state_->hand_state().get_all_responses();
auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); auto follower_hand_resp = follower_state_->hand_state().get_all_responses();
auto follower_arm_resp = follower_state_->arm_state().get_all_responses();
auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); // set referense
auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); leader_state_->arm_state().set_all_references(follower_arm_resp);
leader_state_->hand_state().set_all_references(follower_hand_resp);
//set referense follower_state_->arm_state().set_all_references(leader_arm_resp);
leader_state_->arm_state().set_all_references(follower_arm_resp); follower_state_->hand_state().set_all_references(leader_hand_resp);
leader_state_->hand_state().set_all_references(follower_hand_resp);
follower_state_->arm_state().set_all_references(leader_arm_resp); auto elapsed_us =
follower_state_->hand_state().set_all_references(leader_hand_resp); std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
prev_time = now;
auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count(); // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
prev_time = now; }
// std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; private:
} std::shared_ptr<RobotSystemState> leader_state_;
std::shared_ptr<RobotSystemState> follower_state_;
Control *control_l_;
Control *control_f_;
};
private: int main(int argc, char **argv) {
std::shared_ptr<RobotSystemState> leader_state_;
std::shared_ptr<RobotSystemState> follower_state_;
Control *control_l_;
Control *control_f_;
};
int main(int argc, char** argv) {
try { try {
std::signal(SIGINT, signal_handler); std::signal(SIGINT, signal_handler);
@ -175,7 +155,10 @@ int main(int argc, char** argv) {
std::string follower_can_interface = "can2"; std::string follower_can_interface = "can2";
if (argc < 3) { 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; return 1;
} }
@ -187,7 +170,8 @@ int main(int argc, char** argv) {
if (argc >= 4) { if (argc >= 4) {
arm_side = argv[3]; arm_side = argv[3];
if (arm_side != "left_arm" && arm_side != "right_arm") { 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; return 1;
} }
} }
@ -210,7 +194,8 @@ int main(int argc, char** argv) {
// Setup dynamics // Setup dynamics
std::string root_link = "openarm_body_link0"; 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 // Output confirmation
std::cout << "=== OpenArm Bilateral Control ===" << std::endl; std::cout << "=== OpenArm Bilateral Control ===" << std::endl;
@ -229,7 +214,7 @@ int main(int argc, char** argv) {
std::vector<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); std::vector<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp");
std::vector<double> leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); std::vector<double> leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd");
std::vector<double> leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); std::vector<double> leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc");
std::vector<double> leader_k = leader_loader.get_vector("LeaderArmParam", "k"); std::vector<double> leader_k = leader_loader.get_vector("LeaderArmParam", "k");
std::vector<double> leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); std::vector<double> leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv");
std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo");
@ -237,7 +222,7 @@ int main(int argc, char** argv) {
std::vector<double> follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); std::vector<double> follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp");
std::vector<double> follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); std::vector<double> follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd");
std::vector<double> follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc"); std::vector<double> follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc");
std::vector<double> follower_k = follower_loader.get_vector("FollowerArmParam", "k"); std::vector<double> follower_k = follower_loader.get_vector("FollowerArmParam", "k");
std::vector<double> follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv"); std::vector<double> follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv");
std::vector<double> follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo"); std::vector<double> follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo");
@ -267,33 +252,37 @@ int main(int argc, char** argv) {
// Declare robot_state (Joint and motor counts are assumed to be equal) // Declare robot_state (Joint and motor counts are assumed to be equal)
std::shared_ptr<RobotSystemState> leader_state = std::shared_ptr<RobotSystemState> leader_state =
std::make_shared<RobotSystemState>(leader_arm_motor_num, leader_hand_motor_num); std::make_shared<RobotSystemState>(leader_arm_motor_num, leader_hand_motor_num);
std::shared_ptr<RobotSystemState> follower_state = std::shared_ptr<RobotSystemState> follower_state =
std::make_shared<RobotSystemState>(follower_arm_motor_num, follower_hand_motor_num); 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 // 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 // set home postion
std::thread thread_l(&Control::AdjustPosition, control_leader); std::thread thread_l(&Control::AdjustPosition, control_leader);
std::thread thread_f(&Control::AdjustPosition, control_follower); std::thread thread_f(&Control::AdjustPosition, control_follower);
thread_l.join(); thread_l.join();
thread_f.join(); thread_f.join();
// Start control process // Start control process
LeaderArmThread leader_thread(leader_state ,control_leader, FREQUENCY); LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY);
FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); 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 // thread start in control
leader_thread.start_thread(); leader_thread.start_thread();
@ -311,9 +300,7 @@ int main(int argc, char** argv) {
leader_openarm->disable_all(); leader_openarm->disable_all();
follower_openarm->disable_all(); follower_openarm->disable_all();
} } catch (const std::exception &e) {
catch(const std::exception& e)
{
std::cerr << e.what() << '\n'; std::cerr << e.what() << '\n';
} }

12
control/openarm_communication_test.cpp
View File

@ -19,9 +19,6 @@
#include <openarm/can/socket/openarm.hpp> #include <openarm/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp> #include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <thread> #include <thread>
#include <csignal>
#include <atomic>
int main(int argc, char** argv) { int main(int argc, char** argv) {
try { try {
@ -29,13 +26,12 @@ int main(int argc, char** argv) {
std::cout << "This example demonstrates the OpenArm API functionality" << std::endl; std::cout << "This example demonstrates the OpenArm API functionality" << std::endl;
std::string can_interface = "can0"; std::string can_interface = "can0";
if (argc > 1 ){ if (argc > 1) {
can_interface = argv[1]; can_interface = argv[1];
} }
std::cout << "[INFO] Using CAN interface: " << can_interface << std::endl; std::cout << "[INFO] Using CAN interface: " << can_interface << std::endl;
// Initialize OpenArm with CAN interface and enable CAN-FD // Initialize OpenArm with CAN interface and enable CAN-FD
std::cout << "Initializing OpenArm CAN..." << std::endl; std::cout << "Initializing OpenArm CAN..." << std::endl;
openarm::can::socket::OpenArm openarm(can_interface, true); // Use CAN-FD on can0 interface 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::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::DM4340,
openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310, 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> send_can_ids = {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07};
std::vector<uint32_t> recv_can_ids = {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}; 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::damiao_motor::MITParam{0, 0, 0, 0, 0},
openarm::damiao_motor::MITParam{0, 0, 0, 0, 0} openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}});
});
openarm.get_gripper().mit_control_all({openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}}); openarm.get_gripper().mit_control_all({openarm::damiao_motor::MITParam{0, 0, 0, 0, 0}});

232
control/openarm_unilateral_control.cpp
View File

@ -12,28 +12,23 @@
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#include <atomic> #include <atomic>
#include <chrono> #include <chrono>
#include <controller/control.hpp>
#include <controller/dynamics.hpp>
#include <csignal> #include <csignal>
#include <iostream>
#include <thread>
#include <csignal>
#include <atomic>
#include <filesystem> #include <filesystem>
#include <iostream>
#include <periodic_timer_thread.hpp>
#include <robot_state.hpp>
#include <openarm/can/socket/openarm.hpp> #include <openarm/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp> #include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <openarm_port/openarm_init.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 <yamlloader.hpp>
#include <controller/control.hpp>
std::atomic<bool> keep_running(true); std::atomic<bool> keep_running(true);
void signal_handler(int signal) { void signal_handler(int signal) {
if (signal == SIGINT) { if (signal == SIGINT) {
std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl; std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl;
@ -41,129 +36,115 @@ void signal_handler(int signal) {
} }
} }
class LeaderArmThread : public PeriodicTimerThread { class LeaderArmThread : public PeriodicTimerThread {
public: 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,
: PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l){} double hz = 500.0)
: PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {}
protected: protected:
void before_start() override { std::cout << "leader start thread " << std::endl; }
void before_start() override { void after_stop() override { std::cout << "leader stop thread " << std::endl; }
std::cout << "leader start thread " << std::endl;
}
void after_stop() override { void on_timer() override {
std::cout << "leader stop thread " << std::endl; static auto prev_time = std::chrono::steady_clock::now();
}
void on_timer() override { control_l_->unilateral_step();
static auto prev_time = std::chrono::steady_clock::now();
control_l_->unilateral_step(); auto now = std::chrono::steady_clock::now();
auto now = std::chrono::steady_clock::now(); auto elapsed_us =
std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
prev_time = now;
auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count(); // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
prev_time = now; }
// std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl;
}
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_l_;
};
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_l_;
};
class FollowerArmThread : public PeriodicTimerThread { class FollowerArmThread : public PeriodicTimerThread {
public: 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,
: PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {} double hz = 500.0)
: PeriodicTimerThread(hz), robot_state_(robot_state), control_f_(control_f) {}
protected: 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 { void on_timer() override {
static auto prev_time = std::chrono::steady_clock::now(); static auto prev_time = std::chrono::steady_clock::now();
control_f_->unilateral_step(); control_f_->unilateral_step();
auto now = std::chrono::steady_clock::now(); auto now = std::chrono::steady_clock::now();
auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count(); auto elapsed_us =
prev_time = now; std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
prev_time = now;
// std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl; // std::cout << "[Follower] Period: " << elapsed_us << " us" << std::endl;
} }
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_f_;
};
private:
std::shared_ptr<RobotSystemState> robot_state_;
Control *control_f_;
};
class AdminThread : public PeriodicTimerThread { class AdminThread : public PeriodicTimerThread {
public: public:
AdminThread(std::shared_ptr<RobotSystemState> leader_state, 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: 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 { void on_timer() override {
static auto prev_time = std::chrono::steady_clock::now();
auto now = std::chrono::steady_clock::now();
static auto prev_time = std::chrono::steady_clock::now(); // get response
auto now = std::chrono::steady_clock::now(); auto leader_arm_resp = leader_state_->arm_state().get_all_responses();
auto follower_arm_resp = follower_state_->arm_state().get_all_responses();
// get response auto leader_hand_resp = leader_state_->hand_state().get_all_responses();
auto leader_arm_resp = leader_state_->arm_state().get_all_responses(); auto follower_hand_resp = follower_state_->hand_state().get_all_responses();
auto follower_arm_resp = follower_state_->arm_state().get_all_responses();
auto leader_hand_resp = leader_state_->hand_state().get_all_responses(); // set referense
auto follower_hand_resp = follower_state_->hand_state().get_all_responses(); leader_state_->arm_state().set_all_references(follower_arm_resp);
leader_state_->hand_state().set_all_references(follower_hand_resp);
//set referense follower_state_->arm_state().set_all_references(leader_arm_resp);
leader_state_->arm_state().set_all_references(follower_arm_resp); follower_state_->hand_state().set_all_references(leader_hand_resp);
leader_state_->hand_state().set_all_references(follower_hand_resp);
follower_state_->arm_state().set_all_references(leader_arm_resp); auto elapsed_us =
follower_state_->hand_state().set_all_references(leader_hand_resp); std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count();
prev_time = now;
auto elapsed_us = std::chrono::duration_cast<std::chrono::microseconds>(now - prev_time).count(); // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
prev_time = now; }
// std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; private:
} std::shared_ptr<RobotSystemState> leader_state_;
std::shared_ptr<RobotSystemState> follower_state_;
Control *control_l_;
Control *control_f_;
};
private: int main(int argc, char **argv) {
std::shared_ptr<RobotSystemState> leader_state_;
std::shared_ptr<RobotSystemState> follower_state_;
Control *control_l_;
Control *control_f_;
};
int main(int argc, char** argv) {
try { try {
std::signal(SIGINT, signal_handler); std::signal(SIGINT, signal_handler);
@ -175,7 +156,10 @@ int main(int argc, char** argv) {
std::string follower_can_interface = "can2"; std::string follower_can_interface = "can2";
if (argc < 3) { 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; return 1;
} }
@ -187,7 +171,8 @@ int main(int argc, char** argv) {
if (argc >= 4) { if (argc >= 4) {
arm_side = argv[3]; arm_side = argv[3];
if (arm_side != "left_arm" && arm_side != "right_arm") { 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; return 1;
} }
} }
@ -210,7 +195,8 @@ int main(int argc, char** argv) {
// Setup dynamics // Setup dynamics
std::string root_link = "openarm_body_link0"; 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 // Output confirmation
std::cout << "=== OpenArm Unilateral Control ===" << std::endl; std::cout << "=== OpenArm Unilateral Control ===" << std::endl;
@ -229,7 +215,7 @@ int main(int argc, char** argv) {
std::vector<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); std::vector<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp");
std::vector<double> leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); std::vector<double> leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd");
std::vector<double> leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); std::vector<double> leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc");
std::vector<double> leader_k = leader_loader.get_vector("LeaderArmParam", "k"); std::vector<double> leader_k = leader_loader.get_vector("LeaderArmParam", "k");
std::vector<double> leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); std::vector<double> leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv");
std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo");
@ -237,7 +223,7 @@ int main(int argc, char** argv) {
std::vector<double> follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); std::vector<double> follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp");
std::vector<double> follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); std::vector<double> follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd");
std::vector<double> follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc"); std::vector<double> follower_Fc = follower_loader.get_vector("FollowerArmParam", "Fc");
std::vector<double> follower_k = follower_loader.get_vector("FollowerArmParam", "k"); std::vector<double> follower_k = follower_loader.get_vector("FollowerArmParam", "k");
std::vector<double> follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv"); std::vector<double> follower_Fv = follower_loader.get_vector("FollowerArmParam", "Fv");
std::vector<double> follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo"); std::vector<double> follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo");
@ -267,32 +253,36 @@ int main(int argc, char** argv) {
// Declare robot_state (Joint and motor counts are assumed to be equal) // Declare robot_state (Joint and motor counts are assumed to be equal)
std::shared_ptr<RobotSystemState> leader_state = std::shared_ptr<RobotSystemState> leader_state =
std::make_shared<RobotSystemState>(leader_arm_motor_num, leader_hand_motor_num); std::make_shared<RobotSystemState>(leader_arm_motor_num, leader_hand_motor_num);
std::shared_ptr<RobotSystemState> follower_state = std::shared_ptr<RobotSystemState> follower_state =
std::make_shared<RobotSystemState>(follower_arm_motor_num, follower_hand_motor_num); 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 // set home postion
std::thread thread_l(&Control::AdjustPosition, control_leader); std::thread thread_l(&Control::AdjustPosition, control_leader);
std::thread thread_f(&Control::AdjustPosition, control_follower); std::thread thread_f(&Control::AdjustPosition, control_follower);
thread_l.join(); thread_l.join();
thread_f.join(); thread_f.join();
// Start control process // Start control process
LeaderArmThread leader_thread(leader_state ,control_leader, FREQUENCY); LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY);
FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); 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(); leader_thread.start_thread();
follower_thread.start_thread(); follower_thread.start_thread();
@ -309,9 +299,7 @@ int main(int argc, char** argv) {
leader_openarm->disable_all(); leader_openarm->disable_all();
follower_openarm->disable_all(); follower_openarm->disable_all();
} } catch (const std::exception &e) {
catch(const std::exception& e)
{
std::cerr << e.what() << '\n'; std::cerr << e.what() << '\n';
} }

64
script/launch_bilateral.sh
View File

@ -15,34 +15,34 @@
# limitations under the License. # limitations under the License.
# ========= Configuration ========= # ========= Configuration =========
ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm
LEADER_CAN_IF=$2 # Optional: leader CAN interface LEADER_CAN_IF=$2 # Optional: leader CAN interface
FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface
ARM_TYPE="v10" # Fixed for now ARM_TYPE="v10" # Fixed for now
TMPDIR="/tmp/openarm_urdf_gen" TMPDIR="/tmp/openarm_urdf_gen"
# Validate arm side # Validate arm side
if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then
echo "[ERROR] Invalid arm_side: $ARM_SIDE" echo "[ERROR] Invalid arm_side: $ARM_SIDE"
echo "Usage: $0 <arm_side: right_arm|left_arm> [leader_can_if] [follower_can_if]" echo "Usage: $0 <arm_side: right_arm|left_arm> [leader_can_if] [follower_can_if]"
exit 1 exit 1
fi fi
# Set default CAN interfaces if not provided # Set default CAN interfaces if not provided
if [ -z "$LEADER_CAN_IF" ]; then if [ -z "$LEADER_CAN_IF" ]; then
if [ "$ARM_SIDE" = "right_arm" ]; then if [ "$ARM_SIDE" = "right_arm" ]; then
LEADER_CAN_IF="can0" LEADER_CAN_IF="can0"
else else
LEADER_CAN_IF="can1" LEADER_CAN_IF="can1"
fi fi
fi fi
if [ -z "$FOLLOWER_CAN_IF" ]; then if [ -z "$FOLLOWER_CAN_IF" ]; then
if [ "$ARM_SIDE" = "right_arm" ]; then if [ "$ARM_SIDE" = "right_arm" ]; then
FOLLOWER_CAN_IF="can2" FOLLOWER_CAN_IF="can2"
else else
FOLLOWER_CAN_IF="can3" FOLLOWER_CAN_IF="can3"
fi fi
fi fi
# File paths # File paths
@ -56,45 +56,43 @@ echo $BIN_PATH
# ================================ # ================================
# Check workspace # Check workspace
if [ ! -d "$WS_DIR" ]; then if [ ! -d "$WS_DIR" ]; then
echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 echo "[ERROR] Could not find workspace at: $WS_DIR" >&2
echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2
echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2
exit 1 exit 1
fi fi
# Check openarm_description package # Check openarm_description package
if [ ! -d "$WS_DIR/src/openarm_description" ]; then if [ ! -d "$WS_DIR/src/openarm_description" ]; then
echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2
echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2
exit 1 exit 1
fi fi
# Check xacro # Check xacro
if [ ! -f "$XACRO_PATH" ]; then if [ ! -f "$XACRO_PATH" ]; then
echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2
exit 1 exit 1
fi fi
# Check binary # Check binary
if [ ! -f "$BIN_PATH" ]; then if [ ! -f "$BIN_PATH" ]; then
echo "[ERROR] Compiled binary not found at: $BIN_PATH" echo "[ERROR] Compiled binary not found at: $BIN_PATH"
exit 1 exit 1
fi fi
# Source ROS 2 # Source ROS 2
# shellcheck source=/dev/null
source "$WS_DIR/install/setup.bash" source "$WS_DIR/install/setup.bash"
# Generate URDFs # Generate URDFs
echo "[INFO] Generating URDFs using xacro..." echo "[INFO] Generating URDFs using xacro..."
mkdir -p "$TMPDIR" 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." echo "[ERROR] Failed to generate URDFs."
exit 1 exit 1
fi fi
cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH"
# Run binary # Run binary
echo "[INFO] Launching bilateral control..." echo "[INFO] Launching bilateral control..."

30
script/launch_grav_comp.sh
View File

@ -24,44 +24,42 @@ XACRO_FILE="${ARM_TYPE}.urdf.xacro"
WS_DIR=~/openarm_ros2_ws WS_DIR=~/openarm_ros2_ws
XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE"
URDF_OUT="$TMPDIR/$URDF_NAME" URDF_OUT="$TMPDIR/$URDF_NAME"
BIN_PATH=~/openarm_teleop/build/gravity_comp # adjust if needed BIN_PATH=~/openarm_teleop/build/gravity_comp # adjust if needed
# =============================== # ===============================
# Check workspace # Check workspace
if [ ! -d "$WS_DIR" ]; then if [ ! -d "$WS_DIR" ]; then
echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 echo "[ERROR] Could not find workspace at: $WS_DIR" >&2
echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2
echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2
exit 1 exit 1
fi fi
# Check openarm_description package # Check openarm_description package
if [ ! -d "$WS_DIR/src/openarm_description" ]; then if [ ! -d "$WS_DIR/src/openarm_description" ]; then
echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2
echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2
exit 1 exit 1
fi fi
# Check xacro # Check xacro
if [ ! -f "$XACRO_PATH" ]; then if [ ! -f "$XACRO_PATH" ]; then
echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2
exit 1 exit 1
fi fi
# Check build binary # Check build binary
if [ ! -f "$BIN_PATH" ]; then if [ ! -f "$BIN_PATH" ]; then
echo "[ERROR] Compiled binary not found at: $BIN_PATH" echo "[ERROR] Compiled binary not found at: $BIN_PATH"
exit 1 exit 1
fi fi
# Generate URDF # Generate URDF
echo "[INFO] Generating URDF using xacro..." echo "[INFO] Generating URDF using xacro..."
# shellcheck source=/dev/null
source $WS_DIR/install/setup.bash source $WS_DIR/install/setup.bash
mkdir -p "$TMPDIR" 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." echo "[ERROR] Failed to generate URDF."
exit 1 exit 1
fi fi

63
script/launch_unilateral.sh
View File

@ -15,34 +15,34 @@
# limitations under the License. # limitations under the License.
# ========= Configuration ========= # ========= Configuration =========
ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm ARM_SIDE=${1:-right_arm} # Required: left_arm or right_arm
LEADER_CAN_IF=$2 # Optional: leader CAN interface LEADER_CAN_IF=$2 # Optional: leader CAN interface
FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface FOLLOWER_CAN_IF=$3 # Optional: follower CAN interface
ARM_TYPE="v10" # Fixed for now ARM_TYPE="v10" # Fixed for now
TMPDIR="/tmp/openarm_urdf_gen" TMPDIR="/tmp/openarm_urdf_gen"
# Validate arm side # Validate arm side
if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then
echo "[ERROR] Invalid arm_side: $ARM_SIDE" echo "[ERROR] Invalid arm_side: $ARM_SIDE"
echo "Usage: $0 <arm_side: right_arm|left_arm> [leader_can_if] [follower_can_if]" echo "Usage: $0 <arm_side: right_arm|left_arm> [leader_can_if] [follower_can_if]"
exit 1 exit 1
fi fi
# Set default CAN interfaces if not provided # Set default CAN interfaces if not provided
if [ -z "$LEADER_CAN_IF" ]; then if [ -z "$LEADER_CAN_IF" ]; then
if [ "$ARM_SIDE" = "right_arm" ]; then if [ "$ARM_SIDE" = "right_arm" ]; then
LEADER_CAN_IF="can0" LEADER_CAN_IF="can0"
else else
LEADER_CAN_IF="can1" LEADER_CAN_IF="can1"
fi fi
fi fi
if [ -z "$FOLLOWER_CAN_IF" ]; then if [ -z "$FOLLOWER_CAN_IF" ]; then
if [ "$ARM_SIDE" = "right_arm" ]; then if [ "$ARM_SIDE" = "right_arm" ]; then
FOLLOWER_CAN_IF="can2" FOLLOWER_CAN_IF="can2"
else else
FOLLOWER_CAN_IF="can3" FOLLOWER_CAN_IF="can3"
fi fi
fi fi
# File paths # File paths
@ -55,46 +55,45 @@ BIN_PATH=~/openarm_teleop/build/unilateral_control
# Check workspace # Check workspace
if [ ! -d "$WS_DIR" ]; then if [ ! -d "$WS_DIR" ]; then
echo "[ERROR] Could not find workspace at: $WS_DIR" >&2 echo "[ERROR] Could not find workspace at: $WS_DIR" >&2
echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2 echo "We assume the default ROS 2 workspace is ~/openarm_ros2_ws." >&2
echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2 echo "If you are using a different workspace, please update WS_DIR in this launch script." >&2
exit 1 exit 1
fi fi
# Check openarm_description package # Check openarm_description package
if [ ! -d "$WS_DIR/src/openarm_description" ]; then if [ ! -d "$WS_DIR/src/openarm_description" ]; then
echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2 echo "[ERROR] Could not find package: $WS_DIR/src/openarm_description" >&2
echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2 echo "Please make sure to clone openarm_description into $WS_DIR/src/" >&2
exit 1 exit 1
fi fi
# Check xacro # Check xacro
if [ ! -f "$XACRO_PATH" ]; then if [ ! -f "$XACRO_PATH" ]; then
echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2 echo "[ERROR] Could not find ${XACRO_FILE} under $WS_DIR/src/openarm_description/urdf/robot/" >&2
exit 1 exit 1
fi fi
# ================================ # ================================
# Check binary # Check binary
if [ ! -f "$BIN_PATH" ]; then if [ ! -f "$BIN_PATH" ]; then
echo "[ERROR] Compiled binary not found at: $BIN_PATH" echo "[ERROR] Compiled binary not found at: $BIN_PATH"
exit 1 exit 1
fi fi
# Source ROS 2 # Source ROS 2
# shellcheck source=/dev/null
source "$WS_DIR/install/setup.bash" source "$WS_DIR/install/setup.bash"
# Generate URDFs # Generate URDFs
echo "[INFO] Generating URDFs using xacro..." echo "[INFO] Generating URDFs using xacro..."
mkdir -p "$TMPDIR" 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." echo "[ERROR] Failed to generate URDFs."
exit 1 exit 1
fi fi
cp "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH"
# Run binary # Run binary
echo "[INFO] Launching unilateral control..." echo "[INFO] Launching unilateral control..."

810
src/controller/control.cpp
View File

@ -12,38 +12,54 @@
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#include <cmath>
#include <unistd.h>
#include <string.h> #include <string.h>
#include <thread> #include <unistd.h>
#include <cmath>
#include <algorithm> #include <algorithm>
#include <iomanip> #include <cmath>
#include <controller/control.hpp> #include <controller/control.hpp>
#include <controller/dynamics.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)
differentiator_ = new Differentiator(Ts); : openarm_(arm),
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); dynamics_l_(dynamics_l),
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); 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)
differentiator_ = new Differentiator(Ts); : openarm_(arm),
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_); dynamics_l_(dynamics_l),
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_); dynamics_f_(dynamics_f),
robot_state_(robot_state),
Ts_(Ts),
role_(role),
arm_motor_num_(arm_motor_num),
hand_motor_num_(hand_motor_num) {
differentiator_ = new Differentiator(Ts);
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
arm_type_ = arm_type; arm_type_ = arm_type;
} }
Control::~Control() { Control::~Control() {
std::cout << "Control destructed " << std::endl; std::cout << "Control destructed " << std::endl;
delete openarmjointconverter_; delete openarmjointconverter_;
delete differentiator_; delete differentiator_;
} }
// bool Control::Setup(void) // bool Control::Setup(void)
@ -57,137 +73,237 @@ Control::~Control() {
// return true; // return true;
// } // }
void Control::Shutdown(void){ void Control::Shutdown(void) {
std::cout << "control shutdown !!!" << std::endl; std::cout << "control shutdown !!!" << std::endl;
openarm_->disable_all(); 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, Kp_ = Kp;
const std::vector<double>& k, Kd_ = Kd;
const std::vector<double>& Fv, Fc_ = Fc;
const std::vector<double>& Fo) k_ = k;
{ Fv_ = Fv;
Kp_ = Kp; Fo_ = Fo;
Kd_ = Kd; }
Fc_ = Fc;
k_ = k;
Fv_ = Fv;
Fo_ = Fo;
}
bool Control::bilateral_step() {
// get motor status
std::vector<MotorState> arm_motor_states;
const auto& arm_motors = openarm_->get_arm().get_motors();
for (size_t i = 0; i < arm_motors.size(); ++i) {
const auto& motor = arm_motors[i];
arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0});
}
bool Control::bilateral_step() std::vector<MotorState> gripper_motor_states;
{ const auto& gripper_motors = openarm_->get_gripper().get_motors();
// get motor status for (size_t i = 0; i < gripper_motors.size(); ++i) {
std::vector<MotorState> arm_motor_states; const auto& motor = gripper_motors[i];
const auto& arm_motors = openarm_->get_arm().get_motors(); gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0});
for (size_t i = 0; i < arm_motors.size(); ++i) { }
const auto& motor = arm_motors[i];
arm_motor_states.push_back({
motor.get_position(),
motor.get_velocity(),
0
});
}
std::vector<MotorState> gripper_motor_states; // convert joint to motor
const auto& gripper_motors = openarm_->get_gripper().get_motors(); std::vector<JointState> joint_arm_states =
for (size_t i = 0; i < gripper_motors.size(); ++i) { openarmjointconverter_->motor_to_joint(arm_motor_states);
const auto& motor = gripper_motors[i]; std::vector<JointState> joint_gripper_states =
gripper_motor_states.push_back({ openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
motor.get_position(),
motor.get_velocity(),
0
});
}
// convert joint to motor // set reponse
std::vector<JointState> joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states); robot_state_->arm_state().set_all_responses(joint_arm_states);
std::vector<JointState> joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states); robot_state_->hand_state().set_all_responses(joint_gripper_states);
// set reponse size_t arm_dof = robot_state_->arm_state().get_size();
robot_state_->arm_state().set_all_responses(joint_arm_states); size_t gripper_dof = robot_state_->hand_state().get_size();
robot_state_->hand_state().set_all_responses(joint_gripper_states);
size_t arm_dof = robot_state_->arm_state().get_size(); std::vector<double> joint_arm_positions(arm_dof, 0.0);
size_t gripper_dof = robot_state_->hand_state().get_size(); std::vector<double> joint_arm_velocities(arm_dof, 0.0);
std::vector<double> joint_arm_efforts(arm_dof, 0.0);
std::vector<double> joint_arm_positions(arm_dof, 0.0); std::vector<double> joint_gripper_positions(gripper_dof, 0.0);
std::vector<double> joint_arm_velocities(arm_dof, 0.0); std::vector<double> joint_gripper_velocities(gripper_dof, 0.0);
std::vector<double> joint_arm_efforts(arm_dof, 0.0); std::vector<double> joint_gripper_efforts(gripper_dof, 0.0);
std::vector<double> joint_gripper_positions(gripper_dof, 0.0); for (size_t i = 0; i < arm_dof; ++i) {
std::vector<double> joint_gripper_velocities(gripper_dof, 0.0); joint_arm_positions[i] = joint_arm_states[i].position;
std::vector<double> joint_gripper_efforts(gripper_dof, 0.0); joint_arm_velocities[i] = joint_arm_states[i].velocity;
}
for (size_t i = 0; i < arm_dof; ++i) { for (size_t i = 0; i < gripper_dof; ++i) {
joint_arm_positions[i] = joint_arm_states[i].position; joint_gripper_positions[i] = joint_gripper_states[i].position;
joint_arm_velocities[i] = joint_arm_states[i].velocity; joint_gripper_velocities[i] = joint_gripper_states[i].velocity;
} }
for (size_t i = 0; i < gripper_dof; ++i) { std::vector<double> gravity(arm_dof, 0.0);
joint_gripper_positions[i] = joint_gripper_states[i].position; std::vector<double> coriolis(arm_dof, 0.0);
joint_gripper_velocities[i] = joint_gripper_states[i].velocity; std::vector<double> friction(arm_dof + gripper_dof, 0.0);
}
std::vector<double> gravity(arm_dof, 0.0); std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
std::vector<double> coriolis(arm_dof, 0.0); std::vector<JointState> joint_gripper_states_ref =
std::vector<double> friction(arm_dof + gripper_dof, 0.0); robot_state_->hand_state().get_all_references();
std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references(); std::vector<double> joint_arm_positions_ref(arm_dof);
std::vector<JointState> joint_gripper_states_ref = robot_state_->hand_state().get_all_references();
std::vector<double> joint_arm_positions_ref(arm_dof); for (size_t i = 0; i < arm_dof; ++i) {
joint_arm_positions_ref[i] = joint_arm_states_ref[i].position;
}
for (size_t i = 0; i < arm_dof; ++i) { if (role_ == ROLE_LEADER) {
joint_arm_positions_ref[i] = joint_arm_states_ref[i].position; dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data());
} dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(),
coriolis.data());
} else if (role_ == ROLE_FOLLOWER) {
dynamics_f_->GetGravity(joint_arm_positions.data(), gravity.data());
dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(),
coriolis.data());
}
if (role_ == ROLE_LEADER) { // Friction (compute joint friction)
dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data()); for (size_t i = 0; i < joint_arm_velocities.size(); ++i)
dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); ComputeFriction(joint_arm_velocities.data(), friction.data(), i);
for (size_t i = 0; i < joint_gripper_velocities.size(); ++i)
ComputeFriction(joint_gripper_velocities.data(), friction.data(),
joint_arm_velocities.size() + i);
} else if (role_ == ROLE_FOLLOWER) { // set gravity and friciton comp joint torque value
dynamics_f_->GetGravity(joint_arm_positions.data(), gravity.data()); for (size_t i = 0; i < arm_dof; i++) {
dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data()); joint_arm_states_ref[i].effort = gravity[i] + friction[i];
} }
for (size_t i = 0; i < gripper_dof; i++) {
joint_gripper_states_ref[i].effort = friction[i + arm_dof];
}
std::vector<MotorState> motor_arm_states =
openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
std::vector<MotorState> motor_gripper_states =
openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref);
// kp kd q dq tau
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_dof);
for (size_t i = 0; i < arm_dof; ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity,
motor_arm_states[i].effort});
}
// gripper command mit param
std::vector<openarm::damiao_motor::MITParam> gripper_cmds;
gripper_cmds.reserve(gripper_dof);
for (size_t i = 0; i < gripper_dof; ++i) {
gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{
Kp_[i + arm_dof], Kd_[i + arm_dof], motor_gripper_states[i].position,
motor_gripper_states[i].velocity, motor_gripper_states[i].effort});
}
// send command to arm
openarm_->get_arm().mit_control_all(arm_cmds);
// send command to gripper
openarm_->get_gripper().mit_control_all(gripper_cmds);
std::this_thread::sleep_for(std::chrono::microseconds(200));
openarm_->recv_all(220);
return true;
}
bool Control::unilateral_step() {
// get motor status
std::vector<MotorState> arm_motor_states;
for (const auto& motor : openarm_->get_arm().get_motors()) {
arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<MotorState> gripper_motor_states;
for (const auto& motor : openarm_->get_gripper().get_motors()) {
gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
// convert joint to motor
std::vector<JointState> joint_arm_states =
openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_gripper_states =
openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
// set reponse
robot_state_->arm_state().set_all_responses(joint_arm_states);
robot_state_->hand_state().set_all_responses(joint_gripper_states);
size_t arm_dof = robot_state_->arm_state().get_size();
size_t gripper_dof = robot_state_->hand_state().get_size();
std::vector<double> joint_arm_positions(arm_dof, 0.0);
std::vector<double> joint_arm_velocities(arm_dof, 0.0);
std::vector<double> joint_gripper_positions(gripper_dof, 0.0);
std::vector<double> joint_gripper_velocities(gripper_dof, 0.0);
for (size_t i = 0; i < arm_dof; ++i) {
joint_arm_positions[i] = joint_arm_states[i].position;
joint_arm_velocities[i] = joint_arm_states[i].velocity;
}
for (size_t i = 0; i < gripper_dof; ++i) {
joint_gripper_positions[i] = joint_gripper_states[i].position;
joint_gripper_velocities[i] = joint_gripper_states[i].velocity;
}
std::vector<double> gravity(arm_dof, 0.0);
std::vector<double> coriolis(arm_dof, 0.0);
std::vector<double> friction(arm_dof + gripper_dof, 0.0);
if (role_ == ROLE_LEADER) {
// calc dynamics
dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data());
dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(),
coriolis.data());
// Friction (compute joint friction)
for (size_t i = 0; i < joint_arm_velocities.size(); ++i) for (size_t i = 0; i < joint_arm_velocities.size(); ++i)
ComputeFriction(joint_arm_velocities.data(), friction.data(), i); ComputeFriction(joint_arm_velocities.data(), friction.data(), i);
for (size_t i = 0; i < joint_gripper_velocities.size(); ++i) for (size_t i = 0; i < joint_gripper_velocities.size(); ++i)
ComputeFriction(joint_gripper_velocities.data(), friction.data(), joint_arm_velocities.size() + i); ComputeFriction(joint_gripper_velocities.data(), friction.data(), arm_dof + i);
// set gravity and friciton comp joint torque value // arm joint state
for(size_t i = 0 ; i < arm_dof; i++){ std::vector<JointState> joint_arm_state_torque(arm_dof);
joint_arm_states_ref[i].effort = gravity[i] + friction[i]; for (size_t i = 0; i < arm_dof; ++i) {
joint_arm_state_torque[i].position = joint_arm_positions[i];
joint_arm_state_torque[i].velocity = joint_arm_velocities[i];
joint_arm_state_torque[i].effort = gravity[i] + friction[i] * 0.3 + coriolis[i] * 0.1;
} }
for(size_t i = 0 ; i < gripper_dof; i++){ // gripper joint state
joint_gripper_states_ref[i].effort = friction[i + arm_dof]; std::vector<JointState> joint_gripper_state_torque(gripper_dof);
for (size_t i = 0; i < gripper_dof; ++i) {
joint_gripper_state_torque[i].position = joint_gripper_positions[i];
joint_gripper_state_torque[i].velocity = joint_gripper_velocities[i];
joint_gripper_state_torque[i].effort = friction[arm_dof + i] * 0.3;
} }
std::vector<MotorState> motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_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_state_torque);
std::vector<MotorState> motor_gripper_states =
openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque);
// kp kd q dq tau // arm command mit param
std::vector<openarm::damiao_motor::MITParam> arm_cmds; std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_dof); arm_cmds.reserve(arm_dof);
for (size_t i = 0; i < arm_dof; ++i) { for (size_t i = 0; i < arm_dof; ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity, motor_arm_states[i].effort}); arm_cmds.emplace_back(
openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort});
} }
// gripper command mit param // gripper command mit param
std::vector<openarm::damiao_motor::MITParam> gripper_cmds; std::vector<openarm::damiao_motor::MITParam> gripper_cmds;
gripper_cmds.reserve(gripper_dof); gripper_cmds.reserve(gripper_dof);
for (size_t i = 0; i < gripper_dof; ++i) { for (size_t i = 0; i < gripper_dof; ++i) {
gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i + arm_dof],Kd_[i + arm_dof], motor_gripper_states[i].position, motor_gripper_states[i].velocity, motor_gripper_states[i].effort}); gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{
0.0, 0.0, 0.0, 0.0, motor_gripper_states[i].effort});
} }
// send command to arm // send command to arm
@ -195,316 +311,200 @@ bool Control::bilateral_step()
// send command to gripper // send command to gripper
openarm_->get_gripper().mit_control_all(gripper_cmds); openarm_->get_gripper().mit_control_all(gripper_cmds);
std::this_thread::sleep_for(std::chrono::microseconds(200)); openarm_->recv_all(200);
openarm_->recv_all(220);
return true; return true;
}
else if (role_ == ROLE_FOLLOWER) {
std::vector<JointState> joint_arm_states_ref =
robot_state_->arm_state().get_all_references();
std::vector<JointState> joint_hand_states_ref =
robot_state_->hand_state().get_all_references();
// Joint → Motor
std::vector<MotorState> arm_motor_refs =
openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
std::vector<MotorState> hand_motor_refs =
openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref);
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_motor_refs.size());
for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
Kp_[i], Kd_[i], arm_motor_refs[i].position, arm_motor_refs[i].velocity, 0.0});
} }
std::vector<openarm::damiao_motor::MITParam> hand_cmds;
bool Control::unilateral_step(){ hand_cmds.reserve(hand_motor_refs.size());
for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
// get motor status hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
std::vector<MotorState> arm_motor_states; Kp_[i + arm_dof], Kd_[i + arm_dof], hand_motor_refs[i].position,
for (const auto& motor : openarm_->get_arm().get_motors()) { hand_motor_refs[i].velocity, 0.0});
arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<MotorState> gripper_motor_states;
for (const auto& motor : openarm_->get_gripper().get_motors()) {
gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
// convert joint to motor
std::vector<JointState> joint_arm_states = openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_gripper_states = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
// set reponse
robot_state_->arm_state().set_all_responses(joint_arm_states);
robot_state_->hand_state().set_all_responses(joint_gripper_states);
size_t arm_dof = robot_state_->arm_state().get_size();
size_t gripper_dof = robot_state_->hand_state().get_size();
std::vector<double> joint_arm_positions(arm_dof, 0.0);
std::vector<double> joint_arm_velocities(arm_dof, 0.0);
std::vector<double> joint_gripper_positions(gripper_dof, 0.0);
std::vector<double> joint_gripper_velocities(gripper_dof, 0.0);
for (size_t i = 0; i < arm_dof; ++i) {
joint_arm_positions[i] = joint_arm_states[i].position;
joint_arm_velocities[i] = joint_arm_states[i].velocity;
}
for (size_t i = 0; i < gripper_dof; ++i) {
joint_gripper_positions[i] = joint_gripper_states[i].position;
joint_gripper_velocities[i] = joint_gripper_states[i].velocity;
}
std::vector<double> gravity(arm_dof, 0.0);
std::vector<double> coriolis(arm_dof, 0.0);
std::vector<double> friction(arm_dof + gripper_dof, 0.0);
if (role_ == ROLE_LEADER) {
// calc dynamics
dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data());
dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data());
for (size_t i = 0; i < joint_arm_velocities.size(); ++i)
ComputeFriction(joint_arm_velocities.data(), friction.data(), i);
for (size_t i = 0; i < joint_gripper_velocities.size(); ++i)
ComputeFriction(joint_gripper_velocities.data(), friction.data(), arm_dof + i);
// arm joint state
std::vector<JointState> joint_arm_state_torque(arm_dof);
for (size_t i = 0; i < arm_dof; ++i) {
joint_arm_state_torque[i].position = joint_arm_positions[i];
joint_arm_state_torque[i].velocity = joint_arm_velocities[i];
joint_arm_state_torque[i].effort = gravity[i] + friction[i]*0.3 + coriolis[i]*0.1;
}
// gripper joint state
std::vector<JointState> joint_gripper_state_torque(gripper_dof);
for (size_t i = 0; i < gripper_dof; ++i) {
joint_gripper_state_torque[i].position = joint_gripper_positions[i];
joint_gripper_state_torque[i].velocity = joint_gripper_velocities[i];
joint_gripper_state_torque[i].effort = friction[arm_dof + i]*0.3 ;
}
std::vector<MotorState> motor_arm_states = openarmjointconverter_->joint_to_motor(joint_arm_state_torque);
std::vector<MotorState> motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_state_torque);
// arm command mit param
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_dof);
for (size_t i = 0; i < arm_dof; ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort});
}
// gripper command mit param
std::vector<openarm::damiao_motor::MITParam> gripper_cmds;
gripper_cmds.reserve(gripper_dof);
for (size_t i = 0; i < gripper_dof; ++i) {
gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_gripper_states[i].effort});
}
// send command to arm
openarm_->get_arm().mit_control_all(arm_cmds);
// send command to gripper
openarm_->get_gripper().mit_control_all(gripper_cmds);
openarm_->recv_all(200);
return true;
}
else if (role_ == ROLE_FOLLOWER) {
std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
std::vector<JointState> joint_hand_states_ref = robot_state_->hand_state().get_all_references();
// Joint → Motor
std::vector<MotorState> arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_states_ref);
std::vector<MotorState> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_states_ref);
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_motor_refs.size());
for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
Kp_[i],
Kd_[i],
arm_motor_refs[i].position,
arm_motor_refs[i].velocity,
0.0
});
}
std::vector<openarm::damiao_motor::MITParam> hand_cmds;
hand_cmds.reserve(hand_motor_refs.size());
for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
Kp_[i + arm_dof],
Kd_[i + arm_dof],
hand_motor_refs[i].position,
hand_motor_refs[i].velocity,
0.0
});
}
openarm_->get_arm().mit_control_all(arm_cmds);
openarm_->get_gripper().mit_control_all(hand_cmds);
openarm_->recv_all(200);
return true;
}
return true;
} }
void Control::ComputeFriction(const double* velocity, double* friction, size_t index) openarm_->get_arm().mit_control_all(arm_cmds);
{ openarm_->get_gripper().mit_control_all(hand_cmds);
if (TANHFRIC) {
const double amp_tmp = 1.0;
const double coef_tmp = 0.1;
const double v = velocity[index]; openarm_->recv_all(200);
const double Fc = Fc_.at(index);
const double k = k_.at(index);
const double Fv = Fv_.at(index);
const double Fo = Fo_.at(index);
friction[index] = amp_tmp * Fc * std::tanh(coef_tmp * k * v) + Fv * v + Fo; return true;
} else { }
friction[index] = velocity[index] * Dn_.at(index);
} return true;
}
void Control::ComputeFriction(const double* velocity, double* friction, size_t index) {
if (TANHFRIC) {
const double amp_tmp = 1.0;
const double coef_tmp = 0.1;
const double v = velocity[index];
const double Fc = Fc_.at(index);
const double k = k_.at(index);
const double Fv = Fv_.at(index);
const double Fo = Fo_.at(index);
friction[index] = amp_tmp * Fc * std::tanh(coef_tmp * k * v) + Fv * v + Fo;
} else {
friction[index] = velocity[index] * Dn_.at(index);
}
}
bool Control::AdjustPosition(void) {
int nstep = 220;
double alpha;
std::vector<MotorState> arm_motor_states;
for (const auto& motor : openarm_->get_arm().get_motors()) {
arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<MotorState> gripper_motor_states;
for (const auto& motor : openarm_->get_gripper().get_motors()) {
gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<JointState> joint_arm_now =
openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_hand_now =
openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
std::vector<JointState> joint_arm_goal(NMOTORS - 1);
for (size_t i = 0; i < NMOTORS - 1; ++i) {
joint_arm_goal[i].position = INITIAL_POSITION[i];
joint_arm_goal[i].velocity = 0.0;
joint_arm_goal[i].effort = 0.0;
}
std::vector<JointState> joint_hand_goal(joint_hand_now.size());
for (size_t i = 0; i < joint_hand_goal.size(); ++i) {
joint_hand_goal[i].position = 0.0;
joint_hand_goal[i].velocity = 0.0;
joint_hand_goal[i].effort = 0.0;
}
std::vector<double> kp_arm_temp = {50, 50.0, 50.0, 50.0, 10.0, 10.0, 10.0};
std::vector<double> kd_arm_temp = {1.2, 1.2, 1.2, 1.2, 0.3, 0.2, 0.3};
std::vector<double> kp_hand_temp = {10.0};
std::vector<double> kd_hand_temp = {0.5};
for (int step = 0; step < nstep; ++step) {
alpha = static_cast<double>(step + 1) / nstep;
std::vector<JointState> joint_arm_interp(NMOTORS - 1);
for (size_t i = 0; i < NMOTORS - 1; ++i) {
joint_arm_interp[i].position =
joint_arm_goal[i].position * alpha + joint_arm_now[i].position * (1.0 - alpha);
joint_arm_interp[i].velocity = 0.0;
} }
bool Control::AdjustPosition(void) std::vector<JointState> joint_hand_interp(joint_hand_goal.size());
{ for (size_t i = 0; i < joint_hand_interp.size(); ++i) {
int nstep = 220; joint_hand_interp[i].position =
double alpha; 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_states;
for (const auto& motor : openarm_->get_arm().get_motors()) {
arm_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<MotorState> gripper_motor_states;
for (const auto& motor : openarm_->get_gripper().get_motors()) {
gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<JointState> joint_arm_now = openarmjointconverter_->motor_to_joint(arm_motor_states);
std::vector<JointState> joint_hand_now = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states);
std::vector<JointState> joint_arm_goal(NMOTORS-1);
for (size_t i = 0; i < NMOTORS-1; ++i) {
joint_arm_goal[i].position = INITIAL_POSITION[i];
joint_arm_goal[i].velocity = 0.0;
joint_arm_goal[i].effort = 0.0;
}
std::vector<JointState> joint_hand_goal(joint_hand_now.size());
for (size_t i = 0; i < joint_hand_goal.size(); ++i) {
joint_hand_goal[i].position = 0.0;
joint_hand_goal[i].velocity = 0.0;
joint_hand_goal[i].effort = 0.0;
}
std::vector<double> kp_arm_temp = {50, 50.0, 50.0, 50.0, 10.0, 10.0, 10.0};
std::vector<double> kd_arm_temp = {1.2, 1.2, 1.2, 1.2, 0.3, 0.2, 0.3};
std::vector<double> kp_hand_temp = {10.0};
std::vector<double> kd_hand_temp = {0.5};
for (int step = 0; step < nstep; ++step) {
alpha = static_cast<double>(step + 1) / nstep;
std::vector<JointState> joint_arm_interp(NMOTORS-1);
for (size_t i = 0; i < NMOTORS-1; ++i) {
joint_arm_interp[i].position = joint_arm_goal[i].position * alpha + joint_arm_now[i].position * (1.0 - alpha);
joint_arm_interp[i].velocity = 0.0;
}
std::vector<JointState> joint_hand_interp(joint_hand_goal.size());
for (size_t i = 0; i < joint_hand_interp.size(); ++i) {
joint_hand_interp[i].position = joint_hand_goal[i].position * alpha + joint_hand_now[i].position * (1.0 - alpha);
joint_hand_interp[i].velocity = 0.0;
}
std::vector<MotorState> arm_motor_refs = openarmjointconverter_->joint_to_motor(joint_arm_interp);
std::vector<MotorState> hand_motor_refs = openarmgripperjointconverter_->joint_to_motor(joint_hand_interp);
std::vector<openarm::damiao_motor::MITParam> arm_cmds;
arm_cmds.reserve(arm_motor_refs.size());
for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{
kp_arm_temp[i],
kd_arm_temp[i],
arm_motor_refs[i].position,
arm_motor_refs[i].velocity,
0.0
});
}
std::vector<openarm::damiao_motor::MITParam> hand_cmds;
hand_cmds.reserve(hand_motor_refs.size());
for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
kp_hand_temp[i],
kd_hand_temp[i],
hand_motor_refs[i].position,
hand_motor_refs[i].velocity,
0.0
});
}
openarm_->get_arm().mit_control_all(arm_cmds);
openarm_->get_gripper().mit_control_all(hand_cmds);
std::this_thread::sleep_for(std::chrono::milliseconds(10));
openarm_->recv_all();
}
std::vector<MotorState> arm_motor_states_final;
for (const auto& motor : openarm_->get_arm().get_motors()) {
arm_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<MotorState> gripper_motor_states_final;
for (const auto& motor : openarm_->get_gripper().get_motors()) {
gripper_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
std::vector<JointState> joint_arm_final = openarmjointconverter_->motor_to_joint(arm_motor_states_final);
std::vector<JointState> joint_hand_final = openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final);
robot_state_->arm_state().set_all_references(joint_arm_final);
robot_state_->hand_state().set_all_references(joint_hand_final);
return true;
} }
bool Control::DetectVibration(const double* velocity, bool* what_axis) std::vector<MotorState> arm_motor_refs =
{ openarmjointconverter_->joint_to_motor(joint_arm_interp);
bool vibration_detected = false; std::vector<MotorState> hand_motor_refs =
openarmgripperjointconverter_->joint_to_motor(joint_hand_interp);
for (int i = 0; i < NJOINTS; ++i) { std::vector<openarm::damiao_motor::MITParam> arm_cmds;
what_axis[i] = false; arm_cmds.reserve(arm_motor_refs.size());
for (size_t i = 0; i < arm_motor_refs.size(); ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{kp_arm_temp[i], kd_arm_temp[i],
arm_motor_refs[i].position,
arm_motor_refs[i].velocity, 0.0});
}
velocity_buffer_[i].push_back(velocity[i]); std::vector<openarm::damiao_motor::MITParam> hand_cmds;
if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE) hand_cmds.reserve(hand_motor_refs.size());
velocity_buffer_[i].pop_front(); for (size_t i = 0; i < hand_motor_refs.size(); ++i) {
hand_cmds.emplace_back(openarm::damiao_motor::MITParam{
kp_hand_temp[i], kd_hand_temp[i], hand_motor_refs[i].position,
hand_motor_refs[i].velocity, 0.0});
}
if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE) openarm_->get_arm().mit_control_all(arm_cmds);
continue; openarm_->get_gripper().mit_control_all(hand_cmds);
double mean = std::accumulate( std::this_thread::sleep_for(std::chrono::milliseconds(10));
velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0
) / velocity_buffer_[i].size();
double var = 0.0; openarm_->recv_all();
for (double v : velocity_buffer_[i]) { }
var += (v - mean) * (v - mean);
}
double stddev = std::sqrt(var / velocity_buffer_[i].size()); std::vector<MotorState> arm_motor_states_final;
for (const auto& motor : openarm_->get_arm().get_motors()) {
arm_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0});
}
if (stddev > VIB_THRESHOLD) { std::vector<MotorState> gripper_motor_states_final;
what_axis[i] = true; for (const auto& motor : openarm_->get_gripper().get_motors()) {
vibration_detected = true; gripper_motor_states_final.push_back({motor.get_position(), motor.get_velocity(), 0.0});
std::cout << "[VIBRATION] Joint " << i << " stddev: " << stddev << std::endl; }
}
}
return vibration_detected; std::vector<JointState> joint_arm_final =
} openarmjointconverter_->motor_to_joint(arm_motor_states_final);
std::vector<JointState> joint_hand_final =
openarmgripperjointconverter_->motor_to_joint(gripper_motor_states_final);
robot_state_->arm_state().set_all_references(joint_arm_final);
robot_state_->hand_state().set_all_references(joint_hand_final);
return true;
}
bool Control::DetectVibration(const double* velocity, bool* what_axis) {
bool vibration_detected = false;
for (int i = 0; i < NJOINTS; ++i) {
what_axis[i] = false;
velocity_buffer_[i].push_back(velocity[i]);
if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE) velocity_buffer_[i].pop_front();
if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE) continue;
double mean = std::accumulate(velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0) /
velocity_buffer_[i].size();
double var = 0.0;
for (double v : velocity_buffer_[i]) {
var += (v - mean) * (v - mean);
}
double stddev = std::sqrt(var / velocity_buffer_[i].size());
if (stddev > VIB_THRESHOLD) {
what_axis[i] = true;
vibration_detected = true;
std::cout << "[VIBRATION] Joint " << i << " stddev: " << stddev << std::endl;
}
}
return vibration_detected;
}

121
src/controller/control.hpp
View File

@ -15,90 +15,87 @@
#pragma once #pragma once
// #include <sensor_msgs/msg/joint_state.hpp> // #include <sensor_msgs/msg/joint_state.hpp>
#include <utility>
#include <fstream>
#include <deque>
#include <numeric>
#include <memory>
#include <controller/diff.hpp> #include <controller/diff.hpp>
#include <controller/dynamics.hpp> #include <controller/dynamics.hpp>
#include <robot_state.hpp> #include <deque>
#include <fstream>
#include <joint_state_converter.hpp> #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/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp> #include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <openarm_constants.hpp>
#include <robot_state.hpp>
#include <utility>
class Control {
openarm::can::socket::OpenArm *openarm_;
class Control double Ts_;
{ int role_;
openarm::can::socket::OpenArm* openarm_;
double Ts_; size_t arm_motor_num_;
int role_; size_t hand_motor_num_;
size_t arm_motor_num_; Differentiator *differentiator_;
size_t hand_motor_num_; OpenArmJointConverter *openarmjointconverter_;
OpenArmJGripperJointConverter *openarmgripperjointconverter_;
Differentiator *differentiator_; std::shared_ptr<RobotSystemState> robot_state_;
OpenArmJointConverter *openarmjointconverter_;
OpenArmJGripperJointConverter *openarmgripperjointconverter_;
std::shared_ptr<RobotSystemState> robot_state_; std::string arm_type_;
std::string arm_type_; Dynamics *dynamics_f_;
Dynamics *dynamics_l_;
Dynamics *dynamics_f_; double oblique_coordinates_force;
Dynamics *dynamics_l_; double oblique_coordinates_position;
double oblique_coordinates_force; // for easy logging
double oblique_coordinates_position; // std::vector<std::pair<double, double>> velocity_log_; // (differ_velocity, motor_velocity)
// std::string log_file_path_ = "../data/velocity_comparison.csv";
static constexpr int VEL_WINDOW_SIZE = 10;
static constexpr double VIB_THRESHOLD = 0.7; // [rad/s]
std::deque<double> velocity_buffer_[NJOINTS];
// for easy logging public:
// std::vector<std::pair<double, double>> velocity_log_; // (differ_velocity, motor_velocity) Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
// std::string log_file_path_ = "../data/velocity_comparison.csv"; std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
static constexpr int VEL_WINDOW_SIZE = 10; size_t arm_joint_num, size_t hand_motor_num);
static constexpr double VIB_THRESHOLD = 0.7; // [rad/s] Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
std::deque<double> velocity_buffer_[NJOINTS]; 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();
public: std::shared_ptr<RobotSystemState> response_;
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); std::shared_ptr<RobotSystemState> reference_;
Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, std::string arm_type, size_t arm_joint_num, size_t hand_motor_num);
~Control();
std::shared_ptr<RobotSystemState> response_; std::vector<double> Dn_, Kp_, Kd_, Fc_, k_, Fv_, Fo_;
std::shared_ptr<RobotSystemState> reference_;
std::vector<double> Dn_, Kp_, Kd_,Fc_, k_, Fv_, Fo_; // bool Setup(void);
void Setstate(int state);
void Shutdown(void);
// bool Setup(void); void SetParameter(const std::vector<double> &Kp, const std::vector<double> &Kd,
void Setstate(int state); const std::vector<double> &Fc, const std::vector<double> &k,
void Shutdown(void); const std::vector<double> &Fv, const std::vector<double> &Fo);
void SetParameter( bool AdjustPosition(void);
const std::vector<double>& Kp,
const std::vector<double>& Kd,
const std::vector<double>& Fc,
const std::vector<double>& k,
const std::vector<double>& Fv,
const std::vector<double>& Fo);
bool AdjustPosition(void); // Compute torque based on bilateral
bool bilateral_step();
bool unilateral_step();
// Compute torque based on bilateral // NOTE! Control() class operates on "joints", while the underlying
bool bilateral_step(); // classes operates on "actuators". The following functions map
bool unilateral_step(); // joints to actuators.
// NOTE! Control() class operates on "joints", while the underlying void ComputeJointPosition(const double *motor_position, double *joint_position);
// classes operates on "actuators". The following functions map void ComputeJointVelocity(const double *motor_velocity, double *joint_velocity);
// joints to actuators. void ComputeMotorTorque(const double *joint_torque, double *motor_torque);
void ComputeJointPosition(const double *motor_position, double *joint_position); // void ComputeFriction(const double *velocity, double *friction);
void ComputeJointVelocity(const double *motor_velocity, double *joint_velocity); void ComputeFriction(const double *velocity, double *friction, size_t index);
void ComputeMotorTorque(const double *joint_torque, double *motor_torque); void ComputeGravity(const double *position, double *gravity);
bool DetectVibration(const double *velocity, bool *what_axis);
// void ComputeFriction(const double *velocity, double *friction);
void ComputeFriction(const double* velocity, double* friction, size_t index);
void ComputeGravity(const double *position, double *gravity);
bool DetectVibration(const double* velocity, bool *what_axis);
}; };

87
src/controller/diff.hpp
View File

@ -18,57 +18,52 @@
#include <openarm_constants.hpp> #include <openarm_constants.hpp>
class Differentiator class Differentiator {
{ private:
private: double Ts_; // Sampling time
double Ts_; // Sampling time double velocity_z1_[NMOTORS] = {0.0}; // Velocity (1 step before)
double velocity_z1_[NMOTORS] = {0.0}; // Velocity (1 step before) double position_z1_[NMOTORS] = {0.0}; // Position (1 step before)
double position_z1_[NMOTORS] = {0.0}; // Position (1 step before) double acc_z1_[NMOTORS] = {0.0};
double acc_z1_[NMOTORS] = {0.0}; double acc_[NMOTORS] = {0.0};
double acc_[NMOTORS] = {0.0};
public:
Differentiator(double Ts) : Ts_(Ts) {}
/* public:
* Compute the motor speed by taking the derivative of Differentiator(double Ts) : Ts_(Ts) {}
* the motion.
*/
void Differentiate(const double *position, double *velocity)
{
double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY);
double b = a * CUTOFF_FREQUENCY;
for (int i = 0; i < NMOTORS; i++) { /*
if (position_z1_[i] == 0.0) { * Compute the motor speed by taking the derivative of
position_z1_[i] = position[i]; * the motion.
} */
void Differentiate(const double *position, double *velocity) {
double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY);
double b = a * CUTOFF_FREQUENCY;
velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]); for (int i = 0; i < NMOTORS; i++) {
position_z1_[i] = position[i]; if (position_z1_[i] == 0.0) {
velocity_z1_[i] = velocity[i]; position_z1_[i] = position[i];
} }
} velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]);
position_z1_[i] = position[i];
velocity_z1_[i] = velocity[i];
}
}
void Differentiate_w_obs(const double *position, double *velocity, double *mass, double *input_torque) void Differentiate_w_obs(const double *position, double *velocity, double *mass,
{ double *input_torque) {
double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY);
double b = a * CUTOFF_FREQUENCY;
double a = 1.0 / (1.0 + Ts_ * CUTOFF_FREQUENCY); for (int i = 0; i < NMOTORS; i++) {
double b = a * CUTOFF_FREQUENCY; if (position_z1_[i] == 0.0000000) {
position_z1_[i] = position[i];
acc_z1_[i] = acc_[i];
}
for (int i = 0; i < NMOTORS; i++) { acc_[i] = acc_z1_[i] * a + b * (input_torque[i] / (mass[i]));
if (position_z1_[i] == 0.0000000) { velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]) + acc_[i];
position_z1_[i] = position[i]; position_z1_[i] = position[i];
acc_z1_[i] = acc_[i]; velocity_z1_[i] = velocity[i];
} acc_z1_[i] = acc_[i];
}
acc_[i] = acc_z1_[i] * a + b * (input_torque[i] / (mass[i])); }
velocity[i] = velocity_z1_[i] * a + b * (position[i] - position_z1_[i]) + acc_[i];
position_z1_[i] = position[i];
velocity_z1_[i] = velocity[i];
acc_z1_[i] = acc_[i];
}
}
}; };

266
src/controller/dynamics.cpp
View File

@ -14,18 +14,15 @@
#include <controller/dynamics.hpp> #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->urdf_path = urdf_path; this->start_link = start_link;
this->start_link = start_link; this->end_link = end_link;
this->end_link = end_link;
} }
Dynamics::~Dynamics(){} Dynamics::~Dynamics() {}
bool Dynamics::Init() {
bool Dynamics::Init()
{
std::ifstream file(urdf_path); std::ifstream file(urdf_path);
if (!file.is_open()) { if (!file.is_open()) {
fprintf(stderr, "Failed to open URDF file: %s\n", urdf_path.c_str()); fprintf(stderr, "Failed to open URDF file: %s\n", urdf_path.c_str());
@ -52,192 +49,181 @@ bool Dynamics::Init()
return false; 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()); coriolis_forces.resize(kdl_chain.getNrOfJoints());
gravity_forces.resize(kdl_chain.getNrOfJoints()); gravity_forces.resize(kdl_chain.getNrOfJoints());
inertia_matrix.resize(kdl_chain.getNrOfJoints()); inertia_matrix.resize(kdl_chain.getNrOfJoints());
coriolis_forces.data.setZero(); coriolis_forces.data.setZero();
gravity_forces.data.setZero(); gravity_forces.data.setZero();
inertia_matrix.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; return true;
} }
void Dynamics::GetGravity(const double *motor_position, double *gravity) void Dynamics::GetGravity(const double *motor_position, double *gravity) {
{ const auto njoints = kdl_chain.getNrOfJoints();
const auto njoints = kdl_chain.getNrOfJoints(); KDL::JntArray q_(kdl_chain.getNrOfJoints());
KDL::JntArray q_(kdl_chain.getNrOfJoints()); for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) {
q_(i) = motor_position[i];
}
for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { solver->JntToGravity(q_, gravity_forces);
q_(i) = motor_position[i]; for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) {
} gravity[i] = gravity_forces(i);
}
solver->JntToGravity(q_, gravity_forces);
for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) {
gravity[i] = gravity_forces(i);
}
} }
void Dynamics::GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis) { void Dynamics::GetCoriolis(const double *motor_position, const double *motor_velocity,
KDL::JntArray q_(kdl_chain.getNrOfJoints()); double *coriolis) {
KDL::JntArray q_dot(kdl_chain.getNrOfJoints()); KDL::JntArray q_(kdl_chain.getNrOfJoints());
KDL::JntArray q_dot(kdl_chain.getNrOfJoints());
for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) {
q_(i) = motor_position[i]; q_(i) = motor_position[i];
q_dot(i) = motor_velocity[i]; q_dot(i) = motor_velocity[i];
} }
solver->JntToCoriolis(q_, q_dot, coriolis_forces); solver->JntToCoriolis(q_, q_dot, coriolis_forces);
for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) {
coriolis[i] = coriolis_forces(i); coriolis[i] = coriolis_forces(i);
} }
} }
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::JntArray q_(kdl_chain.getNrOfJoints()); KDL::JntSpaceInertiaMatrix inertia_matrix(kdl_chain.getNrOfJoints());
KDL::JntSpaceInertiaMatrix inertia_matrix(kdl_chain.getNrOfJoints()); for (size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) {
for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) { q_(i) = motor_position[i];
q_(i) = motor_position[i]; }
}
solver->JntToMass(q_, inertia_matrix); solver->JntToMass(q_, inertia_matrix);
for(size_t i = 0; i < kdl_chain.getNrOfJoints(); i++) {
inertia_diag[i] = inertia_matrix(i, i);
}
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());
KDL::JntArray q_(kdl_chain.getNrOfJoints()); for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) {
for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { q_(i) = motor_position[i];
q_(i) = motor_position[i]; }
KDL::Jacobian kdl_jac(kdl_chain.getNrOfJoints());
KDL::ChainJntToJacSolver jac_solver(kdl_chain);
jac_solver.JntToJac(q_, kdl_jac);
jacobian = Eigen::MatrixXd(6, kdl_chain.getNrOfJoints());
for (size_t i = 0; i < 6; ++i) {
for (size_t j = 0; j < kdl_chain.getNrOfJoints(); ++j) {
jacobian(i, j) = kdl_jac(i, j);
} }
}
KDL::Jacobian kdl_jac(kdl_chain.getNrOfJoints());
KDL::ChainJntToJacSolver jac_solver(kdl_chain);
jac_solver.JntToJac(q_, kdl_jac);
jacobian = Eigen::MatrixXd(6, kdl_chain.getNrOfJoints());
for (size_t i = 0; i < 6; ++i) {
for (size_t j = 0; j < kdl_chain.getNrOfJoints(); ++j) {
jacobian(i, j) = kdl_jac(i, j);
}
}
} }
void Dynamics::GetNullSpace(const double* motor_position, Eigen::MatrixXd& nullspace) { void Dynamics::GetNullSpace(const double *motor_position, Eigen::MatrixXd &nullspace) {
const size_t dof = kdl_chain.getNrOfJoints(); const size_t dof = kdl_chain.getNrOfJoints();
bool use_stable_svd = false; bool use_stable_svd = false;
Eigen::MatrixXd J; Eigen::MatrixXd J;
GetJacobian(motor_position, J); GetJacobian(motor_position, J);
Eigen::MatrixXd J_pinv; Eigen::MatrixXd J_pinv;
if (use_stable_svd) { if (use_stable_svd) {
Eigen::JacobiSVD<Eigen::MatrixXd> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); 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 =
Eigen::VectorXd singularValuesInv = svd.singularValues(); 1e-6 * std::max(J.cols(), J.rows()) * svd.singularValues().array().abs().maxCoeff();
for (int i = 0; i < singularValuesInv.size(); ++i) { Eigen::VectorXd singularValuesInv = svd.singularValues();
singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0; for (int i = 0; i < singularValuesInv.size(); ++i) {
} singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0;
J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
} else {
J_pinv = J.transpose() * (J * J.transpose()).inverse();
} }
J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
} else {
J_pinv = J.transpose() * (J * J.transpose()).inverse();
}
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof);
nullspace = I - J_pinv * J; nullspace = I - J_pinv * J;
// std::cout << "[INFO] Null space projector computed.\n"; // std::cout << "[INFO] Null space projector computed.\n";
} }
void Dynamics::GetNullSpaceTauSpace(const double *motor_position, Eigen::MatrixXd &nullspace_T) {
const size_t dof = kdl_chain.getNrOfJoints();
bool use_stable_svd = false;
void Dynamics::GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixXd& nullspace_T) Eigen::MatrixXd J;
{ GetJacobian(motor_position, J);
const size_t dof = kdl_chain.getNrOfJoints();
bool use_stable_svd = false;
Eigen::MatrixXd J; Eigen::MatrixXd J_pinv;
GetJacobian(motor_position, J);
Eigen::MatrixXd J_pinv; if (use_stable_svd) {
Eigen::JacobiSVD<Eigen::MatrixXd> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV);
if (use_stable_svd) { double tol =
Eigen::JacobiSVD<Eigen::MatrixXd> svd(J, Eigen::ComputeThinU | Eigen::ComputeThinV); 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();
Eigen::VectorXd singularValuesInv = svd.singularValues(); for (int i = 0; i < singularValuesInv.size(); ++i) {
for (int i = 0; i < singularValuesInv.size(); ++i) { singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0;
singularValuesInv(i) = (singularValuesInv(i) > tol) ? 1.0 / singularValuesInv(i) : 0.0;
}
J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
} else {
J_pinv = J.transpose() * (J * J.transpose()).inverse();
} }
J_pinv = svd.matrixV() * singularValuesInv.asDiagonal() * svd.matrixU().transpose();
} else {
J_pinv = J.transpose() * (J * J.transpose()).inverse();
}
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof); Eigen::MatrixXd I = Eigen::MatrixXd::Identity(dof, dof);
Eigen::MatrixXd N = I - J_pinv * J; Eigen::MatrixXd N = I - J_pinv * J;
nullspace_T = N.transpose(); nullspace_T = N.transpose();
} }
void Dynamics::GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p) void Dynamics::GetEECordinate(const double *motor_position, Eigen::Matrix3d &R,
{ Eigen::Vector3d &p) {
KDL::JntArray q_(kdl_chain.getNrOfJoints()); KDL::JntArray q_(kdl_chain.getNrOfJoints());
for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) {
q_(i) = motor_position[i]; q_(i) = motor_position[i];
} }
KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain);
KDL::Frame kdl_frame; KDL::Frame kdl_frame;
if (fk_solver.JntToCart(q_, kdl_frame) < 0) { if (fk_solver.JntToCart(q_, kdl_frame) < 0) {
// std::cerr << "[KDL] FK failed in GetEECordinate!" << std::endl; // std::cerr << "[KDL] FK failed in GetEECordinate!" << std::endl;
return; return;
} }
for (int i = 0; i < 3; ++i) 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]; 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()); KDL::JntArray q_(kdl_chain.getNrOfJoints());
for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) { for (size_t i = 0; i < kdl_chain.getNrOfJoints(); ++i) {
q_(i) = motor_position[i]; q_(i) = motor_position[i];
} }
KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain); KDL::ChainFkSolverPos_recursive fk_solver(kdl_chain);
KDL::Frame kdl_frame; KDL::Frame kdl_frame;
if (fk_solver.JntToCart(q_, kdl_frame, kdl_chain.getNrOfSegments() - 1) < 0) {
// std::cerr << "[KDL] FK failed in GetPreEECordinate!" << std::endl;
return;
}
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j)
R(i, j) = kdl_frame.M(i, j);
p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2];
if (fk_solver.JntToCart(q_, kdl_frame, kdl_chain.getNrOfSegments() - 1) < 0) {
// std::cerr << "[KDL] FK failed in GetPreEECordinate!" << std::endl;
return;
}
for (int i = 0; i < 3; ++i)
for (int j = 0; j < 3; ++j) R(i, j) = kdl_frame.M(i, j);
p << kdl_frame.p[0], kdl_frame.p[1], kdl_frame.p[2];
} }

70
src/controller/dynamics.hpp
View File

@ -13,60 +13,60 @@
// limitations under the License. // limitations under the License.
#pragma once #pragma once
#include <unistd.h>
#include <string.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/chain.hpp>
#include <kdl/chaindynparam.hpp> #include <kdl/chaindynparam.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <kdl/chainfksolverpos_recursive.hpp> #include <kdl/chainfksolverpos_recursive.hpp>
#include <kdl/chainjnttojacsolver.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 <sstream>
#include <vector>
/* /*
* Compute gravity and inertia compensation using Orocos * Compute gravity and inertia compensation using Orocos
* Kinematics and Dynamics Library (KDL). * Kinematics and Dynamics Library (KDL).
*/ */
class Dynamics class Dynamics {
{ private:
private: std::shared_ptr<urdf::ModelInterface> urdf_model_interface;
std::shared_ptr<urdf::ModelInterface> urdf_model_interface;
std::string urdf_path; std::string urdf_path;
std::string start_link; std::string start_link;
std::string end_link; std::string end_link;
KDL::JntSpaceInertiaMatrix inertia_matrix; KDL::JntSpaceInertiaMatrix inertia_matrix;
KDL::JntArray q; KDL::JntArray q;
KDL::JntArray q_d; KDL::JntArray q_d;
KDL::JntArray coriolis_forces; KDL::JntArray coriolis_forces;
KDL::JntArray gravity_forces; KDL::JntArray gravity_forces;
KDL::JntArray biasangle; KDL::JntArray biasangle;
KDL::Tree kdl_tree; KDL::Tree kdl_tree;
KDL::Chain kdl_chain; KDL::Chain kdl_chain;
std::unique_ptr<KDL::ChainDynParam> solver; std::unique_ptr<KDL::ChainDynParam> solver;
public: public:
Dynamics(std::string urdf_path, std::string start_link, std::string end_link); Dynamics(std::string urdf_path, std::string start_link, std::string end_link);
~Dynamics(); ~Dynamics();
bool Init(); bool Init();
void GetGravity(const double *motor_position, double *gravity); void GetGravity(const double *motor_position, double *gravity);
void GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis); void GetCoriolis(const double *motor_position, const double *motor_velocity, double *coriolis);
void GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag); void GetMassMatrixDiagonal(const double *motor_position, double *inertia_diag);
void GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian); void GetJacobian(const double *motor_position, Eigen::MatrixXd &jacobian);
void GetNullSpace(const double *motor_positon, Eigen::MatrixXd &nullspace); void GetNullSpace(const double *motor_positon, Eigen::MatrixXd &nullspace);
void GetNullSpaceTauSpace(const double* motor_position, Eigen::MatrixXd& nullspace_T); void GetNullSpaceTauSpace(const double *motor_position, Eigen::MatrixXd &nullspace_T);
void GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); void GetEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p);
void GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p); void GetPreEECordinate(const double *motor_position, Eigen::Matrix3d &R, Eigen::Vector3d &p);
}; };

116
src/joint_state_converter.hpp
View File

@ -14,8 +14,8 @@
#pragma once #pragma once
#include <vector>
#include <robot_state.hpp> #include <robot_state.hpp>
#include <vector>
// Represents the state of a single joint // Represents the state of a single joint
// struct JointState { // struct JointState {
@ -28,7 +28,7 @@
struct MotorState { struct MotorState {
double position = 0.0; double position = 0.0;
double velocity = 0.0; double velocity = 0.0;
double effort = 0.0; double effort = 0.0;
}; };
// Abstract base class for converting between motor and joint states // Abstract base class for converting between motor and joint states
@ -37,71 +37,71 @@ public:
virtual ~MotorJointConverter() = default; virtual ~MotorJointConverter() = default;
// MotorState vector → JointState vector // 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 // 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; virtual size_t get_joint_count() const = 0;
}; };
// assume motor num equals to joint num // assume motor num equals to joint num
class OpenArmJointConverter : public MotorJointConverter { class OpenArmJointConverter : public MotorJointConverter {
public: public:
explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) { explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) {
std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl; std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl;
} }
std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
// std::cout << "joint num conv : " << m.size() << std::endl;
std::vector<JointState> j(m.size()); std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
for (size_t i = 0; i < m.size(); ++i){ // std::cout << "joint num conv : " << m.size() << std::endl;
j[i] = {m[i].position, m[i].velocity, m[i].effort};
} std::vector<JointState> j(m.size());
for (size_t i = 0; i < m.size(); ++i) {
return j; j[i] = {m[i].position, m[i].velocity, m[i].effort};
} }
std::vector<MotorState> joint_to_motor(const std::vector<JointState>& j) const override { return j;
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}; std::vector<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
return m; std::vector<MotorState> m(j.size());
for (size_t i = 0; i < j.size(); ++i) m[i] = {j[i].position, j[i].velocity, j[i].effort};
return m;
}
size_t get_joint_count() const override { return joint_count_; }
private:
size_t joint_count_;
};
// assume motor num equals to joint num
class OpenArmJGripperJointConverter : public MotorJointConverter {
public:
explicit OpenArmJGripperJointConverter(size_t joint_count) : joint_count_(joint_count) {
std::cout << "Gripper joint converter joint_count is : " << joint_count << std::endl;
}
std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
std::vector<JointState> j(m.size());
for (size_t i = 0; i < m.size(); ++i) {
j[i] = {m[i].position, m[i].velocity, m[i].effort};
} }
return j;
size_t get_joint_count() const override { return joint_count_; } }
private: std::vector<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
size_t joint_count_; 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};
// assume motor num equals to joint num }
class OpenArmJGripperJointConverter : public MotorJointConverter { return m;
public: }
explicit OpenArmJGripperJointConverter(size_t joint_count) : joint_count_(joint_count) {
std::cout << "Gripper joint converter joint_count is : " << joint_count << std::endl; size_t get_joint_count() const override { return joint_count_; }
}
private:
std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override { size_t joint_count_;
std::vector<JointState> j(m.size()); };
for (size_t i = 0; i < m.size(); ++i) {
j[i] = {m[i].position, m[i].velocity, m[i].effort};
}
return j;
}
std::vector<MotorState> joint_to_motor(const std::vector<JointState>& j) const override {
std::vector<MotorState> m(j.size());
for (size_t i = 0; i < j.size(); ++i) {
m[i] = {j[i].position, j[i].velocity, j[i].effort};
}
return m;
}
size_t get_joint_count() const override { return joint_count_; }
private:
size_t joint_count_;
};

69
src/openarm_constants.hpp
View File

@ -14,11 +14,12 @@
#pragma once #pragma once
#include <unistd.h>
#include <time.h> #include <time.h>
#include <unistd.h>
#include <iostream> #include <iostream>
#include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <vector> #include <vector>
#include <openarm/damiao_motor//dm_motor_constants.hpp>
constexpr double PI = 3.14159265358979323846; constexpr double PI = 3.14159265358979323846;
@ -43,53 +44,51 @@ constexpr double PI = 3.14159265358979323846;
#define ELBOWLIMIT 0.0 #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 // 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 // sefaty limit velocity
static const double velocity_limit_L[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0}; static const double velocity_limit_L[] = {8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0};
static const double velocity_limit_F[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0}; static const double velocity_limit_F[] = {8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0};
// sefaty limit effort // sefaty limit effort
static const double effort_limit_L[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0}; static const double effort_limit_L[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0};
static const double effort_limit_F[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0}; static const double effort_limit_F[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0};
// Motor configuration structure // Motor configuration structure
struct MotorConfig { struct MotorConfig {
std::vector<openarm::damiao_motor::MotorType> arm_motor_types; std::vector<openarm::damiao_motor::MotorType> arm_motor_types;
std::vector<uint32_t> arm_send_can_ids; std::vector<uint32_t> arm_send_can_ids;
std::vector<uint32_t> arm_recv_can_ids; std::vector<uint32_t> arm_recv_can_ids;
openarm::damiao_motor::MotorType gripper_motor_type; openarm::damiao_motor::MotorType gripper_motor_type;
uint32_t gripper_send_can_id; uint32_t gripper_send_can_id;
uint32_t gripper_recv_can_id; uint32_t gripper_recv_can_id;
}; };
// Global default motor configuration // Global default motor configuration
static const MotorConfig DEFAULT_MOTOR_CONFIG = { static const MotorConfig DEFAULT_MOTOR_CONFIG = {
// Standard 7-DOF arm motor configuration // 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,
openarm::damiao_motor::MotorType::DM4310},
// Standard CAN IDs for arm motors // Standard CAN IDs for arm motors
{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07}, {0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07},
{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17}, {0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17},
// Standard gripper configuration // Standard gripper configuration
openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310,
0x08, 0x08,
0x18}; 0x18};
// opening function // opening function
inline void printOpenArmBanner() { inline void printOpenArmBanner() {

58
src/openarm_port/joint_mapper.cpp
View File

@ -13,6 +13,7 @@
// limitations under the License. // limitations under the License.
#include "joint_mapper.hpp" #include "joint_mapper.hpp"
#include <cmath> #include <cmath>
JointMapper::JointMapper() {} JointMapper::JointMapper() {}
@ -20,38 +21,35 @@ JointMapper::JointMapper() {}
JointMapper::~JointMapper() {} JointMapper::~JointMapper() {}
// Only copying for now // 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[0] = motor_position[0]; joint_position[1] = motor_position[1];
joint_position[1] = motor_position[1]; joint_position[2] = motor_position[2];
joint_position[2] = motor_position[2]; joint_position[3] = motor_position[3];
joint_position[3] = motor_position[3]; joint_position[4] = motor_position[4];
joint_position[4] = motor_position[4]; joint_position[5] = motor_position[5];
joint_position[5] = motor_position[5]; joint_position[6] = motor_position[6];
joint_position[6] = motor_position[6]; joint_position[7] = motor_position[7];
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[0] = motor_velocity[0]; joint_velocity[1] = motor_velocity[1];
joint_velocity[1] = motor_velocity[1]; joint_velocity[2] = motor_velocity[2];
joint_velocity[2] = motor_velocity[2]; joint_velocity[3] = motor_velocity[3];
joint_velocity[3] = motor_velocity[3]; joint_velocity[4] = motor_velocity[4];
joint_velocity[4] = motor_velocity[4]; joint_velocity[5] = motor_velocity[5];
joint_velocity[5] = motor_velocity[5]; joint_velocity[6] = motor_velocity[6];
joint_velocity[6] = motor_velocity[6]; joint_velocity[7] = motor_velocity[7];
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[0] = joint_torque[0]; motor_torque[1] = joint_torque[1];
motor_torque[1] = joint_torque[1]; motor_torque[2] = joint_torque[2];
motor_torque[2] = joint_torque[2]; motor_torque[3] = joint_torque[3];
motor_torque[3] = joint_torque[3]; motor_torque[4] = joint_torque[4];
motor_torque[4] = joint_torque[4]; motor_torque[5] = joint_torque[5];
motor_torque[5] = joint_torque[5]; motor_torque[6] = joint_torque[6];
motor_torque[6] = joint_torque[6]; motor_torque[7] = joint_torque[7];
motor_torque[7] = joint_torque[7];
} }

12
src/openarm_port/joint_mapper.hpp
View File

@ -18,12 +18,10 @@
class JointMapper { class JointMapper {
public: public:
JointMapper(); JointMapper();
~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, void joint_to_motor_torque(const double *joint_torque, double *motor_torque);
double *joint_velocity);
void joint_to_motor_torque(const double *joint_torque, double *motor_torque);
}; };

96
src/openarm_port/openarm_init.cpp
View File

@ -13,75 +13,69 @@
// limitations under the License. // limitations under the License.
#include "openarm_init.hpp" #include "openarm_init.hpp"
#include "../openarm_constants.hpp" #include "../openarm_constants.hpp"
namespace openarm_init { 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) {
bool enable_debug) { MotorConfig config = DEFAULT_MOTOR_CONFIG;
return initialize_openarm(can_device, config, 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,
const MotorConfig &config, bool enable_debug) {
bool enable_debug) { // Create OpenArm instance
openarm::can::socket::OpenArm *openarm =
new openarm::can::socket::OpenArm(can_device, enable_debug);
// Create OpenArm instance // Perform common initialization
openarm::can::socket::OpenArm *openarm = initialize_(openarm, config, enable_debug);
new openarm::can::socket::OpenArm(can_device, enable_debug);
// Perform common initialization return openarm;
initialize_(openarm, config, enable_debug);
return openarm;
} }
void OpenArmInitializer::initialize_(openarm::can::socket::OpenArm *openarm, 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;
}
if (enable_debug) { // Initialize arm motors
std::cout << "Initializing arm motors..." << std::endl; openarm->init_arm_motors(config.arm_motor_types, config.arm_send_can_ids,
} config.arm_recv_can_ids);
// Initialize arm motors if (enable_debug) {
openarm->init_arm_motors(config.arm_motor_types, config.arm_send_can_ids, std::cout << "Initializing gripper motor..." << std::endl;
config.arm_recv_can_ids); }
if (enable_debug) { // Initialize gripper motor
std::cout << "Initializing gripper motor..." << std::endl; openarm->init_gripper_motor(config.gripper_motor_type, config.gripper_send_can_id,
} config.gripper_recv_can_id);
// Initialize gripper motor // Set callback mode for all motors
openarm->init_gripper_motor(config.gripper_motor_type, openarm->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE);
config.gripper_send_can_id,
config.gripper_recv_can_id);
// Set callback mode for all motors if (enable_debug) {
openarm->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); std::cout << "Enabling motors..." << std::endl;
}
if (enable_debug) { // Enable all motors with appropriate delays
std::cout << "Enabling motors..." << std::endl; openarm->enable_all();
} std::this_thread::sleep_for(std::chrono::milliseconds(100));
openarm->recv_all();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// Enable all motors with appropriate delays // Print motor counts for verification
openarm->enable_all(); if (enable_debug) {
std::this_thread::sleep_for(std::chrono::milliseconds(100)); size_t arm_motor_num = openarm->get_arm().get_motors().size();
openarm->recv_all(); size_t gripper_motor_num = openarm->get_gripper().get_motors().size();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
// Print motor counts for verification std::cout << "Arm motor count: " << arm_motor_num << std::endl;
if (enable_debug) { std::cout << "Gripper motor count: " << gripper_motor_num << std::endl;
size_t arm_motor_num = openarm->get_arm().get_motors().size(); }
size_t gripper_motor_num = openarm->get_gripper().get_motors().size();
std::cout << "Arm motor count: " << arm_motor_num << std::endl;
std::cout << "Gripper motor count: " << gripper_motor_num << std::endl;
}
} }
} // namespace openarm_init } // namespace openarm_init

51
src/openarm_port/openarm_init.hpp
View File

@ -14,43 +14,44 @@
#pragma once #pragma once
#include "../openarm_constants.hpp"
#include <chrono> #include <chrono>
#include <iostream> #include <iostream>
#include <openarm/can/socket/openarm.hpp> #include <openarm/can/socket/openarm.hpp>
#include <string> #include <string>
#include <thread> #include <thread>
#include "../openarm_constants.hpp"
namespace openarm_init { namespace openarm_init {
class OpenArmInitializer { class OpenArmInitializer {
public: public:
/** /**
* @brief Initialize OpenArm with default configuration * @brief Initialize OpenArm with default configuration
* @param can_device CAN device name (e.g., "can0", "can1") * @param can_device CAN device name (e.g., "can0", "can1")
* @param enable_debug Enable debug output * @param enable_debug Enable debug output
* @return Initialized OpenArm pointer (caller owns memory) * @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 * @brief Initialize OpenArm with custom motor configuration
* @param can_device CAN device name * @param can_device CAN device name
* @param config Custom motor configuration * @param config Custom motor configuration
* @param enable_debug Enable debug output * @param enable_debug Enable debug output
* @return Initialized OpenArm pointer (caller owns memory) * @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); bool enable_debug = true);
private: private:
/** /**
* @brief Common initialization steps for OpenArm * @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 } // namespace openarm_init

45
src/periodic_timer_thread.hpp
View File

@ -14,55 +14,42 @@
#pragma once #pragma once
#include <pthread.h> #include <pthread.h>
#include <time.h>
#include <unistd.h>
#include <atomic> #include <atomic>
#include <chrono> #include <chrono>
#include <thread>
#include <iostream> #include <iostream>
#include <stdexcept> #include <stdexcept>
#include <unistd.h> #include <thread>
#include <time.h>
class PeriodicTimerThread { class PeriodicTimerThread {
public: public:
explicit PeriodicTimerThread(double hz = 1000.0) explicit PeriodicTimerThread(double hz = 1000.0) : is_running_(false) {
: is_running_(false)
{
if (hz <= 0.0) { if (hz <= 0.0) {
throw std::invalid_argument("Hz must be positive"); throw std::invalid_argument("Hz must be positive");
} }
period_us_.store(static_cast<int>(1e6 / hz)); 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) { void set_period(double hz) {
if (hz > 0.0) { if (hz > 0.0) {
period_us_.store(static_cast<int>(1e6 / hz)); period_us_.store(static_cast<int>(1e6 / hz));
} }
} }
protected: protected:
virtual void on_timer() = 0; 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() {} virtual void after_stop() {}
@ -82,7 +69,6 @@ protected:
} }
private: private:
void start_thread_base() { void start_thread_base() {
before_start(); before_start();
is_running_ = true; is_running_ = true;
@ -106,7 +92,7 @@ private:
void timer_thread() { void timer_thread() {
struct timespec next_time; struct timespec next_time;
clock_gettime(CLOCK_MONOTONIC, &next_time); clock_gettime(CLOCK_MONOTONIC, &next_time);
while (is_running_) { while (is_running_) {
auto start = std::chrono::steady_clock::now(); auto start = std::chrono::steady_clock::now();
@ -117,9 +103,9 @@ private:
} }
auto end = std::chrono::steady_clock::now(); 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(); int period_us = period_us_.load();
next_time.tv_nsec += period_us * 1000; next_time.tv_nsec += period_us * 1000;
while (next_time.tv_nsec >= 1000000000) { while (next_time.tv_nsec >= 1000000000) {
@ -129,7 +115,6 @@ private:
clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, nullptr); clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_time, nullptr);
} }
} }
pthread_t thread_{}; pthread_t thread_{};
std::atomic<bool> is_running_; std::atomic<bool> is_running_;

149
src/robot_state.hpp
View File

@ -14,8 +14,8 @@
#pragma once #pragma once
#include <vector>
#include <mutex> #include <mutex>
#include <vector>
// Represents the state of a single joint: position, velocity, and effort. // Represents the state of a single joint: position, velocity, and effort.
struct JointState { struct JointState {
@ -26,74 +26,72 @@ struct JointState {
// Manages reference and response states for a robot component (e.g., arm, hand). // Manages reference and response states for a robot component (e.g., arm, hand).
class RobotState { class RobotState {
public: public:
RobotState() = default; 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 ---
// --- Set/Get reference (target) joint states --- void set_reference(size_t index, const JointState& state) {
void set_reference(size_t index, const JointState& state) { std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_); if (index < reference_.size()) {
if (index < reference_.size()) { reference_[index] = state;
reference_[index] = state;
}
} }
}
void set_all_references(const std::vector<JointState>& states) {
std::lock_guard<std::mutex> lock(mutex_); void set_all_references(const std::vector<JointState>& states) {
reference_ = states; std::lock_guard<std::mutex> lock(mutex_);
reference_ = states;
}
JointState get_reference(size_t index) const {
std::lock_guard<std::mutex> lock(mutex_);
return index < reference_.size() ? reference_[index] : JointState{};
}
std::vector<JointState> get_all_references() const {
std::lock_guard<std::mutex> lock(mutex_);
return reference_;
}
void set_response(size_t index, const JointState& state) {
std::lock_guard<std::mutex> lock(mutex_);
if (index < response_.size()) {
response_[index] = state;
} }
}
JointState get_reference(size_t index) const {
std::lock_guard<std::mutex> lock(mutex_); void set_all_responses(const std::vector<JointState>& states) {
return index < reference_.size() ? reference_[index] : JointState{}; std::lock_guard<std::mutex> lock(mutex_);
} response_ = states;
}
std::vector<JointState> get_all_references() const {
std::lock_guard<std::mutex> lock(mutex_); JointState get_response(size_t index) const {
return reference_; std::lock_guard<std::mutex> lock(mutex_);
} return index < response_.size() ? response_[index] : JointState{};
}
void set_response(size_t index, const JointState& state) {
std::lock_guard<std::mutex> lock(mutex_); std::vector<JointState> get_all_responses() const {
if (index < response_.size()) { std::lock_guard<std::mutex> lock(mutex_);
response_[index] = state; return response_;
} }
}
void resize(size_t new_size) {
void set_all_responses(const std::vector<JointState>& states) { std::lock_guard<std::mutex> lock(mutex_);
std::lock_guard<std::mutex> lock(mutex_); reference_.resize(new_size);
response_ = states; response_.resize(new_size);
} }
JointState get_response(size_t index) const { size_t get_size() const {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
return index < response_.size() ? response_[index] : JointState{}; return response_.size(); // assume same size for both
} }
std::vector<JointState> get_all_responses() const { private:
std::lock_guard<std::mutex> lock(mutex_); mutable std::mutex mutex_;
return response_; std::vector<JointState> response_;
} std::vector<JointState> reference_;
};
void resize(size_t new_size) {
std::lock_guard<std::mutex> lock(mutex_);
reference_.resize(new_size);
response_.resize(new_size);
}
size_t get_size() const {
std::lock_guard<std::mutex> lock(mutex_);
return response_.size(); // assume same size for both
}
private:
mutable std::mutex mutex_;
std::vector<JointState> response_;
std::vector<JointState> reference_;
};
// Manages the joint states of robot components (arm, hand). // Manages the joint states of robot components (arm, hand).
class RobotSystemState { class RobotSystemState {
@ -120,14 +118,14 @@ public:
void set_all_references(const std::vector<JointState>& all_refs) { void set_all_references(const std::vector<JointState>& all_refs) {
const size_t arm_size = arm_state_.get_size(); const size_t arm_size = arm_state_.get_size();
const size_t hand_size = hand_state_.get_size(); const size_t hand_size = hand_state_.get_size();
if (all_refs.size() != arm_size + hand_size) { if (all_refs.size() != arm_size + hand_size) {
throw std::runtime_error("set_all_references: size mismatch."); throw std::runtime_error("set_all_references: size mismatch.");
} }
std::vector<JointState> arm_refs(all_refs.begin(), all_refs.begin() + arm_size); std::vector<JointState> arm_refs(all_refs.begin(), all_refs.begin() + arm_size);
std::vector<JointState> hand_refs(all_refs.begin() + arm_size, all_refs.end()); std::vector<JointState> hand_refs(all_refs.begin() + arm_size, all_refs.end());
arm_state_.set_all_references(arm_refs); arm_state_.set_all_references(arm_refs);
hand_state_.set_all_references(hand_refs); hand_state_.set_all_references(hand_refs);
} }
@ -141,7 +139,7 @@ public:
combined.insert(combined.end(), hand.begin(), hand.end()); combined.insert(combined.end(), hand.begin(), hand.end());
return combined; return combined;
} }
void set_all_responses(const std::vector<JointState>& all_responses) { void set_all_responses(const std::vector<JointState>& all_responses) {
const size_t arm_size = arm_state_.get_size(); const size_t arm_size = arm_state_.get_size();
const size_t hand_size = hand_state_.get_size(); const size_t hand_size = hand_state_.get_size();
@ -153,18 +151,15 @@ public:
if (all_responses.size() != arm_size + hand_size) { if (all_responses.size() != arm_size + hand_size) {
throw std::runtime_error("set_all_responses: size mismatch."); throw std::runtime_error("set_all_responses: size mismatch.");
} }
std::vector<JointState> arm_res(all_responses.begin(), all_responses.begin() + arm_size); std::vector<JointState> arm_res(all_responses.begin(), all_responses.begin() + arm_size);
std::vector<JointState> hand_res(all_responses.begin() + arm_size, all_responses.end()); std::vector<JointState> hand_res(all_responses.begin() + arm_size, all_responses.end());
arm_state_.set_all_responses(arm_res); arm_state_.set_all_responses(arm_res);
hand_state_.set_all_responses(hand_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: private:
RobotState arm_state_; RobotState arm_state_;

7
src/yamlloader.hpp
View File

@ -12,13 +12,13 @@
// See the License for the specific language governing permissions and // See the License for the specific language governing permissions and
// limitations under the License. // limitations under the License.
#pragma once #pragma once
#include <yaml-cpp/yaml.h> #include <yaml-cpp/yaml.h>
#include <stdexcept>
#include <string> #include <string>
#include <vector> #include <vector>
#include <stdexcept>
class YamlLoader { class YamlLoader {
public: public:
@ -26,7 +26,8 @@ public:
try { try {
root_ = YAML::LoadFile(filepath); root_ = YAML::LoadFile(filepath);
} catch (const std::exception& e) { } 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());
} }
} }