public Vector2dZUpFrame(String string, ReferenceFrame parentFrame) { super(string, parentFrame); xAxis = new FrameVector2d(parentFrame); }
public Vector2dZUpFrame(String string, ReferenceFrame parentFrame) { super(string, parentFrame); xAxis = new FrameVector2d(parentFrame); }
protected FrameVector2d createEmptyFrameTuple2d() { return new FrameVector2d(); }
public static FrameVector2d generateRandomFrameVector2d(Random random, ReferenceFrame zUpFrame) { double randomAngle = RandomTools.generateRandomDouble(random, -Math.PI, Math.PI); FrameVector2d randomVector = new FrameVector2d(zUpFrame, Math.cos(randomAngle), Math.sin(randomAngle)); return randomVector; }
public final FrameVector2d getFrameVector2dCopy() { return new FrameVector2d(getFrameTuple2d()); }
public FrameVector2d perpendicularFrameVector() { return new FrameVector2d(referenceFrame, line.perpendicularVector()); }
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; }
@Override public FrameVector2d getFinalHeadingTarget() { FrameVector2d finalHeading = new FrameVector2d(ReferenceFrame.getWorldFrame(), Math.cos(desiredHeadingFinal.getDoubleValue()), Math.sin(desiredHeadingFinal.getDoubleValue())); return finalHeading; }
/** * Creates a new FrameVector2d based on the x and y components of this FrameVector */ public FrameVector2d toFrameVector2d() { return new FrameVector2d(this.getReferenceFrame(), this.getX(), this.getY()); }
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; }
public final FrameVector2d getFrameVector2dCopy() { return new FrameVector2d(getReferenceFrame(), getX(), getY()); }
private double computeFrameOrientationRelativeToWalkingPath(ReferenceFrame referenceFrame) { this.walkPathVector.sub(this.targetLocation, robotYoPose.getPosition()); fullRobotModel.updateFrames(); FrameVector2d frameHeadingVector = new FrameVector2d(referenceFrame, 1.0, 0.0); frameHeadingVector.changeFrame(worldFrame); double ret = -frameHeadingVector.angle(walkPathVector.getFrameVector2dCopy()); if (DEBUG) { PrintTools.debug(this, "FrameHeadingVector : " + frameHeadingVector); PrintTools.debug(this, "WalkPathVector : " + walkPathVector); PrintTools.debug(this, "OrientationToWalkPath : " + ret); } return ret; }
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())); }
/** * yawAboutPoint * * @param pointToYawAbout FramePoint2d * @param yaw double * @return CartesianPositionFootstep */ public void yawAboutPoint(FramePoint2d pointToYawAbout, FramePoint2d pointToPack, double yaw) { if (temporaryPointForYawing == null) temporaryPointForYawing = new FrameVector2d(this); else temporaryPointForYawing.setIncludingFrame(this); temporaryPointForYawing.sub(pointToYawAbout); temporaryTransformToDesiredFrame.setIdentity(); temporaryTransformToDesiredFrame.setRotationYawAndZeroTranslation(yaw); temporaryPointForYawing.applyTransform(temporaryTransformToDesiredFrame); pointToPack.setIncludingFrame(pointToYawAbout); pointToPack.add(temporaryPointForYawing); }
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(), pickUpLocation.getX(), pickUpLocation.getY()); FramePoint2d robotPosition = new FramePoint2d(midZupFrame, 0.0, 0.0); robotPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameVector2d walkingDirection = new FrameVector2d(ReferenceFrame.getWorldFrame()); walkingDirection.set(ballPosition2d); walkingDirection.sub(robotPosition); walkingDirection.normalize(); double walkingYaw = Math.atan2(walkingDirection.getY(), walkingDirection.getX()); //get a point offset from the ball double x = ballPosition2d.getX() - walkingDirection.getX() * standingDistance; double y = ballPosition2d.getY() - walkingDirection.getY() * standingDistance; double rotationAngle = Math.toRadians(55); //rotate that point around the ball so that the robot stands to the side. double newX = ballPosition2d.getX() + (x - ballPosition2d.getX()) * Math.cos(rotationAngle) - (y - ballPosition2d.getY()) * Math.sin(rotationAngle); double newY = ballPosition2d.getY() + (x - ballPosition2d.getX()) * Math.sin(rotationAngle) + (y - ballPosition2d.getY()) * Math.cos(rotationAngle); FramePose2d poseToWalkTo = new FramePose2d(ReferenceFrame.getWorldFrame(), new Point2d(newX, newY), walkingYaw); return poseToWalkTo; }
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 void sequenceMediumWarmup() { FramePoint2d center = new FramePoint2d(midFeetZUpFrame); FrameVector2d shiftScaleVector = new FrameVector2d(midFeetZUpFrame, 0.1, 0.7); FrameConvexPolygon2d supportPolygon = new FrameConvexPolygon2d(yoSupportPolygon.getFrameConvexPolygon2d()); supportPolygon.changeFrameAndProjectToXYPlane(midFeetZUpFrame); FrameVector2d desiredPelvisOffset = new FrameVector2d(midFeetZUpFrame); for (int i = 0; i < supportPolygon.getNumberOfVertices(); i++) { supportPolygon.getFrameVertex(i, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, shiftScaleVector.getX() * desiredPelvisOffset.getX(), shiftScaleVector.getY() * desiredPelvisOffset.getY(), 0.0); sequenceSquats(); sequenceChestRotations(0.55); //TODO increase/decrease limit? sequencePelvisRotations(0.3); //TODO increase/decrease limit? } // Get back to the first vertex again supportPolygon.getFrameVertex(0, desiredPelvisOffset); desiredPelvisOffset.sub(center); submitDesiredPelvisPositionOffset(false, pelvisShiftScaleFactor.getX() * desiredPelvisOffset.getX(), pelvisShiftScaleFactor.getY() * desiredPelvisOffset.getY(), 0.0); submitChestHomeCommand(false); submitPelvisHomeCommand(false); }
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; }
@Override protected void setBehaviorInput() { TextToSpeechPacket p1 = new TextToSpeechPacket("Walking To Point Two"); sendPacket(p1); walkToPoint2.changeFrame(ReferenceFrame.getWorldFrame()); FramePoint2d walkPosition2d = new FramePoint2d(ReferenceFrame.getWorldFrame(), walkToPoint2.getX(), walkToPoint2.getY()); FramePoint2d robotPosition = new FramePoint2d(midZupFrame, 0.0, 0.0); robotPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameVector2d walkingDirection = new FrameVector2d(ReferenceFrame.getWorldFrame()); walkingDirection.set(walkPosition2d); walkingDirection.sub(robotPosition); walkingDirection.normalize(); float walkingYaw = (float) Math.atan2(walkingDirection.getY(), walkingDirection.getX()); Quaternion q = new Quaternion(new float[] {0, 0, walkingYaw}); FramePose poseToWalkTo = new FramePose(ReferenceFrame.getWorldFrame(), new Point3d(walkToPoint2.getX(), walkToPoint2.getY(), 0), JMEDataTypeUtils.jMEQuaternionToVecMathQuat4d(q)); atlasPrimitiveActions.walkToLocationPlannedBehavior.setTarget(poseToWalkTo); } };