public YoGraphicPolygon(String name, YoFrameConvexPolygon2D yoFrameConvexPolygon2d, YoFramePoseUsingYawPitchRoll framePose, double scale, AppearanceDefinition appearance) { this(name, yoFrameConvexPolygon2d, framePose.getPosition(), framePose.getOrientation(), scale, appearance); }
public void get(Vector3D translation) { translation.set(yoFramePose.getPosition()); } }
public double getDistance(YoFramePoseUsingYawPitchRoll goalYoPose) { return position.distance(goalYoPose.getPosition()); }
public double getZ() { return getPosition().getZ(); }
public YoDouble getYoZ() { return getPosition().getYoZ(); }
public double getY() { return getPosition().getY(); }
public YoDouble getYoX() { return getPosition().getYoX(); }
public double getX() { return getPosition().getX(); }
public YoDouble getYoY() { return getPosition().getYoY(); }
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()); }
/** * 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()); }
private double computeFrameOrientationRelativeToWalkingPath(ReferenceFrame referenceFrame) { this.walkPathVector.sub(this.targetLocation, robotYoPose.getPosition()); fullRobotModel.updateFrames(); FrameVector2D frameHeadingVector = new FrameVector2D(referenceFrame, 1.0, 0.0); frameHeadingVector.changeFrame(worldFrame); double ret = -Math.abs(frameHeadingVector.angle(new FrameVector2D(walkPathVector))); if (DEBUG) { PrintTools.debug(this, "FrameHeadingVector : " + frameHeadingVector); PrintTools.debug(this, "WalkPathVector : " + walkPathVector); PrintTools.debug(this, "OrientationToWalkPath : " + ret); } return ret; }
@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()); }
double zRange = 2.0; sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoX(), footPosition.getX() - xyRange / 2.0, footPosition.getX() + xyRange / 2.0); sliderBoard.setSlider(sliderChannel++, yoFramePose.getPosition().getYoY(), footPosition.getY() - xyRange / 2.0, footPosition.getY() + xyRange / 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);
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(); } } }
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());