private FramePose getRobotFootPose(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) {
List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide);
Joint ankleJoint = gcPoints.get(0).getParentJoint();
RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform();
ankleJoint.getTransformToWorld(ankleTransformToWorld);
FramePose ret = new FramePose();
ret.setPose(ankleTransformToWorld);
return ret;
}
public SlipRandomOnNextStepPerturber(HumanoidFloatingRootJointRobot robot) {
super("SlipRandomOnNextStepPerturber");
String name = "SlipRandom";
this.robot = robot;
groundContactPointsSlippers = new SideDependentList<GroundContactPointsSlipper>();
for (RobotSide robotSide : RobotSide.values()) {
DoubleYoVariable touchdownTimeForSlip =
new DoubleYoVariable(
robotSide.getCamelCaseNameForStartOfExpression()
+ "TouchdownTimeForSlip"
+ robotSide.getCamelCaseNameForMiddleOfExpression(),
registry);
touchdownTimeForSlipMap.put(robotSide, touchdownTimeForSlip);
EnumYoVariable<SlipState> slipState =
new EnumYoVariable<SlipState>(
name + "SlipState" + robotSide.getCamelCaseNameForMiddleOfExpression(),
registry,
SlipState.class);
slipState.set(SlipState.NO_CONTACT);
slipStateMap.put(robotSide, slipState);
groundContactPointsMap.put(robotSide, robot.getFootGroundContactPoints(robotSide));
GroundContactPointsSlipper groundContactPointsSlipper =
new GroundContactPointsSlipper(robotSide.getLowerCaseName());
groundContactPointsSlippers.put(robotSide, groundContactPointsSlipper);
this.addRobotController(groundContactPointsSlipper);
}
this.minSlipAfterTimeDelta = new DoubleYoVariable(name + "MinSlipAfterTimeDelta", registry);
this.maxSlipAfterTimeDelta = new DoubleYoVariable(name + "MaxSlipAfterTimeDelta", registry);
this.nextSlipAfterTimeDelta = new DoubleYoVariable(name + "NextSlipAfterTimeDelta", registry);
this.minSlipPercentSlipPerTick =
new DoubleYoVariable(name + "MinSlipPercentSlipPerTick", registry);
this.maxSlipPercentSlipPerTick =
new DoubleYoVariable(name + "MaxSlipPercentSlipPerTick", registry);
this.nextSlipPercentSlipPerTick =
new DoubleYoVariable(name + "NextSlipPercentSlipPerTick", registry);
this.slipNextStep = new BooleanYoVariable(name + "SlipNextStep", registry);
maxTranslationToSlipNextStep =
new YoFrameVector(
name + "MaxTranslationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
minTranslationToSlipNextStep =
new YoFrameVector(
name + "MinTranslationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
nextTranslationToSlip =
new YoFrameVector(name + "NextTranslationToSlip", ReferenceFrame.getWorldFrame(), registry);
maxRotationToSlipNextStep =
new YoFrameOrientation(
name + "MaxRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
minRotationToSlipNextStep =
new YoFrameOrientation(
name + "MinRotationToSlipNextStep", ReferenceFrame.getWorldFrame(), registry);
nextRotationToSlip =
new YoFrameOrientation(
name + "NextRotationToSlip", ReferenceFrame.getWorldFrame(), registry);
setTranslationRangeToSlipNextStep(new double[] {0.0, 0.0, 0.0}, new double[] {0.05, 0.05, 0.0});
setRotationRangeToSlipNextStep(new double[] {0.0, 0.0, 0.0}, new double[] {0.3, 0.15, 0.1});
setSlipAfterStepTimeDeltaRange(0.01, 0.10);
setSlipPercentSlipPerTickRange(0.01, 0.05);
setProbabilityOfSlip(1.0);
}