/** * 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()); }
/** * Computes the 2D vector perpendicular to the given 2D {@code vector} such that: * <ul> * <li> {@code vector2d.dot(perpendicularVector2d) == 0.0}. * <li> {@code vector2d.angle(perpendicularVector2d) == Math.PI / 2.0}. * </ul> * <p> * WARNING: the 3D arguments are projected onto the XY-plane to perform the actual computation in 2D. * </p> * * @param vector the vector to compute in the xy-plane the perpendicular of. Not modified. * @param perpendicularVectorToPack a vector in which the x and y components of the 2D perpendicular vector are stored. Modified. */ // FIXME this is just bad. public static void getPerpendicularVector2d(FrameVector vector, FrameVector perpendicularVectorToPack) { perpendicularVectorToPack.set(-vector.getY(), vector.getX(), perpendicularVectorToPack.getZ()); }
private void estimateYawDriftPerFoot() { if (numberOfFeetTrusted.getIntegerValue() != numberOfFeet) { for (int i = 0; i < numberOfFeet; i++) estimatedYawDriftPerFoot.get(allFeet.get(i)).set(Double.NaN); return; } for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); referenceAverageFootPosition.getFrameTupleIncludingFrame(averagePosition); referenceFootPositions.get(foot).getFrameTupleIncludingFrame(footPosition); referenceAverageToFootPosition.setToZero(worldFrame); referenceAverageToFootPosition.sub(footPosition, averagePosition); currentAverageFootPosition.getFrameTupleIncludingFrame(averagePosition); currentFootPositions.get(foot).getFrameTupleIncludingFrame(footPosition); currentAverageToFootPosition.setToZero(worldFrame); currentAverageToFootPosition.sub(footPosition, averagePosition); double refenceAngle = Math.atan2(referenceAverageToFootPosition.getY(), referenceAverageToFootPosition.getX()); double currentAngle = Math.atan2(currentAverageToFootPosition.getY(), currentAverageToFootPosition.getX()); estimatedYawDriftPerFoot.get(foot).set(AngleTools.computeAngleDifferenceMinusPiToPi(currentAngle, refenceAngle)); } }
public void update(FrameVector vectorUnfiltered) { x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { matrix.set(rowStart + 0, 0, jointAngularVelocity.getX()); matrix.set(rowStart + 1, 0, jointAngularVelocity.getY()); matrix.set(rowStart + 2, 0, jointAngularVelocity.getZ()); }
@Override public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart) { matrix.set(rowStart + 0, 0, jointAngularAccelerationDesired.getX()); matrix.set(rowStart + 1, 0, jointAngularAccelerationDesired.getY()); matrix.set(rowStart + 2, 0, jointAngularAccelerationDesired.getZ()); }
public static double getRadialVelocity(FramePoint position, FrameVector velocity) { position.checkReferenceFrameMatch(velocity); double x = position.getX(); double y = position.getY(); double xd = velocity.getX(); double yd = velocity.getY(); double radius = getRadius(position); return (x * xd + y * yd) / radius; }
@Override public void getTauMatrix(DenseMatrix64F matrix) { matrix.set(0, 0, jointTorque.getX()); matrix.set(1, 0, jointTorque.getY()); matrix.set(2, 0, jointTorque.getZ()); }
public void update(FrameVector vectorUnfiltered) { checkReferenceFrameMatch(vectorUnfiltered); x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
public static double getAngularVelocity(FramePoint position, FrameVector velocity) { position.checkReferenceFrameMatch(velocity); double x = position.getX(); double y = position.getY(); double xd = velocity.getX(); double yd = velocity.getY(); double radiusSquared = MathTools.square(x) + MathTools.square(y); return (x * yd - y * xd) / radiusSquared; } }
public void update(FrameVector vectorSource) { checkReferenceFrameMatch(vectorSource); x.update(vectorSource.getX()); y.update(vectorSource.getY()); z.update(vectorSource.getZ()); }
public void update(FrameVector vectorUnfiltered) { checkReferenceFrameMatch(vectorUnfiltered); x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
private void doYoVectorCrossProduct(YoFrameVector v1, YoFrameVector v2, YoFrameVector vecToPack) { temp.cross(v1.getFrameTuple(), v2.getFrameTuple()); if (temp.length() > 0) { temp.normalize(); } vecToPack.set(world, temp.getX(), temp.getY(), temp.getZ()); } }
public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint) { filteredYoAngularMomentum.getFrameTuple(angularMomentum); ReferenceFrame comFrame = angularMomentum.getReferenceFrame(); localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint); localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame); double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity); adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX()); adjustedDesiredCapturePoint.scale(scaleFactor); adjustedDesiredCapturePoint.add(localDesiredCapturePoint); adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame()); }
public static void computeWrench(Wrench groundReactionWrenchToPack, FrameVector force, FramePoint2d cop, double normalTorque) { ReferenceFrame referenceFrame = cop.getReferenceFrame(); force.changeFrame(referenceFrame); // r x f + tauN Vector3d torque = new Vector3d(); torque.setX(cop.getY() * force.getZ()); torque.setY(-cop.getX() * force.getZ()); torque.setZ(cop.getX() * force.getY() - cop.getY() * force.getX() + normalTorque); groundReactionWrenchToPack.set(referenceFrame, force.getVector(), torque); }
vectorBetweenFrames2d.set(vectorBetweenFrames.getX(), vectorBetweenFrames.getY());
@Override protected void setBehaviorInput() { TextToSpeechPacket p1 = new TextToSpeechPacket("Walking To Point One"); sendPacket(p1); walkToPoint1.changeFrame(ReferenceFrame.getWorldFrame()); FramePoint walkPosition2d = new FramePoint(ReferenceFrame.getWorldFrame(), walkToPoint1.getX(), walkToPoint1.getY(), 0); FramePoint robotPosition = new FramePoint(midZupFrame, 0.0, 0.0, 0.0); robotPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameVector walkingDirection = new FrameVector(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(walkToPoint1.getX(), walkToPoint1.getY(), 0), JMEDataTypeUtils.jMEQuaternionToVecMathQuat4d(q)); atlasPrimitiveActions.walkToLocationPlannedBehavior.setTarget(poseToWalkTo); } };
public void initialize() { swingTime.set(swingTimeProvider.getValue()); timeIntoStep.set(swingTime.getDoubleValue() - swingTimeRemainingProvider.getValue()); positionSources[0].getPosition(tempPosition); tempPosition.changeFrame(desiredPosition.getReferenceFrame()); double x0 = tempPosition.getX(); double y0 = tempPosition.getY(); velocitySources[0].get(tempVector); tempVector.changeFrame(desiredPosition.getReferenceFrame()); double xd0 = tempVector.getX(); double yd0 = tempVector.getY(); positionSources[1].getPosition(tempPosition); tempPosition.changeFrame(desiredPosition.getReferenceFrame()); double xFinal = tempPosition.getX(); double yFinal = tempPosition.getY(); nominalTrajectoryGenerator.compute(timeIntoStep.getDoubleValue()); nominalTrajectoryGenerator.getAcceleration(tempVector); tempVector.changeFrame(desiredPosition.getReferenceFrame()); double xdd0 = tempVector.getX(); double ydd0 = tempVector.getY(); xPolynomial.setQuintic(timeIntoStep.getDoubleValue(), swingTime.getDoubleValue(), x0, xd0, xdd0, xFinal, 0.0, 0.0); yPolynomial.setQuintic(timeIntoStep.getDoubleValue(), swingTime.getDoubleValue(), y0, yd0, ydd0, yFinal, 0.0, 0.0); if (VISUALIZE) { visualizeTrajectory(); } }
/** * Will return a point on a circle around the origin. The point will be in between the given directions and at * a position specified by the alpha value. E.g. an alpha value of 0.5 will result in the point being in the * middle of the given directions. */ public void getPointBetweenVectorsAtDistanceFromOriginCircular(FrameVector2d directionA, FrameVector2d directionB, double alpha, double radius, FramePoint2d midpoint, FramePoint2d pointToPack) { directionA.checkReferenceFrameMatch(directionB.getReferenceFrame()); directionA.checkReferenceFrameMatch(midpoint.getReferenceFrame()); alpha = MathTools.clipToMinMax(alpha, 0.0, 1.0); double angleBetweenDirections = directionA.angle(directionB); double angleBetweenDirectionsToSetLine = angleBetweenDirections * alpha; rotatedFromA.setToZero(directionA.getReferenceFrame()); rotatedFromA.set(directionA.getX(), directionA.getY(), 0.0); axisAngle.set(negZRotationAxis, angleBetweenDirectionsToSetLine); rotation.setRotation(axisAngle); rotatedFromA.applyTransform(rotation); rotatedFromA.normalize(); rotatedFromA.scale(radius); pointToPack.changeFrame(rotatedFromA.getReferenceFrame()); pointToPack.set(rotatedFromA.getX(), rotatedFromA.getY()); pointToPack.add(midpoint); } }
public FrameVector getCurrentDesiredVelocity(double timeInMove) { FramePoint Xfh = getCurrentDesiredPoint(timeInMove + stepSizeforNumericalCalculation); FramePoint Xf2h = getCurrentDesiredPoint(timeInMove + 2.0 * stepSizeforNumericalCalculation); FramePoint Xrh = getCurrentDesiredPoint(timeInMove - stepSizeforNumericalCalculation); FramePoint Xr2h = getCurrentDesiredPoint(timeInMove - 2.0 * stepSizeforNumericalCalculation); FrameVector ret = new FrameVector(Xfh.getReferenceFrame()); ret.setX((-Xf2h.getX() + 8.0 * Xfh.getX() - 8.0 * Xrh.getX() + Xr2h.getX()) / (12.0 * stepSizeforNumericalCalculation)); ret.setY((-Xf2h.getY() + 8.0 * Xfh.getY() - 8.0 * Xrh.getY() + Xr2h.getY()) / (12.0 * stepSizeforNumericalCalculation)); ret.setZ((-Xf2h.getZ() + 8.0 * Xfh.getZ() - 8.0 * Xrh.getZ() + Xr2h.getZ()) / (12.0 * stepSizeforNumericalCalculation)); // Edge cases result in half the expected value, so multiply by 2 if ((timeInMove <= stepSizeforNumericalCalculation) || (this.moveDuration - timeInMove <= stepSizeforNumericalCalculation)) { ret.setX(ret.getX() * 2); ret.setY(ret.getY() * 2); ret.setZ(ret.getZ() * 2); } return ret; }