增加关节传感器控制机械臂

CMakeLists.txt

@@ -68,8 +68,10 @@ add_executable(gravity_comp control/gravity_compasation.cpp)
 add_executable(comm_test control/openarm_communication_test.cpp)
 add_executable(unilateral_control control/openarm_unilateral_control.cpp)
 add_executable(bilateral_control control/openarm_bilateral_control.cpp)
+add_executable(joint_sensor_control control/openarm_joint_sensor.cpp)
 
 target_link_libraries(gravity_comp PRIVATE openarm_teleop_lib)
 target_link_libraries(comm_test PRIVATE openarm_teleop_lib)
 target_link_libraries(unilateral_control PRIVATE openarm_teleop_lib)
 target_link_libraries(bilateral_control PRIVATE openarm_teleop_lib)
+target_link_libraries(joint_sensor_control PRIVATE openarm_teleop_lib)

control/gravity_compasation.cpp

@@ -37,7 +37,7 @@ int main(int argc, char** argv) {
         std::signal(SIGINT, signal_handler);
 
         std::string arm_side = "right_arm";
-        std::string can_interface = "can0";
+        std::string can_interface = "can4";
 
         if (argc < 4) {
             std::cerr << "Usage: " << argv[0] << " <arm_side> <can_interface> <urdf_path>"
@@ -78,7 +78,7 @@ int main(int argc, char** argv) {
         openarm::can::socket::OpenArm* openarm =
             openarm_init::OpenArmInitializer::initialize_openarm(can_interface, true);
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
         auto start_time = std::chrono::high_resolution_clock::now();
         auto last_hz_display = start_time;
         int frame_count = 0;
@@ -133,7 +133,7 @@ int main(int argc, char** argv) {
             openarm->recv_all();
         }
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 
         openarm->disable_all();
         openarm->recv_all();

control/openarm_bilateral_control.cpp

@@ -207,8 +207,8 @@ int main(int argc, char **argv) {
         std::cout << "Root link         : " << root_link << std::endl;
         std::cout << "Leaf link         : " << leaf_link << std::endl;
 
-        YamlLoader leader_loader("config/leader.yaml");
+        YamlLoader leader_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/leader.yaml");
-        YamlLoader follower_loader("config/follower.yaml");
+        YamlLoader follower_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/follower.yaml");
 
         // Leader parameters
         std::vector<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp");
@@ -290,7 +290,7 @@ int main(int argc, char **argv) {
         admin_thread.start_thread();
 
         while (keep_running) {
-            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
         }
 
         leader_thread.stop_thread();

control/openarm_joint_sensor.cpp

@@ -0,0 +1,457 @@
+// 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.
+
+#include <atomic>
+#include <chrono>
+#include <controller/control.hpp>
+#include <controller/dynamics.hpp>
+#include <csignal>
+#include <filesystem>
+#include <iostream>
+#include <openarm/can/socket/openarm.hpp>
+#include <openarm/damiao_motor/dm_motor_constants.hpp>
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <openarm_port/openarm_init.hpp>
+#include <periodic_timer_thread.hpp>
+#include <poll.h>
+#include <robot_state.hpp>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <thread>
+#include <unistd.h>
+#include <yamlloader.hpp>
+
+std::atomic<bool> keep_running(true);
+std::atomic<double> sensor_angle_rad(0.0);
+std::atomic<double> sensor_velocity_rad_s(0.0);
+std::atomic<bool> sensor_angle_ready(false);
+
+constexpr canid_t SENSOR_CAN_ID = 0x01;  // TODO: replace with your sensor CAN ID
+constexpr double SENSOR_SCALE = 1e-3;     // TODO: replace with your sensor scaling
+constexpr double SENSOR_OFFSET = 0.0;
+constexpr double SENSOR_FILTER_ALPHA = 0.05;  // 0~1, larger = less smoothing
+constexpr double SENSOR_VEL_FILTER_ALPHA = 0.2;  // 0~1, larger = less smoothing
+
+int open_can_socket(const std::string &can_interface) {
+    int sock = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    if (sock < 0) {
+        throw std::runtime_error("Failed to open CAN socket");
+    }
+
+    struct ifreq ifr;
+    std::snprintf(ifr.ifr_name, IFNAMSIZ, "%s", can_interface.c_str());
+    if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) {
+        close(sock);
+        throw std::runtime_error("Failed to get CAN interface index");
+    }
+
+    struct sockaddr_can addr;
+    std::memset(&addr, 0, sizeof(addr));
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    if (bind(sock, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr)) < 0) {
+        close(sock);
+        throw std::runtime_error("Failed to bind CAN socket");
+    }
+
+    struct can_filter filter;
+    filter.can_id = SENSOR_CAN_ID;
+    filter.can_mask = CAN_SFF_MASK;
+    if (setsockopt(sock, SOL_CAN_RAW, CAN_RAW_FILTER, &filter, sizeof(filter)) < 0) {
+        close(sock);
+        throw std::runtime_error("Failed to set CAN filter");
+    }
+
+    return sock;
+}
+
+bool read_sensor(int sock, double &angle_rad_out, double &velocity_rad_s_out) {
+    struct pollfd pfd;
+    pfd.fd = sock;
+    pfd.events = POLLIN;
+    int ret = poll(&pfd, 1, 10);
+    if (ret <= 0 || !(pfd.revents & POLLIN)) {
+        return false;
+    }
+
+    struct can_frame frame;
+    int nbytes = read(sock, &frame, sizeof(frame));
+    if (nbytes != static_cast<int>(sizeof(frame))) {
+        return false;
+    }
+
+    const canid_t frame_id = frame.can_id & CAN_SFF_MASK;
+    if (frame_id != SENSOR_CAN_ID || frame.can_dlc < 4) {
+        return false;
+    }
+
+    int32_t rawPos = 0;
+    rawPos |= static_cast<int32_t>(frame.data[1]) << 8;
+    rawPos |= static_cast<int32_t>(frame.data[2]);
+    int16_t rawVel = 0;
+    rawVel |= static_cast<int32_t>(frame.data[3]) << 8;
+    rawVel |= static_cast<int32_t>(frame.data[4]);
+
+    velocity_rad_s_out = (static_cast<double>(rawVel) / 100.0 / 4096.0) * 360.0 * (M_PI / 180.0);  // Example conversion, adjust as needed
+
+    angle_rad_out = (static_cast<double>(rawPos) / 4096.0) * 360.0;  // Example conversion, adjust as needed
+    angle_rad_out = angle_rad_out * (M_PI / 180.0) - 3.14;
+    return true;
+}
+
+void signal_handler(int signal) {
+    if (signal == SIGINT) {
+        std::cout << "\nCtrl+C detected. Exiting loop..." << std::endl;
+        keep_running = false;
+    }
+}
+
+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) {}
+
+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<std::chrono::microseconds>(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<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) {}
+
+protected:
+    void before_start() override { std::cout << "follower start 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();
+        if (sensor_angle_ready.load()) {
+            // control_f_->SetArmJointReference(0, 0, 0.01);
+            control_f_->SetArmJointReference(0, sensor_angle_rad.load(),
+                                             sensor_velocity_rad_s.load());
+            std::cout << "Sensor angle (rad): " << sensor_angle_rad.load() << std::endl;
+            std::cout << "Sensor velocity (rad/s): " << sensor_velocity_rad_s.load() << std::endl;
+        }
+        // control_f_->SetArmJointReference(0, 0.2, 0.01);  // Example: set joint 0 to 0 radians
+        
+        control_f_->unilateral_step();
+
+        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 << "Leader joint 0 position: " << robot_state_->arm_state().get_reference(0).position
+        //           << std::endl;
+        // std::cout << "Leader joint 0 velocity: " << robot_state_->arm_state().get_reference(0).velocity
+        //           << std::endl;
+        // 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) {}
+
+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<std::chrono::microseconds>(now - prev_time).count();
+        prev_time = now;
+
+        // std::cout << "[Admin] Period: " << elapsed_us << " us" << std::endl;
+    }
+
+private:
+    std::shared_ptr<RobotSystemState> leader_state_;
+    std::shared_ptr<RobotSystemState> follower_state_;
+    Control *control_l_;
+    Control *control_f_;
+};
+
+int main(int argc, char **argv) {
+    try {
+        std::signal(SIGINT, signal_handler);
+
+        // default configration
+        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";
+
+        if (argc < 3) {
+            std::cerr
+                << "Usage: " << argv[0]
+                     << " <leader_urdf_path> <follower_urdf_path> [arm_side] [leader_can] [follower_can]"
+                         " [sensor_can]"
+                << std::endl;
+            return 1;
+        }
+
+        // Required: URDF paths
+        leader_urdf_path = argv[1];
+        follower_urdf_path = argv[2];
+
+        // Optional: arm_side
+        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;
+                return 1;
+            }
+        }
+
+        // Optional: CAN interfaces
+        if (argc >= 6) {
+            leader_can_interface = argv[4];
+            follower_can_interface = argv[5];
+        }
+
+        // Optional: sensor CAN interface
+        if (argc >= 7) {
+            sensor_can_interface = argv[6];
+        }
+
+        // URDF file existence check
+        if (!std::filesystem::exists(leader_urdf_path)) {
+            std::cerr << "[ERROR] Leader URDF not found: " << leader_urdf_path << std::endl;
+            return 1;
+        }
+        if (!std::filesystem::exists(follower_urdf_path)) {
+            std::cerr << "[ERROR] Follower URDF not found: " << follower_urdf_path << std::endl;
+            return 1;
+        }
+
+        // Setup dynamics
+        std::string root_link = "openarm_body_link0";
+        std::string leaf_link =
+            (arm_side == "left_arm") ? "openarm_left_hand" : "openarm_right_hand";
+
+        // 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;
+        std::cout << "Root link         : " << root_link << std::endl;
+        std::cout << "Leaf link         : " << leaf_link << std::endl;
+        std::cout << "Sensor CAN        : " << sensor_can_interface << std::endl;
+
+        int sensor_can_socket = open_can_socket(sensor_can_interface);
+        std::thread can_read_thread([&]() {
+            double filtered_angle = 0.0;
+            bool filter_initialized = false;
+            double filtered_velocity = 0.0;
+            double prev_angle = 0.0;
+            auto prev_time = std::chrono::steady_clock::now();
+            double velocity = 0.0;
+            while (keep_running) {
+                double angle = 0.0;
+                if (read_sensor(sensor_can_socket,  angle, velocity)) {
+                    auto now = std::chrono::steady_clock::now();
+                    double dt =
+                        std::chrono::duration_cast<std::chrono::duration<double>>(now - prev_time)
+                            .count();
+                    // std::cout << "dt: " << dt << " s, raw angle: " << angle << " rad" << std::endl;
+                    prev_time = now;
+
+                    if (!filter_initialized) {
+                        filtered_angle = angle;
+                        filter_initialized = true;
+                        prev_angle = angle;
+                        filtered_velocity = 0.0;
+                    } else {
+                        filtered_angle = SENSOR_FILTER_ALPHA * angle +
+                                         (1.0 - SENSOR_FILTER_ALPHA) * filtered_angle;
+
+                        if (dt > 1e-6) {
+                            filtered_velocity = SENSOR_VEL_FILTER_ALPHA * velocity +
+                                                (1.0 - SENSOR_VEL_FILTER_ALPHA) *
+                                                    filtered_velocity;
+                        }
+                        prev_angle = angle;
+                    }
+                    sensor_angle_rad.store(filtered_angle);
+                    sensor_velocity_rad_s.store(filtered_velocity);
+                    sensor_angle_ready.store(true);
+                }
+            }
+        });
+
+        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<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_Fv = leader_loader.get_vector("LeaderArmParam", "Fv");
+        std::vector<double> leader_Fo = leader_loader.get_vector("LeaderArmParam", "Fo");
+
+        // Follower parameters
+        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_Fv = follower_loader.get_vector("FollowerArmParam", "Fv");
+        std::vector<double> follower_Fo = follower_loader.get_vector("FollowerArmParam", "Fo");
+
+        Dynamics *leader_arm_dynamics = new Dynamics(leader_urdf_path, root_link, leaf_link);
+        leader_arm_dynamics->Init();
+
+        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<RobotSystemState> leader_state =
+            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_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);
+
+        // 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);
+        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()) {
+            can_read_thread.join();
+        }
+        close(sensor_can_socket);
+
+    } catch (const std::exception &e) {
+        std::cerr << e.what() << '\n';
+    }
+
+    return 0;
+}

