增加腰关节

CMakeLists.txt

@@ -33,6 +33,7 @@ add_library(
   openarm_can
   src/openarm/can/socket/arm_component.cpp
   src/openarm/can/socket/gripper_component.cpp
+  src/openarm/can/socket/waist_component.cpp
   src/openarm/can/socket/openarm.cpp
   src/openarm/canbus/can_device_collection.cpp
   src/openarm/canbus/can_socket.cpp
@@ -62,6 +63,7 @@ if(USE_FILE_SET_HEADERS)
            FILES
            include/openarm/can/socket/arm_component.hpp
            include/openarm/can/socket/gripper_component.hpp
+           include/openarm/can/socket/waist_component.hpp
            include/openarm/can/socket/openarm.hpp
            include/openarm/canbus/can_device.hpp
            include/openarm/canbus/can_device_collection.hpp

examples/demo.cpp

@@ -27,7 +27,7 @@ int main() {
 
         // Initialize OpenArm with CAN interface and enable CAN-FD
         std::cout << "Initializing OpenArm CAN..." << std::endl;
-        openarm::can::socket::OpenArm openarm("can0", true);  // Use CAN-FD on can0 interface
+        openarm::can::socket::OpenArm openarm("can0", false);  // Use CAN-FD on can0 interface
 
         // Initialize arm motors
         std::vector<openarm::damiao_motor::MotorType> motor_types = {
@@ -40,6 +40,11 @@ int main() {
         std::cout << "Initializing gripper..." << std::endl;
         openarm.init_gripper_motor(openarm::damiao_motor::MotorType::DM4310, 0x08, 0x18);
 
+        // Initialize waist
+        std::cout << "Initializing waist..." << std::endl;
+        openarm.init_waist_motor(openarm::damiao_motor::MotorType::DM10010L, 0x09, 0x19);
+
+
         // Set callback mode to ignore and enable all motors
         openarm.set_callback_mode_all(openarm::damiao_motor::CallbackMode::IGNORE);
 
@@ -68,6 +73,11 @@ int main() {
                       << motor.get_param(static_cast<int>(openarm::damiao_motor::RID::MST_ID))
                       << std::endl;
         }
+        for (const auto& motor : openarm.get_waist().get_motors()) {
+            std::cout << "Waist Motor: " << motor.get_send_can_id() << " ID: "
+                      << motor.get_param(static_cast<int>(openarm::damiao_motor::RID::MST_ID))
+                      << std::endl;
+        }
 
         // Set device mode to state and control motor
         std::cout << "\n=== Controlling Motors ===" << std::endl;
@@ -88,6 +98,11 @@ int main() {
         openarm.get_gripper().close();
         openarm.recv_all(1000);
 
+        // Control waist
+        std::cout << "Rotating waist..." << std::endl;
+        openarm.get_waist().mit_control_all({openarm::damiao_motor::MITParam{2, 1, 0, 0, 0}});
+        openarm.recv_all(1000);
+
         for (int i = 0; i < 10; i++) {
             std::this_thread::sleep_for(std::chrono::milliseconds(100));
 
@@ -104,6 +119,11 @@ int main() {
                 std::cout << "Gripper Motor: " << motor.get_send_can_id()
                           << " position: " << motor.get_position() << std::endl;
             }
+            // Display waist state
+            for (const auto& motor : openarm.get_waist().get_motors()) {
+                std::cout << "Waist Motor: " << motor.get_send_can_id()
+                          << " position: " << motor.get_position() << std::endl;
+            }
         }
 
         openarm.disable_all();

include/openarm/can/socket/openarm.hpp

@@ -21,6 +21,7 @@
 #include "../../canbus/can_socket.hpp"
 #include "arm_component.hpp"
 #include "gripper_component.hpp"
+#include "waist_component.hpp"
 
 namespace openarm::can::socket {
 class OpenArm {
@@ -38,10 +39,13 @@ public:
 
     void init_gripper_motor(damiao_motor::MotorType motor_type, uint32_t send_can_id,
                             uint32_t recv_can_id);
+    void init_waist_motor(damiao_motor::MotorType motor_type, uint32_t send_can_id,
+                            uint32_t recv_can_id);
 
     // Component access
     ArmComponent& get_arm() { return *arm_; }
     GripperComponent& get_gripper() { return *gripper_; }
+    WaistComponent& get_waist() { return *waist_; }
     canbus::CANDeviceCollection& get_master_can_device_collection() {
         return *master_can_device_collection_;
     }
@@ -65,6 +69,7 @@ private:
     std::unique_ptr<canbus::CANSocket> can_socket_;
     std::unique_ptr<ArmComponent> arm_;
     std::unique_ptr<GripperComponent> gripper_;
+    std::unique_ptr<WaistComponent> waist_;
     std::unique_ptr<canbus::CANDeviceCollection> master_can_device_collection_;
     std::vector<damiao_motor::DMDeviceCollection*> sub_dm_device_collections_;
     void register_dm_device_collection(damiao_motor::DMDeviceCollection& device_collection);

include/openarm/can/socket/waist_component.hpp

@@ -0,0 +1,70 @@
+// 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.
+
+#pragma once
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+#include <memory>
+
+#include "../../damiao_motor/dm_motor.hpp"
+#include "../../damiao_motor/dm_motor_device_collection.hpp"
+
+namespace openarm::can::socket {
+
+class WaistComponent : public damiao_motor::DMDeviceCollection {
+public:
+    WaistComponent(canbus::CANSocket& can_socket);
+    ~WaistComponent() = default;
+
+    void init_motor_device(damiao_motor::MotorType motor_type, uint32_t send_can_id,
+                           uint32_t recv_can_id, bool use_fd);
+
+    // Waist-specific controls
+    void open(double kp = 50.0, double kd = 1.0);
+    void close(double kp = 50.0, double kd = 1.0);
+    damiao_motor::Motor* get_motor() const { return motor_.get(); }
+
+private:
+    std::unique_ptr<damiao_motor::Motor> motor_;
+    std::shared_ptr<damiao_motor::DMCANDevice> motor_device_;
+
+    void set_position(double position, double kp = 50.0, double kd = 1.0);
+
+    // The actual physical waist uses a rotational mechanism, this mapping is an
+    // approximation.
+    double waist_to_motor_position(double waist_position) {
+        // Map waist position (0.0=closed, 1.0=open) to motor position
+        return (waist_position - waist_open_position_) /
+                   (waist_closed_position_ - waist_open_position_) *
+                   (motor_closed_position_ - motor_open_position_) +
+               motor_open_position_;
+    }
+
+    double motor_to_waist_position(double motor_position) {
+        // Map motor position back to waist position (0.0=closed, 1.0=open)
+        return (motor_position - motor_open_position_) /
+                   (motor_closed_position_ - motor_open_position_) *
+                   (waist_closed_position_ - waist_open_position_) +
+               waist_open_position_;
+    }
+
+    // Waist configuration
+    double waist_open_position_ = 1.0;
+    double waist_closed_position_ = 0.0;
+    double motor_open_position_ = -1.0472;  // 60 degrees
+    double motor_closed_position_ = 0.0;
+};
+
+}  // namespace openarm::can::socket

setup/configure_socketcan_4_arms.sh

@@ -68,7 +68,7 @@ while [[ $# -gt 0 ]]; do
     esac
 done
 
-CAN_DEVICES=("can0" "can1" "can2" "can3")
+CAN_DEVICES=("can4" "can5" "can6" "can7")
 # for each device, check if it exists and configure it
 for CAN_IF in "${CAN_DEVICES[@]}"; do
     echo "Configuring $CAN_IF..."

setup/set_zero.sh

@@ -146,7 +146,7 @@ main() {
     if [ "$all_flag" = true ]; then
         echo "Sending set zero messages to all motor with CAN IDs from 001 to 008"
         echo "=========================================="
-        for i in {1..8}; do
+        for i in {1..9}; do
             local padded_id
             padded_id=$(printf "%03d" "$i")
             send_can_messages "$padded_id" "$CAN_IF"

src/openarm/can/socket/openarm.cpp

@@ -26,6 +26,8 @@ OpenArm::OpenArm(const std::string& can_interface, bool enable_fd)
     master_can_device_collection_ = std::make_unique<canbus::CANDeviceCollection>(*can_socket_);
     arm_ = std::make_unique<ArmComponent>(*can_socket_);
     gripper_ = std::make_unique<GripperComponent>(*can_socket_);
+    waist_ = std::make_unique<WaistComponent>(*can_socket_);
+
 }
 
 void OpenArm::init_arm_motors(const std::vector<damiao_motor::MotorType>& motor_types,
@@ -48,6 +50,12 @@ void OpenArm::init_gripper_motor(damiao_motor::MotorType motor_type, uint32_t se
     register_dm_device_collection(*gripper_);
 }
 
+void OpenArm::init_waist_motor(damiao_motor::MotorType motor_type, uint32_t send_can_id,
+                               uint32_t recv_can_id) {
+    waist_->init_motor_device(motor_type, send_can_id, recv_can_id, enable_fd_);
+    register_dm_device_collection(*waist_);
+}
+
 void OpenArm::register_dm_device_collection(damiao_motor::DMDeviceCollection& device_collection) {
     for (const auto& [id, device] : device_collection.get_device_collection().get_devices()) {
         master_can_device_collection_->add_device(device);

src/openarm/can/socket/waist_component.cpp

@@ -0,0 +1,48 @@
+// 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 <linux/can.h>
+#include <linux/can/raw.h>
+
+#include <iostream>
+#include <openarm/can/socket/waist_component.hpp>
+
+namespace openarm::can::socket {
+
+WaistComponent::WaistComponent(canbus::CANSocket& can_socket)
+    : DMDeviceCollection(can_socket) {}
+
+void WaistComponent::init_motor_device(damiao_motor::MotorType motor_type, uint32_t send_can_id,
+                                       uint32_t recv_can_id, bool use_fd) {
+    // Create the motor
+    motor_ = std::make_unique<damiao_motor::Motor>(motor_type, send_can_id, recv_can_id);
+    // Create the device with a reference to the motor
+    motor_device_ = std::make_shared<damiao_motor::DMCANDevice>(*motor_, CAN_SFF_MASK, use_fd);
+    get_device_collection().add_device(motor_device_);
+}
+
+void WaistComponent::open(double kp, double kd) { set_position(waist_open_position_, kp, kd); }
+
+void WaistComponent::close(double kp, double kd) {
+    set_position(waist_closed_position_, kp, kd);
+}
+
+void WaistComponent::set_position(double waist_position, double kp, double kd) {
+    if (!motor_device_) return;
+
+    // MIT control to desired position (zero velocity and torque)
+    mit_control_one(
+        0, damiao_motor::MITParam{kp, kd, waist_to_motor_position(waist_position), 0.0, 0.0});
+}
+}  // namespace openarm::can::socket