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

Add approximate gripper position (#49)

Browse Source 
- Main change: Add approximate gripper position
- Minor change: Add more comments about TODOs and appropriate waits
This commit is contained in:
Yue Yin 2025-07-24 20:05:06 +09:00 committed by GitHub
parent 444a998cf8
commit 3e1f9792b0
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 32 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
openarm_hardware/include/openarm_hardware/v10_simple_hardware.hpp
View File

@ -14,6 +14,7 @@
#pragma once #pragma once
#include <chrono>
#include <memory> #include <memory>
#include <openarm/can/socket/openarm.hpp> #include <openarm/can/socket/openarm.hpp>
#include <openarm/damiao_motor/dm_motor_constants.hpp> #include <openarm/damiao_motor/dm_motor_constants.hpp>
@ -105,6 +106,13 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface {
const std::vector<double> DEFAULT_KD = {2.75, 2.5, 0.7, 0.4, const std::vector<double> DEFAULT_KD = {2.75, 2.5, 0.7, 0.4,
0.7, 0.6, 0.5, 0.1}; 0.7, 0.6, 0.5, 0.1};
const double GRIPPER_JOINT_0_POSITION = 0.044;
const double GRIPPER_JOINT_1_POSITION = 0.0;
const double GRIPPER_MOTOR_0_RADIANS = 0.0;
const double GRIPPER_MOTOR_1_RADIANS = -1.0472;
const double GRIPPER_DEFAULT_KP = 5.0;
const double GRIPPER_DEFAULT_KD = 0.1;
// Configuration // Configuration
std::string can_interface_; std::string can_interface_;
std::string arm_prefix_; std::string arm_prefix_;

51
openarm_hardware/src/v10_simple_hardware.cpp
View File

@ -64,14 +64,14 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) {
"Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s", "Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s",
can_interface_.c_str(), arm_prefix_.c_str(), can_interface_.c_str(), arm_prefix_.c_str(),
hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled"); hand_ ? "enabled" : "disabled", can_fd_ ? "enabled" : "disabled");
return true; return true;
} }
void OpenArm_v10HW::generate_joint_names() { void OpenArm_v10HW::generate_joint_names() {
joint_names_.clear(); joint_names_.clear();
// TODO: read from urdf properly in the future // TODO: read from urdf properly and sort in the future.
// Generate arm joint names: openarm_{arm_prefix}joint{N} // Currently, the joint names are hardcoded for order consistency to align
// with hardware. Generate arm joint names: openarm_{arm_prefix}joint{N}
for (size_t i = 1; i <= ARM_DOF; ++i) { for (size_t i = 1; i <= ARM_DOF; ++i) {
std::string joint_name = std::string joint_name =
"openarm_" + arm_prefix_ + "joint" + std::to_string(i); "openarm_" + arm_prefix_ + "joint" + std::to_string(i);
@ -154,7 +154,6 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_init(
hardware_interface::CallbackReturn OpenArm_v10HW::on_configure( hardware_interface::CallbackReturn OpenArm_v10HW::on_configure(
const rclcpp_lifecycle::State& /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/) {
// Set callback mode to ignore during configuration // Set callback mode to ignore during configuration
openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::IGNORE);
openarm_->refresh_all(); openarm_->refresh_all();
std::this_thread::sleep_for(std::chrono::milliseconds(100)); std::this_thread::sleep_for(std::chrono::milliseconds(100));
openarm_->recv_all(); openarm_->recv_all();
@ -165,7 +164,6 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_configure(
std::vector<hardware_interface::StateInterface> std::vector<hardware_interface::StateInterface>
OpenArm_v10HW::export_state_interfaces() { OpenArm_v10HW::export_state_interfaces() {
std::vector<hardware_interface::StateInterface> state_interfaces; std::vector<hardware_interface::StateInterface> state_interfaces;
for (size_t i = 0; i < joint_names_.size(); ++i) { for (size_t i = 0; i < joint_names_.size(); ++i) {
state_interfaces.emplace_back(hardware_interface::StateInterface( state_interfaces.emplace_back(hardware_interface::StateInterface(
joint_names_[i], hardware_interface::HW_IF_POSITION, &pos_states_[i])); joint_names_[i], hardware_interface::HW_IF_POSITION, &pos_states_[i]));
@ -181,7 +179,7 @@ OpenArm_v10HW::export_state_interfaces() {
std::vector<hardware_interface::CommandInterface> std::vector<hardware_interface::CommandInterface>
OpenArm_v10HW::export_command_interfaces() { OpenArm_v10HW::export_command_interfaces() {
std::vector<hardware_interface::CommandInterface> command_interfaces; std::vector<hardware_interface::CommandInterface> command_interfaces;
// TODO: consider exposing only needed interfaces to avoid undefined behavior.
for (size_t i = 0; i < joint_names_.size(); ++i) { for (size_t i = 0; i < joint_names_.size(); ++i) {
command_interfaces.emplace_back(hardware_interface::CommandInterface( command_interfaces.emplace_back(hardware_interface::CommandInterface(
joint_names_[i], hardware_interface::HW_IF_POSITION, joint_names_[i], hardware_interface::HW_IF_POSITION,
@ -199,14 +197,12 @@ OpenArm_v10HW::export_command_interfaces() {
hardware_interface::CallbackReturn OpenArm_v10HW::on_activate( hardware_interface::CallbackReturn OpenArm_v10HW::on_activate(
const rclcpp_lifecycle::State& /*previous_state*/) { const rclcpp_lifecycle::State& /*previous_state*/) {
RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "Activating OpenArm V10..."); RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "Activating OpenArm V10...");
// Set callback mode to state and enable all motors (like full_arm.cpp)
openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE); openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE);
openarm_->enable_all(); openarm_->enable_all();
std::this_thread::sleep_for(std::chrono::milliseconds(100)); std::this_thread::sleep_for(std::chrono::milliseconds(100));
openarm_->recv_all(); openarm_->recv_all();
// Return to zero position smoothly // Return to zero position
return_to_zero(); return_to_zero();
RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 activated"); RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 activated");
@ -220,6 +216,7 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_deactivate(
// Disable all motors (like full_arm.cpp exit) // Disable all motors (like full_arm.cpp exit)
openarm_->disable_all(); openarm_->disable_all();
std::this_thread::sleep_for(std::chrono::milliseconds(100));
openarm_->recv_all(); openarm_->recv_all();
RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 deactivated"); RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), "OpenArm V10 deactivated");
@ -229,7 +226,6 @@ hardware_interface::CallbackReturn OpenArm_v10HW::on_deactivate(
hardware_interface::return_type OpenArm_v10HW::read( hardware_interface::return_type OpenArm_v10HW::read(
const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) { const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/) {
// Receive all motor states // Receive all motor states
openarm_->set_callback_mode_all(openarm::damiao_motor::CallbackMode::STATE);
openarm_->refresh_all(); openarm_->refresh_all();
openarm_->recv_all(); openarm_->recv_all();
@ -245,13 +241,12 @@ hardware_interface::return_type OpenArm_v10HW::read(
if (hand_ && joint_names_.size() > ARM_DOF) { if (hand_ && joint_names_.size() > ARM_DOF) {
const auto& gripper_motors = openarm_->get_gripper().get_motors(); const auto& gripper_motors = openarm_->get_gripper().get_motors();
if (!gripper_motors.empty()) { if (!gripper_motors.empty()) {
// TODO the mappings are unimplemented, need to actually implement the // TODO the mappings are approximates
// mappings for pos, vel, tau Convert motor position (radians) to joint // Convert motor position (radians) to joint value (0-0.044m)
// value (0-1)
double motor_pos = gripper_motors[0].get_position(); double motor_pos = gripper_motors[0].get_position();
pos_states_[ARM_DOF] = motor_radians_to_joint(motor_pos); pos_states_[ARM_DOF] = motor_radians_to_joint(motor_pos);
// Velocity and torque can be passed through directly for now // Unimplemented: Velocity and torque mapping
vel_states_[ARM_DOF] = 0; // gripper_motors[0].get_velocity(); vel_states_[ARM_DOF] = 0; // gripper_motors[0].get_velocity();
tau_states_[ARM_DOF] = 0; // gripper_motors[0].get_torque(); tau_states_[ARM_DOF] = 0; // gripper_motors[0].get_torque();
} }
@ -271,13 +266,12 @@ hardware_interface::return_type OpenArm_v10HW::write(
openarm_->get_arm().mit_control_all(arm_params); openarm_->get_arm().mit_control_all(arm_params);
// Control gripper if enabled // Control gripper if enabled
if (hand_ && joint_names_.size() > ARM_DOF) { if (hand_ && joint_names_.size() > ARM_DOF) {
// TODO the mappings are unimplemented. // TODO the true mappings are unimplemented.
// Convert joint value (0-1) to motor position (radians)
double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]); double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]);
openarm_->get_gripper().mit_control_all({{5.0, 1.0, motor_command, 0, 0}}); openarm_->get_gripper().mit_control_all(
{{GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, motor_command, 0, 0}});
} }
std::this_thread::sleep_for(std::chrono::microseconds(300)); openarm_->recv_all(1000);
openarm_->recv_all();
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
} }
@ -294,23 +288,26 @@ void OpenArm_v10HW::return_to_zero() {
// Return gripper to zero if enabled // Return gripper to zero if enabled
if (hand_) { if (hand_) {
openarm_->get_gripper().mit_control_all({{5.0, 1.0, 0.0, 0.0, 0.0}}); openarm_->get_gripper().mit_control_all(
{{GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, GRIPPER_JOINT_0_POSITION, 0.0,
0.0}});
} }
std::this_thread::sleep_for(std::chrono::microseconds(1000));
// // Wait for motors to settle
std::this_thread::sleep_for(std::chrono::milliseconds(100));
openarm_->recv_all(); openarm_->recv_all();
} }
// Gripper mapping helper functions // Gripper mapping helper functions
double OpenArm_v10HW::joint_to_motor_radians(double joint_value) { double OpenArm_v10HW::joint_to_motor_radians(double joint_value) {
// Joint 0=closed -> motor 0 rad, Joint 1=open -> motor -1.0472 rad // Joint 0=closed -> motor 0 rad, Joint 0.044=open -> motor -1.0472 rad
return joint_value * (-1.0472); // Scale from 0-1 to 0 to -1.0472 return (joint_value / GRIPPER_JOINT_0_POSITION) *
GRIPPER_MOTOR_1_RADIANS; // Scale from 0-0.044 to 0 to -1.0472
} }
double OpenArm_v10HW::motor_radians_to_joint(double motor_radians) { double OpenArm_v10HW::motor_radians_to_joint(double motor_radians) {
// Motor 0 rad=closed -> joint 0, Motor -1.0472 rad=open -> joint 1 // Motor 0 rad=closed -> joint 0, Motor -1.0472 rad=open -> joint 0.044
return motor_radians / (-1.0472); // Scale from 0 to -1.0472 to 0-1 return GRIPPER_JOINT_0_POSITION *
(motor_radians /
GRIPPER_MOTOR_1_RADIANS); // Scale from 0 to -1.0472 to 0-0.044
} }
} // namespace openarm_hardware } // namespace openarm_hardware