public static FramePoint getMidPoint(FramePoint point1, FramePoint point2) { point1.checkReferenceFrameMatch(point2); FramePoint midPoint = new FramePoint(point1.referenceFrame); midPoint.interpolate(point1, point2, 0.5); return midPoint; }
/** * Compute the desired capture point position at a given time. * x<sup>ICP<sub>des</sub></sup> = (e<sup>ω0 t</sup>) x<sup>ICP<sub>0</sub></sup> + (1-e<sup>ω0 t</sup>)x<sup>CMP<sub>0</sub></sup> * @param omega0 the natural frequency ω = √<span style="text-decoration:overline;"> g / z0 </span> of the biped. * @param time * @param initialCapturePoint * @param initialCMP * @param desiredCapturePointToPack */ public static void computeDesiredCapturePointPosition(double omega0, double time, FramePoint initialCapturePoint, FramePoint initialCMP, FramePoint desiredCapturePointToPack) { desiredCapturePointToPack.setToZero(initialCapturePoint.getReferenceFrame()); if (initialCapturePoint.distance(initialCMP) > EPSILON) desiredCapturePointToPack.interpolate(initialCMP, initialCapturePoint, Math.exp(omega0 * time)); else desiredCapturePointToPack.set(initialCapturePoint); }
private void interpolatePointFromInitialToFinalFrame(FramePoint pointTrajectoryFrameToPack, FramePoint pointInitialFrame, FramePoint pointFinalFrame, double percentOfFinal) { pointA.setIncludingFrame(pointInitialFrame); pointB.setIncludingFrame(pointFinalFrame); pointA.changeFrame(trajectoryFrame); pointB.changeFrame(trajectoryFrame); pointTrajectoryFrameToPack.setToZero(trajectoryFrame); pointTrajectoryFrameToPack.interpolate(pointA, pointB, percentOfFinal); }
for (int i = 0; i < numberWaypoints; i++) waypointPositions.get(i).interpolate(initialPosition, finalPosition, waypointProportions[i]); waypointPositions.get(i).setZ(maxStepZ + swingHeight.getDoubleValue()); for (int i = 0; i < numberWaypoints; i++) waypointPositions.get(i).interpolate(initialPosition, finalPosition, waypointProportions[i]); waypointPositions.get(i).add(0.0, 0.0, swingHeight.getDoubleValue());
private void getOriginOfSpatialAxisOfRotationToOtherPose(FramePose otherPose, FrameVector rotationAxis, double rotationAngle, FramePoint originToPack) { otherPose.checkReferenceFrameMatch(rotationAxis); otherPose.checkReferenceFrameMatch(originToPack); double epsilon = 1e-7; boolean rotationAngleEpsilonEqualsZero = Math.abs(rotationAngle) < epsilon; boolean rotationAngleEpsilonEqualsPlusOrMinusPi = Math.abs(Math.abs(rotationAngle) - Math.PI) < epsilon; if (rotationAngleEpsilonEqualsZero) { originToPack.setToZero(referenceFrame); } else if (rotationAngleEpsilonEqualsPlusOrMinusPi) { originToPack.setToZero(referenceFrame); originToPack.interpolate(this.pose.getPoint(), otherPose.pose.getPoint(), 0.5); } else { GeometryTools.getTopVertexOfIsoscelesTriangle(pose.getPoint(), otherPose.pose.getPoint(), rotationAxis.getVector(), rotationAngle, originToPack.getPoint()); } }
private void computeToePoints(RobotSide supportSide) { FrameConvexPolygon2d footDefaultPolygon = footDefaultPolygons.get(supportSide); toePoints[0].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0); toePoints[1].setIncludingFrame(footDefaultPolygon.getReferenceFrame(), Double.NEGATIVE_INFINITY, 0.0, 0.0); for (int i = 0; i < footDefaultPolygon.getNumberOfVertices(); i++) { footDefaultPolygon.getFrameVertex(i, footPoint); if (footPoint.getX() > toePoints[0].getX()) { toePoints[1].set(toePoints[0]); toePoints[0].setXY(footPoint); } else if (footPoint.getX() > toePoints[1].getX()) { toePoints[1].setXY(footPoint); } } middleToePoint.setToZero(footDefaultPolygon.getReferenceFrame()); middleToePoint.interpolate(toePoints[0], toePoints[1], 0.5); }
originalWaypoints.get(0).interpolate(swingStartPosition, swingEndPosition, defaultWaypointProportions[0]); midGroundPoint.set(originalWaypoints.get(0)); originalWaypoints.get(0).add(0.0, 0.0, minimumSwingHeight); adjustedWaypoints.get(0).set(originalWaypoints.get(0)); originalWaypoints.get(1).setToZero(); originalWaypoints.get(1).interpolate(swingStartPosition, swingEndPosition, defaultWaypointProportions[1]); midGroundPoint.add(originalWaypoints.get(1)); originalWaypoints.get(1).add(0.0, 0.0, minimumSwingHeight);
framePointD0.interpolate(transferFromContactFramePosition, transferToContactFramePosition, d0.getX() / sF.getX()); framePointD0.setZ(d0.getY() + offsetHeightAboveGround.getDoubleValue()); framePointD0.changeFrame(worldFrame); framePointDF.interpolate(transferFromContactFramePosition, transferToContactFramePosition, dF.getX() / sF.getX()); framePointDF.setZ(dF.getY() + offsetHeightAboveGround.getDoubleValue()); framePointDF.changeFrame(worldFrame); tempFramePointForViz1.interpolate(transferFromContactFramePosition, transferToContactFramePosition, ((double) i) / ((double) numberOfPoints)); tempFramePointForViz1.changeFrame(worldFrame); Point2d queryPoint = new Point2d(tempFramePointForViz1.getX(), tempFramePointForViz1.getY());