From 3b1f41866fbb46b67d24745db0d22396d810e510 Mon Sep 17 00:00:00 2001
From: Marcus Grieger <marcus@grieger.xyz>
Date: Sat, 3 Feb 2024 23:47:23 +0100
Subject: [PATCH] Added servo startup_value property for safer startup pose
---
.../urdf/hexapod_robot_ros2_control.xacro | 39 ++++++++++---------
hardware/hexapod_robot_hwi.cpp | 5 ++-
2 files changed, 23 insertions(+), 21 deletions(-)
diff --git a/description/urdf/hexapod_robot_ros2_control.xacro b/description/urdf/hexapod_robot_ros2_control.xacro
index 3cd608b..7b48214 100644
--- a/description/urdf/hexapod_robot_ros2_control.xacro
+++ b/description/urdf/hexapod_robot_ros2_control.xacro
@@ -27,13 +27,14 @@
</joint>
</xacro:macro>
- <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope counts_min counts_max">
+ <xacro:macro name="map_joint_name_to_hardware" params="name i2c_address channel counts_intersect counts_slope counts_min counts_max startup_value">
<param name="${name}_joint_i2c_address">${i2c_address}</param>
<param name="${name}_joint_channel">${channel}</param>
<param name="${name}_joint_counts_intersect">${counts_intersect}</param>
<param name="${name}_joint_counts_slope">${counts_slope}</param>
<param name="${name}_joint_counts_min">${counts_min}</param>
<param name="${name}_joint_counts_max">${counts_max}</param>
+ <param name="${name}_joint_startup_value">${startup_value}</param>
</xacro:macro>
<ros2_control name="Hexapod" type="system">
@@ -41,29 +42,29 @@
<hardware>
<plugin>hexapod_robot/HexapodRobotHardwareInterface</plugin>
<param name="i2c_device">/dev/i2c-1</param>
- <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="386" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="189" counts_slope="129" counts_min="125" counts_max="550"/>
+ <xacro:map_joint_name_to_hardware name="c1" i2c_address="0x40" channel="13" counts_intersect="386" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+ <xacro:map_joint_name_to_hardware name="f1" i2c_address="0x40" channel="14" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.4"/>
+ <xacro:map_joint_name_to_hardware name="t1" i2c_address="0x40" channel="15" counts_intersect="189" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
- <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9" counts_intersect="327" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="317" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="227" counts_slope="129" counts_min="125" counts_max="550"/>
+ <xacro:map_joint_name_to_hardware name="c2" i2c_address="0x40" channel="9" counts_intersect="327" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+ <xacro:map_joint_name_to_hardware name="f2" i2c_address="0x40" channel="10" counts_intersect="317" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.4"/>
+ <xacro:map_joint_name_to_hardware name="t2" i2c_address="0x40" channel="11" counts_intersect="227" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
- <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2" counts_intersect="237" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0" counts_intersect="267" counts_slope="129" counts_min="125" counts_max="550"/>
+ <xacro:map_joint_name_to_hardware name="c3" i2c_address="0x40" channel="2" counts_intersect="237" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+ <xacro:map_joint_name_to_hardware name="f3" i2c_address="0x40" channel="1" counts_intersect="307" counts_slope="129" counts_min="125" counts_max="550" startup_value="-1.4"/>
+ <xacro:map_joint_name_to_hardware name="t3" i2c_address="0x40" channel="0" counts_intersect="267" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
- <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="451" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="487" counts_slope="129" counts_min="125" counts_max="550"/>
+ <xacro:map_joint_name_to_hardware name="c4" i2c_address="0x41" channel="13" counts_intersect="451" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+ <xacro:map_joint_name_to_hardware name="f4" i2c_address="0x41" channel="14" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.4"/>
+ <xacro:map_joint_name_to_hardware name="t4" i2c_address="0x41" channel="15" counts_intersect="487" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
- <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6" counts_intersect="357" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4" counts_intersect="437" counts_slope="129" counts_min="125" counts_max="550"/>
+ <xacro:map_joint_name_to_hardware name="c5" i2c_address="0x41" channel="6" counts_intersect="357" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+ <xacro:map_joint_name_to_hardware name="f5" i2c_address="0x41" channel="5" counts_intersect="367" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.4"/>
+ <xacro:map_joint_name_to_hardware name="t5" i2c_address="0x41" channel="4" counts_intersect="437" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
- <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2" counts_intersect="255" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1" counts_intersect="335" counts_slope="129" counts_min="125" counts_max="550"/>
- <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0" counts_intersect="444" counts_slope="129" counts_min="125" counts_max="550"/>
+ <xacro:map_joint_name_to_hardware name="c6" i2c_address="0x41" channel="2" counts_intersect="255" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
+ <xacro:map_joint_name_to_hardware name="f6" i2c_address="0x41" channel="1" counts_intersect="335" counts_slope="129" counts_min="125" counts_max="550" startup_value="1.4"/>
+ <xacro:map_joint_name_to_hardware name="t6" i2c_address="0x41" channel="0" counts_intersect="444" counts_slope="129" counts_min="125" counts_max="550" startup_value="0"/>
</hardware>
<xacro:create_leg_joints number="1"/>
diff --git a/hardware/hexapod_robot_hwi.cpp b/hardware/hexapod_robot_hwi.cpp
index ea1395f..fb82ab0 100644
--- a/hardware/hexapod_robot_hwi.cpp
+++ b/hardware/hexapod_robot_hwi.cpp
@@ -35,7 +35,6 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
servos_powered_ = false;
for (const hardware_interface::ComponentInfo &joint: info_.joints) {
- joint_commands_[joint.name] = 0.0;
std::string i2c_address_parameter = joint.name + "_i2c_address";
std::string channel_parameter = joint.name + "_channel";
@@ -43,6 +42,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
std::string counts_slope_parameter = joint.name + "_counts_slope";
std::string counts_min_parameter = joint.name + "_counts_min";
std::string counts_max_parameter = joint.name + "_counts_max";
+ std::string startup_value_parameter = joint.name + "_startup_value";
PWMOutput pwm_output;
pwm_output.i2c_address = stoi(info_.hardware_parameters[i2c_address_parameter], nullptr, 0);
@@ -51,6 +51,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
pwm_output.counts_slope = stod(info_.hardware_parameters[counts_slope_parameter]);
pwm_output.counts_min = stod(info_.hardware_parameters[counts_min_parameter]);
pwm_output.counts_max = stod(info_.hardware_parameters[counts_max_parameter]);
+ joint_commands_[joint.name] = stod(info_.hardware_parameters[startup_value_parameter]);
pca_available_[pwm_output.i2c_address] = false;
joint_pwm_mapping_[joint.name] = pwm_output;
@@ -60,7 +61,7 @@ HexapodRobotHardwareInterface::on_init(const hardware_interface::HardwareInfo &i
i2c_available_ = i2c_open_(info_.hardware_parameters["i2c_device"]);
- pca_initialize_();
+ if(i2c_available_) pca_initialize_();
return hardware_interface::CallbackReturn::SUCCESS;
}