control/openarm_unilateral_control.cpp

@@ -158,7 +158,7 @@ int main(int argc, char **argv) {
         if (argc < 3) {
             std::cerr
                 << "Usage: " << argv[0]
-                << " <leader_urdf_path> <follower_urdf_path> [arm_side] [leader_can] [follower_can]"
+                     << " <leader_urdf_path> <follower_urdf_path> [arm_side] [leader_can] [follower_can]"
                 << std::endl;
             return 1;
         }
@@ -208,8 +208,8 @@ int main(int argc, char **argv) {
         std::cout << "Root link         : " << root_link << std::endl;
         std::cout << "Leaf link         : " << leaf_link << std::endl;
 
-        YamlLoader leader_loader("config/leader.yaml");
+        YamlLoader leader_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/leader.yaml");
-        YamlLoader follower_loader("config/follower.yaml");
+        YamlLoader follower_loader("/home/shen/openarm_ros2_ws/src/openarm_teleop/config/follower.yaml");
 
         // Leader parameters
         std::vector<double> leader_kp = leader_loader.get_vector("LeaderArmParam", "Kp");

script/launch_grav_comp.sh

@@ -24,7 +24,7 @@ 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="$WS_DIR/src/openarm_teleop/build/gravity_comp" # adjust if needed
 # ===============================
 # Check workspace
 if [ ! -d "$WS_DIR" ]; then

script/launch_unilateral.sh

@@ -51,7 +51,7 @@ 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=~/openarm_teleop/build/unilateral_control
+BIN_PATH="$WS_DIR/src/openarm_teleop/build/unilateral_control"
 
 # Check workspace
 if [ ! -d "$WS_DIR" ]; then

src/controller/control.cpp

@@ -90,6 +90,35 @@ void Control::SetParameter(const std::vector<double>& Kp, const std::vector<doub
     Fo_ = Fo;
 }
 
+void Control::SetArmJointReference(size_t index, double position_rad, double velocity_rad_s,
+                                   double effort) {
+    if (!robot_state_) {
+        return;
+    }
+
+    const size_t arm_size = robot_state_->arm_state().get_size();
+    if (index >= arm_size) {
+        return;
+    }
+
+    const size_t limit_size = sizeof(position_limit_min_L) / sizeof(position_limit_min_L[0]);
+    double clamped_position = position_rad;
+    if (index < limit_size) {
+        const double min_limit =
+            (role_ == ROLE_LEADER) ? position_limit_min_L[index] : position_limit_min_F[index];
+        const double max_limit =
+            (role_ == ROLE_LEADER) ? position_limit_max_L[index] : position_limit_max_F[index];
+        clamped_position = std::clamp(position_rad, min_limit, max_limit);
+    }
+
+    JointState target;
+    target.position = clamped_position;
+    target.velocity = velocity_rad_s;
+    target.effort = effort;
+
+    robot_state_->arm_state().set_reference(index, target);
+}
+
 bool Control::bilateral_step() {
     // get motor status
     std::vector<MotorState> arm_motor_states;

src/controller/control.hpp

@@ -80,6 +80,10 @@ public:
                       const std::vector<double> &Fc, const std::vector<double> &k,
                       const std::vector<double> &Fv, const std::vector<double> &Fo);
 
+    // Set a single arm joint reference in radians (e.g., sensor follow).
+    void SetArmJointReference(size_t index, double position_rad, double velocity_rad_s = 0.0,
+                              double effort = 0.0);
+
     bool AdjustPosition(void);
 
     // Compute torque based on bilateral

src/openarm_constants.hpp

@@ -31,11 +31,11 @@ constexpr double PI = 3.14159265358979323846;
 #define ROLE_LEADER 1
 #define ROLE_FOLLOWER 2
 
-#define CAN0 "can0"
+#define CAN0 "can4"
-#define CAN1 "can1"
+#define CAN1 "can5"
 
-#define CAN2 "can2"
+#define CAN2 "can6"
-#define CAN3 "can3"
+#define CAN3 "can7"
 
 #define TANHFRIC true
 

src/openarm_port/openarm_init.cpp

@@ -21,7 +21,7 @@ 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);
+    return initialize_openarm(can_device, config, false);
 }
 
 openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std::string &can_device,
@@ -29,10 +29,10 @@ openarm::can::socket::OpenArm *OpenArmInitializer::initialize_openarm(const std:
                                                                       bool enable_debug) {
     // Create OpenArm instance
     openarm::can::socket::OpenArm *openarm =
-        new openarm::can::socket::OpenArm(can_device, enable_debug);
+        new openarm::can::socket::OpenArm(can_device, false);
 
     // Perform common initialization
-    initialize_(openarm, config, enable_debug);
+    initialize_(openarm, config, false);
 
     return openarm;
 }