Feature/quickster footstep provider #527

stefanfasano · 2024-12-10T22:07:00Z

This is the initial groundwork for the implementation of Quickster's footstep calculator into the regular walking controller via the CSG (in step generator thread).

Tests included in this PR:

QFPTest; this tests switching between CSG modes while walking in place and standing in place

The next steps include:

New message type to ensure things on the controller side know which CSG mode is in use
Implementation into whole body MPC scheme
Hardware testing on robot
Max speed limits along with max acceleration per step

…fraction and pole

… swing

…te machine to go to TO_STANDING

christiandebuys · 2024-12-18T00:36:08Z

...LevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java

+      balanceManager.computeICPPlan();
+   }
+
+   private boolean haveWeEntered = false;


I am confused by this boolean. Isn't whether or not you are executing the method inside onEntry() enough information to set firstTick in the method? Why are haveWeEntered and firstTick both in handleNewFootstep()?

There's probably a better way to do this whole thing. They don't exactly convey the same information, since first tick could be false if it is the second+ tick or if it has not entered the state yet, and that distinction is important. It is possible for the method that relies on this boolean (handleNewFootstep()) to be called without entering the state (WalkingSingleSupportState) because I have this method tied to a listener in another class (WalkingMessageHandler), and the class that this method belongs to is instantiated twice, once for each leg. So it is possible for this to be called by the listener even if the class (and therefore single support of the proper leg) has not been called.

The thing is, this is probably redundant because I later ended up adding a check to make sure that we only proceed with the handleNewFootstep() method if we are on the proper side of the robot. So I can probably delete the logic that relies on haveWeEntered

james-p-foster · 2025-01-09T20:19:03Z

...LevelHumanoidControl/highLevelStates/walkingController/states/WalkingSingleSupportState.java

      if (feetManager.adjustHeightIfNeeded(nextFootstep))
      {
         walkingMessageHandler.updateVisualizationAfterFootstepAdjustement(nextFootstep);
         feetManager.adjustSwingTrajectory(swingSide, nextFootstep, swingTime);
      }

+      //


Why all the // in this file?

ah some are so I can more easily identify where I made changes, and some are for comments that I meant to write but never did. I'll fix both of those

…lace boolean

…oler-side variables for QFP functionality

rjgriffin42 · 2025-01-23T14:50:56Z

...src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPController.java

@@ -132,13 +136,15 @@ public class ICPController implements ICPControllerInterface
   private final ICPControllerHelper helper = new ICPControllerHelper();

   public ICPController(WalkingControllerParameters walkingControllerParameters,
+                        WalkingMessageHandler walkingMessageHandler,


I am not a fan of this change. The walking message handler is meant to be a high level thing, and the ICPController is a low level thing. I would much rather add a method to the ICPController that allows you to "override" the useCoPFeedback boolean, which is what you're doing, then have that set by a listener that you add, so that this is occuring at a high level, rather than embedded into the ICP Controller. It's a little more convoluted to write, but much more obvious than this object-oriented approach when it comes to tracing what's going on.

rjgriffin42 · 2025-01-23T14:52:13Z

...ava/us/ihmc/commonWalkingControlModules/capturePoint/controller/ICPControllerParameters.java

+   /**
+    * Enabling this boolean allows feedback control of CoP (feedback alpha < 1).
+    */
+   public boolean useCoPFeedback()


so if this is false, but useCMPFeedback is true, it will still move the base of the pendulum around, but will do so by moving the CMP. Is that intentional?

No, should I be turning that false as well?

rjgriffin42 · 2025-01-23T14:55:03Z

...n/java/us/ihmc/commonWalkingControlModules/capturePoint/LinearMomentumRateControlModule.java

@@ -171,7 +171,7 @@ public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox contro
           controllerToolbox.getYoGraphicsListRegistry());
   }

