public void getPoseIncludingFrame(FramePose framePoseToPack) { framePoseToPack.setPoseIncludingFrame(originPose); }
public void getFiducialPose(FramePose framePoseToPack) { framePoseToPack.setPoseIncludingFrame(foundFiducialPose); }
public void get(FramePose framePose) { framePose.setPoseIncludingFrame(configuration); }
public void set(FramePose newPose) { configuration.setPoseIncludingFrame(newPose); }
public void getFramePose(FramePose framePoseToPack) { framePoseToPack.setPoseIncludingFrame(referenceFrame, box3d.getTransformFromShapeFrameUnsafe()); }
public void getLidarPose(FramePose poseToPack) { poseToPack.setPoseIncludingFrame(ReferenceFrame.getWorldFrame(), lidarPosition, lidarOrientation); }
public void getCurrentTestFramePose(FramePose poseToPack) { poseToPack.setPoseIncludingFrame(worldFrame, getCurrentTestFrameTransformToWorld()); }
@Override public void getPose(FramePose framePoseToPack) { yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(currentPosition); yoCurrentOrientation.getFrameOrientationIncludingFrame(currentOrientation); framePoseToPack.setPoseIncludingFrame(currentPosition, currentOrientation); }
@Override public void getPose(FramePose framePoseToPack) { yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(currentPosition); yoCurrentOrientation.getFrameOrientationIncludingFrame(currentOrientation); framePoseToPack.setPoseIncludingFrame(currentPosition, currentOrientation); }
public void compute(FramePoint desiredPosition, FrameOrientation desiredOrientation, FrameVector desiredLinearVelocity, FrameVector desiredAngularVelocity) { desiredControlFramePose.setPoseIncludingFrame(desiredPosition, desiredOrientation); desiredControlFrameTwist.set(originalEndEffectorFrame, originalBaseFrame, originalControlFrame, desiredLinearVelocity, desiredAngularVelocity); compute(desiredControlFramePose, desiredControlFrameTwist); }
public FramePose getFramePose(ReferenceFrame vehicleReferenceFrame, VehicleObject vehicleObject) { FramePose framePose = new FramePose(vehicleReferenceFrame); RigidBodyTransform transform3D = getTransform(vehicleObject); framePose.setPoseIncludingFrame(vehicleReferenceFrame, transform3D); return framePose; }
public FramePose getFramePose(ReferenceFrame vehicleReferenceFrame, VehicleObject vehicleObject) { FramePose framePose = new FramePose(vehicleReferenceFrame); RigidBodyTransform transform3D = getTransform(vehicleObject); framePose.setPoseIncludingFrame(vehicleReferenceFrame, transform3D); return framePose; }
public void compute(FramePose desiredPose, Twist desiredTwist) { jacobian.compute(); desiredControlFramePose.setPoseIncludingFrame(desiredPose); desiredControlFrameTwist.set(desiredTwist); computeJointAnglesAndVelocities(desiredControlFramePose, desiredControlFrameTwist); }
public TrajectoryBasedStopThreadUpdatable(HumanoidRobotDataReceiver robotDataReceiver, AbstractBehavior behavior, double pausePercent, double pauseDuration, double stopPercent, FramePose2d pose2dAtTrajectoryEnd, ReferenceFrame frameToKeepTrackOf) { this(robotDataReceiver, behavior, pausePercent, pauseDuration, stopPercent, new FramePose(), frameToKeepTrackOf); RigidBodyTransform transformToWorldAtTrajectoryEnd = new RigidBodyTransform(); pose2dAtTrajectoryEnd.getPose(transformToWorldAtTrajectoryEnd); poseAtTrajectoryEnd.setPoseIncludingFrame(worldFrame, transformToWorldAtTrajectoryEnd); }
public void convertToFramePose(FramePoint endEffectorPositionIn, FrameOrientation endEffectorOrientationIn, FramePose poseToPack) { endEffectorPosition.setIncludingFrame(endEffectorPositionIn); endEffectorPosition.changeFrame(endEffectorFrame); endEffectorOrientation.setIncludingFrame(endEffectorOrientationIn); endEffectorOrientation.changeFrame(endEffectorFrame); poseToPack.setPoseIncludingFrame(endEffectorPosition, endEffectorOrientation); }
public void setInterpolatorInputs(FramePose startOffsetError, FramePose goalOffsetError, double alphaFilterPosition) { startOffsetErrorPose.setPoseIncludingFrame(startOffsetError); goalOffsetErrorPose.setPoseIncludingFrame(goalOffsetError); if (!isRotationCorrectionEnabled.getBooleanValue()) { startOffsetErrorPose.setYawPitchRoll(0.0, 0.0, 0.0); goalOffsetErrorPose.setYawPitchRoll(0.0, 0.0, 0.0); } //scs feedback only yoStartOffsetErrorPose_InWorldFrame.set(startOffsetErrorPose); yoGoalOffsetErrorPose_InWorldFrame.set(goalOffsetErrorPose); stateEstimatorReferenceFrame.update(); updateStartAndGoalOffsetErrorTranslation(); updateStartAndGoalOffsetErrorRotation(); ///////////////////// initializeAlphaFilter(alphaFilterPosition); updateMaxTranslationAlphaVariationSpeed(); updateMaxRotationAlphaVariationSpeed(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { for (RobotSide robotSide : RobotSide.values) { ReferenceFrame footFrame = getFootFrame(robotSide); footFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame()); FramePose footPose = footPoses.get(robotSide); footPose.setPoseIncludingFrame(ReferenceFrame.getWorldFrame(), tempTransform); footPose.getOrientation(tempYawPitchRoll); tempYawPitchRoll[1] = 0.0; tempYawPitchRoll[2] = 0.0; footPose.setYawPitchRoll(tempYawPitchRoll); } midFootZUpPose.interpolate(leftFootPose, rightFootPose, 0.5); midFootZUpPose.setZ(Math.min(leftFootPose.getZ(), rightFootPose.getZ())); midFootZUpPose.getPose(transformToParent); } };
private void updateControlFrameAndDesireds(ReferenceFrame newControlFrame, boolean initializeToCurrent, FrameSE3TrajectoryPoint trajectoryPointToPack) { trajectoryPointToPack.setToZero(newControlFrame); if (!initializeToCurrent) { positionTrajectoryGenerator.getPosition(tempFramePoint); orientationTrajectoryGenerator.getOrientation(tempFrameOrientation); desiredPose.setPoseIncludingFrame(tempFramePoint, tempFrameOrientation); changeControlFrame(controlFrame, newControlFrame, desiredPose); desiredPose.getPoseIncludingFrame(tempFramePoint, tempFrameOrientation); trajectoryPointToPack.setToZero(desiredPose.getReferenceFrame()); trajectoryPointToPack.setPosition(tempFramePoint); trajectoryPointToPack.setOrientation(tempFrameOrientation); } setControlFrameFixedInEndEffector(newControlFrame); }
currentDistortionPose.setPoseIncludingFrame(initialDistortionPose); distortedPlane.update(); changeFrame(distortedPlane, false); currentDistortionPose.setPoseIncludingFrame(finalDistortionPose); distortedPlane.update(); changeFrame(distortedPlane, false);
@Override public void doAction() { positionTrajectoryGenerator.compute(getTimeInCurrentState()); orientationTrajectoryGenerator.compute(getTimeInCurrentState()); if (positionTrajectoryGenerator.isDone() && !commandQueue.isEmpty()) { double firstTrajectoryPointTime = positionTrajectoryGenerator.getLastWaypointTime(); HandTrajectoryCommand command = commandQueue.poll(); numberOfQueuedCommands.decrement(); initializeTrajectoryGenerators(command, firstTrajectoryPointTime); positionTrajectoryGenerator.compute(getTimeInCurrentState()); orientationTrajectoryGenerator.compute(getTimeInCurrentState()); } positionTrajectoryGenerator.getLinearData(desiredPosition, desiredLinearVelocity, feedForwardLinearAcceleration); orientationTrajectoryGenerator.getAngularData(desiredOrientation, desiredAngularVelocity, feedForwardAngularAcceleration); desiredPose.setPoseIncludingFrame(desiredPosition, desiredOrientation); yoDesiredPose.setAndMatchFrame(desiredPose); spatialFeedbackControlCommand.changeFrameAndSet(desiredPosition, desiredLinearVelocity, feedForwardLinearAcceleration); spatialFeedbackControlCommand.changeFrameAndSet(desiredOrientation, desiredAngularVelocity, feedForwardAngularAcceleration); spatialFeedbackControlCommand.setGains(gains); yoAngularWeight.get(angularWeight); yoLinearWeight.get(linearWeight); spatialFeedbackControlCommand.setWeightsForSolver(angularWeight, linearWeight); }