Skip to content

Commit 5467a96

Browse files
rjgriffin42PotatoPeeler3000TomaszTBAlexander OCU
authored
added the special case for the stairs (#1109)
* Bugfix/fix pelvis rotation on delays (#1106) * started working on a fix for pelvis rotationd elays * rolled back on how the time in the phase is found, and started adding a decay factor so the feedforward values dont go to the moon * fixed a bound inclusiveness problem * added some changes to prefer previous reachable regions to yield more… (#1105) * added some changes to prefer previous reachable regions to yield more consistent, smoother results * reverted line change --------- Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * remove yaw component from angular velocity filtering of the foot (#1107) Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * add a filter for big downward forces on the second threshold (#1108) Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * [Feature] CSG Heartbeat (#1104) * Make ROS2HeartbeatMonitor garbage free * Add heartbeats to CSG objects * Added destroy method to CSGROS2CommunicationHelper and cleaned up usage in the panel --------- Co-authored-by: Alexander OCU <rosie@ihmc.org> Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> * added the special case for the stairs * Add null check to ImageSensor * Bugfix/fix capture region and rate limit (#1110) * fixed the reachability constraint return * properly reset the feedback conditions for the ICP feedback * Create variables for hard coded values so its easier to understand * upped height for going up cinder blocks * cleaned up variable definition in look and step * added a comment --------- Co-authored-by: Nick Kitchel <59104880+PotatoPeeler3000@users.noreply.github.com> Co-authored-by: Tomasz Bialek <103042423+TomaszTB@users.noreply.github.com> Co-authored-by: Alexander OCU <rosie@ihmc.org> Co-authored-by: nkitchel <nkitchel@ihmc.org>
1 parent 8ba7628 commit 5467a96

File tree

7 files changed

+165
-81
lines changed

7 files changed

+165
-81
lines changed

‎ihmc-avatar-interfaces/src/test/java/us/ihmc/avatar/roughTerrainWalking/HumanoidEndToEndStairsTest.java‎

Lines changed: 91 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -85,7 +85,8 @@ public void setUseExperimentalPhysicsEngine(boolean useExperimentalPhysicsEngine
8585
this.useExperimentalPhysicsEngine = useExperimentalPhysicsEngine;
8686
}
8787

88-
public void testStairs(TestInfo testInfo, boolean squareUpSteps, boolean goingUp, double swingDuration, double transferDuration, double heightOffset) throws Exception
88+
public void testStairs(TestInfo testInfo, boolean squareUpSteps, boolean goingUp, double swingDuration, double transferDuration, double heightOffset)
89+
throws Exception
8990
{
9091
testStairs(testInfo, squareUpSteps, goingUp, swingDuration, transferDuration, heightOffset, null);
9192
}
@@ -96,16 +97,17 @@ public void testStairs(TestInfo testInfo,
9697
double swingDuration,
9798
double transferDuration,
9899
double heightOffset,
99-
Consumer<FootstepDataListMessage> corruptor)
100-
throws Exception
100+
Consumer<FootstepDataListMessage> corruptor) throws Exception
101101
{
102102
DRCRobotModel robotModel = getRobotModel();
103103
double actualFootLength = robotModel.getWalkingControllerParameters().getSteppingParameters().getActualFootLength();
104104
double startX = goingUp ? 0.0 : 1.2 + numberOfSteps * stepLength + 0.3;
105105
double startZ = goingUp ? 0.0 : numberOfSteps * stepHeight;
106106

107107
StairsEnvironment environment = new StairsEnvironment(numberOfSteps, stepHeight, stepLength, true);
108-
SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel, environment, simulationTestingParameters);
108+
SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel,
109+
environment,
110+
simulationTestingParameters);
109111
simulationTestHelperFactory.setStartingLocationOffset(new OffsetAndYawRobotInitialSetup(startX, 0, startZ));
110112
simulationTestHelperFactory.setUseImpulseBasedPhysicsEngine(useExperimentalPhysicsEngine);
111113
simulationTestHelper = simulationTestHelperFactory.createAvatarTestingSimulation();
@@ -138,6 +140,91 @@ public void testStairs(TestInfo testInfo,
138140
}
139141
}
140142