-   public LinearMomentumRateControlModule(CenterOfMassStateProvider centerOfMassStateProvider,
+   public LinearMomentumRateControlModule(HighLevelHumanoidControllerToolbox controllerToolbox,


The LinearMomentumRateControlModule was written in such a way that it could be removed from the control thread. By adding the controller toolbox here, that gets messed up. I think the right thing to do would be to add the method to ICPController, like I said, then add that override cop feedback command as a field to the momentum rate input, such that the field is getting set in the motion control stuff based on some listener that's attached to the walking message handler, and then sent to this control module and used. Then you don't have to add the control toolbox or walking message handler to this class.

Good call, will do

rjgriffin42 · 2025-01-23T14:56:26Z

...l-modules/src/main/java/us/ihmc/commonWalkingControlModules/capturePoint/BalanceManager.java

+         maxICPErrorBeforeSingleSupportX += 0.3;
+         maxICPErrorBeforeSingleSupportY += 0.3;
+      }
+
      normalizedICPError.set(MathTools.square(icpError2d.getX() / maxICPErrorBeforeSingleSupportX)
                             + MathTools.square(icpError2d.getY() / maxICPErrorBeforeSingleSupportY));


It would likely be cleaner to instead just set the normalizedICPError to zero in that if statement.

rjgriffin42 · 2025-01-23T14:57:53Z

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/scs2/SCS2AvatarSimulationFactory.java

@@ -460,7 +460,7 @@ private void setupStepGeneratorThread()
      else
      {
         JoystickBasedSteppingPluginFactory joystickPluginFactory = new JoystickBasedSteppingPluginFactory();
-         if (heightMapForFootstepZ.hasValue())
+         if (heightMapForFootstepZ.hasValue() && heightMapForFootstepZ.get() != null)


I don't think this captures how the height map will work on hardware. THe height map should be continuously streamed to the robot. Right now, the step generator will only take a single instance of the height map at construction. This is out of scope of this PR, but I'm pointing out that this won't work as intended on the robot.

sounds good, I'm guessing this isn't specific to the change I made, but an existing issue with this particular implementation right?

rjgriffin42 · 2025-01-23T14:59:43Z

...monWalkingControlModules/capturePoint/stepAdjustment/ErrorBasedStepAdjustmentController.java

   private final FramePoint3D vertexInWorld = new FramePoint3D();
   private final FrameConvexPolygon2D allowableAreaForCoPInFoot = new FrameConvexPolygon2D();

   public ErrorBasedStepAdjustmentController(WalkingControllerParameters walkingControllerParameters,
+                                             WalkingMessageHandler walkingMessageHandler,


This should also be passed in functionally, and not done through the walking message handler. In reality, in the state where you're calling the step adjustment, you should just be able to write an if block there, rather than turning it off inside the step adjustment controller.

Also I don't believe this class is actually used.

ok, so to clarify, basically do what you suggested for ICPController right?

rjgriffin42 · 2025-01-23T15:03:46Z

...c/commonWalkingControlModules/desiredFootStep/footstepGenerator/ContinuousStepGenerator.java


-         if (swingSide == RobotSide.LEFT)
+         if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue())


Can you add a FIXME note to delete this once you've done the rollout stuff inside the QFP?

rjgriffin42 · 2025-01-23T15:05:49Z

ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/FootstepDataMessage.msg

@@ -9,6 +9,9 @@ byte TRAJECTORY_TYPE_OBSTACLE_CLEARANCE = 1
 byte TRAJECTORY_TYPE_CUSTOM = 2
 byte TRAJECTORY_TYPE_WAYPOINTS = 3

+byte CSG_MODE_STANDARD = 0
+byte CSG_MODE_QFP = 1


Would it make more sense for all this stuff to go into the data list message, rather than the message of each individual footstep? It seems like this is either true for the entire list of steps, or its true for none of them.

With the current implementation, the FootstepDataList is a mix of QFP and standard (first one QFP, the rest are standard). I know we want to make the change such that multiple/all steps in the plan are computed with QFP, but I still think there could be instances perhaps when we switch between the two footstep providers, where the list will become mixed again. I'd rather leave this here because I feel it provides a more granular level of control over the timing of the parameter adjustments

rjgriffin42 · 2025-01-23T15:06:16Z

ihmc-interfaces/src/main/messages/ihmc_interfaces/controller_msgs/msg/FootstepDataMessage.msg

+bool should_check_for_reachability false
+
+# If the desired footstep is for walking in place with zero forward or lateral velocity


I think it would be good to make a note that this is only used if the csg_mode is QFP

rjgriffin42 · 2025-01-23T15:06:53Z

...toolkit/src/main/java/us/ihmc/robotics/math/trajectories/C1ContinuousTrajectorySmoother.java

-      trackingStiffness = new DoubleParameter(namePrefix + "TrackingStiffness", registry, 200.0);
-      trackingZeta = new DoubleParameter(namePrefix + "TrackingZeta", registry, 0.8);
+      trackingStiffness = new YoDouble(namePrefix + "TrackingStiffness", registry);
+      trackingStiffness.set(defaultTrackingStiffness);


I would assume the answer is yes, but I want to make sure that you checked all the parameter files to make sure this didn't change anything?

good call, are you referring to the xml param files? Which one(s) do you reckon these parameters would live in?

rjgriffin42 · 2025-01-23T15:07:00Z

...toolkit/src/main/java/us/ihmc/robotics/math/trajectories/C1ContinuousTrajectorySmoother.java


   private final MatrixExponentialCalculator matrixExponentialCalculator = new MatrixExponentialCalculator(2);
   private final DMatrixRMaj closedLoopStateMatrix = new DMatrixRMaj(2, 2);
   private final DMatrixRMaj scaledClosedLoopStateMatrix = new DMatrixRMaj(2, 2);
   private final DMatrixRMaj stateTransitionMatrix = new DMatrixRMaj(2, 2);

+   private final double defaultTrackingStiffness = 200.0;
+   private final double defaultTrackingZeta = 0.8;


can these be static?

rjgriffin42 · 2025-01-23T15:07:53Z

...n/java/us/ihmc/humanoidRobotics/communication/controllerAPI/command/FootstepDataCommand.java

@@ -28,6 +29,7 @@ public class FootstepDataCommand implements Command<FootstepDataCommand, Footste
   private long sequenceId;
   private RobotSide robotSide;
   private TrajectoryType trajectoryType = TrajectoryType.DEFAULT;
+   private ContinuousStepGeneratorMode csgMode = ContinuousStepGeneratorMode.STANDARD;


if they get moved in the message, they should be moved here, too.

what do you mean?

…ut argument to ICPController and others

…nstructor(s)

stefanfasano added 9 commits December 4, 2024 20:39

added necessary classes for QFP

5d0dccc

added QFP to CSG

ef60a8d

QFP steps added to CSG FootstepDataList

340f1ab

footsteps now continuously update throughout swing

92955ed

fixed stageable yovariable params. Did some tuning of double support …

33793fc

…fraction and pole

added ICP param to disable feedback CoP control

8341322

fixed turn walking bug by setting QFP footstep position and orientation

2b83ce2

added support for joystick control in QFP Sim

e89e446

fixed csg wrong support side bug

a09a8ae

stefanfasano requested review from rjgriffin42 and james-p-foster December 10, 2024 22:07

stefanfasano self-assigned this Dec 10, 2024

stefanfasano added 4 commits December 12, 2024 10:26

added boolean to not continously update touchdown location throughout…

022c0e7

… swing

yovariableized StepGeneratorCommandInputManager desired input variables

4b5cc4c

fixed bug in which footsteps weren't polled at right time causing sta…

3532006

…te machine to go to TO_STANDING

added visualizer of desired touchdown position from QFP

a846e1b

stefanfasano requested a review from Beomyeong December 13, 2024 16:55

Merge branch 'develop' into feature/quickster-footstep-provider

d1c11a7

stefanfasano marked this pull request as ready for review December 13, 2024 17:08

stefanfasano requested a review from christiandebuys December 13, 2024 17:12

switched to ALIP+DS model

5a0ee77

stefanfasano requested a review from PotatoPeeler3000 December 16, 2024 22:30

stefanfasano added 2 commits December 17, 2024 13:59

disabled continouous touchdown position update by default

c5bcb14

Merge branch 'develop' into feature/quickster-footstep-provider

ad74d77

christiandebuys reviewed Dec 18, 2024

View reviewed changes

stefanfasano added 3 commits December 18, 2024 14:56

fixed control frame yaw discontinuity bug

c0e9d4e

some parameter tuning for hardware

b7dc596

Merge branch 'develop' into feature/quickster-footstep-provider

e9f6dec

Beomyeong approved these changes Jan 6, 2025

View reviewed changes

Merge branch 'develop' into feature/quickster-footstep-provider

acef97e

added acp viz

5647e3c

james-p-foster approved these changes Jan 9, 2025

View reviewed changes

stefanfasano added 4 commits January 21, 2025 16:02

Merge branch 'develop' into feature/quickster-footstep-provider

55b6e9f

added master toggle switch for all variables that make QFP work

e56cd60

added new fields to FootstepDataMessage for csg mode and isWalkingInP…

9344161

…lace boolean

utilized new FootstepDataMessage fields to toggle all necessary contr…

073f8bd

…oler-side variables for QFP functionality

rjgriffin42 reviewed Jan 23, 2025

View reviewed changes

stefanfasano added 6 commits January 23, 2025 18:37

addressed PR comments regarding removing WalkingMessageHandler as inp…

e9c44c9

…ut argument to ICPController and others

removed WalkingMessageHandler as input to stepAdjustmentController co…

9b20b2a

…nstructor(s)

added extra documentation to new message field in FootstepDataMessage

26631ff

cleaned up some logic. made some variables static. added a FIXME

09e51ef

Merge branch 'develop' into feature/quickster-footstep-provider

bbe87ef

added null check to walkInputProvider

b2ed0f0

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Feature/quickster footstep provider #527

Feature/quickster footstep provider #527

stefanfasano commented Dec 10, 2024 •

edited

Loading

christiandebuys Dec 18, 2024

stefanfasano Dec 18, 2024

james-p-foster Jan 9, 2025

stefanfasano Jan 9, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

stefanfasano Jan 23, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

stefanfasano Jan 23, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

stefanfasano Jan 23, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

stefanfasano Jan 24, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 24, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 24, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025

stefanfasano Jan 24, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 24, 2025

rjgriffin42 Jan 23, 2025

stefanfasano Jan 23, 2025


		if (swingSide == RobotSide.LEFT)
		if (currentCSGMode.getEnumValue() == ContinuousStepGeneratorMode.QFP && quicksterFootstepProvider.hasValue())

		bool should_check_for_reachability false

		# If the desired footstep is for walking in place with zero forward or lateral velocity

Feature/quickster footstep provider #527

Are you sure you want to change the base?

Feature/quickster footstep provider #527

Conversation

stefanfasano commented Dec 10, 2024 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

stefanfasano commented Dec 10, 2024 •

edited

Loading