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

Add formatter (#5)

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

19
.clang-format Normal file
View File

@ -0,0 +1,19 @@
# Copyright 2025 Enactic, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
---
AccessModifierOffset: -4
BasedOnStyle: Google
ColumnLimit: 100
IndentWidth: 4

19
.cmake-format.py Normal file
View File

@ -0,0 +1,19 @@
# Copyright 2025 Enactic, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
with section("format"):
command_case = "lower"
with section("markup"):
enable_markup = False

27
.editorconfig Normal file
View File

@ -0,0 +1,27 @@
# Copyright 2025 Enactic, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# https://editorconfig.org/
root = true
[*]
charset = utf-8
insert_final_newline = true
spelling_language = en
trim_trailing_whitespace = true
[*.sh]
indent_size = 4
indent_style = space

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

@ -25,6 +25,25 @@ concurrency:
group: ${{ github.head_ref || github.sha }}-${{ github.workflow }}
cancel-in-progress: true
jobs:
lint:
name: Lint
runs-on: ubuntu-latest
timeout-minutes: 5
steps:
- uses: actions/checkout@v5
with:
submodules: recursive
- name: Install pre-commit
run: |
python -m pip install pre-commit
- uses: actions/cache@v4
with:
path: ~/.cache/pre-commit
key: pre-commit-${{ hashFiles('.pre-commit-config.yaml') }}
- name: Run pre-commit
run: |
pre-commit run --show-diff-on-failure --color=always --all-files
build:
name: Build
runs-on: ${{ matrix.runs-on }}

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

@ -0,0 +1,42 @@
# Copyright 2025 Enactic, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
repos:
- repo: https://github.com/pre-commit/mirrors-clang-format
rev: v20.1.7
hooks:
- id: clang-format
alias: cpp
- repo: https://github.com/cheshirekow/cmake-format-precommit
rev: v0.6.13
hooks:
- id: cmake-format
alias: cpp
- repo: https://github.com/koalaman/shellcheck-precommit
rev: v0.10.0
hooks:
- id: shellcheck
alias: shell
- repo: https://github.com/scop/pre-commit-shfmt
# v3.11.0-1 or later requires pre-commit 3.2.0 or later but Ubuntu
# 22.04 ships pre-commit 2.17.0. We can update this rev after
# Ubuntu 22.04 reached EOL (June 2027).
rev: v3.10.0-1
hooks:
- id: shfmt
alias: shell
args:
# The default args is "--write --simplify" but we don't use
# "--simplify". Because it's conflicted will ShellCheck.
- "--write"

16
.shellcheckrc Normal file
View File

@ -0,0 +1,16 @@
# Copyright 2025 Enactic, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
external-sources=true
source-path=SCRIPTDIR

30
CMakeLists.txt
View File

@ -40,28 +40,18 @@ endif()
# -----------------------------
# Create static library
# -----------------------------
add_library(openarm_teleop_lib STATIC
src/controller/dynamics.cpp
src/controller/control.cpp
src/openarm_port/openarm_init.cpp
src/openarm_port/joint_mapper.cpp
)
add_library(
openarm_teleop_lib STATIC
src/controller/dynamics.cpp src/controller/control.cpp
src/openarm_port/openarm_init.cpp src/openarm_port/joint_mapper.cpp)
target_include_directories(
openarm_teleop_lib
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/src
)
target_include_directories(openarm_teleop_lib
PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/src)
target_link_libraries(openarm_teleop_lib
PUBLIC
OpenArmCAN::openarm_can
Eigen3::Eigen
${orocos_kdl_LIBRARIES}
${kdl_parser_LIBRARIES}
urdfdom::urdfdom_model
yaml-cpp::yaml-cpp
)
target_link_libraries(
openarm_teleop_lib
PUBLIC OpenArmCAN::openarm_can Eigen3::Eigen ${orocos_kdl_LIBRARIES}
${kdl_parser_LIBRARIES} urdfdom::urdfdom_model yaml-cpp::yaml-cpp)
# -----------------------------
# Executables

47
control/gravity_compasation.cpp
View File

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

235
control/openarm_bilateral_control.cpp
View File

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

12
control/openarm_communication_test.cpp
View File

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

232
control/openarm_unilateral_control.cpp
View File

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

64
script/launch_bilateral.sh
View File

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

30
script/launch_grav_comp.sh
View File

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

63
script/launch_unilateral.sh
View File

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

810
src/controller/control.cpp
View File

@ -12,38 +12,54 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include <cmath>
#include <unistd.h>
#include <string.h>
#include <thread>
#include <cmath>
#include <unistd.h>
#include <algorithm>
#include <iomanip>
#include <cmath>
#include <controller/control.hpp>
#include <controller/dynamics.hpp>
#include <iomanip>
#include <thread>
Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, size_t arm_motor_num, size_t hand_motor_num):
openarm_(arm), dynamics_l_(dynamics_l), dynamics_f_(dynamics_f), robot_state_(robot_state), Ts_(Ts), role_(role), arm_motor_num_(arm_motor_num), hand_motor_num_(hand_motor_num)
{
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,
size_t arm_motor_num, size_t hand_motor_num)
: openarm_(arm),
dynamics_l_(dynamics_l),
dynamics_f_(dynamics_f),
robot_state_(robot_state),
Ts_(Ts),
role_(role),
arm_motor_num_(arm_motor_num),
hand_motor_num_(hand_motor_num) {
differentiator_ = new Differentiator(Ts);
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
}
Control::Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, std::string arm_type, size_t arm_motor_num, size_t hand_motor_num):
openarm_(arm), dynamics_l_(dynamics_l), dynamics_f_(dynamics_f), robot_state_(robot_state), Ts_(Ts), role_(role), arm_motor_num_(arm_motor_num), hand_motor_num_(hand_motor_num)
{
differentiator_ = new Differentiator(Ts);
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
Control::Control(openarm::can::socket::OpenArm* arm, Dynamics* dynamics_l, Dynamics* dynamics_f,
std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
std::string arm_type, size_t arm_motor_num, size_t hand_motor_num)
: openarm_(arm),
dynamics_l_(dynamics_l),
dynamics_f_(dynamics_f),
robot_state_(robot_state),
Ts_(Ts),
role_(role),
arm_motor_num_(arm_motor_num),
hand_motor_num_(hand_motor_num) {
differentiator_ = new Differentiator(Ts);
openarmjointconverter_ = new OpenArmJointConverter(arm_motor_num_);
openarmgripperjointconverter_ = new OpenArmJGripperJointConverter(hand_motor_num_);
arm_type_ = arm_type;
arm_type_ = arm_type;
}
Control::~Control() {
std::cout << "Control destructed " << std::endl;
delete openarmjointconverter_;
delete differentiator_;
std::cout << "Control destructed " << std::endl;
delete openarmjointconverter_;
delete differentiator_;
}
// bool Control::Setup(void)
@ -57,137 +73,237 @@ Control::~Control() {
// return true;
// }
void Control::Shutdown(void){
std::cout << "control shutdown !!!" << std::endl;
void Control::Shutdown(void) {
std::cout << "control shutdown !!!" << std::endl;
openarm_->disable_all();
openarm_->disable_all();
}
void Control::SetParameter(
const std::vector<double>& Kp,
const std::vector<double>& Kd,
const std::vector<double>& Fc,
const std::vector<double>& k,
const std::vector<double>& Fv,
const std::vector<double>& Fo)
{
Kp_ = Kp;
Kd_ = Kd;
Fc_ = Fc;
k_ = k;
Fv_ = Fv;
Fo_ = Fo;
}
void Control::SetParameter(const std::vector<double>& Kp, const std::vector<double>& Kd,
const std::vector<double>& Fc, const std::vector<double>& k,
const std::vector<double>& Fv, const std::vector<double>& Fo) {
Kp_ = Kp;
Kd_ = Kd;
Fc_ = Fc;
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()
{
// 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
});
}
std::vector<MotorState> gripper_motor_states;
const auto& gripper_motors = openarm_->get_gripper().get_motors();
for (size_t i = 0; i < gripper_motors.size(); ++i) {
const auto& motor = gripper_motors[i];
gripper_motor_states.push_back({motor.get_position(), motor.get_velocity(), 0});
}
std::vector<MotorState> gripper_motor_states;
const auto& gripper_motors = openarm_->get_gripper().get_motors();
for (size_t i = 0; i < gripper_motors.size(); ++i) {
const auto& motor = gripper_motors[i];
gripper_motor_states.push_back({
motor.get_position(),
motor.get_velocity(),
0
});
}
// 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);
// 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);
// set reponse
robot_state_->arm_state().set_all_responses(joint_arm_states);
robot_state_->hand_state().set_all_responses(joint_gripper_states);
size_t arm_dof = robot_state_->arm_state().get_size();
size_t gripper_dof = robot_state_->hand_state().get_size();
size_t arm_dof = robot_state_->arm_state().get_size();
size_t gripper_dof = robot_state_->hand_state().get_size();
std::vector<double> joint_arm_positions(arm_dof, 0.0);
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_arm_velocities(arm_dof, 0.0);
std::vector<double> joint_arm_efforts(arm_dof, 0.0);
std::vector<double> joint_gripper_positions(gripper_dof, 0.0);
std::vector<double> joint_gripper_velocities(gripper_dof, 0.0);
std::vector<double> joint_gripper_efforts(gripper_dof, 0.0);
std::vector<double> joint_gripper_positions(gripper_dof, 0.0);
std::vector<double> joint_gripper_velocities(gripper_dof, 0.0);
std::vector<double> joint_gripper_efforts(gripper_dof, 0.0);
for (size_t i = 0; i < arm_dof; ++i) {
joint_arm_positions[i] = joint_arm_states[i].position;
joint_arm_velocities[i] = joint_arm_states[i].velocity;
}
for (size_t i = 0; i < arm_dof; ++i) {
joint_arm_positions[i] = joint_arm_states[i].position;
joint_arm_velocities[i] = joint_arm_states[i].velocity;
}
for (size_t i = 0; i < gripper_dof; ++i) {
joint_gripper_positions[i] = joint_gripper_states[i].position;
joint_gripper_velocities[i] = joint_gripper_states[i].velocity;
}
for (size_t i = 0; i < gripper_dof; ++i) {
joint_gripper_positions[i] = joint_gripper_states[i].position;
joint_gripper_velocities[i] = joint_gripper_states[i].velocity;
}
std::vector<double> gravity(arm_dof, 0.0);
std::vector<double> coriolis(arm_dof, 0.0);
std::vector<double> friction(arm_dof + gripper_dof, 0.0);
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);
std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
std::vector<JointState> joint_gripper_states_ref =
robot_state_->hand_state().get_all_references();
std::vector<JointState> joint_arm_states_ref = robot_state_->arm_state().get_all_references();
std::vector<JointState> joint_gripper_states_ref = robot_state_->hand_state().get_all_references();
std::vector<double> joint_arm_positions_ref(arm_dof);
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) {
joint_arm_positions_ref[i] = joint_arm_states_ref[i].position;
}
if (role_ == ROLE_LEADER) {
dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data());
dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(),
coriolis.data());
} else if (role_ == ROLE_FOLLOWER) {
dynamics_f_->GetGravity(joint_arm_positions.data(), gravity.data());
dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(),
coriolis.data());
}
if (role_ == ROLE_LEADER) {
dynamics_l_->GetGravity(joint_arm_positions.data(), gravity.data());
dynamics_l_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data());
// Friction (compute joint friction)
for (size_t i = 0; i < joint_arm_velocities.size(); ++i)
ComputeFriction(joint_arm_velocities.data(), friction.data(), i);
for (size_t i = 0; i < joint_gripper_velocities.size(); ++i)
ComputeFriction(joint_gripper_velocities.data(), friction.data(),
joint_arm_velocities.size() + i);
} else if (role_ == ROLE_FOLLOWER) {
dynamics_f_->GetGravity(joint_arm_positions.data(), gravity.data());
dynamics_f_->GetCoriolis(joint_arm_positions.data(), joint_arm_velocities.data(), coriolis.data());
}
// set gravity and friciton comp joint torque value
for (size_t i = 0; i < arm_dof; i++) {
joint_arm_states_ref[i].effort = gravity[i] + friction[i];
}
for (size_t i = 0; i < gripper_dof; i++) {
joint_gripper_states_ref[i].effort = friction[i + arm_dof];
}
std::vector<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)
ComputeFriction(joint_arm_velocities.data(), friction.data(), i);
ComputeFriction(joint_arm_velocities.data(), friction.data(), i);
for (size_t i = 0; i < joint_gripper_velocities.size(); ++i)
ComputeFriction(joint_gripper_velocities.data(), friction.data(), joint_arm_velocities.size() + i);
ComputeFriction(joint_gripper_velocities.data(), friction.data(), arm_dof + i);
// set gravity and friciton comp joint torque value
for(size_t i = 0 ; i < arm_dof; i++){
joint_arm_states_ref[i].effort = gravity[i] + friction[i];
// arm joint state
std::vector<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;
}
for(size_t i = 0 ; i < gripper_dof; i++){
joint_gripper_states_ref[i].effort = friction[i + arm_dof];
// 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_states_ref);
std::vector<MotorState> motor_gripper_states = openarmgripperjointconverter_->joint_to_motor(joint_gripper_states_ref);
std::vector<MotorState> motor_arm_states =
openarmjointconverter_->joint_to_motor(joint_arm_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;
arm_cmds.reserve(arm_dof);
for (size_t i = 0; i < arm_dof; ++i) {
arm_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i], Kd_[i], motor_arm_states[i].position, motor_arm_states[i].velocity, motor_arm_states[i].effort});
arm_cmds.emplace_back(
openarm::damiao_motor::MITParam{0.0, 0.0, 0.0, 0.0, motor_arm_states[i].effort});
}
// gripper command mit param
std::vector<openarm::damiao_motor::MITParam> gripper_cmds;
gripper_cmds.reserve(gripper_dof);
for (size_t i = 0; i < gripper_dof; ++i) {
gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{Kp_[i + arm_dof],Kd_[i + arm_dof], motor_gripper_states[i].position, motor_gripper_states[i].velocity, motor_gripper_states[i].effort});
gripper_cmds.emplace_back(openarm::damiao_motor::MITParam{
0.0, 0.0, 0.0, 0.0, motor_gripper_states[i].effort});
}
// send command to arm
@ -195,316 +311,200 @@ bool Control::bilateral_step()
// send command to gripper
openarm_->get_gripper().mit_control_all(gripper_cmds);
std::this_thread::sleep_for(std::chrono::microseconds(200));
openarm_->recv_all(220);
openarm_->recv_all(200);
return true;
}
else if (role_ == ROLE_FOLLOWER) {
std::vector<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});
}
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());
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;
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});
}
void Control::ComputeFriction(const double* velocity, double* friction, size_t index)
{
if (TANHFRIC) {
const double amp_tmp = 1.0;
const double coef_tmp = 0.1;
openarm_->get_arm().mit_control_all(arm_cmds);
openarm_->get_gripper().mit_control_all(hand_cmds);
const double v = velocity[index];
const double Fc = Fc_.at(index);
const double k = k_.at(index);
const double Fv = Fv_.at(index);
const double Fo = Fo_.at(index);
openarm_->recv_all(200);
friction[index] = amp_tmp * Fc * std::tanh(coef_tmp * k * v) + Fv * v + Fo;
} else {
friction[index] = velocity[index] * Dn_.at(index);
}
return true;
}
return true;
}
void Control::ComputeFriction(const double* velocity, double* friction, size_t index) {
if (TANHFRIC) {
const double amp_tmp = 1.0;
const double coef_tmp = 0.1;
const double v = velocity[index];
const double Fc = Fc_.at(index);
const double k = k_.at(index);
const double Fv = Fv_.at(index);
const double Fo = Fo_.at(index);
friction[index] = amp_tmp * Fc * std::tanh(coef_tmp * k * v) + Fv * v + Fo;
} else {
friction[index] = velocity[index] * Dn_.at(index);
}
}
bool Control::AdjustPosition(void) {
int nstep = 220;
double alpha;
std::vector<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)
{
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;
}
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;
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;
}
bool Control::DetectVibration(const double* velocity, bool* what_axis)
{
bool vibration_detected = false;
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);
for (int i = 0; i < NJOINTS; ++i) {
what_axis[i] = false;
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});
}
velocity_buffer_[i].push_back(velocity[i]);
if (velocity_buffer_[i].size() > VEL_WINDOW_SIZE)
velocity_buffer_[i].pop_front();
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});
}
if (velocity_buffer_[i].size() < VEL_WINDOW_SIZE)
continue;
openarm_->get_arm().mit_control_all(arm_cmds);
openarm_->get_gripper().mit_control_all(hand_cmds);
double mean = std::accumulate(
velocity_buffer_[i].begin(), velocity_buffer_[i].end(), 0.0
) / velocity_buffer_[i].size();
std::this_thread::sleep_for(std::chrono::milliseconds(10));
double var = 0.0;
for (double v : velocity_buffer_[i]) {
var += (v - mean) * (v - mean);
}
openarm_->recv_all();
}
double stddev = std::sqrt(var / velocity_buffer_[i].size());
std::vector<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) {
what_axis[i] = true;
vibration_detected = true;
std::cout << "[VIBRATION] Joint " << i << " stddev: " << stddev << std::endl;
}
}
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});
}
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
// #include <sensor_msgs/msg/joint_state.hpp>
#include <utility>
#include <fstream>
#include <deque>
#include <numeric>
#include <memory>
#include <controller/diff.hpp>
#include <controller/dynamics.hpp>
#include <robot_state.hpp>
#include <deque>
#include <fstream>
#include <joint_state_converter.hpp>
#include <openarm_constants.hpp>
#include <robot_state.hpp>
#include <memory>
#include <numeric>
#include <openarm/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <openarm_constants.hpp>
#include <robot_state.hpp>
#include <utility>
class Control {
openarm::can::socket::OpenArm *openarm_;
class Control
{
openarm::can::socket::OpenArm* openarm_;
double Ts_;
int role_;
double Ts_;
int role_;
size_t arm_motor_num_;
size_t hand_motor_num_;
size_t arm_motor_num_;
size_t hand_motor_num_;
Differentiator *differentiator_;
OpenArmJointConverter *openarmjointconverter_;
OpenArmJGripperJointConverter *openarmgripperjointconverter_;
Differentiator *differentiator_;
OpenArmJointConverter *openarmjointconverter_;
OpenArmJGripperJointConverter *openarmgripperjointconverter_;
std::shared_ptr<RobotSystemState> robot_state_;
std::shared_ptr<RobotSystemState> robot_state_;
std::string arm_type_;
std::string arm_type_;
Dynamics *dynamics_f_;
Dynamics *dynamics_l_;
Dynamics *dynamics_f_;
Dynamics *dynamics_l_;
double oblique_coordinates_force;
double oblique_coordinates_position;
double oblique_coordinates_force;
double oblique_coordinates_position;
// for easy logging
// std::vector<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
// 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];
public:
Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
size_t arm_joint_num, size_t hand_motor_num);
Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f,
std::shared_ptr<RobotSystemState> robot_state, double Ts, int role,
std::string arm_type, size_t arm_joint_num, size_t hand_motor_num);
~Control();
public:
Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, size_t arm_joint_num, size_t hand_motor_num);
Control(openarm::can::socket::OpenArm *arm, Dynamics *dynamics_l, Dynamics *dynamics_f, std::shared_ptr<RobotSystemState> robot_state, double Ts, int role, std::string arm_type, size_t arm_joint_num, size_t hand_motor_num);
~Control();
std::shared_ptr<RobotSystemState> response_;
std::shared_ptr<RobotSystemState> reference_;
std::shared_ptr<RobotSystemState> response_;
std::shared_ptr<RobotSystemState> reference_;
std::vector<double> Dn_, Kp_, Kd_, Fc_, k_, Fv_, Fo_;
std::vector<double> Dn_, Kp_, Kd_,Fc_, k_, Fv_, Fo_;
// bool Setup(void);
void Setstate(int state);
void Shutdown(void);
// bool Setup(void);
void Setstate(int state);
void Shutdown(void);
void SetParameter(const std::vector<double> &Kp, const std::vector<double> &Kd,
const std::vector<double> &Fc, const std::vector<double> &k,
const std::vector<double> &Fv, const std::vector<double> &Fo);
void SetParameter(
const std::vector<double>& Kp,
const std::vector<double>& Kd,
const std::vector<double>& Fc,
const std::vector<double>& k,
const std::vector<double>& Fv,
const std::vector<double>& Fo);
bool AdjustPosition(void);
bool AdjustPosition(void);
// Compute torque based on bilateral
bool bilateral_step();
bool unilateral_step();
// Compute torque based on bilateral
bool bilateral_step();
bool unilateral_step();
// NOTE! Control() class operates on "joints", while the underlying
// classes operates on "actuators". The following functions map
// joints to actuators.
// NOTE! Control() class operates on "joints", while the underlying
// classes operates on "actuators". The following functions map
// joints to actuators.
void ComputeJointPosition(const double *motor_position, double *joint_position);
void ComputeJointVelocity(const double *motor_velocity, double *joint_velocity);
void ComputeMotorTorque(const double *joint_torque, double *motor_torque);
void ComputeJointPosition(const double *motor_position, double *joint_position);
void ComputeJointVelocity(const double *motor_velocity, double *joint_velocity);
void ComputeMotorTorque(const double *joint_torque, double *motor_torque);
// void ComputeFriction(const double *velocity, double *friction);
void ComputeFriction(const double* velocity, double* friction, size_t index);
void ComputeGravity(const double *position, double *gravity);
bool DetectVibration(const double* velocity, bool *what_axis);
// void ComputeFriction(const double *velocity, double *friction);
void ComputeFriction(const double *velocity, double *friction, size_t index);
void ComputeGravity(const double *position, double *gravity);
bool DetectVibration(const double *velocity, bool *what_axis);
};

