private double computeFrameOrientationRelativeToWalkingPath(ReferenceFrame referenceFrame) { this.walkPathVector.sub(this.targetLocation, robotYoPose.getPosition()); fullRobotModel.updateFrames(); FrameVector2d frameHeadingVector = new FrameVector2d(referenceFrame, 1.0, 0.0); frameHeadingVector.changeFrame(worldFrame); double ret = -frameHeadingVector.angle(walkPathVector.getFrameVector2dCopy()); if (DEBUG) { PrintTools.debug(this, "FrameHeadingVector : " + frameHeadingVector); PrintTools.debug(this, "WalkPathVector : " + walkPathVector); PrintTools.debug(this, "OrientationToWalkPath : " + ret); } return ret; }
/** * 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); } }
double angle = icpToCMPVector.angle(icpToCandidateVector); if (angle < 1.0e-7 && projectionClose) double angle = finalICPToICPVector.angle(icpToCandidateVector); if (angle < 1.0e-7) icpToCandidateVector.sub(vertex, capturePoint); double newAngle = icpToCandidateVector.angle(finalICPToICPVector); if (newAngle < angle)