public DoubleYoVariable getYoZ() { return getPosition().getYoZ(); }
public DoubleYoVariable getYoZ() { return getPosition().getYoZ(); }
public YoVariable<?>[] getVariables() { YoVariable<?>[] vars = new YoVariable[9]; int i = 0; vars[i++] = pointOne.getYoX(); vars[i++] = pointOne.getYoY(); vars[i++] = pointOne.getYoZ(); vars[i++] = pointTwo.getYoX(); vars[i++] = pointTwo.getYoY(); vars[i++] = pointTwo.getYoZ(); vars[i++] = pointThree.getYoX(); vars[i++] = pointThree.getYoY(); vars[i++] = pointThree.getYoZ(); return vars; }
public YoGraphicLineSegment(String name, YoFramePoint startPoint, YoFramePoint endPoint, double scale, AppearanceDefinition appearance, boolean drawArrowhead) { this(name, startPoint.getYoX(), startPoint.getYoY(), startPoint.getYoZ(), endPoint.getYoX(), endPoint.getYoY(), endPoint.getYoZ(), scale, appearance, drawArrowhead); if ((!startPoint.getReferenceFrame().isWorldFrame()) || (!endPoint.getReferenceFrame().isWorldFrame())) { System.err.println("Warning: Should be in a World Frame to create a DynamicGraphicLineSegment. startPoint = " + startPoint + ", endPoint = " + endPoint); } }
public YoGraphicPosition(String name, YoFramePoint framePoint, double scale, AppearanceDefinition appearance, GraphicType type) { super(name); framePoint.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); this.x = framePoint.getYoX(); this.y = framePoint.getYoY(); this.z = framePoint.getYoZ(); this.scale = scale; this.type = type; this.appearance = appearance; }
public YoGraphicVector(String name, YoFramePoint startPoint, YoFrameVector frameVector, double scale, AppearanceDefinition appearance, boolean drawArrowhead) { this(name, startPoint.getYoX(), startPoint.getYoY(), startPoint.getYoZ(), frameVector.getYoX(), frameVector.getYoY(), frameVector.getYoZ(), scale, appearance, drawArrowhead); if ((!startPoint.getReferenceFrame().isWorldFrame()) || (!frameVector.getReferenceFrame().isWorldFrame())) { System.err.println("Warning: Should be in a World Frame to create a DynamicGraphicVector. startPoint = " + startPoint + ", frameVector = " + frameVector); } }
public YoGraphicCylinder(String name, YoFramePoint startPoint, YoFrameVector frameVector, AppearanceDefinition appearance, double lineThickness) { this(name, startPoint.getYoX(), startPoint.getYoY(), startPoint.getYoZ(), frameVector.getYoX(), frameVector.getYoY(), frameVector.getYoZ(), appearance, lineThickness); if ((!startPoint.getReferenceFrame().isWorldFrame()) || (!frameVector.getReferenceFrame().isWorldFrame())) { System.err.println("Warning: Should be in a World Frame to create a DynamicGraphicCylinder. startPoint = " + startPoint + ", frameVector = " + frameVector); } }
public static BacklashCompensatingVelocityYoFrameVector createBacklashCompensatingVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, DoubleYoVariable slopTime, YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) { BacklashCompensatingVelocityYoVariable xDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, slopTime, registry); BacklashCompensatingVelocityYoVariable yDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, slopTime, registry); BacklashCompensatingVelocityYoVariable zDot = new BacklashCompensatingVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, slopTime, registry); BacklashCompensatingVelocityYoFrameVector ret = new BacklashCompensatingVelocityYoFrameVector(xDot, yDot, zDot, registry, yoFramePointToDifferentiate); return ret; }
public YoGraphicCoordinateSystem(String name, YoFramePoint framePoint, YoFrameOrientation orientation, double scale) { super(name); ReferenceFrame.getWorldFrame().checkReferenceFrameMatch(framePoint); framePoint.checkReferenceFrameMatch(orientation.getReferenceFrame()); x = framePoint.getYoX(); y = framePoint.getYoY(); z = framePoint.getYoZ(); yaw = orientation.getYaw(); pitch = orientation.getPitch(); roll = orientation.getRoll(); this.scale = scale; }
public static AlphaFilteredYoFrameVector createAlphaFilteredYoFrameVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable alpha, YoFramePoint unfilteredPosition) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoY()); AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPosition.getYoZ()); AlphaFilteredYoFrameVector ret = new AlphaFilteredYoFrameVector(x, y, z, unfilteredPosition.getReferenceFrame()); return ret; }
public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alphaX, double alphaY, double alphaZ, YoFramePoint unfilteredPoint) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alphaY, unfilteredPoint.getYoY()); AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alphaZ, unfilteredPoint.getYoZ()); AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); return ret; }
public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable alphaX, DoubleYoVariable alphaY, DoubleYoVariable alphaZ, YoFramePoint unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alphaX, unfilteredPoint.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alphaY, unfilteredPoint.getYoY()); AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alphaZ, unfilteredPoint.getYoZ()); AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); return ret; }
public static TimestampedVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable timestamp, double dt, YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate, double epsilonChange) { TimestampedVelocityYoVariable xDot = new TimestampedVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", yoFramePointToDifferentiate.getYoX(), timestamp, registry, epsilonChange); TimestampedVelocityYoVariable yDot = new TimestampedVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", yoFramePointToDifferentiate.getYoY(), timestamp, registry, epsilonChange); TimestampedVelocityYoVariable zDot = new TimestampedVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", yoFramePointToDifferentiate.getYoZ(), timestamp, registry, epsilonChange); TimestampedVelocityYoFrameVector ret = new TimestampedVelocityYoFrameVector(xDot, yDot, zDot, yoFramePointToDifferentiate.getReferenceFrame()); return ret; }
public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleYoVariable alpha, YoFramePoint unfilteredPoint) { // alpha is a YoVariable AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoZ()); AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); return ret; }
public static AlphaFilteredYoFramePoint createAlphaFilteredYoFramePoint(String namePrefix, String nameSuffix, YoVariableRegistry registry, double alpha, YoFramePoint unfilteredPoint) { // alpha is a double AlphaFilteredYoVariable x = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoX()); AlphaFilteredYoVariable y = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoY()); AlphaFilteredYoVariable z = new AlphaFilteredYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), registry, alpha, unfilteredPoint.getYoZ()); AlphaFilteredYoFramePoint ret = new AlphaFilteredYoFramePoint(x, y, z, unfilteredPoint.getReferenceFrame()); return ret; }
public static FilteredVelocityYoFrameVector createFilteredVelocityYoFrameVector(String namePrefix, String nameSuffix, DoubleYoVariable alpha, double dt, YoVariableRegistry registry, YoFramePoint yoFramePointToDifferentiate) { FilteredVelocityYoVariable xDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createXName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoX(), dt, registry); FilteredVelocityYoVariable yDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createYName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoY(), dt, registry); FilteredVelocityYoVariable zDot = new FilteredVelocityYoVariable(YoFrameVariableNameTools.createZName(namePrefix, nameSuffix), "", alpha, yoFramePointToDifferentiate.getYoZ(), dt, registry); FilteredVelocityYoFrameVector ret = new FilteredVelocityYoFrameVector(xDot, yDot, zDot, alpha, dt, registry, yoFramePointToDifferentiate.getReferenceFrame()); return ret; }
@Override public YoVariable<?>[] getVariables() { //poly + framePoint + frameOrientation YoVariable<?>[] vars = new YoVariable[1 + 2 * yoFrameConvexPolygon2d.getMaxNumberOfVertices() + 6]; int i = 0; vars[i++] = yoFrameConvexPolygon2d.getYoNumberVertices(); for (YoFramePoint2d p : yoFrameConvexPolygon2d.getYoFramePoints()) { vars[i++] = p.getYoX(); vars[i++] = p.getYoY(); } vars[i++] = yoFramePoint.getYoX(); vars[i++] = yoFramePoint.getYoY(); vars[i++] = yoFramePoint.getYoZ(); vars[i++] = yoFrameOrientation.getYaw(); vars[i++] = yoFrameOrientation.getPitch(); vars[i++] = yoFrameOrientation.getRoll(); return vars; }
sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoZ(), footPosition.getZ() - zRange / 2.0, footPosition.getZ() + zRange / 2.0);
sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoZ(), footPosition.getZ() - zRange / 2.0, footPosition.getZ() + zRange / 2.0);
sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoX(), handPosition.getX() - xyRange / 2.0, handPosition.getX() + xyRange / 2.0); sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoY(), handPosition.getY() - xyRange / 2.0, handPosition.getY() + xyRange / 2.0); sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoZ(), handPosition.getZ() - zRange / 2.0, handPosition.getZ() + zRange / 2.0);