public FramePoint getFinalICP() { return finalICP.getFrameTuple(); } }
public void getPosition(FramePoint positionToPack) { position.getFrameTuple(positionToPack); }
/** * GC-free but unsafe accessor. */ public FramePoint getPosition() { return position.getFrameTuple(); }
public void getFinalPosition(FramePoint finalPosition) { this.finalPosition.getFrameTuple(finalPosition); }
public void getPosition(FramePoint positionToPack) { position.getFrameTuple(positionToPack); }
public double getXYPlaneDistance(FramePoint framePoint) { return getFrameTuple().getXYPlaneDistance(framePoint); }
public double getXYPlaneDistance(YoFramePoint yoFramePoint) { return getXYPlaneDistance(yoFramePoint.getFrameTuple()); }
public double distance(FramePoint framePoint) { return getFrameTuple().distance(framePoint); }
public double distance(YoFramePoint yoFramePoint) { return distance(yoFramePoint.getFrameTuple()); }
public double getXYPlaneDistance(FramePoint2d framePoint2d) { return getFrameTuple().getXYPlaneDistance(framePoint2d); }
@Override public FramePoint getData() { yoFramePoint.getFrameTuple(super.getData()); return super.getData(); }
private void computeTwoCMPOffsets(ArrayList<YoFramePoint2d> upcomingFootstepLocations, int numberOfFootstepsToConsider) { for (int i = 0; i < numberOfFootstepsToConsider; i++) { FrameVector2d entryOffset = entryOffsets.get(i); FrameVector2d exitOffset = exitOffsets.get(i); entryOffset.setToZero(worldFrame); exitOffset.setToZero(worldFrame); entryOffset.setByProjectionOntoXYPlane(referenceCMPsCalculator.getEntryCMPs().get(i + 1).getFrameTuple()); exitOffset.setByProjectionOntoXYPlane(referenceCMPsCalculator.getExitCMPs().get(i + 1).getFrameTuple()); entryOffset.sub(upcomingFootstepLocations.get(i).getFrameTuple2d()); exitOffset.sub(upcomingFootstepLocations.get(i).getFrameTuple2d()); } }
public void set(YoFramePose yoFramePose) { set(yoFramePose.getPosition().getFrameTuple(), yoFramePose.getOrientation().getFrameOrientation()); }
public void set(YoFramePose yoFramePose) { set(yoFramePose.getPosition().getFrameTuple(), yoFramePose.getOrientation().getFrameOrientation()); }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(position.getReferenceFrame()); framePoseToPack.setPosition(position.getFrameTuple()); orientation.get(temp); framePoseToPack.setOrientation(temp); }
public void computeFinalICPRecursion(FramePoint2d finalICPRecursionToPack, int numberOfFootstepsToConsider, boolean useTwoCMPs, boolean isInTransfer, double omega0) { computeFinalICP(numberOfFootstepsToConsider, useTwoCMPs, isInTransfer, omega0); finalICPRecursionToPack.setByProjectionOntoXYPlane(finalICP.getFrameTuple()); finalICPRecursionToPack.scale(footstepRecursionMultiplierCalculator.getFinalICPRecursionMultiplier()); finalICPRecursionToPack.scale(footstepRecursionMultiplierCalculator.getCurrentStateProjectionMultiplier()); }
@Override protected void putYoValuesIntoFrameWaypoint() { frameWaypoint.setToZero(getReferenceFrame()); frameWaypoint.setTime(time.getDoubleValue()); frameWaypoint.setPosition(position.getFrameTuple()); frameWaypoint.setLinearVelocity(linearVelocity.getFrameTuple()); }
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); }
public void update() { toolBody.getInertia().setMass(objectMass.getDoubleValue()); temporaryPoint.setIncludingFrame(objectCenterOfMass.getFrameTuple()); temporaryPoint.changeFrame(elevatorFrame); toolFrame.setPositionAndUpdate(temporaryPoint); // Visualization toolFramePoint.setToZero(toolFrame); toolFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); objectCenterOfMassInWorld.set(toolFramePoint); }
@Override protected void putYoValuesIntoFrameWaypoint() { frameWaypoint.setToZero(getReferenceFrame()); frameWaypoint.setTime(time.getDoubleValue()); frameWaypoint.setPosition(position.getFrameTuple()); frameWaypoint.setOrientation(orientation.getFrameOrientation()); frameWaypoint.setLinearVelocity(linearVelocity.getFrameTuple()); frameWaypoint.setAngularVelocity(angularVelocity.getFrameTuple()); }