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

Make gain configurable (#79)

Browse Source 
Make gain configurable in HardwareInfo of the description file.

Need https://github.com/enactic/openarm_description/pull/38.
This commit is contained in:
Daijiro Fukuda 2026-01-15 18:07:21 +09:00 committed by GitHub
parent eeca95ed4b
commit 73ef898387
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
2 changed files with 22 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -100,17 +100,16 @@ class OpenArm_v10HW : public hardware_interface::SystemInterface {
const uint32_t DEFAULT_GRIPPER_SEND_CAN_ID = 0x08; const uint32_t DEFAULT_GRIPPER_SEND_CAN_ID = 0x08;
const uint32_t DEFAULT_GRIPPER_RECV_CAN_ID = 0x18; const uint32_t DEFAULT_GRIPPER_RECV_CAN_ID = 0x18;
// Default gains // Gains
const std::vector<double> DEFAULT_KP = {70.0, 70.0, 70.0, 60.0, std::vector<double> kp_ = {70.0, 70.0, 70.0, 60.0, 10.0, 10.0, 10.0};
10.0, 10.0, 10.0}; std::vector<double> kd_ = {2.75, 2.5, 2.0, 2.0, 0.7, 0.6, 0.5};
const std::vector<double> DEFAULT_KD = {2.75, 2.5, 2.0, 2.0, 0.7, 0.6, 0.5};
const double GRIPPER_JOINT_0_POSITION = 0.044; const double GRIPPER_JOINT_0_POSITION = 0.044;
const double GRIPPER_JOINT_1_POSITION = 0.0; const double GRIPPER_JOINT_1_POSITION = 0.0;
const double GRIPPER_MOTOR_0_RADIANS = 0.0; const double GRIPPER_MOTOR_0_RADIANS = 0.0;
const double GRIPPER_MOTOR_1_RADIANS = -1.0472; const double GRIPPER_MOTOR_1_RADIANS = -1.0472;
const double GRIPPER_DEFAULT_KP = 5.0; const double GRIPPER_KP = 5.0;
const double GRIPPER_DEFAULT_KD = 0.1; const double GRIPPER_KD = 0.1;
// Configuration // Configuration
std::string can_interface_; std::string can_interface_;

23
openarm_hardware/src/v10_simple_hardware.cpp
View File

@ -60,6 +60,18 @@ bool OpenArm_v10HW::parse_config(const hardware_interface::HardwareInfo& info) {
can_fd_ = (value == "true"); can_fd_ = (value == "true");
} }
// Parse control gains
for (size_t i = 1; i <= ARM_DOF; ++i) {
it = info.hardware_parameters.find("kp" + std::to_string(i));
if (it != info.hardware_parameters.end()) {
kp_[i - 1] = std::stod(it->second);
}
it = info.hardware_parameters.find("kd" + std::to_string(i));
if (it != info.hardware_parameters.end()) {
kd_[i - 1] = std::stod(it->second);
}
}
RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"), RCLCPP_INFO(rclcpp::get_logger("OpenArm_v10HW"),
"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(),
@ -260,8 +272,8 @@ hardware_interface::return_type OpenArm_v10HW::write(
// Control arm motors with MIT control // Control arm motors with MIT control
std::vector<openarm::damiao_motor::MITParam> arm_params; std::vector<openarm::damiao_motor::MITParam> arm_params;
for (size_t i = 0; i < ARM_DOF; ++i) { for (size_t i = 0; i < ARM_DOF; ++i) {
arm_params.push_back({DEFAULT_KP[i], DEFAULT_KD[i], pos_commands_[i], arm_params.push_back(
vel_commands_[i], tau_commands_[i]}); {kp_[i], kd_[i], pos_commands_[i], vel_commands_[i], tau_commands_[i]});
} }
openarm_->get_arm().mit_control_all(arm_params); openarm_->get_arm().mit_control_all(arm_params);
// Control gripper if enabled // Control gripper if enabled
@ -269,7 +281,7 @@ hardware_interface::return_type OpenArm_v10HW::write(
// TODO the true mappings are unimplemented. // TODO the true mappings are unimplemented.
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( openarm_->get_gripper().mit_control_all(
{{GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, motor_command, 0, 0}}); {{GRIPPER_KP, GRIPPER_KD, motor_command, 0, 0}});
} }
openarm_->recv_all(1000); openarm_->recv_all(1000);
return hardware_interface::return_type::OK; return hardware_interface::return_type::OK;
@ -282,15 +294,14 @@ void OpenArm_v10HW::return_to_zero() {
// Return arm to zero with MIT control // Return arm to zero with MIT control
std::vector<openarm::damiao_motor::MITParam> arm_params; std::vector<openarm::damiao_motor::MITParam> arm_params;
for (size_t i = 0; i < ARM_DOF; ++i) { for (size_t i = 0; i < ARM_DOF; ++i) {
arm_params.push_back({DEFAULT_KP[i], DEFAULT_KD[i], 0.0, 0.0, 0.0}); arm_params.push_back({kp_[i], kd_[i], 0.0, 0.0, 0.0});
} }
openarm_->get_arm().mit_control_all(arm_params); openarm_->get_arm().mit_control_all(arm_params);
// Return gripper to zero if enabled // Return gripper to zero if enabled
if (hand_) { if (hand_) {
openarm_->get_gripper().mit_control_all( openarm_->get_gripper().mit_control_all(
{{GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, GRIPPER_JOINT_0_POSITION, 0.0, {{GRIPPER_KP, GRIPPER_KD, GRIPPER_JOINT_0_POSITION, 0.0, 0.0}});
0.0}});
} }
std::this_thread::sleep_for(std::chrono::microseconds(1000)); std::this_thread::sleep_for(std::chrono::microseconds(1000));
openarm_->recv_all(); openarm_->recv_all();