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_RECV_CAN_ID = 0x18;
// Default gains
const std::vector<double> DEFAULT_KP = {70.0, 70.0, 70.0, 60.0,
10.0, 10.0, 10.0};
const std::vector<double> DEFAULT_KD = {2.75, 2.5, 2.0, 2.0, 0.7, 0.6, 0.5};
// Gains
std::vector<double> kp_ = {70.0, 70.0, 70.0, 60.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 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;
const double GRIPPER_KP = 5.0;
const double GRIPPER_KD = 0.1;
// Configuration
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");
}
// 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"),
"Configuration: CAN=%s, arm_prefix=%s, hand=%s, can_fd=%s",
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
std::vector<openarm::damiao_motor::MITParam> arm_params;
for (size_t i = 0; i < ARM_DOF; ++i) {
arm_params.push_back({DEFAULT_KP[i], DEFAULT_KD[i], pos_commands_[i],
vel_commands_[i], tau_commands_[i]});
arm_params.push_back(
{kp_[i], kd_[i], pos_commands_[i], vel_commands_[i], tau_commands_[i]});
}
openarm_->get_arm().mit_control_all(arm_params);
// Control gripper if enabled
@ -269,7 +281,7 @@ hardware_interface::return_type OpenArm_v10HW::write(
// TODO the true mappings are unimplemented.
double motor_command = joint_to_motor_radians(pos_commands_[ARM_DOF]);
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);
return hardware_interface::return_type::OK;
@ -282,15 +294,14 @@ void OpenArm_v10HW::return_to_zero() {
// Return arm to zero with MIT control
std::vector<openarm::damiao_motor::MITParam> arm_params;
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);
// Return gripper to zero if enabled
if (hand_) {
openarm_->get_gripper().mit_control_all(
{{GRIPPER_DEFAULT_KP, GRIPPER_DEFAULT_KD, GRIPPER_JOINT_0_POSITION, 0.0,
0.0}});
{{GRIPPER_KP, GRIPPER_KD, GRIPPER_JOINT_0_POSITION, 0.0, 0.0}});
}
std::this_thread::sleep_for(std::chrono::microseconds(1000));
openarm_->recv_all();