/**
* Quadruped version, assumes flat ground
*
* @param footName name for yovariable registry and yovariable names
* @param quadrupedFeet all the feet
* @param switchZThreshold difference in z between the lowest foot and the foot inquestion before
* assuming foot is in contact
* @param totalRobotWeight
* @param quadrant the foot in question
* @param parentRegistry
*/
public KinematicsBasedFootSwitch(
String footName,
QuadrantDependentList<? extends ContactablePlaneBody> quadrupedFeet,
double switchZThreshold,
double totalRobotWeight,
RobotQuadrant quadrant,
YoVariableRegistry parentRegistry) {
registry = new YoVariableRegistry(footName + getClass().getSimpleName());
foot = quadrupedFeet.get(quadrant);
ContactablePlaneBody acrossBodyFrontFoot =
quadrupedFeet.get(quadrant.getAcrossBodyFrontQuadrant());
ContactablePlaneBody acrossBodyHindFoot =
quadrupedFeet.get(quadrant.getAcrossBodyHindQuadrant());
ContactablePlaneBody sameSideFoot = quadrupedFeet.get(quadrant.getSameSideQuadrant());
otherFeet = new ContactablePlaneBody[] {acrossBodyFrontFoot, acrossBodyHindFoot, sameSideFoot};
this.totalRobotWeight = totalRobotWeight;
hitGround = new BooleanYoVariable(footName + "hitGround", registry);
fixedOnGround = new BooleanYoVariable(footName + "fixedOnGround", registry);
soleZ = new DoubleYoVariable(footName + "soleZ", registry);
ankleZ = new DoubleYoVariable(footName + "ankleZ", registry);
this.switchZThreshold = new DoubleYoVariable(footName + "footSwitchZThreshold", registry);
this.switchZThreshold.set(switchZThreshold);
yoResolvedCoP = new YoFramePoint2d(footName + "ResolvedCoP", "", foot.getSoleFrame(), registry);
parentRegistry.addChild(registry);
}
@DeployableTestMethod(estimatedDuration = 0.1)
@Test(timeout = 30000)
public void testConstructorsGettersAndSetters() {
QuadrupedSupportPolygon quadrupedSupportPolygon = new QuadrupedSupportPolygon();
QuadrantDependentList<Tuple3d> footPoints = new QuadrantDependentList<>();
footPoints.set(RobotQuadrant.HIND_LEFT, new Point3d(0.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.HIND_RIGHT, new Point3d(1.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.FRONT_LEFT, new Point3d(0.0, 1.0, 0.0));
footPoints.set(RobotQuadrant.FRONT_RIGHT, new Point3d(1.0, 1.0, 0.0));
QuadrantDependentList<FramePoint> framePoints = new QuadrantDependentList<>();
for (RobotQuadrant robotQuadrant : RobotQuadrant.values) {
framePoints.set(
robotQuadrant,
new FramePoint(ReferenceFrame.getWorldFrame(), footPoints.get(robotQuadrant)));
}
quadrupedSupportPolygon = new QuadrupedSupportPolygon(framePoints);
quadrupedSupportPolygon = new QuadrupedSupportPolygon(quadrupedSupportPolygon);
for (RobotQuadrant robotQuadrant : RobotQuadrant.values) {
quadrupedSupportPolygon.setFootstep(
robotQuadrant,
new FramePoint(ReferenceFrame.getWorldFrame(), footPoints.get(robotQuadrant)));
}
quadrupedSupportPolygon.printOutPolygon(getClass().getMethods()[0].getName());
for (RobotQuadrant robotQuadrant : RobotQuadrant.values) {
Point3d tuple3dToPack = new Point3d();
quadrupedSupportPolygon.getFootstep(robotQuadrant).get(tuple3dToPack);
assertEquals("Point not equal", footPoints.get(robotQuadrant), tuple3dToPack);
}
assertEquals(
"RefFrame getter not work",
ReferenceFrame.getWorldFrame(),
quadrupedSupportPolygon.getReferenceFrame());
System.out.println(quadrupedSupportPolygon.toString());
quadrupedSupportPolygon.changeFrame(ReferenceFrame.getWorldFrame());
}
private QuadrupedSupportPolygon create3LegPolygon() {
QuadrantDependentList<Tuple3d> footPoints = new QuadrantDependentList<>();
footPoints.set(RobotQuadrant.HIND_LEFT, new Point3d(0.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.HIND_RIGHT, new Point3d(1.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.FRONT_RIGHT, new Point3d(1.0, 1.0, 0.0));
QuadrantDependentList<FramePoint> framePoints = new QuadrantDependentList<>();
for (RobotQuadrant robotQuadrant : footPoints.quadrants()) {
framePoints.set(
robotQuadrant,
new FramePoint(ReferenceFrame.getWorldFrame(), footPoints.get(robotQuadrant)));
}
return new QuadrupedSupportPolygon(framePoints);
}