@Override public void changeFrame(ReferenceFrame desiredFrame) { if (desiredFrame == referenceFrame) return; position.changeFrame(desiredFrame); orientation.changeFrame(desiredFrame); referenceFrame = desiredFrame; }
protected FramePoint2d displacePosition(FramePoint2d footstepPosition2d, double forwardHeading, double offsetForward, double offsetLeft) { double xOffset = offsetForward * Math.cos(forwardHeading) - offsetLeft * Math.sin(forwardHeading); double yOffset = offsetForward * Math.sin(forwardHeading) + offsetLeft * Math.cos(forwardHeading); FrameVector2d offsetVector = new FrameVector2d(WORLD_FRAME, xOffset, yOffset); footstepPosition2d.changeFrame(WORLD_FRAME); footstepPosition2d.add(offsetVector); return footstepPosition2d; }
public static void projectOntoPolygonAndCheckDistance(FramePoint2d point, FrameConvexPolygon2d polygon, double epsilon) { ReferenceFrame originalReferenceFrame = point.getReferenceFrame(); point.changeFrame(polygon.getReferenceFrame()); FramePoint2d originalPoint = new FramePoint2d(point); polygon.orthogonalProjection(point); double distance = originalPoint.distance(point); if (distance > epsilon) throw new RuntimeException("point outside polygon by " + distance); point.changeFrame(originalReferenceFrame); }
private void setReferenceFootstepLocation(int footstepIndex, FramePoint2d referenceFootstepLocation) { referenceFootstepLocation.changeFrame(worldFrame); referenceFootstepLocations.get(footstepIndex).set(0, 0, referenceFootstepLocation.getX()); referenceFootstepLocations.get(footstepIndex).set(1, 0, referenceFootstepLocation.getY()); }
public void resetFootstepRegularization(int footstepIndex, FramePoint2d previousFootstepLocation) { previousFootstepLocation.changeFrame(worldFrame); previousFootstepLocations.get(footstepIndex).set(0, 0, previousFootstepLocation.getX()); previousFootstepLocations.get(footstepIndex).set(1, 0, previousFootstepLocation.getY()); }
/** * Add a vertex to this polygon after doing {@code newVertex.changeFrame(this.referenceFrame)}. * Note that this method recycles memory. * @param newVertex {@code FramePoint2d} the new vertex (it is not modified). */ public void addVertexAndChangeFrame(FramePoint2d newVertex) { tempPoint2d.setIncludingFrame(newVertex); tempPoint2d.changeFrame(referenceFrame); convexPolygon.addVertex(tempPoint2d.getPoint()); }
@Override public StraightLineOverheadPath changeFrameCopy(ReferenceFrame desiredFrame) { FramePoint2d newStartPoint = new FramePoint2d(startPoint); newStartPoint.changeFrame(desiredFrame); FrameOrientation2d newStartOrientation = new FrameOrientation2d(orientation); newStartOrientation.changeFrame(desiredFrame); FramePose2d newStartPose = new FramePose2d(newStartPoint, newStartOrientation); FramePoint2d newEndPoint = new FramePoint2d(endPoint); newEndPoint.changeFrame(desiredFrame); return new StraightLineOverheadPath(newStartPose, newEndPoint); }
protected FramePoint2d offsetFootstepFromPath(RobotSide currentFootstepSide, FramePoint2d footstepPosition2d, double footHeading, double offsetAmount) { double sideWaysHeading = footHeading + Math.PI / 2.0; FrameVector2d offsetVector = new FrameVector2d(WORLD_FRAME, Math.cos(sideWaysHeading), Math.sin(sideWaysHeading)); offsetVector.scale(currentFootstepSide.negateIfRightSide(offsetAmount)); FramePoint2d footstepPosition = new FramePoint2d(footstepPosition2d); footstepPosition.changeFrame(WORLD_FRAME); footstepPosition.add(offsetVector); return footstepPosition; }
@Override public void projectCMPIntoSupportPolygonIfOutside(FramePoint2d capturePoint, FrameConvexPolygon2d supportPolygon, FramePoint2d finalDesiredCapturePoint, FramePoint2d desiredCMPToPack) { ReferenceFrame returnFrame = desiredCMPToPack.getReferenceFrame(); desiredCMPToPack.changeFrame(supportPolygon.getReferenceFrame()); capturePoint.changeFrame(supportPolygon.getReferenceFrame()); projectedCMP.setToZero(supportPolygon.getReferenceFrame()); projectedCMP.setX(desiredCMPToPack.getX()); projectedCMP.setY(supportPolygon.getCentroid().getY()); projectCMPIntoSupportPolygonIfOutsideLocal(supportPolygon, desiredCMPToPack); desiredCMPToPack.changeFrame(returnFrame); capturePoint.changeFrame(returnFrame); }
public void updatePointAndPolygon(FrameConvexPolygon2d polygon, FramePoint2d pointInAnyFrame) { FramePoint2d temp = new FramePoint2d(pointInAnyFrame); temp.changeFrame(polygon.getReferenceFrame()); updatePointAndPolygon(polygon, temp.getPoint()); }
private void addSquareUp(ArrayList<FramePose2d> footstepList, FramePoint2d robotPosition) { robotPosition.changeFrame(stanceFootFrame); if (Math.abs(robotPosition.getX()) > 0.001) throw new RuntimeException("Can not square up for given position."); robotPosition.changeFrame(stanceFootFrame); FramePose2d footstepPose = new FramePose2d(stanceFootFrame); footstepPose.setY(2.0*robotPosition.getY()); if(lastStepSide.equals(RobotSide.LEFT) && footstepPose.getY() > 0) throw new RuntimeException("Left foot can not be placed on right side of right foot"); if (lastStepSide.equals(RobotSide.RIGHT) && footstepPose.getY() < 0) throw new RuntimeException("Right foot can not be placed on left side of left foot"); footstepPose.changeFrame(ReferenceFrame.getWorldFrame()); footstepList.add(footstepPose); stanceFootFrame.setPoseAndUpdate(footstepPose); lastStepSide = lastStepSide.getOppositeSide(); }
@Override public void changeFrame(ReferenceFrame desiredFrame) { get(point2d); ReferenceFrame currentReferenceFrame = multipleFramesHelper.switchCurrentReferenceFrame(desiredFrame); framePoint2d.setIncludingFrame(currentReferenceFrame, point2d); framePoint2d.changeFrame(desiredFrame); framePoint2d.get(point2d); set(point2d); }
public void setPoseAndUpdate(FramePoint2d position, FrameOrientation2d orientation) { position.changeFrame(originPose.getReferenceFrame()); originPose.setPosition(position); orientation.changeFrame(originPose.getReferenceFrame()); originPose.setOrientation(orientation); this.update(); }
@Override public TurningOverheadPath changeFrameCopy(ReferenceFrame desiredFrame) { FramePoint2d newStartPoint = new FramePoint2d(point); newStartPoint.changeFrame(desiredFrame); FrameOrientation2d newStartOrientation = new FrameOrientation2d(startOrientation); newStartOrientation.changeFrame(desiredFrame); FramePose2d newStartPose = new FramePose2d(newStartPoint, newStartOrientation); FrameOrientation2d newEndOrientation = new FrameOrientation2d(endOrientation); newEndOrientation.changeFrame(desiredFrame); return new TurningOverheadPath(newStartPose, newEndOrientation); }
private void computeDistanceAndAngleToDestination(ReferenceFrame supportAnkleZUpFrame, RobotSide swingLegSide, FramePoint2d desiredDestination) { FramePoint2d destinationInAnkleFrame = new FramePoint2d(desiredDestination); destinationInAnkleFrame.changeFrame(supportAnkleZUpFrame); FramePoint2d squaredUpMidpointInAnkleFrame = new FramePoint2d(supportAnkleZUpFrame, 0.0, swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue() / 2.0)); FrameVector2d midpointToDestination = new FrameVector2d(destinationInAnkleFrame); midpointToDestination.sub(squaredUpMidpointInAnkleFrame); distanceToDestination.set(midpointToDestination.length()); angleToDestination.set(Math.atan2(midpointToDestination.getY(), midpointToDestination.getX())); }
public void computeAchievedCMP(FrameVector achievedLinearMomentumRate, FramePoint2d achievedCMPToPack) { if (achievedLinearMomentumRate.containsNaN()) return; centerOfMass2d.setToZero(centerOfMassFrame); centerOfMass2d.changeFrame(worldFrame); achievedCoMAcceleration2d.setByProjectionOntoXYPlaneIncludingFrame(achievedLinearMomentumRate); achievedCoMAcceleration2d.scale(1.0 / totalMass); achievedCoMAcceleration2d.changeFrame(worldFrame); achievedCMPToPack.set(achievedCoMAcceleration2d); achievedCMPToPack.scale(-1.0 / (omega0 * omega0)); achievedCMPToPack.add(centerOfMass2d); }
/** * 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); }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
public FrameVector2d computeDesiredMomentumXY(FramePoint2d desiredCoMXY, MutableDouble errorMagnitude) { FrameVector2d ret = new FrameVector2d(); FramePoint2d errorCoMXY = new FramePoint2d(desiredCoMXY); errorCoMXY.changeFrame(referenceFrames.getCenterOfMassFrame()); errorMagnitude.setValue(MathTools.square(errorCoMXY.getX()) + MathTools.square(errorCoMXY.getY())); errorMagnitude.setValue(Math.sqrt(errorMagnitude.doubleValue())); errorCoMXY.scale(1.0 / updateDT); ret.setIncludingFrame(errorCoMXY); ret.scale(toolbox.getTotalRobotMass()); return ret; }
private FramePose2d getoffsetPoint() { FramePoint2d ballPosition2d = new FramePoint2d(ReferenceFrame.getWorldFrame(), sphereDetectionBehavior.getBallLocation().getX(), sphereDetectionBehavior.getBallLocation().getY()); FramePoint2d robotPosition = new FramePoint2d(midZupFrame, 0.0, 0.0); robotPosition.changeFrame(worldFrame); FrameVector2d walkingDirection = new FrameVector2d(worldFrame); walkingDirection.set(ballPosition2d); walkingDirection.sub(robotPosition); walkingDirection.normalize(); double walkingYaw = Math.atan2(walkingDirection.getY(), walkingDirection.getX()); double x = ballPosition2d.getX() - walkingDirection.getX() * standingDistance; double y = ballPosition2d.getY() - walkingDirection.getY() * standingDistance; FramePose2d poseToWalkTo = new FramePose2d(worldFrame, new Point2d(x, y), walkingYaw); return poseToWalkTo; }