/** * Creates a copy of {@code this} by finding the duplicated {@code YoVariable}s in the given * {@link YoVariableRegistry}. * <p> * This method does not duplicate {@code YoVariable}s. Assuming the given registry is a duplicate * of the registry that was used to create {@code this}, this method searches for the duplicated * {@code YoVariable}s and use them to duplicate {@code this}. * </p> * * @param newRegistry YoVariableRegistry to duplicate {@code this} to. * @return the duplicate of {@code this}. */ public YoFramePose3D duplicate(YoVariableRegistry newRegistry) { return new YoFramePose3D(position.duplicate(newRegistry), orientation.duplicate(newRegistry)); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { locatedFiducialPoseInWorldFrame.get(transformToParent); } };
public YoGraphicCoordinateSystem(String name, YoFramePose3D yoFramePose, double scale, AppearanceDefinition arrowAppearance) { this(name, yoFramePose.getPosition(), yoFramePose.getOrientation(), scale, arrowAppearance); }
detectorReferenceFrame.update(); cameraPose.setOrientation(cameraOrientationInWorldXForward); cameraPose.setPosition(cameraPositionInWorld); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); locatedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); reportedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame);
/** * Sets this pose to represent the same geometry as the given {@code yoFramePose}. * * @param yoFramePose the pose used to set this. Not modified. */ public void set(YoFramePoseUsingYawPitchRoll yoFramePose) { set(yoFramePose.getPosition(), yoFramePose.getOrientation().getFrameOrientation()); }
/** * Provides a {@code String} representation of this pose as follows:<br> * Pose 3D: position = (x, y, z), orientation = (x, y, z, s)-worldFrame * * @return the {@code String} representing this pose. */ @Override public String toString() { return EuclidGeometryIOTools.getPose3DString(this) + "-" + getReferenceFrame(); } }
detectorReferenceFrame.update(); cameraPose.setOrientation(cameraOrientationInWorldXForward); cameraPose.setPosition(cameraPositionInWorld); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); locatedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame); tempFiducialDetectorFrame.changeFrame(ReferenceFrame.getWorldFrame()); reportedFiducialPoseInWorldFrame.set(tempFiducialDetectorFrame);
currentMeshIndex.set(currentIndex); planarRegionToProcess.getTransformToWorld(regionTransform); currentRegionPose.set(regionTransform); currentRegionId.set(planarRegionToProcess.getRegionId());
YoFramePoint3D position = new YoFramePoint3D(x, y, z, worldFrame); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, worldFrame); currentRegionPose = new YoFramePose3D(position, orientation);
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { locatedFiducialPoseInWorldFrame.get(transformToParent); } };
public YoGraphicPolygon(String name, YoFrameConvexPolygon2D yoFrameConvexPolygon2d, YoFramePose3D framePose, double scale, AppearanceDefinition appearance) { this(name, yoFrameConvexPolygon2d, framePose.getPosition(), framePose.getOrientation(), scale, appearance); }
int numberOfPolynomials = random.nextInt(20) + 3; YoFramePose3D poseToPolynomialFrame = new YoFramePose3D(name + "Pose", ReferenceFrame.getWorldFrame(), registry);
@Override protected void computeRotationTranslation(AffineTransform transform) { if (getCurrentGraphicType() == TrajectoryGraphicType.HIDE) return; if (poseToWorldFrame != null) { poseToWorldFrame.get(rigidBodyTransform); transform.set(rigidBodyTransform); } else { transform.setIdentity(); } update(); }
isPlanarRegionsListComplete = new YoBoolean(name + "IsComplete", registry); clear = new YoBoolean(name + "Clear", registry); currentRegionPose = new YoFramePose3D(name + "CurrentRegionPose", worldFrame, registry);
currentRegionPose.get(transform);
private static YoFramePose3D findYoFramePose3D(String nameSpace, String namePrefix, String nameSuffix, SimulationConstructionSet scs) { YoDouble x = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createXName(namePrefix, nameSuffix)); YoDouble y = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createYName(namePrefix, nameSuffix)); YoDouble z = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createZName(namePrefix, nameSuffix)); YoFramePoint3D position = new YoFramePoint3D(x, y, z, ReferenceFrame.getWorldFrame()); YoDouble qx = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQxName(namePrefix, nameSuffix)); YoDouble qy = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQyName(namePrefix, nameSuffix)); YoDouble qz = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQzName(namePrefix, nameSuffix)); YoDouble qs = (YoDouble) scs.getVariable(nameSpace, YoFrameVariableNameTools.createQsName(namePrefix, nameSuffix)); YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); return new YoFramePose3D(position, orientation); }
public HumanoidKinematicsToolboxController(CommandInputManager commandInputManager, StatusMessageOutputManager statusOutputManager, FullHumanoidRobotModel desiredFullRobotModel, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { super(commandInputManager, statusOutputManager, desiredFullRobotModel.getRootJoint(), getAllJointsExcludingHands(desiredFullRobotModel), createListOfControllableRigidBodies(desiredFullRobotModel), yoGraphicsListRegistry, parentRegistry); this.desiredFullRobotModel = desiredFullRobotModel; footWeight.set(200.0); momentumWeight.set(1.0); for (RobotSide robotSide : RobotSide.values) { if (desiredFullRobotModel.getHand(robotSide) != null) setupVisualization(desiredFullRobotModel.getHand(robotSide)); setupVisualization(desiredFullRobotModel.getFoot(robotSide)); } for (RobotSide robotSide : RobotSide.values) { String side = robotSide.getCamelCaseNameForMiddleOfExpression(); String sidePrefix = robotSide.getCamelCaseNameForStartOfExpression(); isFootInSupport.put(robotSide, new YoBoolean("is" + side + "FootInSupport", registry)); initialFootPoses.put(robotSide, new YoFramePose3D(sidePrefix + "FootInitial", worldFrame, registry)); } populateJointLimitReductionFactors(); }
YoDouble qs = (YoDouble) yoVariables[index++]; YoFrameQuaternion orientation = new YoFrameQuaternion(qx, qy, qz, qs, ReferenceFrame.getWorldFrame()); poseToWorldFrame = new YoFramePose3D(position, orientation);
int numberOfSteps = 3; YoFramePose3D footstepSolution = new YoFramePose3D("footstepSolution", ReferenceFrame.getWorldFrame(), registry); YoFramePoint2D unclippedFootstepSolution = new YoFramePoint2D("unclippedFootstepSolution", ReferenceFrame.getWorldFrame(), registry); FramePose3D footstepPose = new FramePose3D();
double stanceWidth = 0.2; int numberOfSteps = 3; YoFramePose3D foostepSolution = new YoFramePose3D("footstepSolution", ReferenceFrame.getWorldFrame(), registry); YoFramePoint2D unclippedFootstepSolution = new YoFramePoint2D("unclippedFootstepSolution", ReferenceFrame.getWorldFrame(), registry); FramePose3D foostepPose = new FramePose3D();