public void setCurrentDesiredFootstep(Footstep currentDesiredFootstep) { this.currentDesiredFootstep.setPose(currentDesiredFootstep); }
public Footstep getFootstepAtCurrentLocation(RobotSide robotSide) { tempPose.setToZero(contactableFeet.get(robotSide).getFrameAfterParentJoint()); tempPose.changeFrame(worldFrame); Footstep footstep = footstepsAtCurrentLocation.get(robotSide); footstep.setPose(tempPose); return footstep; }
public void sendFootStepMessages(int numberOfFootstepsToPlan) { RobotSide robotSide = RobotSide.LEFT; FramePoint3D footstepLocation = new FramePoint3D(); FrameQuaternion footstepOrientation = new FrameQuaternion(); upcomingFootstepsData.clear(); for (int i = 1; i < numberOfFootstepsToPlan + 1; i++) { Footstep footstep = new Footstep(robotSide); footstepLocation.set(i * stepLength, robotSide.negateIfRightSide(stepWidth), 0.0); footstep.setPose(footstepLocation, footstepOrientation); FootstepTiming timing = new FootstepTiming(swingTime, transferTime); upcomingFootstepsData.add(new FootstepData(footstep, timing)); robotSide = robotSide.getOppositeSide(); } numberOfUpcomingFootsteps.set(upcomingFootstepsData.size()); }
@Override public boolean getUpcomingFootstepSolution(Footstep footstepToPack) { if (icpOptimizationController.getNumberOfFootstepsToConsider() > 0) { footstepToPack.getPose(footstepPose); icpOptimizationController.getFootstepSolution(0, footstepPositionSolution); footstepPose.setXYFromPosition2d(footstepPositionSolution); footstepToPack.setPose(footstepPose); } return icpOptimizationController.wasFootstepAdjusted(); }
public Footstep createFootstep(RobotSide robotSide, FramePose footstepPose) { RigidBody foot = contactableFeet.get(robotSide).getRigidBody(); ReferenceFrame soleFrame = contactableFeet.get(robotSide).getSoleFrame(); Footstep ret = new Footstep(foot, robotSide, soleFrame); ret.setPose(footstepPose); ret.setPredictedContactPointsFromFramePoint2ds(contactableFeet.get(robotSide).getContactPoints2d()); return ret; } }
private void setupInputs() { clear(); RobotSide side = RobotSide.LEFT; for (int i = 0; i < testParameters.getNumberOfFootstepsToConsider(); i++) { // Update the current location based on walking direction tempFrameVector.setIncludingFrame(walkingDirectionUnitVector); tempFrameVector.scale(stepLength); currentLocation.add(tempFrameVector); // Create new footstep based on updated CoM location Footstep newFootstep = new Footstep(); newFootstep.setRobotSide(side); getFootLocationFromCoMLocation(tempFramePoint1, side, currentLocation, walkingDirectionUnitVector, stepLength, stepWidth); // Set calculated pose to footstep newFootstep.setPose(tempFramePoint1, robotOrientation); upcomingFootstepsData.add(new FootstepData(newFootstep, new FootstepTiming(swingTime, transferTime))); side = side.getOppositeSide(); } numberOfUpcomingFootsteps.set(upcomingFootstepsData.size()); }
footstepToAdjust.setPose(adjustedPosition, adjustedOrientation); break; case AT_SOLE_FRAME:
private int checkForEndingOfAdjustment(double omega0) { int numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue()); if (timeRemainingInState.getDoubleValue() < remainingTimeToStopAdjusting.getDoubleValue() && localUseStepAdjustment && !isStanding.getBooleanValue() && !isInTransfer.getBooleanValue()) { //record current locations for (int i = 0; i < numberOfFootstepsToConsider; i++) { upcomingFootstepLocations.get(i).set(footstepSolutions.get(i)); Footstep footstep = upcomingFootsteps.get(i); footstep.getPose(footstepPose); footstepPose.setXYFromPosition2d(footstepSolutions.get(i).getFrameTuple2d()); footstep.setPose(footstepPose); inputHandler.addFootstepToPlan(footstep); } localUseStepAdjustment = false; numberOfFootstepsToConsider = clipNumberOfFootstepsToConsiderToProblem(this.numberOfFootstepsToConsider.getIntegerValue()); footstepRecursionMultiplierCalculator.resetTimes(); footstepRecursionMultiplierCalculator.submitTimes(0, 0.0, singleSupportDuration.getDoubleValue()); for (int i = 1; i < numberOfFootstepsToConsider + 1; i++) footstepRecursionMultiplierCalculator.submitTimes(i, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue()); footstepRecursionMultiplierCalculator.submitTimes(numberOfFootstepsToConsider + 1, doubleSupportDuration.getDoubleValue(), singleSupportDuration.getDoubleValue()); footstepRecursionMultiplierCalculator.computeRecursionMultipliers(numberOfFootstepsToConsider, isInTransfer.getBooleanValue(), localUseTwoCMPs, omega0); } return numberOfFootstepsToConsider; }
@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); }
@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; }
@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 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; }