From c059e08e9b164e9804f924c2fafbc168ff3a5272 Mon Sep 17 00:00:00 2001 From: shen <664376944@qq.com> Date: Thu, 26 Mar 2026 14:30:43 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E8=85=B0=E5=85=B3=E8=8A=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 2 + examples/demo.cpp | 22 +++++- include/openarm/can/socket/openarm.hpp | 5 ++ .../openarm/can/socket/waist_component.hpp | 70 +++++++++++++++++++ setup/configure_socketcan_4_arms.sh | 2 +- setup/set_zero.sh | 2 +- src/openarm/can/socket/openarm.cpp | 8 +++ src/openarm/can/socket/waist_component.cpp | 48 +++++++++++++ 8 files changed, 156 insertions(+), 3 deletions(-) create mode 100644 include/openarm/can/socket/waist_component.hpp create mode 100644 src/openarm/can/socket/waist_component.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 151d901..ba25186 100755 --- a/CMakeLists.txt +++ b/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 diff --git a/examples/demo.cpp b/examples/demo.cpp index 8cce261..a1a0192 100644 --- a/examples/demo.cpp +++ b/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 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(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(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(); diff --git a/include/openarm/can/socket/openarm.hpp b/include/openarm/can/socket/openarm.hpp index 666e259..ed02bb0 100644 --- a/include/openarm/can/socket/openarm.hpp +++ b/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 can_socket_; std::unique_ptr arm_; std::unique_ptr gripper_; + std::unique_ptr waist_; std::unique_ptr master_can_device_collection_; std::vector sub_dm_device_collections_; void register_dm_device_collection(damiao_motor::DMDeviceCollection& device_collection); diff --git a/include/openarm/can/socket/waist_component.hpp b/include/openarm/can/socket/waist_component.hpp new file mode 100644 index 0000000..5011c79 --- /dev/null +++ b/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 +#include + +#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 motor_; + std::shared_ptr 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 diff --git a/setup/configure_socketcan_4_arms.sh b/setup/configure_socketcan_4_arms.sh index 402b397..e6cc7ad 100755 --- a/setup/configure_socketcan_4_arms.sh +++ b/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..." diff --git a/setup/set_zero.sh b/setup/set_zero.sh index 22ea828..55a3bcb 100755 --- a/setup/set_zero.sh +++ b/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" diff --git a/src/openarm/can/socket/openarm.cpp b/src/openarm/can/socket/openarm.cpp index 9a0f342..e2912fd 100644 --- a/src/openarm/can/socket/openarm.cpp +++ b/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(*can_socket_); arm_ = std::make_unique(*can_socket_); gripper_ = std::make_unique(*can_socket_); + waist_ = std::make_unique(*can_socket_); + } void OpenArm::init_arm_motors(const std::vector& 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); diff --git a/src/openarm/can/socket/waist_component.cpp b/src/openarm/can/socket/waist_component.cpp new file mode 100644 index 0000000..7c83930 --- /dev/null +++ b/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 +#include + +#include +#include + +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(motor_type, send_can_id, recv_can_id); + // Create the device with a reference to the motor + motor_device_ = std::make_shared(*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