Footstep replanning ignores the real robot pose — feature request / advice?

[rohanpsingh]

Hello,

I'm using BWC inside a multi-state FSM controller that interleaves walking with manipulation: roughly WalkA → manipulate → WalkB → manipulate → WalkC .... Each walking state computes a target world pose and then calls
footManager_->walkToRelativePose(...) to plan a new gait, when the footstep queue is empty.

In simulation (mc_mujoco), the real robot drifts away from the control robot between walks (sim slipping, contact-model imperfections, manipulation contacts disturbing the stance, etc.). I would like each new walk to be planned starting from where the real robot is actually standing, not from where the previous plan said it should stand.

What BWC currently does (as I understand it)

FootManager::reset() seeds targetFootPoses_ from ctl().robot().surfacePose(...) once, and after that they're only mutated by the swing trajectory. Crucially, walkToRelativePose uses

const sva::PTransformd & initialFootMidpose =      
      projGround(sva::interpolate(targetFootPoses_.at(Foot::Left), 
                                  targetFootPoses_.at(Foot::Right), 0.5));

as the origin of the new plan. So the plan is anchored to the control robot's last commanded foot poses — any drift between control and real accumulates across walks and is never corrected.

I noticed CentroidalManager deliberately exposes hooks for closing the loop on the real robot (useActualStateForMpc, useActualComForWrenchDist, useTargetPoseForControlRobotAnchorFrame, ...) but the FootManager has no analogous switch.

Question

Is there already an idiomatic mc_rtc / BWC way to do this that I'm missing?

Suggested fix (if there isn't already a way)

A small public API on FootManager to re-seed the planning origin from a given robot would solve this cleanly, e.g.:

  /// Re-seed targetFootPoses_ from `robot.surfacePose(...)`.
  /// Only valid when the footstep queue is empty (i.e., not mid-walk).
  void FootManager::resetFootOrigin(const mc_rbdyn::Robot & robot);

Then a user could call footManager_->resetFootOrigin(ctl().realRobot()) just before walkToRelativePose to anchor the new plan to reality. Could also be a config flag along the lines of useActualFootPoseForReplan checked at the top of walkToRelativePose.

[mmurooka]

Hi, @rohanpsingh

The integration between BWC and the footstep planner remained mostly conceptual, so the issue you pointed out can indeed happen. There is currently no built-in option to address it.

I think your proposed solution is also a reasonable approach.

That said, accurately estimating where the humanoid actually is in the real world would additionally require something like SLAM. I am not entirely sure whether mc_rtc is designed with the assumption that the robot's global pose estimated by SLAM would be reflected back into the real robot state.

[mehdi-benallegue]

Hello Rohan, how are you?

I am not an expert in BWC but I know this issue in a general way.

Are you working in position control mode ? In such a case you know that the control occurs in an open loop where the control state results from the integration of the reference accelerations. And this includes the floating base. This means that if the estimated robot (that was misnamed realRobot in the code) drifts from the real robot. The control at the QP level is not aware of it. We do update the stabilizer (and possibliy the MPC) with the state of the robot such as orientation and CoM pose, which then uses them to generate reference forces and CoM trajecories to correct for them. This means that if you use an odometry or a motion capture, this drift may lead to a too big difference and make the robot fall (which is surprisingly not what you are reporting here as an issue). This means also that the feet have to be controlled in the control space.

In IS-MPC waling controller implemented by Antonin Dallard, we solve that by adding a slow drift reference in the contact pose that makes the robot virtually slide to the RealRobot foot location. Therefore in that case if you have slow drifting robot or slow driftin estimation they will mostly align, but if there is a discontinuity in the discrepancy between (estimated-) realRobot and (control-) Robot (for example in the case of a SLAM loop closure), the robot would likely fall...

There might be mechanisms in BWC that can deal with this issue as well but I don't know them.

Are you using position control mode?
Only simulation or also real robot?
What is your estimator pipeline and what sensors are used?
Do you expect to have discontinuities in the estimated position of the robot?

[mehdi-benallegue]

Wow we replied in the same minute without planning it... ^_^

I am not entirely sure whether mc_rtc is designed with the assumption that the robot's global pose estimated by SLAM would be reflected back into the real robot state.

I agree that the initial design of mc_rtc has many flaws including this one. And we spend a lot of time trying to bend it to include more versatile assumptions. But I think that if SLAM creates slow varying drift from the control robot, there are simple ways to fix that, and for now the torque control mode is actually ready to cope with such loop closure. But in case of a big discontinuity, we are not fully equipped. For instance we don't have straightforward tasks in the egocentric frame, or in the contact anchor frame, which would have been more convenient to use for balance and manipulation in that case...

[rohanpsingh]

Hi @mehdi-benallegue I'm doing great, hope you're doing good too!

Thank you to both of you for responding!

I'm basically trying to implement an mc_rtc controller for door opening with the JVRC1 robot. It's position control, and I do not have any plans to deploy on the real robot. Yet I want it to faithfully demonstrate how a real deployment would work, assuming there's SLAM/mocap to estimate the robot's global position.

This is what my observation pipeline looks like:

ObserverPipelines:
  name: MainObserverPipeline
  gui: true
  observers:
    - type: Encoder
    - type: Attitude
      required: false
    - type: KinematicInertial
      config:
        anchorFrame:
          maxAnchorFrameDiscontinuity: 0.05 # [m]

But I think in order for mc_rtc to have realRobot.posW() reflect the FloatingBase sensor (i.e. the actual simulator/observed floating-base pose), I'll also have to set type: BodySensor update: true.

We do update the stabilizer (and possibliy the MPC) with the state of the robot such as orientation and CoM pose, which then uses them to generate reference forces and CoM trajecories to correct for them. This means that if you use an odometry or a motion capture, this drift may lead to a too big difference and make the robot fall (which is surprisingly not what you are reporting here as an issue).

Well, I simply want the footstep plan to be generated relative to the current frame of the robot, and leave the balance control as is. If I understand you correctly, there's just no way to make the robot go from precisely point A to B even if I have an accurate odometry?

[rohanpsingh]

I agree that the initial design of mc_rtc has many flaws including this one. And we spend a lot of time trying to bend it to include more versatile assumptions.

Perhaps it's time for mc_rtc 2.0? 😄

[mehdi-benallegue]

Sorry my message went immediately with ctrl+enter....

Perhaps it's time for mc_rtc 2.0? 😄

The current release of mc_rtc is v2.14, which is a big improvement over version 1, especially in terms of versatility.
Release v2.14.0
So we went some distance off that initial design, but still some work needs to be done still :)

