/** * Computes the complete minimum rotation from {@code zUp = (0, 0, 1)} to the given {@code vector} and packs it into an {@link AxisAngle4d}. * The rotation axis if perpendicular to both vectors. * The rotation angle is computed as the angle from the {@code zUp} to the {@code vector}: * <br> {@code rotationAngle = zUp.angle(vector)}. </br> * Note: the vector does not need to be unit length. * <p> * Edge cases: * <ul> * <li> the vector is aligned with {@code zUp}: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * <li> the vector is collinear pointing opposite direction of {@code zUp}: the rotation angle is equal to {@code Math.PI} and the rotation axis is set to: (1, 0, 0). * <li> if the length of the given normal is below {@code 1.0E-7}: the rotation angle is equal to {@code 0.0} and the rotation axis is set to: (1, 0, 0). * </ul> * </p> * <p> * Note: The calculation becomes less accurate as the two vectors are more collinear. * </p> * <p> * WARNING: This method generates garbage. * </p> * * @param vector the 3D vector that is rotated with respect to {@code zUp}. Not modified. * @return the minimum rotation from {@code zUp} to the given {@code vector}. */ public static AxisAngle4d getAxisAngleFromZUpToVector(Vector3d vector) { AxisAngle4d axisAngle = new AxisAngle4d(); getAxisAngleFromZUpToVector(vector, axisAngle); return axisAngle; }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { positionError.changeFrame(bodyFrame); GeometryTools.getAxisAngleFromZUpToVector(positionError.getVector(), rotationToControlFrame); transformToParent.setRotationAndZeroTranslation(rotationToControlFrame); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { circleOrigin.get(localTranslation); rotationAxis.get(localRotationAxis); GeometryTools.getAxisAngleFromZUpToVector(localRotationAxis, localAxisAngle); transformToParent.set(localAxisAngle, localTranslation); } };
private void computeNormalContactVectorRotation(Matrix3d normalContactVectorRotationMatrixToPack) { yoPlaneContactState.getContactNormalFrameVector(contactNormalVector); contactNormalVector.changeFrame(planeFrame); contactNormalVector.normalize(); GeometryTools.getAxisAngleFromZUpToVector(contactNormalVector.getVector(), normalContactVectorRotation); normalContactVectorRotationMatrixToPack.set(normalContactVectorRotation); }
public static PlanarRegion convertToPlanarRegion(PlanarRegionMessage planarRegionMessage) { RigidBodyTransform transformToWorld = new RigidBodyTransform(); Vector3d regionOrigin = new Vector3d(planarRegionMessage.getRegionOrigin()); Vector3d regionNormal = new Vector3d(planarRegionMessage.getRegionNormal()); AxisAngle4d regionOrientation = GeometryTools.getAxisAngleFromZUpToVector(regionNormal); transformToWorld.set(regionOrientation, regionOrigin); List<Point2f[]> messageHullsVertices = planarRegionMessage.getConcaveHullsVertices(); List<Point2d[]> concaveHullsVertices = new ArrayList<>(); for (int hullIndex = 0; hullIndex < messageHullsVertices.size(); hullIndex++) { Point2f[] messageHullVertices = messageHullsVertices.get(hullIndex); Point2d[] hullVertices = new Point2d[messageHullVertices.length]; for (int vertexIndex = 0; vertexIndex < messageHullVertices.length; vertexIndex++) { hullVertices[vertexIndex] = new Point2d(messageHullVertices[vertexIndex]); } concaveHullsVertices.add(hullVertices); } List<ConvexPolygon2d> planarRegionConvexPolygons = new ArrayList<>(); List<Point2f[]> convexPolygonsVertices = planarRegionMessage.getConvexPolygonsVertices(); for (int polygonIndex = 0; polygonIndex < convexPolygonsVertices.size(); polygonIndex++) { ConvexPolygon2d convexPolygon = new ConvexPolygon2d(convexPolygonsVertices.get(polygonIndex)); planarRegionConvexPolygons.add(convexPolygon); } PlanarRegion planarRegion = new PlanarRegion(transformToWorld, concaveHullsVertices, planarRegionConvexPolygons); planarRegion.setRegionId(planarRegionMessage.getRegionId()); return planarRegion; }
/** * Creates a transform that transforms to the given point and rotates to make the z axis align with the * normal vector. */ private static RigidBodyTransform createTransformFromPointAndZAxis(FramePoint point, FrameVector zAxis) { RigidBodyTransform ret = new RigidBodyTransform(); ret.setRotation(GeometryTools.getAxisAngleFromZUpToVector(zAxis.getVectorCopy())); Vector3d translation = new Vector3d(); point.get(translation); ret.setTranslation(translation); return ret; }
public void setInitialClearance(FrameVector initialDirection, double leaveDistance) { this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld); rotationPlane.setIncludingFrame(this.initialDirection.getReferenceFrame(), axisAngleToWorld); this.leaveDistance.set(leaveDistance); }
public void setFinalApproach(FrameVector finalDirection, double approachDistance) { this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, axisAngleToWorld); rotationPlane.setIncludingFrame(this.finalDirection.getReferenceFrame(), axisAngleToWorld); this.approachDistance.set(approachDistance); }
public void setInitialLeadOut(FramePoint initialPosition, FrameVector initialDirection, double leaveDistance) { this.initialPosition.set(initialPosition); this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); initialDistortionPose.setToZero(this.initialPosition.getReferenceFrame()); initialDistortionPose.setPosition(initialPosition); initialDistortionPose.setOrientation(tempAxisAngle); this.leaveDistance.set(leaveDistance); }
public void setFinalLeadIn(FramePoint finalPosition, FrameVector finalDirection, double approachDistance) { this.finalPosition.set(finalPosition); this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); finalDistortionPose.setToZero(this.finalPosition.getReferenceFrame()); finalDistortionPose.setPosition(finalPosition); finalDistortionPose.setOrientation(tempAxisAngle); this.approachDistance.set(approachDistance); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempPoint.setToZero(frameBeforeHipPitchJoint); tempPoint.changeFrame(endEffectorFrame); footToHipAxis.setIncludingFrame(tempPoint); footToHipAxis.changeFrame(getParent()); GeometryTools.getAxisAngleFromZUpToVector(footToHipAxis.getVector(), anklePitchRotationToParentFrame); anklePitchPosition.setToZero(endEffectorFrame); anklePitchPosition.changeFrame(getParent()); anklePitchPosition.get(anklePitchToParentFrame); transformToParent.setRotationAndZeroTranslation(anklePitchRotationToParentFrame); transformToParent.setTranslation(anklePitchToParentFrame); } };
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { tempPoint.setToZero(frameBeforeHipPitchJoint); tempPoint.changeFrame(endEffectorFrame); footToHipAxis.setIncludingFrame(tempPoint); footToHipAxis.changeFrame(getParent()); GeometryTools.getAxisAngleFromZUpToVector(footToHipAxis.getVector(), hipPitchRotationToParentFrame); hipPitchPosition.setToZero(frameBeforeHipPitchJoint); hipPitchPosition.changeFrame(getParent()); hipPitchPosition.get(hipPitchToParentFrame); transformToParent.setRotationAndZeroTranslation(hipPitchRotationToParentFrame); transformToParent.setTranslation(hipPitchToParentFrame); } };
public static RigidBodyTransform computeHandstepTransform(boolean rotateZIntoX, Tuple3d position, Vector3d surfaceNormal, double rotationAngleAboutNormal) { surfaceNormal.normalize(); AxisAngle4d rotationAxisAngle = new AxisAngle4d(); GeometryTools.getAxisAngleFromZUpToVector(surfaceNormal, rotationAxisAngle); AxisAngle4d rotationAboutNormal = new AxisAngle4d(surfaceNormal, rotationAngleAboutNormal); RigidBodyTransform transformOne = new RigidBodyTransform(); transformOne.setRotationAndZeroTranslation(rotationAboutNormal); RigidBodyTransform transformTwo = new RigidBodyTransform(); transformTwo.setRotationAndZeroTranslation(rotationAxisAngle); transformOne.multiply(transformTwo); if (rotateZIntoX) { RigidBodyTransform transformThree = new RigidBodyTransform(); transformThree.setRotationPitchAndZeroTranslation(Math.PI / 2.0); transformOne.multiply(transformThree); } transformOne.setTranslation(new Vector3d(position)); return transformOne; } }