public void set(FramePose framePose) { set(framePose, true); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { locatedFiducialPoseInWorldFrame.getPose(transformToParent); } };
@Override public void set(YoFramePoseUsingQuaternions other) { setPosition(other.position); setOrientation(other.orientation); }
public void getFramePoseIncludingFrame(FramePose framePoseToPack) { framePoseToPack.setToZero(getReferenceFrame()); getFramePose(framePoseToPack); }
detectorReferenceFrame.update(); cameraPose.setOrientation(cameraOrientationInWorldXForward); cameraPose.setPosition(cameraPositionInWorld); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); locatedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); reportedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame);
public FindGoalBehavior(DoubleYoVariable yoTime, CommunicationBridge behaviorCommunicationBridge, FullHumanoidRobotModel fullRobotModel, HumanoidReferenceFrames referenceFrames, GoalDetectorBehaviorService fiducialDetectorBehaviorService) { super(FollowFiducialBehavior.class.getSimpleName(), behaviorCommunicationBridge); this.fullRobotModel = fullRobotModel; this.referenceFrames = referenceFrames; foundFiducialYoFramePose = new YoFramePoseUsingQuaternions(prefix + "FoundGoalPose", ReferenceFrame.getWorldFrame(), registry); this.fiducialDetectorBehaviorService = fiducialDetectorBehaviorService; addBehaviorService(fiducialDetectorBehaviorService); headPitchToFindFucdicial.set(0.6); headTrajectorySentTimer = new YoTimer(yoTime); headTrajectorySentTimer.start(); // behaviorCommunicationBridge.attachNetworkListeningQueue(robotConfigurationDataQueue, RobotConfigurationData.class); // behaviorCommunicationBridge.attachNetworkListeningQueue(footstepStatusQueue, FootstepStatus.class); // behaviorCommunicationBridge.attachNetworkListeningQueue(walkingStatusQueue, WalkingStatusMessage.class); // behaviorCommunicationBridge.attachNetworkListeningQueue(planarRegionsListQueue, PlanarRegionsListMessage.class); }
public void setPose(RigidBodyTransform rigidBodyTransform) { setPose(rigidBodyTransform, true); }
public DoubleYoVariable getYoQz() { return getOrientation().getYoQz(); } }
public void getReportedFiducialPoseWorldFrame(FramePose framePoseToPack) { reportedFiducialPoseInWorldFrame.getFramePoseIncludingFrame(framePoseToPack); }
detectorReferenceFrame.update(); cameraPose.setOrientation(cameraOrientationInWorldXForward); cameraPose.setPosition(cameraPositionInWorld); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); locatedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); reportedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame);
YoFramePoint position = new YoFramePoint(x, y, z, worldFrame); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, worldFrame); currentRegionPose = new YoFramePoseUsingQuaternions(position, orientation);
currentMeshIndex.set(currentIndex); planarRegionToProcess.getTransformToWorld(regionTransform); currentRegionPose.setPose(regionTransform); currentRegionId.set(planarRegionToProcess.getRegionId());
public DoubleYoVariable getYoQs() { return getOrientation().getYoQs(); }
public void getReportedFiducialPoseWorldFrame(FramePose framePoseToPack) { reportedFiducialPoseInWorldFrame.getFramePoseIncludingFrame(framePoseToPack); }
public void set(YoFramePose yoFramePose) { set(yoFramePose.getPosition().getFrameTuple(), yoFramePose.getOrientation().getFrameOrientation()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { locatedFiducialPoseInWorldFrame.getPose(transformToParent); } };
currentRegionId = new IntegerYoVariable(name + "CurrentRegionId", registry); isPlanarRegionsListComplete = new BooleanYoVariable(name + "IsComplete", registry); currentRegionPose = new YoFramePoseUsingQuaternions(name + "CurrentRegionPose", worldFrame, registry);
public DoubleYoVariable getYoQx() { return getOrientation().getYoQx(); }
@Override public void doControl() { if (fiducialDetectorBehaviorService.getGoalHasBeenLocated()) { fiducialDetectorBehaviorService.getReportedGoalPoseWorldFrame(foundFiducialPose); foundFiducialYoFramePose.set(foundFiducialPose); foundFiducial.set(true); Point3d position = new Point3d(); foundFiducialPose.getPosition(position); sendTextToSpeechPacket("Target object located at " + position); } else { sendTextToSpeechPacket("Target object not located"); } if (headTrajectorySentTimer.totalElapsed() < 2.0) { return; } pitchHeadToFindFiducial(); // pitchHeadToCenterFiducial(); }
currentRegionPose.getPose(transform);