Back to your question I see, with the current observation loop, you don't have any drift from the observed robot and the control robot models (because kinematic inertial assumes that the contacts have the same cordinates in both frames), but you will have a difference between the actual robot and the observed robot. You are right that if you need to remove that drift, you would need to use BodySensor and find an alternative solution for the real robot.

In that case I am not sure to understand completely what you want to do. But I see two options

• If you want to maintain the current observer pipeline(this keep no drift between the estimated and the control robots): This requires no modification of the balance control but in that case you would either
  • need to work in relative cordinates and report the desired translation and rotation as targets in the control frame. Which is probably is the simplest
  • Or more complicated just want to have a separate instance of the trajectory planner that knows the actual position of the robot and generates the plan and then you would translate it in the frame of the control robot. I see this only relevant if you want to check the trajectory for collisions or something.
• if you want to move to a perfect-odometry and use body sensor: you could implement a solution like what we did with Antonin, and just add a task on the feet to move them towards the RealRobot feet position slowly. I can show you how it is done in IS-MPC if you are interested
• Switch to IS MPC, but we have not yet made a nice release of that for bad reasons.

Sorry for chaotic message,

[ThomasDuvinage]

Hello @rohanpsingh,

We encountered the same issue recently. Few solutions can be considered :

• Updating BaselineFootstepPlanner to take as input any robot (control, real, slam, ...). Since SLAM observer is creating a new robot which can be retrieved from the Datastore it can be a first solution be doesn't consider potential issues mentioned in the next point.
• Update state_observation / mc_state_observation to better consider SLAM observer. As mentioned by @mehdi-benallegue different solutions exist but not yet existing (I don't know when). So far existing SLAM observer doesn't support discontinuities.
• Update mc_rtc to better differentiate which robot to use at different stage. This implies a large refactoring that should be considered when moving to Pinnochio.

Sorry for only describing the current state and not bringing any solution on the table. It is indeed time for a big update, handling pinnochio would be a good time for this ;-)

[rohanpsingh]

Thanks all for the suggestions. I spent some time trying to come up with the solution but unfortunately, did not succeed. It is a bit surprising, however, this hasn't surfaced before. Looking forward the new version of mc_rtc (and the release of IS MPC)!

[arntanguy]

Thanks all for the suggestions. I spent some time trying to come up with the solution but unfortunately, did not succeed. It is a bit surprising, however, this hasn't surfaced before. Looking forward the new version of mc_rtc (and the release of IS MPC)!

Well it has, many times in fact. It started during my thesis trying to close the control loop on in fact SLAM 6D pose feedback on the floating base, which raised many issues with the current QP formulation. There are a few main issues with closing the loop on the QP in position control:

