public YoGraphicPolygon(String name, YoFrameConvexPolygon2D yoFrameConvexPolygon2d, YoFramePoseUsingYawPitchRoll framePose, double scale, AppearanceDefinition appearance) { this(name, yoFrameConvexPolygon2d, framePose.getPosition(), framePose.getOrientation(), scale, appearance); }
public YoDouble getYoRoll() { return getOrientation().getRoll(); }
public void get(Quaternion rotation) { yoFramePose.getOrientation().getQuaternion(rotation); }
public YoDouble getYoYaw() { return getOrientation().getYaw(); }
public YoDouble getYoPitch() { return getOrientation().getPitch(); }
public YoGraphicCoordinateSystem(String name, YoFramePoseUsingYawPitchRoll yoFramePose, double scale, AppearanceDefinition arrowAppearance) { this(name, yoFramePose.getPosition(), yoFramePose.getOrientation(), scale, arrowAppearance); }
public YoGraphicShape(String name, Graphics3DObject linkGraphics, YoFramePoseUsingYawPitchRoll framePose, double scale) { this(name, linkGraphics, framePose.getPosition(), framePose.getOrientation(), scale); }
public void add(YoFramePoseUsingYawPitchRoll yoFramePose) { getPosition().add(yoFramePose.getPosition()); getOrientation().add(yoFramePose.getOrientation()); }
public double getRoll() { return getOrientation().getRoll().getDoubleValue(); }
public double getPitch() { return getOrientation().getPitch().getDoubleValue(); }
public double getYaw() { return getOrientation().getYaw().getDoubleValue(); }
/** * 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()); }
public void set(YoFramePoseUsingYawPitchRoll yoFramePose) { set(yoFramePose.getPosition(), yoFramePose.getOrientation().getFrameOrientation()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { yoFramePose.getOrientation().getQuaternion(rotation); transformToParent.setRotation(rotation); YoFramePoint3D yoFramePoint = yoFramePose.getPosition(); transformToParent.setTranslation(yoFramePoint.getX(), yoFramePoint.getY(), yoFramePoint.getZ()); }
sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoZ(), footPosition.getZ() - zRange / 2.0, footPosition.getZ() + zRange / 2.0); sliderBoard.setSlider(sliderChannel++, yoFramePose.getOrientation().getYaw(), -Math.PI, Math.PI); sliderBoard.setSlider(sliderChannel++, yoFramePose.getOrientation().getPitch(), -Math.PI, Math.PI); sliderBoard.setSlider(sliderChannel++, yoFramePose.getOrientation().getRoll(), -Math.PI, Math.PI);
sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoZ(), handPosition.getZ() - zRange / 2.0, handPosition.getZ() + zRange / 2.0); sliderBoard.setSlider(sliderChannel++, yoFramePose.getOrientation().getYaw(), -Math.PI, Math.PI); sliderBoard.setSlider(sliderChannel++, yoFramePose.getOrientation().getPitch(), -Math.PI, Math.PI); sliderBoard.setSlider(sliderChannel++, yoFramePose.getOrientation().getRoll(), -Math.PI, Math.PI);
public void notifyOfVariableChange(YoVariable<?> v) { if (legInverseKinematicsCalculators == null) return; reader.read(); sdfRobot.update(); if (sliderSpace.getEnumValue() == SliderSpace.CARTESIAN) { for (RobotSide robotSide : RobotSide.values()) { YoFramePoseUsingYawPitchRoll footIK = feetIKs.get(robotSide); FramePoint3D position = new FramePoint3D(footIK.getPosition()); FrameQuaternion orientation = footIK.getOrientation().getFrameOrientationCopy(); FramePose3D framePose = new FramePose3D(position, orientation); framePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame()); framePose.get(desiredTransform); legInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); YoFramePoseUsingYawPitchRoll handIK = handIKs.get(robotSide); position = new FramePoint3D(handIK.getPosition()); orientation = handIK.getOrientation().getFrameOrientationCopy(); framePose = new FramePose3D(position, orientation); framePose.changeFrame(fullRobotModel.getChest().getBodyFixedFrame()); framePose.get(desiredTransform); armInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); } writer.updateRobotConfigurationBasedOnFullRobotModel(); } } }
visiblePolygons.add(polygon); visiblePolygonPoses.add(pose); YoGraphicPolygon visualization = new YoGraphicPolygon("Polygon" + i, polygon, pose.getPosition(), pose.getOrientation(), 1.0, 0.02, new YoAppearanceRGBColor(Color.BLUE, 0.8)); polygonVisualizations.add(visualization);
visiblePolygons.add(polygon); visiblePolygonPoses.add(pose); YoGraphicPolygon visualization = new YoGraphicPolygon("Polygon" + i, polygon, pose.getPosition(), pose.getOrientation(), 1.0, 0.02, new YoAppearanceRGBColor(Color.BLUE, 0.8)); polygonVisualizations.add(visualization);
AppearanceDefinition bubble = YoAppearance.LightBlue(); bubble.setTransparency(0.5); collisionSphere = new YoGraphicEllipsoid("CollisionSphere", solePose.getPosition(), solePose.getOrientation(), bubble, new Vector3D()); stanceFootGraphic = new YoGraphicPolygon("StanceFootGraphic", footPolygon.getNumberOfVertices(), registry, true, 1.0, YoAppearance.Blue()); swingStartGraphic = new YoGraphicPolygon("SwingStartGraphic", footPolygon.getNumberOfVertices(), registry, true, 1.0, YoAppearance.Green());