Skip to content

Add new heavy chassis controller first edition #225

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
OceanCing wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Add new heavy chassis controller first edition #225

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions rm_chassis_controllers/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,7 @@ add_library(${PROJECT_NAME}
src/swerve.cpp
src/balance.cpp
src/active_suspension.cpp
src/auto_suspension.cpp
${BIPEDAL_SOURCES}
)

Expand Down
93 changes: 93 additions & 0 deletions rm_chassis_controllers/include/rm_chassis_controllers/auto_suspension.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
//
// Created by root on 2026/10/12.
//

#pragma once

#include "rm_chassis_controllers/chassis_base.h"
#include "rm_chassis_controllers/omni.h"
#include <effort_controllers/joint_position_controller.h>
#include <hardware_interface/imu_sensor_interface.h>
#include <rm_msgs/ChassisCmd.h>
#include <rm_msgs/ChassisAutoSuspension.h>
#include <std_msgs/Bool.h>
#include <rm_common/ros_utilities.h>
#include <string>
#include <Eigen/QR>
#include <pluginlib/class_list_macros.hpp>

namespace rm_chassis_controllers
{
enum class State
{
DOWN = 0,
MID = 1,
UP = 2
};

enum class Prompt
{
RESET = 0,
CLIMB
};

class AutoSuspensionController : public OmniController
{
public:
AutoSuspensionController() = default;
bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override;
void AutoSuspensionCallback(const rm_msgs::ChassisAutoSuspension& msg);
void ChassisFollowCallBack(const rm_msgs::ChassisCmd& msg);

private:
void reset(const ros::Duration& period);
void climb(const ros::Duration& period);
void drop(const ros::Duration& period);
void moveJoint(const ros::Time& time, const ros::Duration& period) override;

std::vector<std::shared_ptr<effort_controllers::JointPositionController>> active_sus_ctrls_;
std::vector<std::shared_ptr<effort_controllers::JointPositionController>> flipper_ctrls_;
std::vector<hardware_interface::JointHandle> active_suspension_joints_{};
std::vector<hardware_interface::JointHandle> flipper_joints_{};

ros::Subscriber chassis_follow_sub_;
ros::Subscriber cmd_sub_;

hardware_interface::ImuSensorHandle imu_sensor_handle_;

double sus_current_pos_{ 0. };
double sus_target_pos_{ 0. };
double sus_last_target_{ 0. };
double sus_delta_target_{ 0. };
double flipper_target_pos_{ 0. };
double feedforward_offset{ 0. };
double feedforward_effort{ 0. };
double stretch_coff_A_{ 0. };
double stretch_coff_k_{ 0. };
double shrink_coff_A_{ 0. };
double shrink_coff_k_{ 0. };
double feedforward_effect_time_{ 0. };
double static_effort{ 0. };
double base_roll_{}, base_pitch_{}, base_yaw_{}, base_roll_rate_{}, base_pitch_rate_{}, base_yaw_rate_{};
double pitch_threshold_ = 0.0;
double pos_tolerance_ = 0.0;
double flipper_cmd_vel_coff_ = 0.0, sus_cmd_vel_coff_ = 0.0;
double sus_upper_pos_{}, sus_lower_pos_{}, flipper_upper_pos_{}, flipper_lower_pos_{};
bool has_imu_{};
bool use_manual_ = false;
bool should_release_ = false;
int reset_seq_ = 0;
int last_reset_seq_ = 0;
std::string imu_name_{};
std::string chassis_follow_frame_;

ros::Time feedforward_timer;
tf2::Quaternion odom2base_q;
tf2::Transform odom2base;
State current_state_{ State::DOWN };
State last_state_{ State::DOWN };
Prompt prompt_{ Prompt::RESET };

realtime_tools::RealtimeBuffer<rm_msgs::ChassisAutoSuspension> cmd_buffer_;
};
} // namespace rm_chassis_controllers
9 changes: 9 additions & 0 deletions rm_chassis_controllers/rm_chassis_controllers_plugins.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,4 +55,13 @@
</description>
</class>

<class name="rm_chassis_controllers/AutoSuspensionController"
type="rm_chassis_controllers::AutoSuspensionController"
base_class_type="controller_interface::ControllerBase">
<description>
The AutoSuspensionController is RoboMaster heavy robot Chassis controller. It expects a
EffortJointInterface type of hardware interface.
</description>
</class>

</library>
215 changes: 215 additions & 0 deletions rm_chassis_controllers/src/auto_suspension.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,215 @@
//
// Created by root on 2026/10/12.
//