143+
/**
144+
* This tests a set of stairs that require the robot square up on each step, and are also exactly one 9" cinder block high.
145+
*/
146+
public void testSpecialStairs(boolean goingUp,
147+
double swingDuration,
148+
double transferDuration) throws Exception
149+
{
150+
DRCRobotModel robotModel = getRobotModel();
151+
double startX = 0.4;
152+
double startZ = goingUp ? 0.0 : numberOfSteps * stepHeight;
153+
154+
// 9" high step
155+
double stepLength = 0.3;
156+
double stepHeight = 9 * 2.54 / 100.0;
157+
double stanceWidth = 0.22;
158+
159+
StairsEnvironment environment = new StairsEnvironment(2, stepHeight, stepLength, true);
160+
SCS2AvatarTestingSimulationFactory simulationTestHelperFactory = SCS2AvatarTestingSimulationFactory.createDefaultTestSimulationFactory(robotModel,
161+
environment,
162+
simulationTestingParameters);
163+
simulationTestHelperFactory.setStartingLocationOffset(new OffsetAndYawRobotInitialSetup(startX, 0, startZ));
164+
simulationTestHelperFactory.setUseImpulseBasedPhysicsEngine(useExperimentalPhysicsEngine);
165+
simulationTestHelper = simulationTestHelperFactory.createAvatarTestingSimulation();
166+
simulationTestHelper.start();
167+
168+
simulationTestHelper.setCameraFocusPosition(startX, 0.0, 0.8 + startZ);
169+
simulationTestHelper.setCameraPosition(startX, -5.0, 0.8 + startZ);
170+
171+
assertTrue(simulationTestHelper.simulateNow(1.0));
172+
173+
FootstepDataListMessage firstStepUpList = new FootstepDataListMessage();
174+
FootstepDataMessage firstStepUp = firstStepUpList.getFootstepDataList().add();
175+
firstStepUp.setRobotSide(RobotSide.LEFT.toByte());
176+
firstStepUp.getLocation().setX(startX + stepLength);
177+
firstStepUp.getLocation().setY(stanceWidth / 2.0);
178+
firstStepUp.getLocation().setZ(stepHeight);
179+
firstStepUp.setSwingDuration(swingDuration);
180+
firstStepUp.setTransferDuration(transferDuration);
181+
182+
publishFootstepsAndSimulate(robotModel, firstStepUpList);
183+
184+
assertTrue(simulationTestHelper.simulateNow(1.0));
185+
186+
FootstepDataListMessage firstSquareUpList = new FootstepDataListMessage();
187+
FootstepDataMessage firstSquareUp = firstSquareUpList.getFootstepDataList().add();
188+
firstSquareUp.setRobotSide(RobotSide.RIGHT.toByte());
189+
firstSquareUp.getLocation().setX(startX + stepLength);
190+
firstSquareUp.getLocation().setY(-stanceWidth / 2.0);
191+
firstSquareUp.getLocation().setZ(stepHeight);
192+
firstSquareUp.setSwingDuration(swingDuration);
193+
firstSquareUp.setTransferDuration(transferDuration);
194+
195+
publishFootstepsAndSimulate(robotModel, firstSquareUpList);
196+
197+
assertTrue(simulationTestHelper.simulateNow(1.0));
198+
199+
FootstepDataListMessage secondStepUpList = new FootstepDataListMessage();
200+
FootstepDataMessage secondStepUp = secondStepUpList.getFootstepDataList().add();
201+
secondStepUp.setRobotSide(RobotSide.LEFT.toByte());
202+
secondStepUp.getLocation().setX(2.0 * startX + stepLength);
203+
secondStepUp.getLocation().setY(stanceWidth / 2.0);
204+
secondStepUp.getLocation().setZ(2.0 * stepHeight);
205+
secondStepUp.setSwingDuration(swingDuration);
206+
secondStepUp.setTransferDuration(transferDuration);
207+
208+
publishFootstepsAndSimulate(robotModel, secondStepUpList);
209+
210+
assertTrue(simulationTestHelper.simulateNow(1.0));
211+
212+
213+
FootstepDataListMessage secondSquareUpList = new FootstepDataListMessage();
214+
FootstepDataMessage secondSquareUp = secondSquareUpList.getFootstepDataList().add();
215+
secondSquareUp.setRobotSide(RobotSide.RIGHT.toByte());
216+
secondSquareUp.getLocation().setX(2.0 * startX + stepLength);
217+
secondSquareUp.getLocation().setY(-stanceWidth / 2.0);
218+
secondSquareUp.getLocation().setZ(2.0 * stepHeight);
219+
secondSquareUp.setSwingDuration(swingDuration);
220+
secondSquareUp.setTransferDuration(transferDuration);
221+
222+
publishFootstepsAndSimulate(robotModel, secondSquareUpList);
223+
224+
assertTrue(simulationTestHelper.simulateNow(1.0));
225+
226+
}
227+
141228
private void publishHeightOffset(double heightOffset)
142229
{
143230
if (!Double.isFinite(heightOffset) || EuclidCoreTools.epsilonEquals(0.0, heightOffset, 1.0e-3))

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/heightPlanning/LookAheadCoMHeightTrajectoryGenerator.java‎

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,9 @@
4444
public class LookAheadCoMHeightTrajectoryGenerator implements SCS2YoGraphicHolder
4545
{
4646
private static final double defaultPercentageInOffset = 0.05;
47+
private static final double THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP = 0.1;
48+
private static final double THRESHOLD_FOR_STEP_UP_HEIGHT = 0.05;
49+
4750
private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
4851

4952
private boolean visualize = true;
@@ -422,11 +425,13 @@ private void computeWaypoints(FramePoint3DReadOnly startCoMPosition,
422425
fourthMidpoint.setXY(fourthMidpointX, fourthMidpointY);
423426
endWaypoint.setXY(endWaypointX, endWaypointY);
424427

425-
// Handle the case where the feet are basically squared up when stepping up. In that case, the max height needs to be pinned by the "from" foot, as the
428+
// Handle the case where the feet are basically squared up when stepping up, but are at different heights In that case, the max height needs to be pinned
429+
// by the "from" foot, as the
426430
// way the phase variable works means we can approach what is meant to be swing on the "to" foot while still in the "from" foot.
427-
if (Math.abs(transferToPosition.getX()) < 0.1 && endGroundHeight > startGroundHeight)
431+
if (Math.abs(transferToPosition.getX()) < THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP && endGroundHeight > startGroundHeight
432+
&& transferToPosition.getZ() > transferFromPosition.getZ() + THRESHOLD_FOR_STEP_UP_HEIGHT)
428433
{
429-
double blend = Math.abs(transferToPosition.getX()) / 0.1;
434+
double blend = Math.abs(transferToPosition.getX()) / THRESHOLD_FOR_DETECTING_AN_IN_PLACE_STEP;
430435
double minHeight = Math.min(startGroundHeight + extraToeOffHeight, endGroundHeight);
431436
endGroundHeight = EuclidCoreTools.interpolate(minHeight, endGroundHeight, blend);
432437
}

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/StandingState.java‎

Lines changed: 31 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
import us.ihmc.commonWalkingControlModules.messageHandlers.WalkingMessageHandler;
1313
import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox;
1414
import us.ihmc.communication.controllerAPI.CommandInputManager;
15+
import us.ihmc.humanoidRobotics.footstep.Footstep;
1516
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics;
1617
import us.ihmc.robotModels.FullHumanoidRobotModel;
1718
import us.ihmc.robotics.robotSide.RobotSide;
@@ -34,6 +35,8 @@ public class StandingState extends WalkingState
3435
private final SideDependentList<RigidBodyControlManager> handManagers = new SideDependentList<>();
3536
private final FeetManager feetManager;
3637

38+
private RobotSide supportingSide;
39+
3740
public StandingState(CommandInputManager commandInputManager,
3841
WalkingMessageHandler walkingMessageHandler,
3942
TouchdownErrorCompensator touchdownErrorCompensator,
@@ -74,7 +77,7 @@ public StandingState(CommandInputManager commandInputManager,
7477
public void doAction(double timeInState)
7578
{
7679
if (!holdDesiredHeightConstantWhenStanding)
77-
comHeightManager.setSupportLeg(RobotSide.LEFT);
80+
comHeightManager.setSupportLeg(supportingSide);
7881
balanceManager.computeICPPlan();
7982
controllerToolbox.getWalkingTrajectoryPath().updateTrajectory(feetManager.getCurrentConstraintType(RobotSide.LEFT),
8083
feetManager.getCurrentConstraintType(RobotSide.RIGHT),
@@ -99,10 +102,17 @@ public void onEntry()
99102
if (holdDesiredHeightConstantWhenStanding)
100103
{
101104
comHeightManager.initializeToNominalDesiredHeight();
105+
supportingSide = RobotSide.LEFT;
102106
}
103107
else
104108
{
105-
TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(RobotSide.RIGHT);
109+
Footstep footstepLeft = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.LEFT);
110+
Footstep footstepRight = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.RIGHT);
111+
112+
supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight);
113+
supportingSide = supportingSide == null ? RobotSide.LEFT : supportingSide;
114+
115+
TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide.getOppositeSide());
106116
comHeightManager.initialize(transferToAndNextFootstepsDataForDoubleSupport, 0.0);
107117
}
108118

@@ -151,4 +161,23 @@ public boolean isDone(double timeInState)
151161
{
152162
return true;
153163
}
164+
165+
private RobotSide getSideCarryingMostWeight(Footstep leftFootstep, Footstep rightFootstep)
166+
{
167+
WalkingStateEnum previousWalkingState = getPreviousWalkingStateEnum();
168+
if (previousWalkingState == null)
169+
return null;
170+
171+
RobotSide mostSupportingSide = null;
172+
boolean leftStepLower = leftFootstep.getZ() <= rightFootstep.getZ();
173+
boolean rightStepLower = leftFootstep.getZ() > rightFootstep.getZ();
174+
if (leftStepLower)
175+
mostSupportingSide = RobotSide.LEFT;
176+
else if (rightStepLower)
177+
mostSupportingSide = RobotSide.RIGHT;
178+
else if (previousWalkingState.getTransferToSide() != null)
179+
mostSupportingSide = previousWalkingState.getTransferToSide().getOppositeSide();
180+
181+
return mostSupportingSide;
182+
}
154183
}

‎ihmc-common-walking-control-modules/src/main/java/us/ihmc/commonWalkingControlModules/highLevelHumanoidControl/highLevelStates/walkingController/states/TransferToStandingState.java‎

Lines changed: 5 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
import us.ihmc.commonWalkingControlModules.momentumBasedController.HighLevelHumanoidControllerToolbox;
1212
import us.ihmc.commonWalkingControlModules.referenceFrames.WalkingTrajectoryPath;
1313
import us.ihmc.euclid.referenceFrame.FramePoint2D;
14+
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
1415
import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
1516
import us.ihmc.euclid.tuple3D.Point3D;
1617
import us.ihmc.humanoidRobotics.footstep.Footstep;
@@ -33,6 +34,7 @@ public class TransferToStandingState extends WalkingState
3334
private final FeetManager feetManager;
3435

3536
private final Point3D midFootPosition = new Point3D();
37+
private RobotSide supportingSide;
3638

3739
public TransferToStandingState(WalkingMessageHandler walkingMessageHandler,
3840
TouchdownErrorCompensator touchdownErrorCompensator,
@@ -65,7 +67,7 @@ public void doAction(double timeInState)
6567
switchToPointToeOffIfAlreadyInLine();
6668

6769
// Always do this so that when a foot slips or is loaded in the air, the height gets adjusted.
68-
comHeightManager.setSupportLeg(RobotSide.LEFT);
70+
comHeightManager.setSupportLeg(supportingSide);
6971
}
7072

7173
private final FramePoint2D desiredCoP = new FramePoint2D();
@@ -171,14 +173,14 @@ else if (previousStateEnum.getTransferToSide() != null)
171173

172174
Footstep footstepLeft = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.LEFT);
173175
Footstep footstepRight = walkingMessageHandler.getFootstepAtCurrentLocation(RobotSide.RIGHT);
174-
RobotSide supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight);
176+
supportingSide = getSideCarryingMostWeight(footstepLeft, footstepRight);
175177
supportingSide = supportingSide == null ? RobotSide.RIGHT : supportingSide;
176178

