public void setOrientationAndUpdate(Quat4d quat4d) { originPose.setOrientation(quat4d); this.update(); }
public void setOrientationAndUpdate(AxisAngle4d axisAngle4d) { originPose.setOrientation(axisAngle4d); this.update(); }
public void setPose(FramePoint position, FrameOrientation orientation) { setPosition(position); setOrientation(orientation); }
public void setOrientationAndUpdate(FrameOrientation frameOrientation) { frameOrientation.checkReferenceFrameMatch(parentFrame); originPose.setOrientation(frameOrientation); this.update(); }
public void getFramePose(FramePose framePoseToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); framePoseToPack.setPosition(tempFramePoint); framePoseToPack.setOrientation(tempFrameOrientation); }
public void getFramePose(FramePose framePoseToPack) { position.getFrameTupleIncludingFrame(tempFramePoint); orientation.getFrameOrientationIncludingFrame(tempFrameOrientation); framePoseToPack.setPosition(tempFramePoint); framePoseToPack.setOrientation(tempFrameOrientation); }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(position.getReferenceFrame()); framePoseToPack.setPosition(position.getFrameTuple()); orientation.get(temp); framePoseToPack.setOrientation(temp); }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(currentPosition.getReferenceFrame()); framePoseToPack.setPosition(currentPosition.getFrameTuple()); currentOrientation.get(temp); framePoseToPack.setOrientation(temp); }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void getPose(FramePose framePoseToPack) { positionTrajectoryGenerator.getPosition(tempPosition); framePoseToPack.changeFrame(tempPosition.getReferenceFrame()); framePoseToPack.setPosition(tempPosition); orientationTrajectoryGenerator.getOrientation(tempOrientation); framePoseToPack.setOrientation(tempOrientation); }
public void getPose(FramePose framePoseToPack) { framePoseToPack.changeFrame(currentPosition.getReferenceFrame()); framePoseToPack.setPosition(currentPosition.getFrameTuple()); currentOrientation.get(temp); framePoseToPack.setOrientation(temp); }
public void getPose(FramePose poseToPack) { checkReferenceFrameMatch(poseToPack); SE3Waypoint waypointData = geometryObject.waypointData; poseToPack.setPosition(waypointData.getEuclideanWaypoint().getPosition()); poseToPack.setOrientation(waypointData.getSO3Waypoint().getOrientation()); }
@Override public void getReportedGoalPoseWorldFrame(FramePose framePoseToPack) { ReferenceFrame midFeetZUpFrame = referenceFrames.getMidFeetZUpFrame(); FramePoint goalPosition = new FramePoint(midFeetZUpFrame, constantGoalLocationInMidFeetZUpFrame); goalPosition.changeFrame(ReferenceFrame.getWorldFrame()); FrameOrientation goalOrientation = new FrameOrientation(midFeetZUpFrame); goalOrientation.changeFrame(ReferenceFrame.getWorldFrame()); framePoseToPack.setPosition(goalPosition); framePoseToPack.setOrientation(goalOrientation); }
public void setInitialLeadOut(FramePoint initialPosition, FrameVector initialDirection, double leaveDistance) { this.initialPosition.set(initialPosition); this.initialDirection.set(initialDirection); this.initialDirection.normalize(); this.initialDirection.get(tempVector); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); initialDistortionPose.setToZero(this.initialPosition.getReferenceFrame()); initialDistortionPose.setPosition(initialPosition); initialDistortionPose.setOrientation(tempAxisAngle); this.leaveDistance.set(leaveDistance); }
public void setFinalLeadIn(FramePoint finalPosition, FrameVector finalDirection, double approachDistance) { this.finalPosition.set(finalPosition); this.finalDirection.set(finalDirection); this.finalDirection.normalize(); this.finalDirection.get(tempVector); tempVector.negate(); GeometryTools.getAxisAngleFromZUpToVector(tempVector, tempAxisAngle); finalDistortionPose.setToZero(this.finalPosition.getReferenceFrame()); finalDistortionPose.setPosition(finalPosition); finalDistortionPose.setOrientation(tempAxisAngle); this.approachDistance.set(approachDistance); }
@Override protected boolean initialize() { isDone.set(false); requestedPlanarRegions.set(false); toolboxTime.set(0.0); timeReceivedPlanarRegion.set(0.0); FootstepPlanningRequestPacket request = latestRequestReference.getAndSet(null); if (request == null) return false; usePlanarRegions.set(!request.assumeFlatGround); FramePose initialStancePose = new FramePose(ReferenceFrame.getWorldFrame()); initialStancePose.setPosition(new Point3d(request.stanceFootPositionInWorld)); initialStancePose.setOrientation(new Quat4d(request.stanceFootOrientationInWorld)); FramePose goalPose = new FramePose(ReferenceFrame.getWorldFrame()); goalPose.setPosition(new Point3d(request.goalPositionInWorld)); goalPose.setOrientation(new Quat4d(request.goalOrientationInWorld)); FootstepPlanner planner = plannerMap.get(activePlanner.getEnumValue()); planner.setInitialStanceFoot(initialStancePose, request.initialStanceSide); FootstepPlannerGoal goal = new FootstepPlannerGoal(); goal.setFootstepPlannerGoalType(FootstepPlannerGoalType.POSE_BETWEEN_FEET); goal.setGoalPoseBetweenFeet(goalPose); planner.setGoal(goal); return true; }
goalPose.setOrientation(goalOrientation);
palmPose.setOrientation(palmOrientationWithRespectToHandFrame);