private void computeEntryCMPForFootstep(FramePoint entryCMPToPack, Footstep footstep, FramePoint2d centroidInSoleFrameOfPreviousSupportFoot, YoFramePoint previousExitCMP) { ReferenceFrame soleFrame = footstep.getSoleReferenceFrame(); List<Point2d> predictedContactPoints = footstep.getPredictedContactPoints(); RobotSide robotSide = footstep.getRobotSide(); if (predictedContactPoints != null) tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, predictedContactPoints); else tempSupportPolygon.setIncludingFrameAndUpdate(soleFrame, defaultFootPolygons.get(robotSide)); computeEntryCMP(entryCMPToPack, robotSide, soleFrame, tempSupportPolygon, centroidInSoleFrameOfPreviousSupportFoot, previousExitCMP); }
public static Footstep[] createFootsteps(int numberOfSteps) { Footstep[] footsteps = new Footstep[numberOfSteps]; for (int i = 0; i < numberOfSteps; i++) { footsteps[i] = new Footstep(); } return footsteps; }
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; }
public void updateContactPointsForUpcomingFootstep(Footstep nextFootstep) { RobotSide robotSide = nextFootstep.getRobotSide(); List<Point2d> predictedContactPoints = nextFootstep.getPredictedContactPoints(); if ((predictedContactPoints != null) && (!predictedContactPoints.isEmpty())) { setFootPlaneContactPoints(robotSide, predictedContactPoints); } else { resetFootPlaneContactPoint(robotSide); } }
public void set(ArrayList<Footstep> footsteps, double swingTime, double transferTime) { FootstepDataListMessage footstepDataList = new FootstepDataListMessage(swingTime,transferTime); for (int i = 0; i < footsteps.size(); i++) { Footstep footstep = footsteps.get(i); Point3d location = new Point3d(footstep.getX(), footstep.getY(), footstep.getZ()); Quat4d orientation = new Quat4d(); footstep.getOrientation(orientation); RobotSide footstepSide = footstep.getRobotSide(); FootstepDataMessage footstepData = new FootstepDataMessage(footstepSide, location, orientation); footstepDataList.add(footstepData); } set(footstepDataList); }
private ArrayList<Footstep> concatenateFootstepPaths(ArrayList<Footstep> firstSetOfSteps, ArrayList<Footstep> secondSetOfSteps) { int indexOfLastStepOfFirstSegment = firstSetOfSteps.size() - 1; RobotSide sideOfLastFootstepOfFirstSegment = firstSetOfSteps.get(indexOfLastStepOfFirstSegment).getRobotSide(); RobotSide sideOfFirstFootstepOfSecondSegment = secondSetOfSteps.get(0).getRobotSide(); if (sideOfLastFootstepOfFirstSegment == sideOfFirstFootstepOfSecondSegment) firstSetOfSteps.remove(indexOfLastStepOfFirstSegment); for (Footstep footstep : secondSetOfSteps) { firstSetOfSteps.add(footstep); } return firstSetOfSteps; }
@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; }
public FootstepDataMessage(Footstep footstep, FootstepTiming timing) robotSide = footstep.getRobotSide(); location = new Point3d(); orientation = new Quat4d(); footstep.getPositionInWorldFrame(location); footstep.getOrientationInWorldFrame(orientation); List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints(); if (footstepContactPoints != null) trajectoryType = footstep.getTrajectoryType(); swingHeight = footstep.getSwingHeight(); if (footstep.getSwingWaypoints().size() != 0) trajectoryWaypoints = new Point3d[footstep.getSwingWaypoints().size()]; for (int i = 0; i < footstep.getSwingWaypoints().size(); i++) trajectoryWaypoints[i] = new Point3d(footstep.getSwingWaypoints().get(i));
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 Footstep.FootstepType snapFootstep(Footstep footstep, HeightMapWithPoints heightMap){ FootstepDataMessage originalFootstep = new FootstepDataMessage(footstep); //set to the sole pose Vector3d position = new Vector3d(); Quat4d orientation = new Quat4d(); RigidBodyTransform solePose = new RigidBodyTransform(); footstep.getSolePose(solePose); solePose.get(orientation, position); originalFootstep.setLocation(new Point3d(position)); originalFootstep.setOrientation(orientation); //get the footstep Footstep.FootstepType type = snapFootstep(originalFootstep, heightMap); footstep.setPredictedContactPointsFromPoint2ds(originalFootstep.getPredictedContactPoints()); footstep.setFootstepType(type); FramePose solePoseInWorld = new FramePose(ReferenceFrame.getWorldFrame(), originalFootstep.getLocation(), originalFootstep.getOrientation()); footstep.setSolePose(solePoseInWorld); footstep.setSwingHeight(originalFootstep.getSwingHeight()); footstep.setTrajectoryType(originalFootstep.getTrajectoryType()); return type; }
swingStartPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide())); stanceFootPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide().getOppositeSide())); footstep.getPose(swingEndPose); FootstepDataMessage footstepDataMessage = HumanoidMessageTools.createFootstepDataMessage(footstep.getRobotSide(), new Point3D(swingEndPose.getPosition()), new Quaternion(swingEndPose.getOrientation())); MessageTools.copyData(waypoints, footstepDataMessage.getCustomPositionWaypoints()); if (footstep.getFootstepType() == FootstepType.PARTIAL_FOOTSTEP) partialFootholdPolygon.addVertices(Vertex2DSupplier.asVertex2DSupplier(footstep.getPredictedContactPoints())); partialFootholdPolygon.update();
transferToFootstep.setZ(transferToFootstep.getZ() + RandomNumbers.nextDouble(random, 0.0, maxZChange)); previousFootstep = transferToFootstep; FootSpoof transferFromFootSpoof = contactableFeet.get(transferFromFootstep.getRobotSide()); FramePoint3D transferFromFootFramePoint = new FramePoint3D(); transferFromFootstep.getPosition(transferFromFootFramePoint); FrameQuaternion transferFromFootOrientation = new FrameQuaternion(); transferFromFootstep.getOrientation(transferFromFootOrientation); transferFromFootSpoof.setPose(transferFromFootFramePoint, transferFromFootOrientation); FootSpoof transferToFootSpoof = contactableFeet.get(transferToFootstep.getRobotSide()); FramePoint3D transferToFootFramePoint = new FramePoint3D(); transferToFootstep.getPosition(transferToFootFramePoint); FrameQuaternion transferToFootOrientation = new FrameQuaternion(); transferToFootstep.getOrientation(transferToFootOrientation); transferToFootSpoof.setPose(transferToFootFramePoint, transferToFootOrientation); transferToAndNextFootstepsData.setNextFootstep(upcomingFootstep); transferToAndNextFootstepsData.setTransferToSide(transferToFootstep.getRobotSide()); RobotSide supportLeg = transferFromFootstep.getRobotSide(); supportLegFrameSide.set(supportLeg); supportLeg = transferToFootstep.getRobotSide(); FramePoint2D transferFromFootPosition = new FramePoint2D(transferFromFootstep.getFootstepPose().getPosition()); FramePoint2D transferToFootPosition = new FramePoint2D(transferToFootstep.getFootstepPose().getPosition());
return false; if (footstepToAdjust.getRobotSide() != requestedFootstepAdjustment.getRobotSide()) PrintTools.warn(this, "RobotSide does not match: side of footstep to be adjusted: " + footstepToAdjust.getRobotSide() + ", side of adjusted footstep: " + requestedFootstepAdjustment.getRobotSide()); hasNewFootstepAdjustment.set(false); requestedFootstepAdjustment.clear(); footstepToAdjust.setPose(adjustedPosition, adjustedOrientation); break; case AT_SOLE_FRAME: footstepToAdjust.setSolePose(adjustedPosition, adjustedOrientation); break; default: for (int i = 0; i < footstepToAdjust.getPredictedContactPoints().size(); i++) contactPoints.add(footstepToAdjust.getPredictedContactPoints().get(i)); footstepToAdjust.setPredictedContactPointsFromPoint2ds(contactPoints);
public AdjustFootstepMessage(Footstep footstep) { uniqueId = VALID_MESSAGE_DEFAULT_ID; origin = FootstepOrigin.AT_ANKLE_FRAME; robotSide = footstep.getRobotSide(); location = new Point3d(); orientation = new Quat4d(); footstep.getPositionInWorldFrame(location); footstep.getOrientationInWorldFrame(orientation); List<Point2d> footstepContactPoints = footstep.getPredictedContactPoints(); if (footstepContactPoints != null) { if (predictedContactPoints == null) { predictedContactPoints = new ArrayList<>(); } else { predictedContactPoints.clear(); } for (Point2d contactPoint : footstepContactPoints) { predictedContactPoints.add((Point2d) contactPoint.clone()); } } else { predictedContactPoints = null; } }
private FootstepDataListMessage createFootstepDataList(ArrayList<Footstep> desiredFootsteps) { FootstepDataListMessage ret = new FootstepDataListMessage(); for (int i = 0; i < desiredFootsteps.size(); i++) { Footstep footstep = desiredFootsteps.get(i); FramePoint3D position = new FramePoint3D(); FrameQuaternion orientation = new FrameQuaternion(); footstep.getPose(position, orientation); RobotSide footstepSide = footstep.getRobotSide(); FootstepDataMessage footstepData = HumanoidMessageTools.createFootstepDataMessage(footstepSide, position, orientation); ret.getFootstepDataList().add().set(footstepData); } return ret; }
swingStartPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide())); stanceFootPose.setToZero(humanoidReferenceFrames.getSoleFrame(footstepList.get(0).getRobotSide().getOppositeSide())); footstep.getSolePose(swingEndPose); FootstepDataMessage footstepDataMessage = new FootstepDataMessage(footstep.getRobotSide(), new Point3d(swingEndPose.getPositionUnsafe()), new Quat4d(swingEndPose.getOrientationUnsafe())); footstepDataMessage.setOrigin(FootstepOrigin.AT_SOLE_FRAME); footstepDataMessage.setTrajectoryWaypoints(waypoints); if (footstep.getFootstepType() == FootstepType.PARTIAL_FOOTSTEP) partialFootholdPolygon.addVertices(footstep.getPredictedContactPoints(), footstep.getPredictedContactPoints().size()); partialFootholdPolygon.update();
private FrameVector3D getTranslationVector(Footstep stance, double circleCenterOffset) { RobotSide stanceSide = stance.getRobotSide(); double yCenterDisplacementTowardsExpectedOtherFoot = (stanceSide == RobotSide.LEFT) ? -circleCenterOffset : circleCenterOffset; FrameVector3D translationFromFootCenterToCircleCenter = new FrameVector3D(stance.getSoleReferenceFrame(), 0.0, yCenterDisplacementTowardsExpectedOtherFoot, 0.0); translationFromFootCenterToCircleCenter.changeFrame(ReferenceFrame.getWorldFrame()); return translationFromFootCenterToCircleCenter; }
/** * This function takes a footstep and calculates the touch-down polygon in the * desired reference frame */ private void calculateTouchdownFootPolygon(Footstep footstep, ReferenceFrame desiredFrame, FrameConvexPolygon2d polygonToPack) { footstep.getPositionIncludingFrame(centroid3d); centroid3d.getFramePoint2d(centroid2d); centroid2d.changeFrame(desiredFrame); polygonToPack.setIncludingFrameAndUpdate(footstep.getSoleReferenceFrame(), defaultSupportPolygons.get(footstep.getRobotSide())); polygonToPack.changeFrameAndProjectToXYPlane(desiredFrame); // shrink the polygon for safety by pulling all the corner points towards the center polygonToPack.scale(centroid2d, SHRINK_TOUCHDOWN_POLYGON_FACTOR); }
private void checkFootMeetsAngleRequirement(Footstep stance, Footstep swingEnd) { FramePose3D stancePose = new FramePose3D(); stance.getPose(stancePose); FramePose3D swingEndPose = new FramePose3D(); swingEnd.getPose(swingEndPose); FrameOrientation2D stanceOrientation = new FrameOrientation2D(stancePose.getOrientation()); FrameOrientation2D swingOrientation = new FrameOrientation2D(swingEndPose.getOrientation()); double yawDifference = swingOrientation.difference(stanceOrientation); double allowedDifference = (Math.abs(yawDifference) - footTwistLimitInRadians) - 3e-16; if (allowedDifference > 0) valid = false; }
@Override public void receivedPacket(FootstepDataListMessage packet) { boolean adjustable = packet.getAreFootstepsAdjustable(); List<FootstepDataMessage> footstepDataList = packet.getFootstepDataList(); for (int i = 0; i < footstepDataList.size(); i++) { FootstepDataMessage footstepData = footstepDataList.get(i); FramePose3D footstepPose = new FramePose3D(ReferenceFrame.getWorldFrame(), footstepData.getLocation(), footstepData.getOrientation()); Footstep footstep = new Footstep(robotSide, footstepPose, true, adjustable); footstep.setPredictedContactPoints(HumanoidMessageTools.unpackPredictedContactPoints(footstepData)); footstep.setTrajectoryType(TrajectoryType.fromByte(footstepData.getTrajectoryType())); footstep.setSwingHeight(footstepData.getSwingHeight()); reconstructedFootstepPath.add(footstep); } }