public void getPosition(FramePoint positionToPack) { configuration.getPositionIncludingFrame(positionToPack); }
public void getPosition(FramePoint positionToPack) { configuration.getPositionIncludingFrame(positionToPack); }
public void getPositionIncludingFrame(FramePoint framePointToPack) { originPose.getPositionIncludingFrame(framePointToPack); }
public void getPoseIncludingFrame(FramePoint framePointToPack, FrameOrientation orientationToPack) { getPositionIncludingFrame(framePointToPack); getOrientationIncludingFrame(orientationToPack); }
public void setInitialPose(FramePose initialPose) { initialPose.getPositionIncludingFrame(tempPosition); initialPose.getOrientationIncludingFrame(tempOrientation); positionTrajectoryGenerator.setInitialPosition(tempPosition); orientationTrajectoryGenerator.setInitialOrientation(tempOrientation); }
public void setFinalLeadIn(FramePose finalPose, FrameVector finalDirection, double approachDistance) { finalPose.getPositionIncludingFrame(tempPosition); finalPose.getOrientationIncludingFrame(tempOrientation); positionTrajectoryGenerator.setFinalLeadIn(tempPosition, finalDirection, approachDistance); orientationTrajectoryGenerator.setFinalOrientation(tempOrientation); }
public void setFinalPose(FramePose finalPose) { finalPose.getPositionIncludingFrame(tempPosition); finalPose.getOrientationIncludingFrame(tempOrientation); positionTrajectoryGenerator.setFinalPosition(tempPosition); orientationTrajectoryGenerator.setFinalOrientation(tempOrientation); }
public void setInitialPose(FramePose initialPose) { initialPose.getPositionIncludingFrame(tempPosition); initialPose.getOrientationIncludingFrame(tempOrientation); positionTrajectoryGenerator.setInitialPosition(tempPosition); orientationTrajectoryGenerator.setInitialOrientation(tempOrientation); }
public void setInitialLeadOut(FramePose initialPose, FrameVector initialDirection, double leaveDistance) { initialPose.getPositionIncludingFrame(tempPosition); initialPose.getOrientationIncludingFrame(tempOrientation); positionTrajectoryGenerator.setInitialLeadOut(tempPosition, initialDirection, leaveDistance); orientationTrajectoryGenerator.setInitialOrientation(tempOrientation); }
public void setFinalPose(FramePose finalPose) { finalPose.getPositionIncludingFrame(tempPosition); finalPose.getOrientationIncludingFrame(tempOrientation); positionTrajectoryGenerator.setFinalPosition(tempPosition); orientationTrajectoryGenerator.setFinalOrientation(tempOrientation); }
public void set(FramePose framePose, boolean notifyListeners) { framePose.checkReferenceFrameMatch(getReferenceFrame()); framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
public void set(FramePose framePose, boolean notifyListeners) { framePose.checkReferenceFrameMatch(getReferenceFrame()); framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
public boolean checkIfErrorIsTooBig(FramePose correctedPelvisPoseInWorldFrame, FramePose iterativeClosestPointInWorldFramePose, boolean isRotationCorrectionEnabled) { correctedPelvisPoseReferenceFrame.setPoseAndUpdate(correctedPelvisPoseInWorldFrame); iterativeClosestPointInWorldFramePose.getOrientationIncludingFrame(iterativeClosestPointOrientation); iterativeClosestPointInWorldFramePose.getPositionIncludingFrame(iterativeClosestPointTranslation); iterativeClosestPointOrientation.changeFrame(correctedPelvisPoseReferenceFrame); iterativeClosestPointTranslation.changeFrame(correctedPelvisPoseReferenceFrame); iterativeClosestPointOrientation.getAxisAngle(axisAngleForError); angleError.set(Math.abs(axisAngleForError.getAngle())); translationErrorX.set(Math.abs(iterativeClosestPointTranslation.getX())); translationErrorY.set(Math.abs(iterativeClosestPointTranslation.getY())); translationErrorZ.set(Math.abs(iterativeClosestPointTranslation.getZ())); if(isRotationCorrectionEnabled && Math.abs(axisAngleForError.getAngle()) > Math.toRadians(maximumErrorAngleInDegrees.getDoubleValue())) return true; if(Math.abs(iterativeClosestPointTranslation.getX()) > maximumErrorTranslation.getDoubleValue()) return true; if(Math.abs(iterativeClosestPointTranslation.getY()) > maximumErrorTranslation.getDoubleValue()) return true; if(Math.abs(iterativeClosestPointTranslation.getZ()) > maximumErrorTranslation.getDoubleValue()) return true; return false; }
public void setAndMatchFrame(FramePose framePose, boolean notifyListeners) { framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); tempFramePoint.changeFrame(getReferenceFrame()); tempFrameOrientation.changeFrame(getReferenceFrame()); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
public boolean computeIteratively(FramePose desiredPose, Twist desiredTwist, double maxIterations, double epsilon) { for(int i = 0; i < maxIterations; i++) { compute(desiredPose, desiredTwist); desiredPose.getPositionIncludingFrame(tempPoint); tempPoint.changeFrame(localControlFrame); double translationDistance = tempPoint.getDistanceFromOrigin(); desiredPose.getOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(localControlFrame); tempOrientation.getAxisAngle(tempAxisAngle); double angle = Math.abs(AngleTools.trimAngleMinusPiToPi(tempAxisAngle.getAngle())); if(translationDistance < epsilon && angle < epsilon) { return true; } } return false; }
public void setAndMatchFrame(FramePose framePose, boolean notifyListeners) { framePose.getPositionIncludingFrame(tempFramePoint); framePose.getOrientationIncludingFrame(tempFrameOrientation); tempFramePoint.changeFrame(getReferenceFrame()); tempFrameOrientation.changeFrame(getReferenceFrame()); position.set(tempFramePoint, notifyListeners); orientation.set(tempFrameOrientation, notifyListeners); }
public void setPose(FramePose pose) { pose.getPositionIncludingFrame(tempPoint); tempPoint.changeFrame(this.position.getReferenceFrame()); this.position.set(tempPoint); pose.getOrientationIncludingFrame(tempOrientation); tempOrientation.changeFrame(this.orientation.getReferenceFrame()); this.orientation.set(tempOrientation); } }
private void updateSensorPosition() { sensorFrame.update(); sensorPose.setPose(sensorFrame.getTransformToDesiredFrame(world)); sensorPose.getPositionIncludingFrame(temp); yoSensorPositionInWorld.set(temp.getReferenceFrame(), temp.getX(), temp.getY(), temp.getZ()); }
/** * Computes linear portion of twist to pack */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
/** * This method provides a twist feedback controller, intended for spatial velocity control. * @param twistToPack twist to return * @param desiredPose desired pose that we want to achieve. * @param desiredTwist feed forward twist from a reference trajectory */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }