Skip to content

Commit 36b2197

Browse files
author
Tim Seyde
committed
Added behavior to test fixed number of steps forward walk on Atlas (comparison new ICP planner w/ and w/o angular momentum)
1 parent 763b0d6 commit 36b2197

File tree

6 files changed

+230
-3
lines changed

6 files changed

+230
-3
lines changed

‎CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/dynamicReachability/SmoothCoMIntegrationToolbox.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,7 @@
1515

1616
public class SmoothCoMIntegrationToolbox
1717
{
18-
private static final int defaultSize = 1000;
18+
private static final int defaultSize = 100;
1919

2020
private final DenseMatrix64F tPowersDerivativeVector = new DenseMatrix64F(defaultSize, 1);
2121
private final DenseMatrix64F tPowersDerivativeVectorTranspose = new DenseMatrix64F(defaultSize, 1);

‎CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/instantaneousCapturePoint/smoothCMP/SmoothCMPBasedICPPlanner.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -89,7 +89,7 @@ public SmoothCMPBasedICPPlanner(FullHumanoidRobotModel fullRobotModel, BipedSupp
8989

9090
public void initializeParameters(ICPPlannerParameters icpPlannerParameters)
9191
{
92-
initializeParameters(icpPlannerParameters, false);
92+
initializeParameters(icpPlannerParameters, true);
9393
}
9494

9595
public void initializeParameters(ICPPlannerParameters icpPlannerParameters, boolean computePredictedAngularMomentum)

‎CommonWalkingControlModules/src/us/ihmc/commonWalkingControlModules/instantaneousCapturePoint/smoothICPGenerator/SmoothCapturePointToolbox.java‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,7 @@ public class SmoothCapturePointToolbox
2020
{
2121
// private static final ThreadLocal<DenseMatrix64F> dummyName = new ThreadLocal<>();
2222

23-
private static final int defaultSize = 1000;
23+
private static final int defaultSize = 100;
2424

2525
private final DenseMatrix64F tPowersDerivativeVector = new DenseMatrix64F(defaultSize, 1);
2626
private final DenseMatrix64F tPowersDerivativeVectorTranspose = new DenseMatrix64F(defaultSize, 1);

‎IHMCHumanoidBehaviors/src/us/ihmc/humanoidBehaviors/IHMCHumanoidBehaviorManager.java‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@
2020
import us.ihmc.humanoidBehaviors.behaviors.complexBehaviors.WalkToGoalBehavior;
2121
import us.ihmc.humanoidBehaviors.behaviors.debug.PartialFootholdBehavior;
2222
import us.ihmc.humanoidBehaviors.behaviors.debug.TestICPOptimizationBehavior;
23+
import us.ihmc.humanoidBehaviors.behaviors.debug.TestSmoothICPPlannerBehavior;
2324
import us.ihmc.humanoidBehaviors.behaviors.diagnostic.DiagnosticBehavior;
2425
import us.ihmc.humanoidBehaviors.behaviors.examples.ExampleComplexBehaviorStateMachine;
2526
import us.ihmc.humanoidBehaviors.behaviors.fiducialLocation.FollowFiducialBehavior;
@@ -235,6 +236,9 @@ private void createAndRegisterBehaviors(BehaviorDispatcher<HumanoidBehaviorType>
235236

236237
dispatcher.addBehavior(HumanoidBehaviorType.TEST_ICP_OPTIMIZATION,
237238
new TestICPOptimizationBehavior(behaviorCommunicationBridge, referenceFrames, yoTime));
239+
240+
dispatcher.addBehavior(HumanoidBehaviorType.TEST_SMOOTH_ICP_PLANNER,
241+
new TestSmoothICPPlannerBehavior(behaviorCommunicationBridge, yoTime, yoDoubleSupport, fullRobotModel, referenceFrames, wholeBodyControllerParameters, atlasPrimitiveActions));
238242

239243
dispatcher.addBehavior(HumanoidBehaviorType.PUSH_AND_WALK,
240244
new PushAndWalkBehavior(behaviorCommunicationBridge, referenceFrames, fullRobotModel, walkingControllerParameters, yoGraphicsListRegistry));

‎IHMCHumanoidBehaviors/src/us/ihmc/humanoidBehaviors/behaviors/debug/TestSmoothICPPlannerBehavior.java‎

Lines changed: 222 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,222 @@
1+
package us.ihmc.humanoidBehaviors.behaviors.debug;
2+
3+
import us.ihmc.communication.packets.TextToSpeechPacket;
4+
import us.ihmc.euclid.referenceFrame.FramePoint3D;
5+
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
6+
import us.ihmc.euclid.tuple3D.Point3D;
7+
import us.ihmc.euclid.tuple4D.Quaternion;
8+
import us.ihmc.humanoidBehaviors.behaviors.complexBehaviors.ResetRobotBehavior;
9+
import us.ihmc.humanoidBehaviors.behaviors.debug.TestSmoothICPPlannerBehavior.TestSmoothICPPlannerBehaviorState;
10+
import us.ihmc.humanoidBehaviors.behaviors.primitives.AtlasPrimitiveActions;
11+
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.BehaviorAction;
12+
import us.ihmc.humanoidBehaviors.behaviors.simpleBehaviors.SimpleDoNothingBehavior;
13+
import us.ihmc.humanoidBehaviors.communication.CommunicationBridge;
14+
import us.ihmc.humanoidBehaviors.stateMachine.StateMachineBehavior;
15+
import us.ihmc.humanoidRobotics.communication.packets.dataobjects.HandConfiguration;
16+
import us.ihmc.humanoidRobotics.communication.packets.manipulation.ArmTrajectoryMessage;
17+
import us.ihmc.humanoidRobotics.communication.packets.manipulation.HandDesiredConfigurationMessage;
18+
import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataListMessage;
19+
import us.ihmc.humanoidRobotics.communication.packets.walking.FootstepDataMessage;
20+
import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames;
21+
import us.ihmc.robotModels.FullHumanoidRobotModel;
22+
import us.ihmc.robotics.geometry.FrameOrientation;
23+
import us.ihmc.robotics.geometry.FramePose;
24+
import us.ihmc.robotics.robotSide.RobotSide;
25+
import us.ihmc.wholeBodyController.WholeBodyControllerParameters;
26+
import us.ihmc.yoVariables.variable.YoBoolean;
27+
import us.ihmc.yoVariables.variable.YoDouble;
28+
import us.ihmc.yoVariables.variable.YoInteger;
29+
30+
public class TestSmoothICPPlannerBehavior extends StateMachineBehavior<TestSmoothICPPlannerBehaviorState>
31+
{
32+
public enum TestSmoothICPPlannerBehaviorState
33+
{
34+
STOPPED,
35+
SETUP_ROBOT,
36+
CONFIRM_WALK,
37+
WALK_FORWARD,
38+
RESET_ROBOT,
39+
DONE
40+
}
41+
42+
private final ResetRobotBehavior resetRobotBehavior;
43+
44+
private final AtlasPrimitiveActions atlasPrimitiveActions;
45+
46+
private final HumanoidReferenceFrames referenceFrames;
47+
48+
// YoVariables
49+
private final YoInteger numberOfSteps;
50+
private final YoDouble stepLength;
51+
private final YoDouble stepWidth;
52+
private final YoDouble swingTime;
53+
private final YoDouble transferTime;
54+
private final YoDouble initialTransferTime;
55+
private final YoDouble finalTransferTime;
56+
57+
// Default parameters
58+
private final int defaultNumberOfSteps = 6;
59+
private final double defaultStepLength = 0.5;
60+
private final double defaultStepWidth = 0.25;
61+
private final double defaultSwingTime = 1.0;
62+
private final double defaultTransferTime = 0.4;
63+
private final double defaultInitialTransferTime = 1.0;
64+
private final double defaultFinalTransferTime = 1.0;
65+
66+
private RobotSide side = RobotSide.LEFT;
67+
68+
public TestSmoothICPPlannerBehavior(CommunicationBridge communicationBridge, YoDouble yoTime, YoBoolean yoDoubleSupport,
69+
FullHumanoidRobotModel fullRobotModel, HumanoidReferenceFrames referenceFrames, WholeBodyControllerParameters wholeBodyControllerParameters,
70+
AtlasPrimitiveActions atlasPrimitiveActions)
71+
{
72+
super("testSmoothICPPlannerBehavior", TestSmoothICPPlannerBehaviorState.class, yoTime, communicationBridge);
73+
74+
communicationBridge.registerYovaribleForAutoSendToUI(statemachine.getStateYoVariable());
75+
this.atlasPrimitiveActions = atlasPrimitiveActions;
76+
this.referenceFrames = referenceFrames;
77+
78+
numberOfSteps = new YoInteger("TestSmoothICPPlannerNumberOfSteps", registry);
79+
numberOfSteps.set(defaultNumberOfSteps);
80+
stepLength = new YoDouble("TestSmoothICPPlannerStepLength", registry);
81+
stepLength.set(defaultStepLength);
82+
stepWidth = new YoDouble("TestSmoothICPPlannerStepWidth", registry);
83+
stepWidth.set(defaultStepWidth);
84+
swingTime = new YoDouble("TestSmoothICPPlannerSwingTime", registry);
85+
swingTime.set(defaultSwingTime);
86+
transferTime = new YoDouble("TestSmoothICPPlannerTransferTime", registry);
87+
transferTime.set(defaultTransferTime);
88+
initialTransferTime = new YoDouble("TestSmoothICPPlannerInitialTransferTime", registry);
89+
initialTransferTime.set(defaultInitialTransferTime);
90+
finalTransferTime = new YoDouble("TestSmoothICPPlannerFinalTransferTime", registry);
91+
finalTransferTime.set(defaultFinalTransferTime);
92+
93+
resetRobotBehavior = new ResetRobotBehavior(communicationBridge, yoTime);
94+
setupStateMachine();
95+
}
96+
97+
@Override
98+
public void doControl()
99+
{
100+
super.doControl();
101+
}
102+
103+
private void setupStateMachine()
104+
{
105+
BehaviorAction<TestSmoothICPPlannerBehaviorState> resetRobot = new BehaviorAction<TestSmoothICPPlannerBehaviorState>(TestSmoothICPPlannerBehaviorState.RESET_ROBOT,
106+
resetRobotBehavior);
107+
108+
BehaviorAction<TestSmoothICPPlannerBehaviorState> setup = new BehaviorAction<TestSmoothICPPlannerBehaviorState>(TestSmoothICPPlannerBehaviorState.SETUP_ROBOT,
109+
atlasPrimitiveActions.leftHandDesiredConfigurationBehavior, atlasPrimitiveActions.rightHandDesiredConfigurationBehavior, atlasPrimitiveActions.leftArmTrajectoryBehavior,
110+
atlasPrimitiveActions.rightArmTrajectoryBehavior)
111+
{
112+
@Override
113+
protected void setBehaviorInput()
114+
{
115+
HandDesiredConfigurationMessage leftHandMessage = new HandDesiredConfigurationMessage(RobotSide.LEFT, HandConfiguration.CLOSE);
116+
HandDesiredConfigurationMessage rightHandMessage = new HandDesiredConfigurationMessage(RobotSide.RIGHT, HandConfiguration.CLOSE);
117+
118+
atlasPrimitiveActions.rightHandDesiredConfigurationBehavior.setInput(rightHandMessage);
119+
120+
atlasPrimitiveActions.leftHandDesiredConfigurationBehavior.setInput(leftHandMessage);
121+
122+
double[] rightArmPose = new double[] {1.5708, 0.8226007082651046, 1.2241049170121854, -1.546127437107859, -0.8486641166791746, -1.3365746544030488,
123+
1.3376930879072813};
124+
double[] leftArmPose = new double[] {-1.5383305366909918, -0.9340404711083553, 1.9634792241521146, 0.9236260708644913, -0.8710518130931819,
125+
-0.8771109242461594, -1.336089159719967};
126+
127+
ArmTrajectoryMessage rightPoseMessage = new ArmTrajectoryMessage(RobotSide.RIGHT, 2, rightArmPose);
128+
129+
ArmTrajectoryMessage leftPoseMessage = new ArmTrajectoryMessage(RobotSide.LEFT, 2, leftArmPose);
130+
131+
atlasPrimitiveActions.leftArmTrajectoryBehavior.setInput(leftPoseMessage);
132+
atlasPrimitiveActions.rightArmTrajectoryBehavior.setInput(rightPoseMessage);
133+
}
134+
};
135+
136+
BehaviorAction<TestSmoothICPPlannerBehaviorState> waitForConfirmation = new BehaviorAction<TestSmoothICPPlannerBehaviorState>(
137+
TestSmoothICPPlannerBehaviorState.CONFIRM_WALK)
138+
{
139+
@Override
140+
public void doTransitionOutOfAction()
141+
{
142+
super.doTransitionOutOfAction();
143+
}
144+
};
145+
146+
BehaviorAction<TestSmoothICPPlannerBehaviorState> walkForward = new BehaviorAction<TestSmoothICPPlannerBehaviorState>(
147+
TestSmoothICPPlannerBehaviorState.WALK_FORWARD, atlasPrimitiveActions.footstepListBehavior)
148+
{
149+
@Override
150+
protected void setBehaviorInput()
151+
{
152+
FootstepDataListMessage message = setUpFootSteps();
153+
atlasPrimitiveActions.footstepListBehavior.set(message);
154+
}
155+
};
156+
157+
BehaviorAction<TestSmoothICPPlannerBehaviorState> doneState = new BehaviorAction<TestSmoothICPPlannerBehaviorState>(TestSmoothICPPlannerBehaviorState.DONE,
158+
new SimpleDoNothingBehavior(communicationBridge))
159+
{
160+
@Override
161+
protected void setBehaviorInput()
162+
{
163+
TextToSpeechPacket p1 = new TextToSpeechPacket("Finished Walking Forward");
164+
sendPacket(p1);
165+
}
166+
};
167+
168+
statemachine.addStateWithDoneTransition(setup, TestSmoothICPPlannerBehaviorState.CONFIRM_WALK);
169+
statemachine.addStateWithDoneTransition(waitForConfirmation, TestSmoothICPPlannerBehaviorState.WALK_FORWARD);
170+
statemachine.addStateWithDoneTransition(walkForward, TestSmoothICPPlannerBehaviorState.RESET_ROBOT);
171+
statemachine.addStateWithDoneTransition(resetRobot, TestSmoothICPPlannerBehaviorState.DONE);
172+
statemachine.addState(doneState);
173+
statemachine.setStartState(TestSmoothICPPlannerBehaviorState.SETUP_ROBOT);
174+
175+
}
176+
177+
public FootstepDataListMessage setUpFootSteps()
178+
{
179+
FootstepDataListMessage footstepMessage = new FootstepDataListMessage(swingTime.getDoubleValue(), transferTime.getDoubleValue(), finalTransferTime.getDoubleValue());
180+
181+
ReferenceFrame midFeetZUpFrame = referenceFrames.getMidFeetZUpFrame();
182+
183+
for (int currentStep = 0; currentStep < numberOfSteps.getIntegerValue(); currentStep++)
184+
{
185+
Point3D footLocation = new Point3D(stepLength.getDoubleValue() * currentStep, side.negateIfRightSide(stepWidth.getDoubleValue() / 2), 0.0);
186+
Quaternion footOrientation = new Quaternion(0.0, 0.0, 0.0, 1.0);
187+
addFootstep(midFeetZUpFrame, side, footLocation, footOrientation, footstepMessage);
188+
side = side.getOppositeSide();
189+
}
190+
Point3D footLocation = new Point3D(stepLength.getDoubleValue() * (numberOfSteps.getIntegerValue()-1), side.negateIfRightSide(stepWidth.getDoubleValue() / 2), 0.0);
191+
Quaternion footOrientation = new Quaternion(0.0, 0.0, 0.0, 1.0);
192+
addFootstep(midFeetZUpFrame, side, footLocation, footOrientation, footstepMessage);
193+
194+
return footstepMessage;
195+
196+
}
197+
198+
private void addFootstep(ReferenceFrame stepReferenceFrame, RobotSide robotSide, Point3D stepLocation, Quaternion stepOrientation, FootstepDataListMessage footstepMessage)
199+
{
200+
FramePose stepPose = new FramePose();
201+
getRelativeFootstepInWorldFrame(stepReferenceFrame, stepLocation, stepOrientation, stepPose);
202+
FootstepDataMessage footstepData = new FootstepDataMessage(robotSide, stepPose.getFramePointCopy().getPoint(), stepPose.getFrameOrientationCopy().getQuaternion());
203+
204+
footstepMessage.add(footstepData);
205+
}
206+
207+
private void getRelativeFootstepInWorldFrame(ReferenceFrame frame, Point3D point3d, Quaternion quat4d, FramePose framePoseToPack)
208+
{
209+
FramePoint3D position = new FramePoint3D(frame, point3d);
210+
position.changeFrame(ReferenceFrame.getWorldFrame());
211+
FrameOrientation orientation = new FrameOrientation(frame, quat4d);
212+
orientation.changeFrame(ReferenceFrame.getWorldFrame());
213+
214+
framePoseToPack.setPoseIncludingFrame(position, orientation);
215+
}
216+
217+
@Override
218+
public void onBehaviorExited()
219+
{
220+
221+
}
222+
}

‎IHMCHumanoidRobotics/src/us/ihmc/humanoidRobotics/communication/packets/behaviors/HumanoidBehaviorType.java‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ public enum HumanoidBehaviorType
2424
DEBUG_PARTIAL_FOOTHOLDS,
2525
WALK_TO_GOAL_ANYTIME_PLANNER,
2626
TEST_ICP_OPTIMIZATION,
27+
TEST_SMOOTH_ICP_PLANNER,
2728
SOLARPANEL_BEHAVIOR,
2829
PUSH_AND_WALK,
2930
COLLABORATIVE_TASK,

0 commit comments

Comments
 (0)