public double getRoll() { return originPose.getRoll(); }
@Override public void doControl() { if (detectorBehaviorService.getGoalHasBeenLocated()) { detectorBehaviorService.getReportedGoalPoseWorldFrame(foundFiducialPose); Point3d position = new Point3d(); foundFiducialPose.getPosition(position); double x = position.getX(), y = position.getY(), z = position.getZ(); double yaw = Math.toDegrees(foundFiducialPose.getYaw()), pitch = Math.toDegrees(foundFiducialPose.getPitch()), roll = Math.toDegrees(foundFiducialPose.getRoll()); sendTextToSpeechPacket(String.format("Target object located at [%s, %s, %s], rotation (%s, %s, %s)", decimalFormat.format(x), decimalFormat.format(y), decimalFormat.format(z), decimalFormat.format(yaw), decimalFormat.format(pitch), decimalFormat.format(roll))); done.set(true); } else { sendTextToSpeechPacket("Target object not located"); done.set(false); } }
private ContactableSelectableBoxRobot createDebrisRobot(FramePose debrisPose) { debrisPose.checkReferenceFrameMatch(constructionWorldFrame); ContactableSelectableBoxRobot debris = ContactableSelectableBoxRobot.createContactable2By4Robot(debrisName + String.valueOf(id++), debrisDepth, debrisWidth, debrisLength, debrisMass); debris.setPosition(debrisPose.getX(), debrisPose.getY(), debrisPose.getZ()); debris.setYawPitchRoll(debrisPose.getYaw(), debrisPose.getPitch(), debrisPose.getRoll()); return debris; }
public void setConstantPose(FramePose constantPose) { position.checkReferenceFrameMatch(constantPose); position.set(constantPose.getX(), constantPose.getY(), constantPose.getZ()); orientation.set(constantPose.getYaw(), constantPose.getPitch(), constantPose.getRoll()); }