@Override public void setPoint(Point3d point) { this.set(point); }
/** * 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); }
public static List<FramePoint> getWaypointsAtSpecifiedGroundClearance(FramePoint initialPosition, FramePoint finalPosition, double groundClearance, double[] proportionsThroughTrajectoryForGroundClearance) { List<FramePoint> waypoints = new ArrayList<FramePoint>(); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); waypoints.add(new FramePoint(initialPosition.getReferenceFrame())); for (int i = 0; i < 2; i++) { FramePoint waypoint = waypoints.get(i); waypoint.set(initialPosition); FrameVector offsetFromInitial = new FrameVector(waypoint.getReferenceFrame()); offsetFromInitial.set(finalPosition); offsetFromInitial.sub(initialPosition); offsetFromInitial.scale(proportionsThroughTrajectoryForGroundClearance[i]); waypoint.add(offsetFromInitial); waypoint.setZ(waypoints.get(i).getZ() + groundClearance); } return waypoints; }
public static FramePoint computeMinZPointInFrame(RigidBodyTransform footToWorldTransform, List<FramePoint> footPoints, ReferenceFrame bodyFrame, ReferenceFrame frame) { FramePoint minFramePoint = new FramePoint(frame); minFramePoint.setZ(Double.POSITIVE_INFINITY); FramePoint tempFramePoint = new FramePoint(ReferenceFrame.getWorldFrame()); boolean pointFound = false; for (FramePoint footPoint : footPoints) { tempFramePoint.setIncludingFrame(footPoint); tempFramePoint.changeFrame(bodyFrame); tempFramePoint.changeFrameUsingTransform(ReferenceFrame.getWorldFrame(), footToWorldTransform); tempFramePoint.changeFrame(frame); if (tempFramePoint.getZ() < minFramePoint.getZ()) { minFramePoint.set(tempFramePoint); pointFound = true; } } if (!pointFound) throw new RuntimeException(); return minFramePoint; }
@Override public void setPosition(FramePoint position) { this.position.set(position); }
private void setArcLengths() { pi.setToZero(referenceFrame); piPlusOne.setToZero(referenceFrame); arcLengths[0].set(0.0); double tiPlusOne = t0.getDoubleValue(); compute(t0.getDoubleValue()); getPosition(piPlusOne); double differentialDistance; for (int i = 0; i < arcLengthCalculatorDivisions; i++) { pi.set(piPlusOne); tiPlusOne = getTime(i + 1); compute(tiPlusOne); getPosition(piPlusOne); differentialDistance = pi.distance(piPlusOne); arcLengths[i + 1].set(arcLengths[i].getDoubleValue() + differentialDistance); } }
public void setPositionToPointAt(FramePoint positionToPointAt) { positionToPointAt.changeFrame(parentFrame); this.positionToPointAt.set(positionToPointAt); }
public void initializeCoMPositionToActual(FramePoint initialCoMPosition) { initializeToActual = true; centerOfMassPosition.set(initialCoMPosition); yoCenterOfMassPosition.set(initialCoMPosition); }
public void getFirstEndpoint(FramePoint firstEndpointToPack) { checkReferenceFrameMatch(firstEndpointToPack); firstEndpointToPack.set(getFirstEndpoint()); }
public void getSecondEndpoint(FramePoint secondEndpointToPack) { checkReferenceFrameMatch(secondEndpointToPack); secondEndpointToPack.set(getSecondEndpoint()); }
public static FramePoint average(List<? extends FramePoint> framePoints) { ReferenceFrame frame = framePoints.get(0).getReferenceFrame(); FramePoint nextPoint = new FramePoint(frame); FramePoint average = new FramePoint(framePoints.get(0)); for (int i = 1; i < framePoints.size(); i++) { nextPoint.set(framePoints.get(i)); nextPoint.changeFrame(frame); average.add(nextPoint); } average.scale(1.0 / ((double) framePoints.size())); return average; }
private void updateCenterOfMassVelocity(FrameVector centerOfMassPositionDelta) { centerOfMassPosition.set(centerOfMassPositionPort.getData()); centerOfMassPosition.add(centerOfMassPositionDelta); centerOfMassPositionPort.setData(centerOfMassPosition); } }
private void updateCenterOfMassPosition(FrameVector centerOfMassPositionDelta) { centerOfMassPosition.set(centerOfMassPositionPort.getData()); centerOfMassPosition.add(centerOfMassPositionDelta); centerOfMassPositionPort.setData(centerOfMassPosition); } }
public void getPoint(FramePoint pointToPack) { checkReferenceFrameMatch(pointToPack.getReferenceFrame()); this.plane3d.getPoint(temporaryPoint); pointToPack.set(temporaryPoint); }
private void updateClosestAndMostSevereIntersectionPoint(SwingOverPlanarRegionsTrajectoryCollisionType collisionType) { if (collisionType.ordinal() > this.mostSevereCollisionType.getEnumValue().ordinal()) { this.mostSevereCollisionType.set(collisionType); } if (footCollisionSphere.distance(sphereWithConvexPolygonIntersector.getClosestPointOnPolygon()) < footCollisionSphere.distance(closestPolygonPointMap.get(collisionType))) { closestPolygonPointMap.get(collisionType).set(sphereWithConvexPolygonIntersector.getClosestPointOnPolygon()); } }
private void setProcessedSensors() { DenseMatrix64F xState = kalmanFilters.get(Direction.X).getState(); DenseMatrix64F yState = kalmanFilters.get(Direction.Y).getState(); DenseMatrix64F zState = kalmanFilters.get(Direction.Z).getState(); bodyPosition.set(xState.get(positionIndex), yState.get(positionIndex), zState.get(positionIndex)); bodyVelocity.set(xState.get(velocityIndex), yState.get(velocityIndex), zState.get(velocityIndex)); processedBodyPositionSensors.setBodyPosition(bodyPosition); processedBodyPositionSensors.setBodyVelocity(bodyVelocity); }
public void computeNextTick(FramePoint positionToPack, FrameVector velocityToPack, FrameVector accelerationToPack, double deltaT) { double time = this.time.getDoubleValue(); positionToPack.set(trajectory.getPosition(time)); velocityToPack.set(trajectory.getVelocity(time)); accelerationToPack.set(trajectory.getAcceleration(time)); }
public void getIntersectionWithLine(FramePoint pointToPack, FrameLine line) { checkReferenceFrameMatch(line.getReferenceFrame()); checkReferenceFrameMatch(pointToPack.getReferenceFrame()); Point3d intersectionToPack = new Point3d(); plane3d.getIntersectionWithLine(intersectionToPack, line.getPoint(), line.getNormalizedVector()); pointToPack.set(intersectionToPack); }
private void computePositionFromMergingMeasurements() { yoRootJointPosition.getFrameTuple(rootJointPosition); imuBasedLinearStateCalculator.updatePelvisPosition(rootJointPosition, pelvisPositionIMUPart); kinematicsBasedLinearStateCalculator.getPelvisPosition(pelvisPositionKinPart); pelvisPositionIMUPart.scale(alphaIMUAgainstKinematicsForPosition.getDoubleValue()); pelvisPositionKinPart.scale(1.0 - alphaIMUAgainstKinematicsForPosition.getDoubleValue()); rootJointPosition.set(pelvisPositionIMUPart); rootJointPosition.add(pelvisPositionKinPart); yoRootJointPosition.set(rootJointPosition); }