public void normalize() { getFrameTuple2d().normalize(); getYoValuesFromFrameTuple2d(); } }
public void setXAxis(FrameVector2d xAxis) { this.xAxis.setIncludingFrame(xAxis); this.xAxis.changeFrame(parentFrame); this.xAxis.normalize(); update(); }
public void setXAxis(FrameVector2d xAxis) { this.xAxis.setIncludingFrame(xAxis); this.xAxis.changeFrame(parentFrame); this.xAxis.normalize(); update(); }
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; }
private FramePose2d getoffsetPoint() { FramePoint2d ballPosition2d = new FramePoint2d(ReferenceFrame.getWorldFrame(), initialSphereDetectionBehavior.getBallLocation().getX(), initialSphereDetectionBehavior.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()); //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(worldFrame, new Point2d(newX, newY), walkingYaw); return poseToWalkTo; }
private void setXYVectorFromPoseToPoseNormalize(YoFrameVector2d vectorToPack, FramePose fromPose, FramePose toPose) { if (fromPose.epsilonEquals(toPose, 1e-7, Double.MAX_VALUE)) { vectorToPack.set(fromPose.getReferenceFrame(), 0.0, 0.0); } else { FrameVector2d frameTuple2d = vectorToPack.getFrameTuple2d(); frameTuple2d.setByProjectionOntoXYPlane(toPose.getFramePointCopy()); fromPose.checkReferenceFrameMatch(vectorToPack); frameTuple2d.sub(fromPose.getX(), fromPose.getY()); frameTuple2d.normalize(); vectorToPack.setWithoutChecks(frameTuple2d); } }
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; }
vector.normalize(); vector.scale(distanceToBeInside); pointToMove.set(intersections[i]);
double yDirection = Math.signum(shiftingVector.getY()); shiftedLineVector.normalize(); if (Math.abs(theta % Math.PI / 2.0) > 0.1)
if (intersectionToUse == intersections[1]) frameVector2d.scale(-1.0); frameVector2d.normalize(); frameVector2d.scale(amountToBeInside);
@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); } };
/** * This function projects the footstep midpoint in the capture region. * Might be a bit conservative it should be sufficient to slightly overlap the capture region * and the touch-down polygon. */ private void projectFootstepInCaptureRegion(Footstep footstep, FramePoint2d projectionPoint, FrameConvexPolygon2d captureRegion) { projection.setIncludingFrame(projectionPoint); projection.changeFrame(footstep.getParentFrame()); // move the position of the footstep to the capture region centroid nextStep2d.setIncludingFrame(captureRegion.getCentroid()); nextStep2d.changeFrame(footstep.getParentFrame()); // move the position as far away from the projectionPoint as possible direction.setIncludingFrame(nextStep2d); direction.sub(projection); direction.normalize(); direction.scale(10.0); nextStep2d.add(direction); nextStep2d.changeFrame(captureRegion.getReferenceFrame()); captureRegion.orthogonalProjection(nextStep2d); nextStep2d.changeFrame(footstep.getParentFrame()); footstep.setPositionChangeOnlyXY(nextStep2d); calculateTouchdownFootPolygon(footstep, captureRegion.getReferenceFrame(), touchdownFootPolygon); }
footAngularVelocityUnitVector.normalize();