private void updateNextFootsteps(int stepIndex) { int numberOfStepToUpdate = (numberOfFootstepsForTest - stepIndex) < numberOfFootstepsToConsider ? (numberOfFootstepsForTest - stepIndex) : numberOfFootstepsToConsider; int nextStepIndex; for (nextStepIndex = 0; nextStepIndex < numberOfStepToUpdate; nextStepIndex++) { nextFootstepPoses.get(nextStepIndex).set(footstepList.get(stepIndex + nextStepIndex).getFootstepPose()); } for (; nextStepIndex < numberOfFootstepsToConsider; nextStepIndex++) { nextFootstepPoses.get(nextStepIndex).setToNaN(); } }
public FootstepDataListMessage generateFootstepsFromLocationsAndOrientations(RobotSide[] robotSides, double[][][] footstepLocationsAndOrientations, double swingTime, double transferTime) { FootstepDataListMessage footstepDataList = HumanoidMessageTools.createFootstepDataListMessage(swingTime, transferTime); for (int i = 0; i < robotSides.length; i++) { RobotSide robotSide = robotSides[i]; double[][] footstepLocationAndOrientation = footstepLocationsAndOrientations[i]; Footstep footstep = generateFootstepFromLocationAndOrientation(robotSide, footstepLocationAndOrientation); FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(robotSide, footstep.getFootstepPose().getPosition(), footstep.getFootstepPose().getOrientation()); footstepDataList.getFootstepDataList().add().set(footstepData); } return footstepDataList; }
private boolean areFoostepsEqual(Footstep sentFootstep, Footstep receivedFootstep, double epsilon) { boolean arePosesEqual = sentFootstep.getFootstepPose().epsilonEquals(receivedFootstep.getFootstepPose(), epsilon); boolean sameRobotSide = sentFootstep.getRobotSide() == receivedFootstep.getRobotSide(); boolean isTrustHeightTheSame = sentFootstep.getTrustHeight() == receivedFootstep.getTrustHeight(); // FIXME the field is set in FootstepDataListMessage only, which makes impossible to properly convert messages to footsteps. boolean isAdjustableTheSame = true; //sentFootstep.getIsAdjustable() == receivedFootstep.getIsAdjustable(); boolean sameWaypoints = sentFootstep.getCustomPositionWaypoints().size() == receivedFootstep.getCustomPositionWaypoints().size(); if (sameWaypoints) { for (int i = 0; i < sentFootstep.getCustomPositionWaypoints().size(); i++) { FramePoint3D waypoint = sentFootstep.getCustomPositionWaypoints().get(i); FramePoint3D otherWaypoint = receivedFootstep.getCustomPositionWaypoints().get(i); sameWaypoints = sameWaypoints && waypoint.epsilonEquals(otherWaypoint, epsilon); } } boolean sameBlendDuration = MathTools.epsilonEquals(sentFootstep.getSwingTrajectoryBlendDuration(), receivedFootstep.getSwingTrajectoryBlendDuration(), epsilon); return arePosesEqual && sameRobotSide && isTrustHeightTheSame && isAdjustableTheSame && sameWaypoints && sameBlendDuration; }
public static AdjustFootstepMessage createAdjustFootstepMessage(Footstep footstep) { AdjustFootstepMessage message = new AdjustFootstepMessage(); message.setRobotSide(footstep.getRobotSide().toByte()); FramePoint3D location = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(location, orientation); footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getLocation().set(location); message.getOrientation().set(orientation); MessageTools.copyData(footstep.getPredictedContactPoints().stream().map(Point3D::new).collect(Collectors.toList()), message.getPredictedContactPoints2d()); return message; }
@Override public void adjustFootstepWithoutHeightmap(Footstep footstep, double height, Vector3D planeNormal) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FramePose3D footstepPose = new FramePose3D(); footstep.getPose(footstepPose); Point3D position = new Point3D(footstepPose.getPosition()); double yaw = footstep.getFootstepPose().getYaw(); Quaternion orientation = new Quaternion(); RotationTools.computeQuaternionFromYawAndZNormal(yaw, planeNormal, orientation); position.setZ(height); footstepPose.set(new Point3D(position), orientation); footstep.setPose(footstepPose); }
private void updateContactState(int currentStepCount, double percentageOfPhase) { if (inDoubleSupport.getBooleanValue()) { for (RobotSide side : RobotSide.values) contactStates.get(side).setFullyConstrained(); } else { RobotSide swingSide = footstepList.get(currentStepCount).getRobotSide(); contactStates.get(swingSide).clear(); contactStates.get(swingSide.getOppositeSide()).setFullyConstrained(); if (currentStepCount > 0) { swingFootPose.set(footstepList.get(currentStepCount - 1).getFootstepPose()); } else { swingFootPose.set(initialPose); swingFootPose.appendTranslation(0.0, swingSide.negateIfRightSide(stepWidth / 2.0), 0.0); } FramePose3D endOfSwing = footstepList.get(currentStepCount).getFootstepPose(); swingFootPose.interpolate(endOfSwing, percentageOfPhase); FootSpoof foot = feet.get(swingSide); foot.setSoleFrame(swingFootPose); } bipedSupportPolygons.updateUsingContactStates(contactStates); }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0){ throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points"); } footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); footstep.setFootstepType(type); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); if (type == Footstep.FootstepType.FULL_FOOTSTEP && originalFootstep.getPredictedContactPoints2d().size() > 0 ) { throw new RuntimeException(this.getClass().getSimpleName() + "Full Footstep should have null contact points"); } footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); footstep.setFootstepType(type); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
public static FootstepDataMessage createFootstepDataMessage(Footstep footstep) { FootstepDataMessage message = new FootstepDataMessage(); message.setRobotSide(footstep.getRobotSide().toByte()); FramePoint3D location = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(location, orientation); footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getLocation().set(location); message.getOrientation().set(orientation); packPredictedContactPoints(footstep.getPredictedContactPoints(), message); message.setTrajectoryType(footstep.getTrajectoryType().toByte()); message.setSwingHeight(footstep.getSwingHeight()); message.setSwingTrajectoryBlendDuration(footstep.getSwingTrajectoryBlendDuration()); if (footstep.getCustomPositionWaypoints().size() != 0) { for (int i = 0; i < footstep.getCustomPositionWaypoints().size(); i++) { FramePoint3D framePoint = footstep.getCustomPositionWaypoints().get(i); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); message.getCustomPositionWaypoints().add().set(framePoint); } } return message; }
for (int numberOfStepsFromTarget = 0; numberOfStepsFromTarget <= numberOfFootstepsBetweenStartAndTarget; numberOfStepsFromTarget++) FramePose2D currentFootstepPose = new FramePose2D(footsteps.get(numberOfFootsteps - numberOfStepsFromTarget - 1).getFootstepPose()); assertEquals("Current footstep orientation does not match end orientation.", 0.0, currentFootstepPose.getOrientationDistance(targetMidFeetPose2d), ORIENTATION_THRESHOLD);
public Footstep.FootstepType snapFootstep(Footstep footstep, List<Point3D> pointList, double defaultHeight) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, pointList, defaultHeight); footstep.setFootstepType(type); footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConstructor() { FootstepData testObject = new FootstepData(); assertTrue(testObject != null); Footstep testFootstep = new Footstep(RobotSide.LEFT); FootstepTiming testFootstepTiming = new FootstepTiming(0.2, 0.1); testFootstepTiming.setAbsoluteTime(0.1, 0.5); testObject.set(testFootstep, testFootstepTiming); assertTrue(testObject.getFootstep() == testFootstep); assertTrue(testObject.getStepTime() == testFootstepTiming.getStepTime()); assertTrue(testObject.getSwingSide() == testFootstep.getRobotSide()); assertTrue(testObject.getSupportSide() == testFootstep.getRobotSide().getOppositeSide()); assertTrue(testObject.getSwingTime() == testFootstepTiming.getSwingTime()); assertTrue(testObject.getTransferTime() == testFootstepTiming.getTransferTime()); assertTrue(MathTools.epsilonEquals(testObject.getTransferStartTime(), testFootstepTiming.getExecutionStartTime(), Epsilons.ONE_BILLIONTH)); assertTrue(testObject.getSwingStartTime() == testFootstepTiming.getSwingStartTime()); assertTrue(testObject.getFootstepPoseReferenceFrame() == testFootstep.getFootstepPose().getReferenceFrame()); assertTrue(testObject.getFramePose() == testFootstep.getFootstepPose()); testObject.setSwingTime(0.8); assertTrue(testObject.getSwingTime() == 0.8); Footstep newFootstep = new Footstep(RobotSide.LEFT); testObject.setFootstep(newFootstep); assertTrue(testObject.getFootstep() == newFootstep); }
for (Footstep footstep : footsteps) FramePose2D currentFootstepPose = new FramePose2D(footstep.getFootstepPose()); if (currentFootstepPose.getOrientationDistance(startTargetMidPose2dMean) < ORIENTATION_THRESHOLD) numberOfStepsAlignedWithMeanOrientation++;
@Override public Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap) { // can only snap footsteps in world frame footstep.getFootstepPose().checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); FootstepDataMessage originalFootstep = HumanoidMessageTools.createFootstepDataMessage(footstep); //set to the sole pose FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); originalFootstep.getLocation().set(position); originalFootstep.getOrientation().set(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(originalFootstep)); footstep.setFootstepType(type); FramePose3D solePoseInWorld = new FramePose3D(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setPose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(TrajectoryType.fromByte(originalFootstep.getTrajectoryType())); return type; }
FramePoint2D transferFromFootPosition = new FramePoint2D(transferFromFootstep.getFootstepPose().getPosition()); FramePoint2D transferToFootPosition = new FramePoint2D(transferToFootstep.getFootstepPose().getPosition());
for (Footstep footstep : walkToLocationBehavior.getFootSteps()) FramePose2D currentFootstepPose = new FramePose2D(footstep.getFootstepPose()); assertEquals("Current footstep orientation does not match start orientation.", 0.0, currentFootstepPose.getOrientationDistance(startMidFeetPose2d), ORIENTATION_THRESHOLD);