• We can't just take the current measured state of the robot, compute the joint accelerations for the next iteration, and send these as the desired control, since the difference in position between two iterations is negligible and would therefore not be tracked by any low-level PD control (as we have on HRP robots). Thus we need to have a model that maintains the double integration from computed joint accelerations by the QP to joint positions result over time. This is what robot() is in mc_rtc. Traditionally this robot represent the open-loop state of the QP, that is:
  • Controller is initialized from current measured state of the robot (floating base position is either user-defined in the controller or can be provided from sensor measurements)
  • Tasks/constraints are added, the QP computes a solution qdd that is then double integrated into robot().q
  • Next iteration starts from the double integrated state of robot().mbc().q

Over time the state of the double integrator robot().mbc().q diverges from the real robot state, unless something is done to prevent it. In an attempt to add feedback to this scheme several "closed-loop" modes were introduced:

/** Describe the type of feedback used to control the robot */
enum class MC_SOLVER_DLLAPI FeedbackType
{
  /** No feedback, i.e. open-loop control */
  None,
  /** Synonyn for None */
  OpenLoop = None,
  /** Use encoder values for actuated joints */
  Joints,
  /** Joints + encoder velocity obtained from numerical differentiation */
  JointsWVelocity,
  /** Run in closed loop w.r.t realRobots using the observation pipeline and integrate over the control state of the
     system */
  ObservedRobots,
  /** Synonym for ObservedRobots */
  ClosedLoop = ObservedRobots,
  /** Run in closed loop w.r.t realRobots using the observation pipeline and integrate over the real state of the system
   */
  ClosedLoopIntegrateReal,
  /** Skip the QP entirely */
  SkipQP
};

What this does is that the QP solution is computed from a robot state matching the selected feedback mode (e.g Joints will set robot().mbc().q to the measured encoder values, etc), but the resulting acceleration qdd is still integrated on top of the previous double integration result. In pseudo code:

• store double integrator state robot().mbc()
• set robot().mbc.[q,alpha,alphaD, ...] to measured state according to the chosen feedback mode
• compute the QP result accelerations qdd
• restore double integrator state to robot().mbc(), integrate the qdd accelerations

This raises a few major issues since the solution of the QP is not directly applied:

• The integrator state and real robot state can, and will diverge
  • For example, if we feed back the measured floating base position (from SLAM for example), and add a CoM task with high gains on HRP-2Kai/HRP-4 that have unmodelled spring-dampers in the feet, the control will have diverging oscillations as the spring adds unaccounted for energy.
• Constraints may become infeasible, especially in their current formulation where they are mostly written in planning way rather than accounting for real motion. For example contacts are assumed to be fixed and non sliding when in reality they may move or even loose physical contact, joint limit constraints may become infeasible (we use heuristics to decelerate to 0 velocity within a given interaction distance as the QP does not have enough horizon to know how to optimally descelerate).

Some of these issues were explored and tackled in the thesis of Mohamed Djeha: https://theses.fr/2022UMONS036. This mostly implies reformulating tasks with an integral state to avoid divergence, and reformulating constraints as MPC (read the thesis' abstract for a brief overview). While the implementation was done with Tasks/mc_rtc and showed promising results, it is not currently integrated in the framework (it ough to be.)

The QP feedback issue is lessened when doing in torque control, as this fully removes the need of the double integrator, and we can have a much simpler scheme of measured joint torque -> compute qp solution -> send solution to robot -> repeat, and only need to consider additional issues such as joint friction compensation, imprecise dynamic model, etc.

Thus for now, most feedback has been done on a per-controller basis at a higher level than the QP. Typicall the relevant measured state is observed with the ObserverPipeline which computes and sets realRobot(). This realRobot state is then used in various ways:

• In walking controller it is done in the footstep planning phase where we recompute where the next footsteps should be based on the measured robot pose (your proposed solution here).
• For stabilization this is typically done with a stabilizer based on a LIPM model that takes into account the measured state of the robot and modifies the tasks targets to compensate.
• Walking controllers such as ISMPC additionnaly can compute a closed-loop MPC based on the real robot state, with limitations as it is indeed not trivial to guarantee MPC stability in that case.
• In grasping controllers this is usually a combination of initial pose detection (SLAM, object pose estimation etc) and visual servoing (e.g COMANOID demo, HRP5 torqueing demo). In more complex cases we used real-time object tracking (e.g for the big wheel manipulation demo)
• Etc, this unfortunately always has to be done manually.

These adjustements are quite ad-hoc and introduce significant friction in controller implementations, and often lead to, quite frankly, ugly workarounds.

Your proposed solution here of modifying BaselineWalkiingController's API to use the real robot pose for footstep planning is well in line with that type of "workarounds", and is indeed the easiest way of achieving your goals currently. In an ideal world, we would not need the distinction between robot() and realRobot(), but this implies having a QP formulation that can fully close the loop on real robot data, which I'm afraid still requires significant work, especially for position control. Significant work is underway and soon-to-be-merged regarding torque control improvements.

I hope this helps give some context on the issue, we should probably have a dedicated tutorial page explaining this in more details.