Feature/separate csg values (#1103)

* changed csg status to publish current velocity commands in velocity units

* made dedicated class for ROS2 step generator communication help. integrated that into CSG xbox and CSG rdx panel

* added setup for different forward/backwards max step lengths

* made it so xbox and rdx csg panel don't publish by default all the time. also made velocity commands in csg panel in m/s

* fixed some bugs with csg panel

* added swing height control to rdx csg panel

* added a separate set of stepping parameters for step generation vs control

* did some cleanup of the stepping parameters calls

* switched some of the parameters

---------

Co-authored-by: Stefan Fasano <sfasano@ihmc.org>
Co-authored-by: Alexander OCU <rosie@ihmc.org>

Author: 3 people

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/ContinuousStepController.java

@@ -81,9 +81,13 @@ public class ContinuousStepController
 
    public ContinuousStepController(WalkingControllerParameters walkingControllerParameters)
    {
-      steppingParameters = walkingControllerParameters.getSteppingParameters();
+      steppingParameters = walkingControllerParameters.getSteppingParametersForStepGeneration();
 
-      snapAndWiggleParameters.setFootLength(walkingControllerParameters.getSteppingParameters().getFootLength());
+      double footLength = steppingParameters.getFootLength();
+      double toeWidth = steppingParameters.getToeWidth();
+      double footWidth = steppingParameters.getFootWidth();
+
+      snapAndWiggleParameters.setFootLength(footLength);
       snapAndWiggleSingleStep = new SnapAndWiggleSingleStep(snapAndWiggleParameters);
 
       continuousStepGenerator.setNumberOfTicksBeforeSubmittingFootsteps(0);
@@ -124,10 +128,7 @@ public boolean isUnitVelocity()
       continuousStepGenerator.addFootstepValidityIndicator(this::isSafeDistanceFromObstacle);
       continuousStepGenerator.addFootstepValidityIndicator(this::isSafeStepHeight);
 
-      SteppingParameters steppingParameters = walkingControllerParameters.getSteppingParameters();
-      double footLength = steppingParameters.getFootLength();
-      double toeWidth = steppingParameters.getToeWidth();
-      double footWidth = steppingParameters.getFootWidth();
+
       ConvexPolygon2D footPolygon = new ConvexPolygon2D();
       footPolygon.addVertex(footLength / 2.0, toeWidth / 2.0);
       footPolygon.addVertex(footLength / 2.0, -toeWidth / 2.0);

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/JoystickStepParametersProperty.java

@@ -121,7 +121,7 @@ public void set(WalkingControllerParameters walkingControllerParameters)
          swingDuration = walkingControllerParameters.getDefaultSwingTime();
          transferDuration = walkingControllerParameters.getDefaultTransferTime();
 
-         SteppingParameters steppingParameters = walkingControllerParameters.getSteppingParameters();
+         SteppingParameters steppingParameters = walkingControllerParameters.getSteppingParametersForStepGeneration();
          swingHeight = walkingControllerParameters.getSwingTrajectoryParameters().getMinSwingHeight();
          maxStepLength = steppingParameters.getMaxStepLength();
          defaultStepWidth = steppingParameters.getInPlaceWidth();

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/joystickBasedJavaFXController/XBoxOneCSGPlugin.java

@@ -205,7 +205,7 @@ private void configureCSGParameters(ContinuousStepGeneratorParametersMessage csg
       csgParametersCommand.setSwingDuration(walkingControllerParameters.getDefaultSwingTime());
       csgParametersCommand.setTransferDuration(walkingControllerParameters.getDefaultTransferTime());
 
-      SteppingParameters steppingParameters = walkingControllerParameters.getSteppingParameters();
+      SteppingParameters steppingParameters = walkingControllerParameters.getSteppingParametersForStepGeneration();
       csgParametersCommand.setSwingHeight(walkingControllerParameters.getSwingTrajectoryParameters().getDefaultSwingHeight());
       csgParametersCommand.setMaxStepLengthForwards(steppingParameters.getMaxStepLength());
       csgParametersCommand.setMaxStepLengthBackwards(steppingParameters.getMaxBackwardStepLength());

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/directionalControlToolboxModule/DirectionalControlController.java

@@ -29,7 +29,6 @@
 import us.ihmc.euclid.geometry.interfaces.ConvexPolygon2DReadOnly;
 import us.ihmc.euclid.referenceFrame.FramePose3D;
 import us.ihmc.euclid.referenceFrame.ReferenceFrame;
-import us.ihmc.euclid.referenceFrame.interfaces.FixedFramePose3DBasics;
 import us.ihmc.euclid.referenceFrame.interfaces.FramePose2DReadOnly;
 import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly;
 import us.ihmc.euclid.transform.RigidBodyTransform;
@@ -198,7 +197,7 @@ public DirectionalControlController(FullHumanoidRobotModel robotModel,
       this.footPolygons = getFootPolygons(walkingControllerParameters);
       this.robotUpdater = new RobotModelUpdater(robotModel);
 
-      steppingParameters = walkingControllerParameters.getSteppingParameters();
+      steppingParameters = walkingControllerParameters.getSteppingParametersForStepGeneration();
       stepTime = () -> getSwingDuration() + getTransferDuration();
 
       controlParameters = new JoystickStepParameters(walkingControllerParameters);
@@ -210,7 +209,7 @@ public DirectionalControlController(FullHumanoidRobotModel robotModel,
       controlParameters = userProfileManager.loadProfile("default");
       stepParametersReference = new AtomicReference<JoystickStepParameters>(controlParameters);
 
-      snapAndWiggleParameters.setFootLength(walkingControllerParameters.getSteppingParameters().getFootLength());
+      snapAndWiggleParameters.setFootLength(steppingParameters.getFootLength());
       snapAndWiggleSingleStep = new SnapAndWiggleSingleStep(snapAndWiggleParameters);
 
       continuousStepGenerator.setNumberOfFootstepsToPlan(10);
@@ -399,7 +398,6 @@ public void run()
     */
    private SideDependentList<ConvexPolygon2D> getFootPolygons(WalkingControllerParameters walkingControllerParameters)
    {
-      SteppingParameters steppingParameters = walkingControllerParameters.getSteppingParameters();
       double footLength = steppingParameters.getFootLength();
       double footWidth = steppingParameters.getFootWidth();
       ConvexPolygon2D footPolygon = new ConvexPolygon2D();

ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/networkProcessor/stepConstraintToolboxModule/StepConstraintToolboxController.java

@@ -570,7 +570,7 @@ private void setupStepGeneratorThread()
                                                                                                 .getContactPointParameters(),
                                                                                       robotModel.get()
                                                                                                 .getWalkingControllerParameters()
-                                                                                                .getSteppingParameters());
+                                                                                                .getSteppingParametersForStepGeneration());
             stepSnapperUpdatable.setSnapToHeightMap(true);
          }