#include "rm_chassis_controllers/auto_suspension.h"
#include <algorithm>

namespace rm_chassis_controllers
{
bool AutoSuspensionController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh,
ros::NodeHandle& controller_nh)
{
if (!OmniController::init(robot_hw, root_nh, controller_nh)) return false;

chassis_follow_sub_ =
controller_nh.subscribe("/cmd_chassis", 10, &AutoSuspensionController::ChassisFollowCallBack, this);
cmd_sub_ = controller_nh.subscribe("/cmd_auto_suspension", 1, &AutoSuspensionController::AutoSuspensionCallback, this);

XmlRpc::XmlRpcValue suspension_legs;
controller_nh.getParam("suspension_leg", suspension_legs);
XmlRpc::XmlRpcValue flipper_joints;
controller_nh.getParam("flipper_joints", flipper_joints);
stretch_coff_A_ = getParam(controller_nh, "stretch_coff_A_", 0.);
stretch_coff_k_ = getParam(controller_nh, "stretch_coff_k_", 0.);
shrink_coff_A_ = getParam(controller_nh, "shrink_coff_A_", 0.);
shrink_coff_k_ = getParam(controller_nh, "shrink_coff_k_", 0.);
feedforward_offset = getParam(controller_nh, "feed_forward_offset", 0.);
feedforward_effect_time_ = getParam(controller_nh, "feedforward_effect_time_", 0.);

pitch_threshold_ = getParam(controller_nh, "pitch_threshold", 0.);
pos_tolerance_ = getParam(controller_nh, "pos_tolerance", 0.0);
flipper_cmd_vel_coff_ = getParam(controller_nh, "flipper_cmd_vel_coff", 0.);
sus_cmd_vel_coff_ = getParam(controller_nh, "sus_cmd_vel_coff", 0.);
sus_upper_pos_ = getParam(controller_nh, "sus_upper_pos", 0.);
sus_lower_pos_ = getParam(controller_nh, "sus_lower_pos", 0.);
flipper_upper_pos_ = getParam(controller_nh, "flipper_upper_pos", 0.);
flipper_lower_pos_ = getParam(controller_nh, "flipper_lower_pos", 0.);

for (const auto& suspension_leg : suspension_legs)
{
ros::NodeHandle nh_suspension = ros::NodeHandle(controller_nh, "suspension_leg/" + suspension_leg.first);
active_sus_ctrls_.push_back(std::make_shared<effort_controllers::JointPositionController>());
if (!active_sus_ctrls_.back()->init(effort_joint_interface_, nh_suspension))
{
ROS_ERROR("Failed to init active_suspension_joint");
return false;
}
}

for (const auto& flipper_joint : flipper_joints)
{
ros::NodeHandle nh_flipper = ros::NodeHandle(controller_nh, "flipper_joints/" + flipper_joint.first);
flipper_ctrls_.push_back(std::make_shared<effort_controllers::JointPositionController>());
if (!flipper_ctrls_.back()->init(effort_joint_interface_, nh_flipper))
{
ROS_ERROR("Failed to init flipper_joint");
return false;
}
}

imu_sensor_handle_ = robot_hw->get<hardware_interface::ImuSensorInterface>()->getHandle(
getParam(controller_nh, "imu_name", std::string("chassis_imu")));
return true;
}
void AutoSuspensionController::moveJoint(const ros::Time& time, const ros::Duration& period)
{
OmniController::moveJoint(time, period);
odom2base_q.setValue(imu_sensor_handle_.getOrientation()[0], imu_sensor_handle_.getOrientation()[1],
imu_sensor_handle_.getOrientation()[2], imu_sensor_handle_.getOrientation()[3]);
odom2base.setRotation(odom2base_q);
odom2base.setOrigin(tf2::Vector3(0, 0, 0));
tf2::Matrix3x3(odom2base.getRotation()).getRPY(base_roll_, base_pitch_, base_yaw_);
base_roll_rate_ = imu_sensor_handle_.getAngularVelocity()[0];
base_pitch_rate_ = imu_sensor_handle_.getAngularVelocity()[1];
base_yaw_rate_ = imu_sensor_handle_.getAngularVelocity()[2];
// if (chassis_follow_frame_ == "yaw_reverse" && base_pitch_ < -0.35)
// {
// protect_state_ = true;
// }
// else
// {
// protect_state_ = false;
// }
const auto cmd = *cmd_buffer_.readFromRT();
current_state_ = static_cast<State>(cmd.state);
prompt_ = static_cast<Prompt>(cmd.prompt);
use_manual_ = cmd.use_manual;
reset_seq_ = cmd.reset_seq;

if (use_manual_)
{
flipper_target_pos_ = 0.0;
switch (current_state_)
{
case State::DOWN:
sus_target_pos_ = 0.0;
static_effort = -0.0;
break;
case State::MID:
sus_target_pos_ = 0.40;
static_effort = 8.0;
break;
case State::UP:
sus_target_pos_ = 0.75;
static_effort = 5.0;
break;
}
if (current_state_ != last_state_)
{
last_state_ = current_state_;
feedforward_timer = ros::Time::now();
}
if ((ros::Time::now() - feedforward_timer).toSec() < feedforward_effect_time_)
{
if (current_state_ == State::MID)
feedforward_effort = 1.2 * (stretch_coff_A_ - 55.0 * sus_current_pos_);
else if (current_state_ == State::UP)
feedforward_effort = 1.5 * (stretch_coff_A_ + stretch_coff_k_ * sus_current_pos_);
else if (current_state_ == State::DOWN)
feedforward_effort = shrink_coff_A_ + shrink_coff_k_ * sus_current_pos_;
}
else
feedforward_effort = 0.0;
}
else
{
static_effort = 0.0;
const bool reset_requested = prompt_ != Prompt::CLIMB && reset_seq_ != last_reset_seq_;
if (reset_requested)
{
reset(period);
last_reset_seq_ = reset_seq_;
}
else
{
// Both flipper modes allow automatic leg leveling.
climb(period);
}

if (prompt_ != Prompt::CLIMB)
{
prompt_ = Prompt::RESET;
flipper_target_pos_ = flipper_lower_pos_;
}

const bool extending = !reset_requested && std::abs(pitch_) >= pitch_threshold_ &&
-sus_cmd_vel_coff_ * pitch_ > 0.0;
should_release_ = prompt_ == Prompt::RESET && !extending &&
std::abs(sus_target_pos_ - sus_lower_pos_) <= pos_tolerance_ &&
std::abs(sus_current_pos_ - sus_lower_pos_) <= pos_tolerance_;

const double error = sus_target_pos_ - sus_current_pos_;
if (should_release_)
feedforward_effort = 0.0;
else if (error > pos_tolerance_)
feedforward_effort = stretch_coff_A_ + stretch_coff_k_ * sus_current_pos_;
else if (error < -pos_tolerance_)
feedforward_effort = shrink_coff_A_ + shrink_coff_k_ * sus_current_pos_;
else
feedforward_effort = 0.0;
}

for (auto& ctrl : flipper_ctrls_)
{
ctrl->setCommand(flipper_target_pos_);
ctrl->update(time, period);
}
for (auto& ctrl : active_sus_ctrls_)
{
if (should_release_ && !use_manual_ && prompt_ == Prompt::RESET)
{
ctrl->joint_.setCommand(0.0);
sus_current_pos_ = ctrl->getPosition();
continue;
}
ctrl->setCommand(sus_target_pos_);
ctrl->update(time, period);
sus_current_pos_ = ctrl->getPosition();
auto cmd = ctrl->joint_.getCommand();
cmd += feedforward_effort + feedforward_offset + static_effort;
ctrl->joint_.setCommand(cmd);
}
}

void AutoSuspensionController::reset(const ros::Duration& period)
{
flipper_target_pos_ = flipper_lower_pos_;
sus_target_pos_ = sus_lower_pos_;
}

void AutoSuspensionController::climb(const ros::Duration& period)
{
flipper_target_pos_ = flipper_upper_pos_;
if (std::abs(pitch_) >= pitch_threshold_)
{
sus_target_pos_ += sus_cmd_vel_coff_ * pitch_ * period.toSec();
}
sus_target_pos_ = std::max(sus_lower_pos_, std::min(sus_upper_pos_, sus_target_pos_));
}

void AutoSuspensionController::drop(const ros::Duration& period)
{
}

void AutoSuspensionController::AutoSuspensionCallback(const rm_msgs::ChassisAutoSuspension& msg)
{
cmd_buffer_.writeFromNonRT(msg);
}

void AutoSuspensionController::ChassisFollowCallBack(const rm_msgs::ChassisCmd& msg)
{
chassis_follow_frame_ = msg.follow_source_frame;
}
} // namespace rm_chassis_controllers
PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::AutoSuspensionController, controller_interface::ControllerBase)
Loading