public static void packFramePointInJMEVector(FramePoint original, Vector3f target) { target.set((float) original.getX(), (float) original.getY(), (float) original.getZ()); }
/** * Sets the next ball to the given location, and gives it the given appearance. If all the balls have been set, then does nothing. * @param location FramePoint to set the next ball to. * @param appearance Appearance to give the next ball. */ public void setBall(FramePoint location, AppearanceDefinition appearance, int ballIndex) { //TODO: PDN, note that with current implementation of JME, you can only "set" the appearance once. After that, it will ignore all appearance sets if (!location.getReferenceFrame().isWorldFrame()) throw new RuntimeException(location + " must be in a World Frame!"); setBall(location.getX(), location.getY(), location.getZ(), appearance, ballIndex); }
private void moveUpSlightlyToEnsureVisible(YoGraphicPolygon footstepToExpandViz) { FramePoint framePointToPack = new FramePoint(worldFrame); footstepToExpandViz.getPosition(framePointToPack); framePointToPack.setZ(framePointToPack.getZ() + 0.0025); footstepToExpandViz.setPosition(framePointToPack); }
public void setPosition(FramePoint position) { this.x.set(position.getX()); this.y.set(position.getY()); if (this.z != null) this.z.set(position.getZ()); }
public void setPosition(FramePoint position) { x.set(position.getX()); y.set(position.getY()); z.set(position.getZ()); }
public FrameVector getCurrentDesiredAcceleration(double timeInMove) { FramePoint Xfh = getCurrentDesiredPoint(timeInMove + stepSizeforNumericalCalculation); FramePoint X = getCurrentDesiredPoint(timeInMove); FramePoint Xrh = getCurrentDesiredPoint(timeInMove - stepSizeforNumericalCalculation); FrameVector ret = new FrameVector(X.getReferenceFrame()); ret.setX((Xfh.getX() - 2.0 * X.getX() + Xrh.getX()) / (MathTools.square(stepSizeforNumericalCalculation))); ret.setY((Xfh.getY() - 2.0 * X.getY() + Xrh.getY()) / (MathTools.square(stepSizeforNumericalCalculation))); ret.setZ((Xfh.getZ() - 2.0 * X.getZ() + Xrh.getZ()) / (MathTools.square(stepSizeforNumericalCalculation))); return ret; }
public void getPose(RigidBodyTransform rigidBodyTransformToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.getTransform3D(rigidBodyTransformToPack); rigidBodyTransformToPack.setTranslation(tempFramePoint.getX(), tempFramePoint.getY(), tempFramePoint.getZ()); }
public void update(FramePoint pointUnfiltered) { checkReferenceFrameMatch(pointUnfiltered); x.update(pointUnfiltered.getX()); y.update(pointUnfiltered.getY()); z.update(pointUnfiltered.getZ()); }
public void getPose(RigidBodyTransform rigidBodyTransformToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); tempFrameOrientation.getTransform3D(rigidBodyTransformToPack); rigidBodyTransformToPack.setTranslation(tempFramePoint.getX(), tempFramePoint.getY(), tempFramePoint.getZ()); }
public void update() { virtualLegTangentialFrameHipCentered.update(); virtualLegTangentialFrameAnkleCentered.update(); anklePosition.setToZero(endEffectorFrame); anklePosition.changeFrame(worldFrame); yoCurrentFootPosition.set(anklePosition); anklePosition.changeFrame(virtualLegTangentialFrameHipCentered); currentLegLength.set(-anklePosition.getZ()); }
public void initialize() { currentTime.set(0.0); this.trajectoryTime.set(trajectoryTimeProvider.getValue()); initialPositionProvider.getPosition(tempFramePoint); tempFramePoint.changeFrame(referenceFrame); double y = tempFramePoint.getY(); double x = tempFramePoint.getX(); z.set(tempFramePoint.getZ()); radius.set(Math.sqrt(x * x + y * y)); double initialAngle = Math.atan2(y, x); double finalAngle = initialAngle + desiredRotationAngleProvider.getValue(); anglePolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), initialAngle, 0.0, 0.0, finalAngle, 0.0, 0.0); }
public ReferenceFrame copyAndAimAxisAtPoint(Axis axisToAlign, FramePoint targetToAimAt) { ReferenceFrame initialFrame = targetToAimAt.getReferenceFrame(); targetToAimAt.changeFrame(this); FrameVector targetRelativeToCurrentFrame = new FrameVector(this, targetToAimAt.getX(), targetToAimAt.getY(), targetToAimAt.getZ()); targetToAimAt.changeFrame(initialFrame); return copyAndAlignAxisWithVector(axisToAlign, targetRelativeToCurrentFrame); }
public void solve(CoMHeightPartialDerivativesData coMHeightPartialDerivativesDataToPack, boolean isInDoubleSupport) { getCenterOfMass2d(queryPoint, centerOfMassFrame); solutionPoint.set(queryPoint); solve(coMHeightPartialDerivativesDataToPack, solutionPoint, isInDoubleSupport); coMHeightPartialDerivativesDataToPack.getCoMHeight(tempFramePoint); desiredCoMPosition.set(queryPoint.getX(), queryPoint.getY(), tempFramePoint.getZ()); }
public void initialize(CoMHeightTimeDerivativesData comHeightDataIn) { comHeightDataIn.getComHeight(centerOfMassHeightPoint); smoothComHeight.set(centerOfMassHeightPoint.getZ()); smoothComHeightVelocity.set(comHeightDataIn.getComHeightVelocity()); smoothComHeightAcceleration.set(comHeightDataIn.getComHeightAcceleration()); this.hasBeenInitialized.set(true); }
private void updateSensorPosition() { sensorFrame.update(); sensorPose.setPose(sensorFrame.getTransformToDesiredFrame(world)); sensorPose.getPositionIncludingFrame(temp); yoSensorPositionInWorld.set(temp.getReferenceFrame(), temp.getX(), temp.getY(), temp.getZ()); }
public static void computePseudoCMP3d(FramePoint pseudoCMP3dToPack, FramePoint centerOfMass, FramePoint2d cmp, double fZ, double totalMass, double omega0) { double zCMP = centerOfMass.getZ() - fZ / (totalMass * MathTools.square(omega0)); pseudoCMP3dToPack.setIncludingFrame(cmp.getReferenceFrame(), cmp.getX(), cmp.getY(), 0.0); pseudoCMP3dToPack.changeFrame(centerOfMass.getReferenceFrame()); pseudoCMP3dToPack.setZ(zCMP); }
@Override public void variableChanged(YoVariable<?> v) { if (userDesiredSetPelvisHeightToActual.getBooleanValue()) { ReferenceFrame pelvisFrame = fullRobotModel.getPelvis().getBodyFixedFrame(); FramePose currentPose = new FramePose(pelvisFrame); currentPose.changeFrame(ReferenceFrame.getWorldFrame()); userDesiredPelvisHeight.set(currentPose.getFramePointCopy().getZ()); userDesiredSetPelvisHeightToActual.set(false); } } });
public void computePositionsForVis(double time) { nominalTrajectoryGenerator.compute(time); xPolynomial.compute(time); yPolynomial.compute(time); nominalTrajectoryGenerator.getPosition(nominalTrajectoryPosition); nominalTrajectoryGenerator.getVelocity(nominalTrajectoryVelocity); nominalTrajectoryGenerator.getAcceleration(nominalTrajectoryAcceleration); desiredPosition.setX(xPolynomial.getPosition()); desiredPosition.setY(yPolynomial.getPosition()); desiredPosition.setZ(nominalTrajectoryPosition.getZ()); }
public Graphics3DObject createVisualization(FramePoint voxelLocation, double scale, double reachabilityValue) { ReferenceFrame originalFrame = voxelLocation.getReferenceFrame(); voxelLocation.changeFrame(ReferenceFrame.getWorldFrame()); Graphics3DObject voxelViz = new Graphics3DObject(); AppearanceDefinition appearance = YoAppearance.RGBColorFromHex(Color.HSBtoRGB((float) (0.7 * reachabilityValue), 1.0f, 1.0f)); voxelViz.translate(voxelLocation.getX(), voxelLocation.getY(), voxelLocation.getZ()); voxelViz.addSphere(scale * voxelSize / 2.0, appearance); voxelLocation.changeFrame(originalFrame); return voxelViz; } }
public void getClosestVoxel(FramePoint voxelLocationToPack, FramePoint inputPoint) { checkReferenceFrameMatch(inputPoint); if (!boundingBox.isInside(inputPoint.getPoint())) throw new RuntimeException("The given point is outside the grid"); voxelLocationToPack.setToZero(getReferenceFrame()); voxelLocationToPack.setX(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getX()))); voxelLocationToPack.setY(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getY()))); voxelLocationToPack.setZ(getCoordinateFromIndexUnsafe(getIndexFromCoordinateUnsafe(inputPoint.getZ()))); }