Refactor leg dynamics, control logic, and state management - #215
Open
WiseL00k wants to merge 63 commits into
Open
Refactor leg dynamics, control logic, and state management#215WiseL00k wants to merge 63 commits into
WiseL00k wants to merge 63 commits into
Conversation
…que to force conversion
…ons; update internal state management for leg dynamics
…g state management
…e in computePidLegCommand
There was a problem hiding this comment.
Pull request overview
This PR refactors the bipedal wheel chassis control stack by consolidating leg kinematics/dynamics into the C++ VMC class (removing generated C helpers), and reworking controller/mode state flow to use explicit LegState/ChassisState plus a new BipedalControllerInterface for mode implementations.
Changes:
- Move leg pos/vel/force conversions into
VMCinternal state (LegPos/LegSpd/LegForce) and remove the MATLAB-generatedleg_*.[ch]utilities. - Refactor
BipedalControllerand mode classes to pull state viaBipedalControllerInterface(instead of pushing viaModeBase::update*), and expand parameter grouping (spring + chassis geometry + thresholds). - Add a new
PROTECTmode, additional debug publishing, and update tuning/config (YAML + dynamic_reconfigure).
Reviewed changes
Copilot reviewed 34 out of 34 changed files in this pull request and generated 9 comments.
Show a summary per file
| File | Description |
|---|---|
| rm_chassis_controllers/test/vmc_controller.yaml | Updates VMC controller test params (bias angle, spring params, geometry). |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/VMC.cpp | Refactors VMC API to store internal pos/spd/Jacobian; adds torque→virtual-force conversion. |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_spd.c | Removes generated leg speed code (migrated into VMC). |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_pos.c | Removes generated leg position code (migrated into VMC). |
| rm_chassis_controllers/src/bipedal_wheel_controller/vmc/leg_conv.c | Removes generated leg force/torque conversion code (migrated into VMC). |
| rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp | Updates controller to new VMC API; adds gravity-comp debug + spring force function. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp | Refactors init/state estimation to use LegState/ChassisState; adds services/debug; updates VMC usage. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/upstairs.cpp | Updates Upstairs mode to consume controller state via interface and VMC getters. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp | Updates StandUp mode to new state flow; adds ramp filters and revised leg motion logic. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/sit_down.cpp | Updates SitDown mode execution signature and state access via interface. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/recover.cpp | Updates Recover mode to use interface state; expands recovery decision logic. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/protect.cpp | Adds new Protect mode implementation. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp | Refactors Normal mode to new state model; adjusts jump/unstick/protection logic. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp | Rewires mode construction to pass BipedalControllerInterface; adds PROTECT mode. |
| rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_base.cpp | Removes obsolete state-push helpers from ModeBase. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/VMC.h | Defines LegPos/LegSpd/LegForce and updates VMC public API + stored Jacobian/state. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_spd.h | Removes generated header (migrated into VMC). |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_pos.h | Removes generated header (migrated into VMC). |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_params.h | Removes generated params header (now parameterized elsewhere). |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/vmc/leg_conv.h | Removes generated header (migrated into VMC). |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/series_legged_vmc_controller.h | Updates includes; adds debug publisher and gravity-comp helpers/params. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/helper_functions.h | Adds leg geometry helper functions and a generic clamp helper; updates AB generation. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h | Expands shared definitions: Leg/Chassis state structs, thresholds, new mode, indices. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller.h | Introduces interface inheritance, new params structs, new pubs/subs/srv, new state layout. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/upstairs.h | Updates signature/state types to interface-based operation. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h | Updates signature/state types; adds ramp filters and StandUp command struct. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/sit_down.h | Updates signature to interface-based execution. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/recover.h | Updates signature and recovery state enums; adds leg recovery readiness checks. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/protect.h | Adds Protect mode header. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/normal.h | Updates signature; adds wheel-vel diff PID and new internal flags/thresholds. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_manager.h | Updates constructor to accept controller interface; adds new PID and Protect include. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/mode_base.h | Changes mode execution API to interface-only; stores controller pointer in base. |
| rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_interface.h | Adds the new BipedalControllerInterface used by modes. |
| rm_chassis_controllers/cfg/LQRWeight.cfg | Expands dynamic_reconfigure ranges and adds bias parameters. |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
Comment on lines
+248
to
+249
| double s_left_flip = 1 - s_left; | ||
| double s_right_flip = 1 - s_left; |
Comment on lines
+391
to
393
| double ddot_theta = 0.3 * ((x(1) - last_dot_theta) / period.toSec()) + 0.7 * last_ddot_theta; | ||
| double ddot_leg_len = 0.3 * (d_leg_len[2] - d_leg_len[0]) / 2 * period.toSec() + 0.7 * last_ddot_leg_len; | ||
| last_dot_theta = x(1); |
Comment on lines
+396
to
+397
| double ddot_zw = ddot_zM - ddot_leg_len * cos(x(0)) + 2 * leg_len_spd * x(1) * sin(x(0)) + | ||
| +leg_length * (ddot_theta * sin(x(0)) + leg_length * x(1) * x(1) * cos(x(0))); |
Comment on lines
+45
to
+56
| static double last_L0_spd = J_[0][0] * dphi1 + J_[0][1] * dphi4; | ||
| static double last_theta_spd = J_[1][0] * dphi1 + J_[1][1] * dphi4; | ||
|
|
||
| // 速度映射 | ||
| spd[0] = J[0][0] * dphi1 + J[0][1] * dphi4; // dl0 | ||
| spd[1] = J[1][0] * dphi1 + J[1][1] * dphi4; // dphi0 | ||
| spd_.dL0 = J_[0][0] * dphi1 + J_[0][1] * dphi4; // dl0 | ||
| spd_.dTheta = J_[1][0] * dphi1 + J_[1][1] * dphi4; // dphi0 | ||
|
|
||
| // leg_spd lp filter | ||
| spd_.dL0 = 0.4 * spd_.dL0 + 0.6 * last_L0_spd; | ||
| last_L0_spd = spd_.dL0; | ||
| spd_.dTheta = 0.4 * spd_.dTheta + 0.6 * last_theta_spd; | ||
| last_theta_spd = spd_.dTheta; |
Comment on lines
+92
to
95
| void StandUp::setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const LegOrientation& other_leg_orientation, | ||
| const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation, | ||
| StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time) | ||
| { |
Comment on lines
+107
to
+111
| case LegOrientation::FRONT: | ||
| stop_flag = false; | ||
| if (!arrive_flag) | ||
| arrive_flag = false; | ||
| legCommand.desired_angle = M_PI_2 - 0.35; |
Comment on lines
+52
to
+55
| mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles, | ||
| pid_legs_stand_up_, pid_thetas_))); | ||
| mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles, | ||
| pid_legs_stand_up_, pid_thetas_))); |
Comment on lines
47
to
63
| s2_ = controller_nh.param("s2", 0.0775); | ||
| s3_ = controller_nh.param("s3", 0.205); | ||
| alpha_s_ = controller_nh.param("alpha_s", 0.2); | ||
| vmcPtr_ = std::make_unique<VMC>(l1, l2, 0); |
Comment on lines
133
to
136
| double leftWheelVel{}, rightWheelVel{}, leftWheelVelAbsolute{}, rightWheelVelAbsolute{}, slip_alpha_{ 2.0 }, | ||
| slip_R_wheel_{}, R_wheel_{}; | ||
| int i = 0, sample_times_ = 3; | ||
| int itor = 0, sample_times_ = 0; | ||
| bool slip_flag_{ false }; |
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 39 out of 39 changed files in this pull request and generated 3 comments.
Suppressed comments (8)
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:253
- In OFF_GROUND,
s_right_flipis computed froms_left, so the right leg uses the left leg’s interpolation parameter. This makes the right-leg force profile incorrect whenever left/right leg lengths differ.
ROS_INFO("[balance] ENTER OFF_GROUND");
double s_left_flip = 1 - s_left;
double s_right_flip = 1 - s_left;
F_leg(LEFT) = -control_params_->off_ground_force * (1 - 3 * pow(s_left_flip, 2) + 2 * pow(s_left_flip, 3)) -
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:397
ddot_leg_lenis currently computed as(Δv)/2 * dt, which has the wrong units (it scales with dt instead of dividing by dt). Also,ddot_zwmultipliesleg_lengthtwice in the centripetal term (leg_length * leg_length * ω²), which looks unintended and will inflate support-force estimation.
double ddot_zM = 0.3 * (acc_z - model_params->g) + 0.7 * last_ddot_zM;
double ddot_theta = 0.3 * ((x(1) - last_dot_theta) / period.toSec()) + 0.7 * last_ddot_theta;
double ddot_leg_len = 0.3 * (d_leg_len[2] - d_leg_len[0]) / 2 * period.toSec() + 0.7 * last_ddot_leg_len;
last_dot_theta = x(1);
last_ddot_theta = ddot_theta;
rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp:692
std::coutinreconfigCBwill spam stdout whenever dynamic reconfigure updates. This should be removed or replaced with throttled ROS debug logging.
coeffs_(2, i + 2 * j) * 0.2 + coeffs_(3, i + 2 * j);
}
}
std::cout << "len: 0.2m LQR k: " << std::endl << k << std::endl;
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp:95
arrive_timeis passed by value, so assignments likearrive_time = ros::Time::now()don’t persist back to the caller; this breaks the timing-based transition to BEHIND. Passarrive_timeby reference so the state machine can measure dwell time correctly.
void StandUp::setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const LegOrientation& other_leg_orientation,
const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation,
StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time)
{
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp:55
BalanceMode::UPSTAIRSis inserted intomode_map_twice.std::map::insertwill ignore the second insert, so this is dead code and makes it harder to see the intended mode set.
mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles,
pid_legs_stand_up_, pid_thetas_)));
mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles,
pid_legs_stand_up_, pid_thetas_)));
rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp:451
- This
std::coutblock prints matrices during controller initialization. Writing to stdout in a ROS controller can be noisy and may impact timing; prefer ROS logging (or remove entirely) for debug-only output.
This issue also appears on line 688 of the same file.
{
std::cout << "A: " << std::endl << a << std::endl;
std::cout << "B: " << std::endl << b << std::endl;
std::cout << "len: 0.2m LQR k: " << std::endl << k << std::endl;
}
rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h:38
- Header declaration passes
arrive_timeby value, but the implementation needs to update this timestamp across calls for the stand-up state machine. Make this a reference to match the intended behavior.
void setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const LegOrientation& other_leg_orientation,
const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation,
StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time);
rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/definitions.h:9
definitions.husesEigen::Matrixandgeometry_msgs::Vector3but doesn’t include the corresponding headers. This can break compilation depending on include order; headers should be self-contained.
#include "bipedal_wheel_controller/vmc/VMC.h"
#include <array>
#include <utility>
Comment on lines
+204
to
+205
| F_leg[LEFT] = F_pid_left - F_inertia_left + gravity / cos(left_pos.theta) + F_roll - left_spring_force; | ||
| F_leg[RIGHT] = F_pid_right + F_inertia_right + gravity / cos(right_pos.theta) - F_roll - right_spring_force; |
Comment on lines
+704
to
+709
| double cos_theta3, theta3, ls, Fv; | ||
| cos_theta3 = (l1 * l1 + l2 * l2 - L0 * L0) / (2 * l1 * l2); | ||
| theta3 = acos(cos_theta3); | ||
| ls = sqrt(s2 * s2 + s3 * s3 - 2 * s2 * s3 * cos(theta3 - alpha_s)); | ||
| Fv = Fs * (L0 * s2 * s3 * sin(theta3 - alpha_s)) / (ls * l1 * l2 * sin(theta3)); | ||
| return Fv; |
Comment on lines
+168
to
+174
| double l1 = vmcPtr_->getL1(), l2 = vmcPtr_->getL2(), Fs = spring_force_, s2 = s2_, s3 = s3_, alpha_s = alpha_s_; | ||
| double cos_theta3, theta3, ls, Fv; | ||
| cos_theta3 = (l1 * l1 + l2 * l2 - L0 * L0) / (2 * l1 * l2); | ||
| theta3 = acos(cos_theta3); | ||
| ls = sqrt(s2 * s2 + s3 * s3 - 2 * s2 * s3 * cos(theta3 - alpha_s)); | ||
| Fv = Fs * (L0 * s2 * s3 * sin(theta3 - alpha_s)) / (ls * l1 * l2 * sin(theta3)); | ||
| return Fv; |
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 39 out of 39 changed files in this pull request and generated 1 comment.
Suppressed comments (8)
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:396
ddot_leg_lenis computed with... / 2 * period.toSec()which (due to operator precedence) multiplies by dt instead of dividing by dt. This makes the leg-length acceleration estimate scale incorrectly with the control period and can destabilize support-force estimation.
double ddot_zM = 0.3 * (acc_z - model_params->g) + 0.7 * last_ddot_zM;
double ddot_theta = 0.3 * ((x(1) - last_dot_theta) / period.toSec()) + 0.7 * last_ddot_theta;
double ddot_leg_len = 0.3 * (d_leg_len[2] - d_leg_len[0]) / 2 * period.toSec() + 0.7 * last_ddot_leg_len;
last_dot_theta = x(1);
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:401
- The
ddot_zwexpression has two issues: there is an extra unary+(likely a typo) andleg_lengthis multiplied twice in the last term (leg_length * (... + leg_length * x(1)^2 * cos(...))), changing units and the physical meaning compared to the standard derivation.
double ddot_zw = ddot_zM - ddot_leg_len * cos(x(0)) + 2 * leg_len_spd * x(1) * sin(x(0)) +
+leg_length * (ddot_theta * sin(x(0)) + leg_length * x(1) * x(1) * cos(x(0)));
double Fn = model_params->m_w * ddot_zw + model_params->m_w * model_params->g + P;
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp:95
- The
setUpLegMotiondefinition still takesarrive_timeby value, so updates to the timestamp do not survive outside this function. This breaks the timing-based gate (ros::Time::now() - arrive_time) used to switch to BEHIND.
void StandUp::setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const LegOrientation& other_leg_orientation,
const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation,
StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time)
{
rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp:706
acos(cos_theta3)can produce NaN ifcos_theta3drifts slightly outside [-1, 1] due to floating-point error. This would propagate toFvand can destabilize the controller. Clampcos_theta3before callingacos.
double cos_theta3, theta3, ls, Fv;
cos_theta3 = (l1 * l1 + l2 * l2 - L0 * L0) / (2 * l1 * l2);
theta3 = acos(cos_theta3);
ls = sqrt(s2 * s2 + s3 * s3 - 2 * s2 * s3 * cos(theta3 - alpha_s));
rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp:172
acos(cos_theta3)can return NaN ifcos_theta3is slightly outside [-1, 1] from floating-point error. Clamping avoids NaNs propagating into the computed spring compensation force.
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:253- In OFF_GROUND jump phase,
s_right_flipis computed froms_leftinstead ofs_right, which makes the right-leg trajectory mirror the left-leg value and breaks the intended per-leg interpolation.
This issue also appears in the following locations of the same file:
- line 393
- line 399
ROS_INFO("[balance] ENTER OFF_GROUND");
double s_left_flip = 1 - s_left;
double s_right_flip = 1 - s_left;
F_leg(LEFT) = -control_params_->off_ground_force * (1 - 3 * pow(s_left_flip, 2) + 2 * pow(s_left_flip, 3)) -
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp:55
BalanceMode::UPSTAIRSis inserted intomode_map_twice. Withstd::map::insert, the second insert is ignored (but still constructs/destroys a fullUpstairsinstance), which is unnecessary and can hide mistakes when adding new modes.
mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles,
pid_legs_stand_up_, pid_thetas_)));
mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles,
pid_legs_stand_up_, pid_thetas_)));
rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h:38
setUpLegMotiontakesarrive_timeby value, so the timestamp set on first arrival (arrive_time = ros::Time::now()) does not persist back toleft_arrive_time_/right_arrive_time_. This makes thearrive_time_thresholdcheck unreliable and can prevent the FRONT→BEHIND transition.
void setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const LegOrientation& other_leg_orientation,
const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation,
StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time);
Comment on lines
5
to
10
| #pragma once | ||
|
|
||
| #include "bipedal_wheel_controller/vmc/VMC.h" | ||
| #include <array> | ||
| #include <utility> | ||
|
|
There was a problem hiding this comment.
Pull request overview
Copilot reviewed 39 out of 39 changed files in this pull request and generated no new comments.
Suppressed comments (8)
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:396
ddot_leg_lenuses* period.toSec()(multiplying by dt) instead of dividing by dt, so the estimated acceleration has incorrect units/magnitude.
double ddot_zM = 0.3 * (acc_z - model_params->g) + 0.7 * last_ddot_zM;
double ddot_theta = 0.3 * ((x(1) - last_dot_theta) / period.toSec()) + 0.7 * last_ddot_theta;
double ddot_leg_len = 0.3 * (d_leg_len[2] - d_leg_len[0]) / 2 * period.toSec() + 0.7 * last_ddot_leg_len;
last_dot_theta = x(1);
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:400
ddot_zwcurrently multiplies the centrifugal term by an extraleg_length, which changes the dynamics (and can blow up at large leg lengths). This looks like an accidental extra factor compared to the standard form.
double ddot_zw = ddot_zM - ddot_leg_len * cos(x(0)) + 2 * leg_len_spd * x(1) * sin(x(0)) +
+leg_length * (ddot_theta * sin(x(0)) + leg_length * x(1) * x(1) * cos(x(0)));
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/stand_up.cpp:95
arrive_timeis passed by value, but it is used as state across calls (to measure how long the leg has been in the target region). Passing by value prevents the timestamp from persisting, so the BEHIND transition timing logic will not work as intended.
void StandUp::setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const LegOrientation& other_leg_orientation,
const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation,
StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time)
{
rm_chassis_controllers/src/bipedal_wheel_controller/series_legged_vmc_controller.cpp:174
acos(cos_theta3)can return NaN if numeric error pushescos_theta3slightly outside [-1, 1]. Alsolsorsin(theta3)can approach zero, causing a divide-by-zero and Inf/NaN spring force. Clamp and guard the denominators to keep the controller stable.
rm_chassis_controllers/src/bipedal_wheel_controller/controller.cpp:708acos(cos_theta3)can produce NaN ifcos_theta3drifts outside [-1, 1] due to numeric error. Also the spring-force expression divides bylsandsin(theta3)without guarding against near-zero values, which can create Inf/NaN forces and destabilize control.
double cos_theta3, theta3, ls, Fv;
cos_theta3 = (l1 * l1 + l2 * l2 - L0 * L0) / (2 * l1 * l2);
theta3 = acos(cos_theta3);
ls = sqrt(s2 * s2 + s3 * s3 - 2 * s2 * s3 * cos(theta3 - alpha_s));
Fv = Fs * (L0 * s2 * s3 * sin(theta3 - alpha_s)) / (ls * l1 * l2 * sin(theta3));
return Fv;
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/mode_manager.cpp:55
BalanceMode::UPSTAIRSis inserted intomode_map_twice. The second insert is redundant (and will be ignored bystd::map::insert), which is confusing and makes future edits error-prone.
mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles,
pid_legs_stand_up_, pid_thetas_)));
mode_map_.insert(std::make_pair(BalanceMode::UPSTAIRS, std::make_unique<Upstairs>(controller, joint_handles,
pid_legs_stand_up_, pid_thetas_)));
rm_chassis_controllers/src/bipedal_wheel_controller/controller_mode/normal.cpp:252
- In OFF_GROUND,
s_right_flipis computed froms_left, which makes both legs use the left leg interpolation and breaks symmetry during the off-ground phase.
double s_left_flip = 1 - s_left;
double s_right_flip = 1 - s_left;
rm_chassis_controllers/include/rm_chassis_controllers/bipedal_wheel_controller/controller_mode/stand_up.h:38
arrive_timeis passed by value, but it is used as state across calls (to measure how long the leg has been in the target region). Passing by value prevents the timestamp from persisting, so the BEHIND transition timing logic will not work as intended.
void setUpLegMotion(const Eigen::Matrix<double, STATE_DIM, 1>& x, const LegOrientation& other_leg_orientation,
const double& leg_length, const double& leg_theta, LegOrientation& leg_orientation,
StandUpLegCommand& legCommand, bool& stop_flag, bool& arrive_flag, ros::Time arrive_time);
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.