public void setAndMatchFrame(YoFrameQuaternion yoFrameQuaternion) { setAndMatchFrame(yoFrameQuaternion, true); }
public void setAndMatchFrame(YoFrameOrientation yoFrameOrientation) { setAndMatchFrame(yoFrameOrientation, true); }
public void setAndMatchFrame(FrameOrientation orientation) { setAndMatchFrame(orientation, true); }
public void setAndMatchFrame(FramePoint framePoint, FrameOrientation frameOrientation) { boolean notifyListeners = true; position.setAndMatchFrame(framePoint, notifyListeners); orientation.setAndMatchFrame(frameOrientation, notifyListeners); }
public void setInitialPose(FramePoint initialPosition, FrameOrientation initialOrientation) { this.initialPosition.set(initialPosition); this.initialOrientation.set(initialOrientation); initialOrientationForViz.setAndMatchFrame(initialOrientation); }
public void setFinalPose(FramePoint finalPosition, FrameOrientation finalOrientation) { this.finalPosition.set(finalPosition); this.finalOrientation.set(finalOrientation); finalOrientationForViz.setAndMatchFrame(finalOrientation); tempPosition.setIncludingFrame(finalPosition); tempOrientation.setIncludingFrame(finalOrientation); finalOrientationForViz.setAndMatchFrame(tempOrientation); }
public void setFinalPose(FramePose finalPose) { finalPose.getPoseIncludingFrame(tempPosition, tempOrientation); finalPosition.set(tempPosition); finalOrientation.set(tempOrientation); finalOrientationForViz.setAndMatchFrame(tempOrientation); }
public void setFinalPoseWithoutFinalVelocity(FramePoint finalPosition, FrameOrientation finalOrientation) { this.finalPosition.set(finalPosition); this.finalOrientation.set(finalOrientation); finalPositionForViz.setAndMatchFrame(finalPosition); finalOrientationForViz.setAndMatchFrame(finalOrientation); this.finalVelocity.setToZero(); this.finalAngularVelocity.setToZero(); }
public void setInitialPose(FramePose initialPose) { initialPose.getPoseIncludingFrame(tempPosition, tempOrientation); tempPosition.changeFrame(initialPosition.getReferenceFrame()); initialPosition.set(tempPosition); tempOrientation.changeFrame(initialOrientation.getReferenceFrame()); initialOrientation.set(tempOrientation); initialOrientationForViz.setAndMatchFrame(tempOrientation); }
@Override public void doControl() { diagnosticBehaviorEnabled.set(isControllerReady()); handleAutomaticDiagnosticRoutine(); if (!diagnosticBehaviorEnabled.getBooleanValue()) return; for (RobotSide robotSide : RobotSide.values) { tempFrameOrientation.setToZero(upperArmsFrames.get(robotSide)); currentUpperArmOrientations.get(robotSide).setAndMatchFrame(tempFrameOrientation); tempFrameOrientation.setToZero(fullRobotModel.getHand(robotSide).getBodyFixedFrame()); currentHandOrientations.get(robotSide).setAndMatchFrame(tempFrameOrientation); } handleRequestedSymmetricArmPose(); handleRequestedArmPose(); handleRequestedDiagnostic(); handleIcpOffsetSending(); pipeLine.doControl(); }
goalOffsetFrameOrientation_Rotation.set(updatedGoalOffset_Rotation); goalOffsetFrameOrientation_Rotation.changeFrame(startOffsetErrorReferenceFrame_Rotation); yoGoalOffsetFrameOrientation_Rotation.setAndMatchFrame(goalOffsetFrameOrientation_Rotation); yoInterpolatedOffsetFrameOrientation_Rotation.setAndMatchFrame(interpolatedOffsetFrameOrientation_Rotation);