87
src/controller/diff.hpp
View File

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

266
src/controller/dynamics.cpp
View File

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

70
src/controller/dynamics.hpp
View File

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

116
src/joint_state_converter.hpp
View File

@ -14,8 +14,8 @@
#pragma once
#include <vector>
#include <robot_state.hpp>
#include <vector>
// Represents the state of a single joint
// struct JointState {
@ -28,7 +28,7 @@
struct MotorState {
double position = 0.0;
double velocity = 0.0;
double effort = 0.0;
double effort = 0.0;
};
// Abstract base class for converting between motor and joint states
@ -37,71 +37,71 @@ public:
virtual ~MotorJointConverter() = default;
// MotorState vector → JointState vector
virtual std::vector<JointState> motor_to_joint(const std::vector<MotorState>& motor_states) const = 0;
virtual std::vector<JointState> motor_to_joint(
const std::vector<MotorState>& motor_states) const = 0;
// JointState vector → MotorState vector
virtual std::vector<MotorState> joint_to_motor(const std::vector<JointState>& joint_states) const = 0;
virtual std::vector<MotorState> joint_to_motor(
const std::vector<JointState>& joint_states) const = 0;
virtual size_t get_joint_count() const = 0;
};
// assume motor num equals to joint num
class OpenArmJointConverter : public MotorJointConverter {
public:
explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) {
std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl;
}
std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
// std::cout << "joint num conv : " << m.size() << std::endl;
public:
explicit OpenArmJointConverter(size_t joint_count) : joint_count_(joint_count) {
std::cout << "OpenArm joint converter joinit_count is : " << joint_count << std::endl;
}
std::vector<JointState> j(m.size());
for (size_t i = 0; i < m.size(); ++i){
j[i] = {m[i].position, m[i].velocity, m[i].effort};
std::vector<JointState> motor_to_joint(const std::vector<MotorState>& m) const override {
// std::cout << "joint num conv : " << m.size() << std::endl;
}
return j;
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};
}
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;
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_;
};
// 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};
}
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;
}
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_;
};
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
#include <unistd.h>
#include <time.h>
#include <unistd.h>
#include <iostream>
#include <openarm/damiao_motor/dm_motor_constants.hpp>
#include <vector>
#include <openarm/damiao_motor//dm_motor_constants.hpp>
constexpr double PI = 3.14159265358979323846;
@ -43,53 +44,51 @@ constexpr double PI = 3.14159265358979323846;
#define ELBOWLIMIT 0.0
static const double INITIAL_POSITION[NMOTORS] = {
0, 0, 0, PI/5.0, 0, 0, 0, 0
};
static const double INITIAL_POSITION[NMOTORS] = {0, 0, 0, PI / 5.0, 0, 0, 0, 0};
// safety limit position
static const double position_limit_max_L[] = { (2.0/3.0)*PI, PI, PI/2.0, PI, PI/2.0, PI/2.0, PI/2.0, PI };
static const double position_limit_min_L[] = { -(2.0/3.0)*PI, -PI/2.0, -PI/2.0, ELBOWLIMIT, -PI/2.0, -PI/2.0, -PI/2.0, -PI };
static const double position_limit_max_F[] = { (2.0/3.0)*PI, PI, PI/2.0, PI, PI/2.0, PI/2.0, PI/2.0, PI };
static const double position_limit_min_F[] = { -(2.0/3.0)*PI, -PI/2.0, -PI/2.0, ELBOWLIMIT, -PI/2.0, -PI/2.0, -PI/2.0, -PI };
static const double position_limit_max_L[] = {(2.0 / 3.0) * PI, PI, PI / 2.0, PI,
PI / 2.0, PI / 2.0, PI / 2.0, PI};
static const double position_limit_min_L[] = {-(2.0 / 3.0) * PI, -PI / 2.0, -PI / 2.0, ELBOWLIMIT,
-PI / 2.0, -PI / 2.0, -PI / 2.0, -PI};
static const double position_limit_max_F[] = {(2.0 / 3.0) * PI, PI, PI / 2.0, PI,
PI / 2.0, PI / 2.0, PI / 2.0, PI};
static const double position_limit_min_F[] = {-(2.0 / 3.0) * PI, -PI / 2.0, -PI / 2.0, ELBOWLIMIT,
-PI / 2.0, -PI / 2.0, -PI / 2.0, -PI};
// sefaty limit velocity
static const double velocity_limit_L[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0};
static const double velocity_limit_F[] = {8.0,8.0,8.0,8.0,8.0,8.0,8.0,8.0};
static const double velocity_limit_L[] = {8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0};
static const double velocity_limit_F[] = {8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0, 8.0};
// sefaty limit effort
static const double effort_limit_L[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0};
static const double effort_limit_F[] = {20.0,20.0,20.0,20.0,20.0,20.0,20.0,20.0};
static const double effort_limit_L[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0};
static const double effort_limit_F[] = {20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0};
// Motor configuration structure
struct MotorConfig {
std::vector<openarm::damiao_motor::MotorType> arm_motor_types;
std::vector<uint32_t> arm_send_can_ids;
std::vector<uint32_t> arm_recv_can_ids;
openarm::damiao_motor::MotorType gripper_motor_type;
uint32_t gripper_send_can_id;
uint32_t gripper_recv_can_id;
std::vector<openarm::damiao_motor::MotorType> arm_motor_types;
std::vector<uint32_t> arm_send_can_ids;
std::vector<uint32_t> arm_recv_can_ids;
openarm::damiao_motor::MotorType gripper_motor_type;
uint32_t gripper_send_can_id;
uint32_t gripper_recv_can_id;
};
// Global default motor configuration
static const MotorConfig DEFAULT_MOTOR_CONFIG = {
// Standard 7-DOF arm motor configuration
{openarm::damiao_motor::MotorType::DM8009,
openarm::damiao_motor::MotorType::DM8009,
openarm::damiao_motor::MotorType::DM4340,
openarm::damiao_motor::MotorType::DM4340,
openarm::damiao_motor::MotorType::DM4310,
openarm::damiao_motor::MotorType::DM4310,
openarm::damiao_motor::MotorType::DM4310},
// Standard 7-DOF arm motor configuration
{openarm::damiao_motor::MotorType::DM8009, openarm::damiao_motor::MotorType::DM8009,
openarm::damiao_motor::MotorType::DM4340, openarm::damiao_motor::MotorType::DM4340,
openarm::damiao_motor::MotorType::DM4310, openarm::damiao_motor::MotorType::DM4310,
openarm::damiao_motor::MotorType::DM4310},
// Standard CAN IDs for arm motors
{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07},
{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17},
// Standard CAN IDs for arm motors
{0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07},
{0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17},
// Standard gripper configuration
openarm::damiao_motor::MotorType::DM4310,
0x08,
0x18};
// Standard gripper configuration
openarm::damiao_motor::MotorType::DM4310,
0x08,
0x18};
// opening function
inline void printOpenArmBanner() {

58
src/openarm_port/joint_mapper.cpp
View File

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

12
src/openarm_port/joint_mapper.hpp
View File

@ -18,12 +18,10 @@
class JointMapper {
public:
JointMapper();
~JointMapper();
JointMapper();
~JointMapper();
void motor_to_joint_position(const double *motor_position,
double *joint_position);
void motor_to_joint_velocity(const double *motor_velocity,
double *joint_velocity);
void joint_to_motor_torque(const double *joint_torque, double *motor_torque);
void motor_to_joint_position(const double *motor_position, double *joint_position);
void motor_to_joint_velocity(const double *motor_velocity, double *joint_velocity);
void joint_to_motor_torque(const double *joint_torque, double *motor_torque);
};

96
src/openarm_port/openarm_init.cpp
View File

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

51
src/openarm_port/openarm_init.hpp
View File

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

45
src/periodic_timer_thread.hpp
View File

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

149
src/robot_state.hpp
View File

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

7
src/yamlloader.hpp
View File

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