public PoseReferenceFrame(String frameName, ReferenceFrame parentFrame) { super(frameName, parentFrame); originPose = new FramePose(parentFrame); }
public TrajectoryBasedStopThreadUpdatable(HumanoidRobotDataReceiver robotDataReceiver, AbstractBehavior behavior, double pausePercent, double pauseDuration, double stopPercent, FramePose poseAtTrajectoryEnd, ReferenceFrame frameToKeepTrackOf) { super(robotDataReceiver, behavior, frameToKeepTrackOf); this.initialPose = new FramePose(); this.currentPose = new FramePose(); this.poseAtTrajectoryEnd = poseAtTrajectoryEnd; this.pausePercent = pausePercent; this.pauseDuration = pauseDuration; this.stopPercent = stopPercent; }
public ChangeableConfigurationProvider(FramePose initialConfiguration) { configuration = new FramePose(initialConfiguration); }
public FramePose getCurrentTestFramePoseCopy() { FramePose ret = new FramePose(); getCurrentTestFramePose(ret); return ret; }
public FramePose getRotatedAboutAxisCopy(ReferenceFrame rotationAxisFrame, Axis rotationAxis, double angle) { FramePose ret = new FramePose(this); ret.rotatePoseAboutAxis(rotationAxisFrame, rotationAxis, angle); return ret; }
public FramePose getTestFramePoseCopyAtTransition(BehaviorControlModeEnum controlMode) { FramePose ret = new FramePose(ReferenceFrame.getWorldFrame(), getTestFrameTransformToWorldAtTransition(controlMode)); return ret; }
private void submitFootPosition(boolean parallelize, RobotSide robotSide, FramePoint desiredFootPosition) { FrameOrientation desiredFootOrientation = new FrameOrientation(desiredFootPosition.getReferenceFrame()); FramePose desiredFootPose = new FramePose(desiredFootPosition, desiredFootOrientation); submitFootPose(parallelize, robotSide, desiredFootPose); }
private void submitFootstepPose(boolean parallelize, RobotSide robotSide, FramePose desiredFootstepPose) { FramePose footPose = new FramePose(desiredFootstepPose); FootstepTask footstepTask = new FootstepTask(fullRobotModel, robotSide, footstepListBehavior, footPose); if (parallelize) pipeLine.submitTaskForPallelPipesStage(footstepListBehavior, footstepTask); else pipeLine.submitSingleTaskStage(footstepTask); }
private void submitFootPose(boolean parallelize, RobotSide robotSide, ReferenceFrame referenceFrame, double x, double y, double z, double yaw, double pitch, double roll) { FramePoint framePosition = new FramePoint(referenceFrame, x, y, z); FrameOrientation frameOrientation = new FrameOrientation(referenceFrame, yaw, pitch, roll); FramePose desiredFootPose = new FramePose(framePosition, frameOrientation); submitFootPose(parallelize, robotSide, desiredFootPose); }
public ConstantConfigurationProvider(FramePoint framePoint) { configuration = new FramePose(framePoint, new FrameOrientation(framePoint.getReferenceFrame())); }
private FramePose generateDebrisPose(Point3d positionWithRespectToRobot, double debrisYaw, double debrisPitch, double debrisRoll) { FramePose debrisPose = new FramePose(robotInitialPoseReferenceFrame); Quat4d orientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(debrisYaw, debrisPitch, debrisRoll, orientation); debrisPose.setPose(positionWithRespectToRobot, orientation); debrisPose.changeFrame(constructionWorldFrame); return debrisPose; }
public DRCDebrisEnvironment(Tuple3d robotInitialPosition, double robotInitialYaw) { combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName()); combinedTerrainObject.addTerrainObject(setUpGround("Ground")); Quat4d robotInitialOrientation = new Quat4d(); RotationTools.convertYawPitchRollToQuaternion(robotInitialYaw, 0.0, 0.0, robotInitialOrientation); Point3d robotPosition = new Point3d(robotInitialPosition); FramePose robotInitialPose = new FramePose(constructionWorldFrame, robotPosition, robotInitialOrientation); this.robotInitialPoseReferenceFrame = new PoseReferenceFrame("robotInitialPoseReferenceFrame", robotInitialPose); }
public static Footstep generateStandingFootstep(RobotSide side, RigidBody foot, ReferenceFrame soleFrame) { FramePose footFramePose = new FramePose(foot.getParentJoint().getFrameAfterJoint()); footFramePose.changeFrame(worldFrame); boolean trustHeight = false; PoseReferenceFrame footstepPoseFrame = new PoseReferenceFrame("footstepPoseFrame", footFramePose); Footstep footstep = new Footstep(foot, side, soleFrame, footstepPoseFrame, trustHeight); return footstep; }
public static FramePose poseFormPose2d(FramePose2d pose2d) { FramePose pose = new FramePose(pose2d.getReferenceFrame()); pose.setYawPitchRoll(pose2d.getYaw(), 0.0, 0.0); pose.setX(pose2d.getX()); pose.setY(pose2d.getY()); return pose; }
public void setSoleFrame(FramePoint position, FrameOrientation orientation) { position.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); orientation.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); setSoleFrame(new FramePose(ReferenceFrame.getWorldFrame(), position.getPoint(), orientation.getQuaternionCopy())); }
private FramePose offsetPointFromChestInWorldFrame(double x, double y, double z, double yaw, double pitch, double roll) { FramePoint point1 = new FramePoint(referenceFrames.getChestFrame(), x, y, z); point1.changeFrame(ReferenceFrame.getWorldFrame()); FrameOrientation orient = new FrameOrientation(referenceFrames.getChestFrame(), yaw, pitch, roll); orient.changeFrame(ReferenceFrame.getWorldFrame()); FramePose pose = new FramePose(point1, orient); return pose; }
@Override public void variableChanged(YoVariable<?> v) { if (userDesiredSetPelvisHeightToActual.getBooleanValue()) { ReferenceFrame pelvisFrame = fullRobotModel.getPelvis().getBodyFixedFrame(); FramePose currentPose = new FramePose(pelvisFrame); currentPose.changeFrame(ReferenceFrame.getWorldFrame()); userDesiredPelvisHeight.set(currentPose.getFramePointCopy().getZ()); userDesiredSetPelvisHeightToActual.set(false); } } });
public void computePelvisTrajectoryMessage() { checkIfDataHasBeenSet(); Point3d desiredPosition = new Point3d(); Quat4d desiredOrientation = new Quat4d(); ReferenceFrame pelvisFrame = fullRobotModelToUseForConversion.getRootJoint().getFrameAfterJoint(); FramePose desiredPelvisPose = new FramePose(pelvisFrame); desiredPelvisPose.changeFrame(worldFrame); desiredPelvisPose.getPose(desiredPosition, desiredOrientation); PelvisTrajectoryMessage pelvisTrajectoryMessage = new PelvisTrajectoryMessage(trajectoryTime, desiredPosition, desiredOrientation); output.setPelvisTrajectoryMessage(pelvisTrajectoryMessage); }
@Override public void doTransitionOutOfAction() { FramePose goalPose = new FramePose(); locateGoalBehavior.getReportedGoalPoseWorldFrame(goalPose); setGoalPose(goalPose); super.doTransitionOutOfAction(); sendPacketToUI(new UIPositionCheckerPacket(goalPose.getFramePointCopy().getPoint(), goalPose.getFrameOrientationCopy().getQuaternion())); PrintTools.info("got the goal: \n" + goalPose + " \n requesting planar regions..."); } };
public static FramePose setupStanceFoot(SideDependentList<PoseReferenceFrame> soleFrames, SideDependentList<SixDoFJoint> sixDoFJoints) { FramePose startStanceFootPose = new FramePose(worldFrame, new Point3d(0.0, 0.2, 0.0), new Quat4d()); soleFrames.get(RobotSide.LEFT).setPoseAndUpdate(startStanceFootPose); soleFrames.get(RobotSide.LEFT).update(); Point3d stanceAnklePosition = new Point3d(); startStanceFootPose.getPosition(stanceAnklePosition); sixDoFJoints.get(RobotSide.LEFT).setPosition(stanceAnklePosition); return startStanceFootPose; }