From 80fca8524c3f31479faf8a5087377060f687a70b Mon Sep 17 00:00:00 2001 From: shen <664376944@qq.com> Date: Fri, 27 Feb 2026 11:28:55 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E5=90=AF=E5=8A=A8=E8=84=9A?= =?UTF-8?q?=E6=9C=AC=EF=BC=8C=E5=88=A0=E9=99=A4=E6=97=A0=E7=94=A8=E4=BB=A3?= =?UTF-8?q?=E7=A0=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- control/openarm_joint_sensor.cpp | 114 ------------------------------- script/launch_joint_sensor.sh | 104 ++++++++++++++++++++++++++++ 2 files changed, 104 insertions(+), 114 deletions(-) create mode 100755 script/launch_joint_sensor.sh diff --git a/control/openarm_joint_sensor.cpp b/control/openarm_joint_sensor.cpp index a260678..6700112 100644 --- a/control/openarm_joint_sensor.cpp +++ b/control/openarm_joint_sensor.cpp @@ -120,37 +120,6 @@ void signal_handler(int signal) { } } -class LeaderArmThread : public PeriodicTimerThread { -public: - LeaderArmThread(std::shared_ptr robot_state, Control *control_l, - double hz = 500.0) - : PeriodicTimerThread(hz), robot_state_(robot_state), control_l_(control_l) {} - -protected: - void before_start() override { std::cout << "leader start thread " << std::endl; } - - void after_stop() override { std::cout << "leader stop thread " << std::endl; } - - void on_timer() override { - static auto prev_time = std::chrono::steady_clock::now(); - - control_l_->unilateral_step(); - - auto now = std::chrono::steady_clock::now(); - - auto elapsed_us = - std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; - // robot_state_->arm_state().get_response(0).position; // Example: access joint 0 position - - // std::cout << "[Leader] Period: " << elapsed_us << " us" << std::endl; - } - -private: - std::shared_ptr robot_state_; - Control *control_l_; -}; - class FollowerArmThread : public PeriodicTimerThread { public: FollowerArmThread(std::shared_ptr robot_state, Control *control_f, @@ -192,53 +161,6 @@ private: Control *control_f_; }; -class AdminThread : public PeriodicTimerThread { -public: - AdminThread(std::shared_ptr leader_state, - std::shared_ptr follower_state, Control *control_l, - Control *control_f, double hz = 500.0) - : PeriodicTimerThread(hz), - leader_state_(leader_state), - follower_state_(follower_state), - control_l_(control_l), - control_f_(control_f) {} - -protected: - void before_start() override { std::cout << "admin start thread " << std::endl; } - - void after_stop() override { std::cout << "admin stop thread " << std::endl; } - - void on_timer() override { - static auto prev_time = std::chrono::steady_clock::now(); - auto now = std::chrono::steady_clock::now(); - - // 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(); - - // 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); - - auto elapsed_us = - std::chrono::duration_cast(now - prev_time).count(); - prev_time = now; - - // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl; - } - -private: - std::shared_ptr leader_state_; - std::shared_ptr follower_state_; - Control *control_l_; - Control *control_f_; -}; int main(int argc, char **argv) { try { @@ -248,7 +170,6 @@ int main(int argc, char **argv) { std::string arm_side = "right_arm"; std::string leader_urdf_path; std::string follower_urdf_path; - std::string leader_can_interface = "can0"; std::string follower_can_interface = "can2"; std::string sensor_can_interface = "can0"; @@ -256,7 +177,6 @@ int main(int argc, char **argv) { std::cerr << "Usage: " << argv[0] << " [arm_side] [leader_can] [follower_can]" - " [sensor_can]" << std::endl; return 1; } @@ -277,7 +197,6 @@ int main(int argc, char **argv) { // Optional: CAN interfaces if (argc >= 6) { - leader_can_interface = argv[4]; follower_can_interface = argv[5]; } @@ -304,7 +223,6 @@ int main(int argc, char **argv) { // Output confirmation std::cout << "=== OpenArm Unilateral Control ===" << std::endl; std::cout << "Arm side : " << arm_side << std::endl; - std::cout << "Leader CAN : " << leader_can_interface << std::endl; std::cout << "Follower CAN : " << follower_can_interface << std::endl; std::cout << "Leader URDF path : " << leader_urdf_path << std::endl; std::cout << "Follower URDF path: " << follower_urdf_path << std::endl; @@ -356,14 +274,6 @@ int main(int argc, char **argv) { YamlLoader leader_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/leader.yaml"); YamlLoader follower_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/follower.yaml"); - // Leader parameters - std::vector leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp"); - std::vector leader_kd = leader_loader.get_vector("LeaderArmParam", "Kd"); - std::vector leader_Fc = leader_loader.get_vector("LeaderArmParam", "Fc"); - std::vector leader_k = leader_loader.get_vector("LeaderArmParam", "k"); - std::vector leader_Fv = leader_loader.get_vector("LeaderArmParam", "Fv"); - std::vector leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo"); - // Follower parameters std::vector follower_kp = follower_loader.get_vector("FollowerArmParam", "Kp"); std::vector follower_kd = follower_loader.get_vector("FollowerArmParam", "Kd"); @@ -378,42 +288,26 @@ int main(int argc, char **argv) { Dynamics *follower_arm_dynamics = new Dynamics(follower_urdf_path, root_link, leaf_link); follower_arm_dynamics->Init(); - std::cout << "=== Initializing Leader OpenArm ===" << std::endl; - openarm::can::socket::OpenArm *leader_openarm = - openarm_init::OpenArmInitializer::initialize_openarm(leader_can_interface, true); - std::cout << "=== Initializing Follower OpenArm ===" << std::endl; openarm::can::socket::OpenArm *follower_openarm = openarm_init::OpenArmInitializer::initialize_openarm(follower_can_interface, true); - size_t leader_arm_motor_num = leader_openarm->get_arm().get_motors().size(); size_t follower_arm_motor_num = follower_openarm->get_arm().get_motors().size(); - size_t leader_hand_motor_num = leader_openarm->get_gripper().get_motors().size(); size_t follower_hand_motor_num = follower_openarm->get_gripper().get_motors().size(); - std::cout << "leader arm motor num : " << leader_arm_motor_num << std::endl; std::cout << "follower arm motor num : " << follower_arm_motor_num << std::endl; - std::cout << "leader hand motor num : " << leader_hand_motor_num << std::endl; std::cout << "follower hand motor num : " << follower_hand_motor_num << std::endl; // Declare robot_state (Joint and motor counts are assumed to be equal) - std::shared_ptr leader_state = - std::make_shared(leader_arm_motor_num, leader_hand_motor_num); std::shared_ptr follower_state = std::make_shared(follower_arm_motor_num, follower_hand_motor_num); - Control *control_leader = new Control( - leader_openarm, leader_arm_dynamics, follower_arm_dynamics, leader_state, - 1.0 / FREQUENCY, ROLE_LEADER, arm_side, leader_arm_motor_num, leader_hand_motor_num); Control *control_follower = new Control(follower_openarm, leader_arm_dynamics, follower_arm_dynamics, follower_state, 1.0 / FREQUENCY, ROLE_FOLLOWER, arm_side, follower_arm_motor_num, follower_hand_motor_num); - control_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); @@ -424,24 +318,16 @@ int main(int argc, char **argv) { // thread_f.join(); // Start control process - LeaderArmThread leader_thread(leader_state, control_leader, FREQUENCY); FollowerArmThread follower_thread(follower_state, control_follower, FREQUENCY); - AdminThread admin_thread(leader_state, follower_state, control_leader, control_follower, - FREQUENCY); - leader_thread.start_thread(); follower_thread.start_thread(); - // admin_thread.start_thread(); while (keep_running) { std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } - leader_thread.stop_thread(); follower_thread.stop_thread(); - admin_thread.stop_thread(); - leader_openarm->disable_all(); follower_openarm->disable_all(); if (can_read_thread.joinable()) { diff --git a/script/launch_joint_sensor.sh b/script/launch_joint_sensor.sh new file mode 100755 index 0000000..e1b49c3 --- /dev/null +++ b/script/launch_joint_sensor.sh @@ -0,0 +1,104 @@ +#!/bin/bash +# +# 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. + +# ========= 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 +TMPDIR="/tmp/openarm_urdf_gen" + +# Validate arm side +if [[ "$ARM_SIDE" != "right_arm" && "$ARM_SIDE" != "left_arm" ]]; then + echo "[ERROR] Invalid arm_side: $ARM_SIDE" + echo "Usage: $0 [leader_can_if] [follower_can_if]" + exit 1 +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 +fi + +if [ -z "$FOLLOWER_CAN_IF" ]; then + if [ "$ARM_SIDE" = "right_arm" ]; then + FOLLOWER_CAN_IF="can2" + else + FOLLOWER_CAN_IF="can3" + fi +fi + +# File paths +LEADER_URDF_PATH="$TMPDIR/${ARM_TYPE}_leader.urdf" +FOLLOWER_URDF_PATH="$TMPDIR/${ARM_TYPE}_follower.urdf" +XACRO_FILE="$ARM_TYPE.urdf.xacro" +WS_DIR=~/openarm_ros2_ws +XACRO_PATH="$WS_DIR/src/openarm_description/urdf/robot/$XACRO_FILE" +BIN_PATH="$WS_DIR/src/openarm_teleop/build/joint_sensor_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 +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 +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 +fi + +# ================================ + +# Check binary +if [ ! -f "$BIN_PATH" ]; then + 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" +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..." +"$BIN_PATH" "$LEADER_URDF_PATH" "$FOLLOWER_URDF_PATH" "$ARM_SIDE" "$LEADER_CAN_IF" "$FOLLOWER_CAN_IF" + +# Cleanup +echo "[INFO] Cleaning up temporary files..." +rm -rf "$TMPDIR"