public static void getSupportVector(FrameVector normalizedSupportVectorToPack, double angle, double mu, ReferenceFrame contactPlaneFrame) { double x = mu * Math.cos(angle); double y = mu * Math.sin(angle); double z = 1.0; normalizedSupportVectorToPack.setIncludingFrame(contactPlaneFrame, x, y, z); normalizedSupportVectorToPack.normalize(); }
public void normalize() { getFrameTuple().normalize(); getYoValuesFromFrameTuple(); }
private void computeBasisVector(int basisVectorIndex, Matrix3d normalContactVectorRotationMatrix, FrameVector basisVectorToPack) { double angle = basisVectorIndex * basisVectorAngleIncrement; double mu = yoPlaneContactState.getCoefficientOfFriction(); // Compute the linear part considering a normal contact vector pointing z-up basisVectorToPack.setIncludingFrame(planeFrame, Math.cos(angle) * mu, Math.sin(angle) * mu, 1.0); // Transforming the result to consider the actual normal contact vector normalContactVectorRotationMatrix.transform(basisVectorToPack.getVector()); basisVectorToPack.normalize(); }
private void computeNormalContactVectorRotation(Matrix3d normalContactVectorRotationMatrixToPack) { yoPlaneContactState.getContactNormalFrameVector(contactNormalVector); contactNormalVector.changeFrame(planeFrame); contactNormalVector.normalize(); GeometryTools.getAxisAngleFromZUpToVector(contactNormalVector.getVector(), normalContactVectorRotation); normalContactVectorRotationMatrixToPack.set(normalContactVectorRotation); }
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()); } }
private void setAccelerationEndpointPositions() { for (int i : new int[] {0, 1}) { FrameVector waypointToEndpoint = getWaypointToEndpoint(i); FrameVector oppositeWaypointToEndpoint = getOppositeWaypointToEndpoint(i); double scaleFactor = waypointToEndpoint.dot(oppositeWaypointToEndpoint) / oppositeWaypointToEndpoint.length() * linearSplineLengthFactor.getDoubleValue(); oppositeWaypointToEndpoint.normalize(); oppositeWaypointToEndpoint.scale(scaleFactor); allPositions[accelerationEndpointIndices[i]].set(allPositions[waypointIndices[i]].getFramePointCopy()); allPositions[accelerationEndpointIndices[i]].add(oppositeWaypointToEndpoint); } }
directionOfMotion.normalize();
@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); } };
private void setWaypointAndAccelerationEndpointTimesAndVelocities(double[] arcLengths) { double waypointSpeed = waypointsAreTheSamePoint ? 0.0 : getWaypointSpeed(arcLengths); for (int i = 0; i < 2; i++) { FrameVector waypointVelocity = getOppositeWaypointToEndpoint(i); waypointVelocity.normalize(); waypointVelocity.scale(waypointSpeed * ((i == 0) ? -1 : 1)); allVelocities[waypointIndices[i]].set(waypointVelocity); allVelocities[accelerationEndpointIndices[i]].set(waypointVelocity); } if (waypointsAreTheSamePoint) { double totalArcLength = getTotalArcLength(arcLengths); allTimes[1].set((arcLengths[0] + arcLengths[1]) / totalArcLength * stepTime.getDoubleValue()); for (int i = 2; i < 5; i++) { allTimes[i].set(allTimes[1].getDoubleValue()); } } else { allTimes[1].set(2 * arcLengths[0] / (allVelocities[0].length() + waypointSpeed)); allTimes[2].set(allTimes[1].getDoubleValue() + arcLengths[1] / waypointSpeed); allTimes[3].set(allTimes[2].getDoubleValue() + arcLengths[2] / waypointSpeed); allTimes[4].set(allTimes[3].getDoubleValue() + arcLengths[3] / waypointSpeed); } }
/** * 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); } }
tmpVector.normalize(); tmpVector.scale(deadband);
closestEdgeToCoP.getFrameVector(edgeVector2d); edgeVector.setXYIncludingFrame(edgeVector2d); edgeVector.normalize(); desiredOrientationCopy.setIncludingFrame(desiredOrientation); desiredOrientationCopy.changeFrame(footPolygon.getReferenceFrame());
pointingBackwardVector.normalize(); pointingBackwardVector.scale(0.15); pointingBackwardVector.changeFrame(worldFrame);
FrameVector tempVector = new FrameVector(tempPoint); MathTools.floorToGivenPrecision(tempVector.getVector(), 1.0e-2); tempVector.normalize();
omegaVectorDF.normalize();