177179
double extraToeOffHeight = 0.0;
178180
if (feetManager.getCurrentConstraintType(supportingSide.getOppositeSide()) == FootControlModule.ConstraintType.TOES)
179181
extraToeOffHeight = feetManager.getExtraCoMMaxHeightWithToes();
180182

181-
TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide);
183+
TransferToAndNextFootstepsData transferToAndNextFootstepsDataForDoubleSupport = walkingMessageHandler.createTransferToAndNextFootstepDataForDoubleSupport(supportingSide.getOppositeSide());
182184
comHeightManager.setSupportLeg(supportingSide);
183185
comHeightManager.initialize(transferToAndNextFootstepsDataForDoubleSupport, extraToeOffHeight);
184186

‎open-alexander/src/test/java/us/ihmc/openAlexander/controllerAPI/AlexanderEndToEndStairsTest.java‎

Lines changed: 0 additions & 68 deletions
This file was deleted.

‎open-alexander/src/test/java/us/ihmc/openAlexander/obstacleCourseTests/AlexanderCustomSteppingStonesTest.java‎

Lines changed: 23 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,11 +3,13 @@
33
import org.junit.jupiter.api.Tag;
44
import org.junit.jupiter.api.Test;
55
import org.junit.jupiter.api.TestInfo;
6+
import us.ihmc.commonWalkingControlModules.configurations.WalkingControllerParameters;
67
import us.ihmc.openAlexander.OpenAlexanderVersion;
78
import us.ihmc.openAlexander.OpenAlexanderRobotModel;
89
import us.ihmc.avatar.drcRobot.DRCRobotModel;
910
import us.ihmc.avatar.drcRobot.RobotTarget;
1011
import us.ihmc.avatar.obstacleCourseTests.AvatarCustomSteppingStonesTest;
12+
import us.ihmc.openAlexander.parameters.controller.OpenAlexanderWalkingControllerParameters;
1113
import us.ihmc.simulationConstructionSetTools.tools.CITools;
1214

