public YoReferencePose(String frameName, ReferenceFrame parentFrame, YoVariableRegistry registry) { super(frameName, parentFrame); yoFramePose = new YoFramePose(frameName + "_", this, registry); }
public void set(FramePose framePose) { set(framePose, true); }
public YoGraphicShape(String name, Graphics3DObject linkGraphics, YoFramePose framePose, double scale) { this(name, linkGraphics, framePose.getPosition(), framePose.getOrientation(), scale); }
public void getFramePoseIncludingFrame(FramePose framePoseToPack) { framePoseToPack.setToZero(getReferenceFrame()); getFramePose(framePoseToPack); }
public void set(YoFramePose yoFramePose) { set(yoFramePose.getPosition().getFrameTuple(), yoFramePose.getOrientation().getFrameOrientation()); }
yoWristJointPose = new YoFramePose("wristJointPose", HumanoidReferenceFrames.getWorldFrame(), registry); yoWristJointPose.set(wristJointPose); YoGraphicCoordinateSystem yoWristCoordinateSystem = new YoGraphicCoordinateSystem("wristCoordinateSystemViz", yoWristJointPose, 0.1, YoAppearance.Red()); handControlFramePose = new FramePose(HumanoidReferenceFrames.getWorldFrame(), transform); handControlFramePose.translate(tangentVector); yoHandControlFramePose = new YoFramePose("handControlFrame",HumanoidReferenceFrames.getWorldFrame(), registry); yoHandControlFramePose.set(handControlFramePose); YoGraphicCoordinateSystem yoToolTip = new YoGraphicCoordinateSystem("toolTipViz", yoHandControlFramePose, 0.1, YoAppearance.Yellow());
public void get(Vector3d translation) { yoFramePose.getPosition().get(translation); } }
midFeetZUpFrame = commonHumanoidReferenceFrames.getMidFeetZUpFrame(); pelvisFrame = commonHumanoidReferenceFrames.getPelvisFrame(); userDesiredPelvisPose = new YoFramePose("userDesiredPelvisPose", midFeetZUpFrame, registry); userDesiredPelvisPose.attachVariableChangedListener(new VariableChangedListener() userDesiredPelvisPose.getOrientation().attachVariableChangedListener(new VariableChangedListener()
originalDesiredYoPose = new YoFramePose(prefix + "originalDesiredYoPose", ReferenceFrame.getWorldFrame(), registry); adjustedDesiredPose = new YoFramePose(prefix + "adjustedDesiredPose", ReferenceFrame.getWorldFrame(), registry); desiredTransform = new RigidBodyTransform(); adjustedDesiredPosition = new FramePoint(ReferenceFrame.getWorldFrame()); if (visualize) yoDesiredFootPositionGraphic = new YoGraphicPosition(prefix + "DesiredFootPosition", originalDesiredYoPose.getPosition(), 0.025, YoAppearance.Yellow(), YoGraphicPosition.GraphicType.BALL); yoGraphicsListRegistry.registerYoGraphic("SingularityCollapseAvoidance", yoDesiredFootPositionGraphic); yoCorrectedDesiredFootPositionGraphic = new YoGraphicPosition(prefix + "CorrectedDesiredFootPosition", adjustedDesiredPose.getPosition(), 0.025, YoAppearance.Blue(), YoGraphicPosition.GraphicType.BALL); yoGraphicsListRegistry.registerYoGraphic("SingularityCollapseAvoidance", yoCorrectedDesiredFootPositionGraphic);
yoStartOffsetErrorPose_InWorldFrame.setPosition(updatedStartOffset_Translation); yoStartOffsetErrorPose_InWorldFrame.setOrientation(updatedStartOffset_Rotation_quat); yoGoalOffsetErrorPose_InWorldFrame.setPosition(updatedGoalOffset_Translation); yoGoalOffsetErrorPose_InWorldFrame.setOrientation(updatedGoalOffset_Rotation_quat); yoInterpolatedOffset_InWorldFrame.set(offsetPoseToPack);
public DoubleYoVariable getYoYaw() { return getOrientation().getYaw(); } }
@Override public void variableChanged(YoVariable<?> v) { if (userDesiredSetHandPoseToActual.getBooleanValue()) { ReferenceFrame referenceFrame = getReferenceFrameToUse(); ReferenceFrame wristFrame = fullRobotModel.getEndEffectorFrame(userHandPoseSide.getEnumValue(), LimbName.ARM); FramePose currentPose = new FramePose(wristFrame); currentPose.changeFrame(referenceFrame); userDesiredHandPose.setPosition(currentPose.getFramePointCopy().getPointCopy()); userDesiredHandPose.setOrientation(currentPose.getFrameOrientationCopy().getQuaternionCopy()); userDesiredSetHandPoseToActual.set(false); } } });
this.desiredFootStatusPoses.get(stanceSide).getFramePose(tempStanceFootPose); goalPose.setZ(tempStanceFootPose.getZ()); footstepPlannerGoalPose.set(goalPose); footstepPlannerInitialStepPose.set(tempStanceFootPose);
originalDesiredYoPose.set(originalDesiredPose); originalDesiredPose.changeFrame(jacobian.getBaseFrame()); adjustedDesiredOrientation.setToZero(adjustedFootFrame); adjustedDesiredOrientation.changeFrame(ReferenceFrame.getWorldFrame()); adjustedDesiredPose.setPosition(adjustedDesiredPosition);
public SingleFootstepVisualizer(RobotSide robotSide, int maxContactPoints, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { Integer index = indices.get(robotSide); String namePrefix = robotSide.getLowerCaseName() + "Foot" + index; YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix); this.robotSide = robotSide; ArrayList<Point2d> polyPoints = new ArrayList<Point2d>(); yoContactPoints = new YoFramePoint[maxContactPoints]; for (int i = 0; i < maxContactPoints; i++) { yoContactPoints[i] = new YoFramePoint(namePrefix + "ContactPoint" + i, ReferenceFrame.getWorldFrame(), registry); yoContactPoints[i].set(0.0, 0.0, -1.0); YoGraphicPosition baseControlPointViz = new YoGraphicPosition(namePrefix + "Point" + i, yoContactPoints[i], 0.01, YoAppearance.Blue()); yoGraphicsList.add(baseControlPointViz); polyPoints.add(new Point2d()); } footPolygon = new YoFrameConvexPolygon2d(namePrefix + "yoPolygon", "", ReferenceFrame.getWorldFrame(), maxContactPoints, registry); footPolygon.setConvexPolygon2d(new ConvexPolygon2d(polyPoints)); soleFramePose = new YoFramePose(namePrefix + "polygonPose", "", ReferenceFrame.getWorldFrame(), registry); soleFramePose.setXYZ(0.0, 0.0, -1.0); footPolygonViz = new YoGraphicPolygon(namePrefix + "graphicPolygon", footPolygon, soleFramePose, 1.0, footPolygonAppearances.get(robotSide)); yoGraphicsList.add(footPolygonViz); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(footPolygonViz); } index++; indices.set(robotSide, index); }
public void variableChanged(YoVariable<?> v) { if (userDoFootPose.getBooleanValue()) { userDesiredFootPose.getFramePose(framePose); ReferenceFrame soleZUpFrame = ankleZUpFrames.get(userFootPoseSide.getEnumValue()); soleZUpFrame.update(); framePose.setIncludingFrame(soleZUpFrame, framePose.getGeometryObject()); framePose.changeFrame(ReferenceFrame.getWorldFrame()); System.out.println("framePose " + framePose); FootTrajectoryCommand footTrajectoryControllerCommand = new FootTrajectoryCommand(); FrameSE3TrajectoryPoint trajectoryPoint = new FrameSE3TrajectoryPoint(ReferenceFrame.getWorldFrame()); trajectoryPoint.setTime(userDesiredFootPoseTrajectoryTime.getDoubleValue()); trajectoryPoint.setPosition(framePose.getFramePointCopy()); trajectoryPoint.setOrientation(framePose.getFrameOrientationCopy()); trajectoryPoint.setLinearVelocity(new Vector3d()); trajectoryPoint.setAngularVelocity(new Vector3d()); footTrajectoryControllerCommand.addTrajectoryPoint(trajectoryPoint); footTrajectoryControllerCommand.setRobotSide(userFootPoseSide.getEnumValue()); System.out.println("Submitting " + footTrajectoryControllerCommand); controllerCommandInputManager.submitCommand(footTrajectoryControllerCommand); userDoFootPose.set(false); } } });
yoWristJointPose = new YoFramePose("wristJointPose", HumanoidReferenceFrames.getWorldFrame(), registry); yoWristJointPose.set(wristJointPose); YoGraphicCoordinateSystem yoWristCoordinateSystem = new YoGraphicCoordinateSystem("wristCoordinateSystemViz", yoWristJointPose, 0.1, YoAppearance.Red()); handControlFramePose = new FramePose(HumanoidReferenceFrames.getWorldFrame(), transform); handControlFramePose.translate(tangentVector); yoHandControlFramePose = new YoFramePose("handControlFrame",HumanoidReferenceFrames.getWorldFrame(), registry); yoHandControlFramePose.set(handControlFramePose); YoGraphicCoordinateSystem yoToolTip = new YoGraphicCoordinateSystem("toolTipViz", yoHandControlFramePose, 0.1, YoAppearance.Yellow());
public double getZ() { return getPosition().getZ(); }
public DoubleYoVariable getYoPitch() { return getOrientation().getPitch(); }
Quat4d desiredFootOrientationInWorld = status.getDesiredFootOrientationInWorld(); desiredFootStatusPoses.get(side).setPosition(desiredFootPositionInWorld); desiredFootStatusPoses.get(side).setOrientation(desiredFootOrientationInWorld); actualFootStatusPoses.get(side).setPosition(actualFootPositionInWorld); actualFootStatusPoses.get(side).setOrientation(actualFootOrientationInWorld);