Enhance VMC and SeriesLeggedController with new features and optimizations#208
Merged
2194555 merged 62 commits intorm-controls:masterfrom Mar 15, 2026
Merged
Enhance VMC and SeriesLeggedController with new features and optimizations#2082194555 merged 62 commits intorm-controls:masterfrom
2194555 merged 62 commits intorm-controls:masterfrom
Conversation
Add dynamic params.
Fix powerlimit dynamicparam
There was a problem hiding this comment.
Pull request overview
This PR introduces a new bipedal wheel chassis controller stack (VMC + balance modes) and adds dynamic-reconfigure support for both LQR weights and chassis power limiting, along with test launch/configs to load the new controllers.
Changes:
- Add
BipedalControllerplus mode framework (NORMAL / STAND_UP / SIT_DOWN / RECOVER / UPSTAIRS) and VMC kinematics/force mapping utilities. - Add dynamic reconfigure for LQR weights and chassis power-limit coefficients, wiring runtime updates into controller logic.
- Add test YAML/launch and plugin exports to load the new controllers via
controller_manager.
Reviewed changes
Copilot reviewed 42 out of 42 changed files in this pull request and generated 16 comments.
Show a summary per file
| File | Description |
|---|---|
| rm_chassis_controllers/test/vmc_controller.yaml | Adds test controller configuration for VMCController. |
| rm_chassis_controllers/test/load_vmc_controller.launch | Adds a launch file to load VMC-related controllers for testing. |
| rm_chassis_controllers/src/chassis_base.cpp | Wires dynamic-reconfigure into chassis power limiting and reads coefficients from an RT buffer. |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c | Adds generated leg speed mapping code (C). |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c | Adds generated leg forward-kinematics (C). |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c | Adds generated force/torque-to-joint conversion (C). |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp | Adds C++ VMC implementation (2-link Jacobian-based). |
| rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp | Adds a standalone VMC ROS controller plugin. |
| rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_B.c | Adds generated dynamics input matrix code (C). |
| rm_chassis_controllers/src/bipedal_wheel_controller/dynamics/gen_A.c | Adds generated dynamics state matrix code (C). |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp | Adds UPSTAIRS balance mode behavior. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp | Adds STAND_UP mode behavior. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp | Adds SIT_DOWN mode behavior. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp | Adds RECOVER mode behavior. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp | Adds NORMAL mode behavior including jump/unstick logic. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp | Adds mode manager implementation and PID loading. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp | Adds mode base state update helpers (estimation, kinematics, base state). |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp | Adds main BipedalController implementation and estimation/LQR wiring. |
| rm_chassis_controllers/rm_chassis_controllers_plugins.xml | Exports new controller plugins (BipedalController, VMCController). |
| rm_chassis_controllers/package.xml | Adds dynamic_reconfigure dependency. |
| rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h | Adds power-limit dynamic-reconfigure declarations and makes follow() virtual. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h | Adds C header for leg speed mapping. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h | Adds C header for leg position mapping. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_params.h | Adds leg geometry constants. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv.h | Adds C header for force/torque conversion. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h | Adds C++ VMC interface. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h | Adds VMCController class declaration. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h | Adds shared math/helpers for dynamics, PID leg commands, and quaternion conversion. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_B.h | Adds header for generated gen_B functions. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/dynamics/gen_A.h | Adds header for generated gen_A functions. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h | Adds shared constants/types (modes, states, params). |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h | Declares UPSTAIRS mode class. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h | Declares STAND_UP mode class. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/sit_down.h | Declares SIT_DOWN mode class. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h | Declares RECOVER mode class. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h | Declares NORMAL mode class. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h | Declares ModeManager and PID bundles. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h | Declares common mode base interface/state. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h | Declares BipedalController and LQR dynamic reconfigure interface. |
| rm_chassis_controllers/cfg/PowerLimit.cfg | Adds dynamic-reconfigure config for power-limit coefficients. |
| rm_chassis_controllers/cfg/LQRWeight.cfg | Adds dynamic-reconfigure config for LQR weights. |
| rm_chassis_controllers/CMakeLists.txt | Adds dynamic-reconfigure generation and compiles new bipedal sources. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
You can also share your feedback on Copilot code review. Take the survey.
| rightSupportForceAveragePtr_ = std::make_shared<MovingAverageFilter<double>>(4); | ||
| } | ||
|
|
||
| void Normal::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) |
Comment on lines
+18
to
+20
| void StandUp::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) | ||
| { | ||
| if (!controller->getStateChange()) |
| } | ||
|
|
||
| void Recover::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) | ||
| { |
| virtual ~ModeManager() = default; | ||
| void switchMode(int mode) | ||
| { | ||
| mode_impl = mode_map_[mode]; |
Comment on lines
+213
to
+233
| updateUnstick(left_unstick, right_unstick); | ||
| left_unstick = right_unstick = false; | ||
| if (controller->getCompleteStand() && left_unstick && jump_phase_ != JumpPhase::LEG_RETRACTION) | ||
| { | ||
| F_leg[LEFT] -= F_roll; | ||
| u_left = k_left_unstick * (-x_left); | ||
| } | ||
| if (controller->getCompleteStand() && right_unstick && jump_phase_ != JumpPhase::LEG_RETRACTION) | ||
| { | ||
| F_leg[RIGHT] += F_roll; | ||
| u_right = k_right_unstick * (-x_right); | ||
| } | ||
|
|
||
| // Control | ||
| double left_T[2], right_T[2]; | ||
| controller->getVMCPtr()->leg_conv(F_leg[LEFT], u_left(1) + T_theta_diff, left_angle_[0], left_angle_[1], left_T); | ||
| controller->getVMCPtr()->leg_conv(F_leg[RIGHT], u_right(1) - T_theta_diff, right_angle_[0], right_angle_[1], right_T); | ||
| double left_wheel_cmd = left_unstick ? 0. : u_left(0) - T_yaw; | ||
| double right_wheel_cmd = right_unstick ? 0. : u_right(0) + T_yaw; | ||
| LegCommand left_cmd = { F_leg[LEFT], u_left[1], { left_T[0], left_T[1] } }, | ||
| right_cmd = { F_leg[RIGHT], u_right[1], { right_T[0], right_T[1] } }; |
Comment on lines
+18
to
+20
| void Upstairs::execute(BipedalController* controller, const ros::Time& time, const ros::Duration& period) | ||
| { | ||
| if (!controller->getStateChange()) |
Comment on lines
175
to
+178
| control_toolbox::Pid pid_follow_; | ||
|
|
||
| dynamic_reconfigure::Server<rm_chassis_controllers::PowerLimitConfig>* power_limit_srv_{}; | ||
| realtime_tools::RealtimeBuffer<rm_chassis_controllers::PowerLimitConfig> power_limit_rt_buffer_; |
Comment on lines
394
to
418
| @@ -398,8 +411,8 @@ void ChassisBase<T...>::powerLimit() | |||
| c += square(real_vel); | |||
| } | |||
| } | |||
| a *= effort_coeff_; | |||
| c = c * velocity_coeff_ - power_offset_ - power_limit; | |||
| a *= effort_coeff; | |||
| c = c * vel_coeff - power_offset - power_limit; | |||
| // Root formula for quadratic equation in one variable | |||
| double zoom_coeff = (square(b) - 4 * a * c) > 0 ? ((-b + sqrt(square(b) - 4 * a * c)) / (2 * a)) : 0.; | |||
| for (auto joint : joint_handles_) | |||
Comment on lines
+5
to
+35
| #pragma once | ||
|
|
||
| #include <control_toolbox/pid.h> | ||
|
|
||
| #include "bipedal_wheel_controller/controller_mode/mode_base.h" | ||
| #include "bipedal_wheel_controller/controller_mode/sit_down.h" | ||
| #include "bipedal_wheel_controller/controller_mode/stand_up.h" | ||
| #include "bipedal_wheel_controller/controller_mode/recover.h" | ||
| #include "bipedal_wheel_controller/controller_mode/normal.h" | ||
| #include "bipedal_wheel_controller/controller_mode/upstairs.h" | ||
|
|
||
| namespace rm_chassis_controllers | ||
| { | ||
| class ModeManager | ||
| { | ||
| public: | ||
| ModeManager(ros::NodeHandle& controller_nh, const std::vector<hardware_interface::JointHandle*>& joint_handles); | ||
| virtual ~ModeManager() = default; | ||
| void switchMode(int mode) | ||
| { | ||
| mode_impl = mode_map_[mode]; | ||
| } | ||
| const std::shared_ptr<ModeBase>& getModeImpl() | ||
| { | ||
| return mode_impl; | ||
| } | ||
|
|
||
| private: | ||
| std::shared_ptr<ModeBase> mode_impl; | ||
| std::map<int, std::shared_ptr<ModeBase>> mode_map_; | ||
|
|
| bool BipedalController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, | ||
| ros::NodeHandle& controller_nh) | ||
| { | ||
| ChassisBase::init(robot_hw, root_nh, controller_nh); |
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
No description provided.