1315
public class AlexanderCustomSteppingStonesTest extends AvatarCustomSteppingStonesTest
@@ -66,7 +68,27 @@ public double getStepLength()
6668
@Override
6769
public DRCRobotModel getRobotModel()
6870
{
69-
return new OpenAlexanderRobotModel(OpenAlexanderVersion.V1_FULL_ROBOT, RobotTarget.SCS);
71+
return new OpenAlexanderRobotModel(OpenAlexanderVersion.V1_FULL_ROBOT, RobotTarget.SCS)
72+
{
73+
@Override
74+
public WalkingControllerParameters getWalkingControllerParameters()
75+
{
76+
return new OpenAlexanderWalkingControllerParameters(getRobotVersion(), getTarget(), getJointMap(), getPhysicalProperties())
77+
{
78+
@Override
79+
public double nominalHeightAboveAnkle()
80+
{
81+
return 0.91;
82+
}
83+
84+
@Override
85+
public double maximumHeightAboveAnkle()
86+
{
87+
return 0.93;
88+
}
89+
};
90+
}
91+
};
7092
}
7193

7294
@Override

‎open-alexander/src/test/java/us/ihmc/openAlexander/roughTerrainWalking/AlexanderEndToEndStairsTest.java‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -42,4 +42,11 @@ public void testDownStairs(TestInfo testInfo) throws Exception
4242
{
4343
testStairs(testInfo, false, false, 1.0, 0.35, 0.0);
4444
}
45+
46+
@Test
47+
public void testSpecialStairs() throws Exception
48+
{
49+
testSpecialStairs(true, 0.8, 0.5);
50+
}
51+
4552
}

0 commit comments

Comments
 (0)