public void get(Vector3d translation) { yoFramePose.getPosition().get(translation); } }
public DoubleYoVariable getYoZ() { return getPosition().getYoZ(); }
public YoGraphicCoordinateSystem(String name, YoFramePose yoFramePose, double scale) { this(name, yoFramePose.getPosition(), yoFramePose.getOrientation(), scale); }
public double getZ() { return getPosition().getZ(); }
public YoGraphicShape(String name, Graphics3DObject linkGraphics, YoFramePose framePose, double scale) { this(name, linkGraphics, framePose.getPosition(), framePose.getOrientation(), scale); }
public double getDistance(YoFramePose goalYoPose) { return position.distance(goalYoPose.getPosition()); }
public double getY() { return getPosition().getY(); }
public YoGraphicCoordinateSystem(String name, YoFramePose yoFramePose, double scale, AppearanceDefinition arrowColor) { this(name, yoFramePose.getPosition(), yoFramePose.getOrientation(), scale, arrowColor); }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2d yoFrameConvexPolygon2d, YoFramePose framePose, double scale, AppearanceDefinition appearance) { this(name, yoFrameConvexPolygon2d, framePose.getPosition(), framePose.getOrientation(), scale, appearance); }
public double getDistance(YoFramePose goalYoPose) { return position.distance(goalYoPose.getPosition()); }
public double getX() { return getPosition().getX(); }
public DoubleYoVariable getYoX() { return getPosition().getYoX(); }
public DoubleYoVariable getYoY() { return getPosition().getYoY(); }
public void add(YoFramePose yoFramePose) { getPosition().add(yoFramePose.getPosition()); getOrientation().add(yoFramePose.getOrientation()); }
public YoGraphicPolygon(String name, YoFramePose framePose, int maxNumberOfVertices, YoVariableRegistry registry, double scale, AppearanceDefinition appearance) { this(name, new YoFrameConvexPolygon2d(name + "ConvexPolygon2d", ReferenceFrame.getWorldFrame(), maxNumberOfVertices, registry), framePose.getPosition(), framePose.getOrientation(), scale, appearance); }
public void set(YoFramePose yoFramePose) { set(yoFramePose.getPosition().getFrameTuple(), yoFramePose.getOrientation().getFrameOrientation()); }
public void set(YoFramePose yoFramePose) { set(yoFramePose.getPosition().getFrameTuple(), 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 = -frameHeadingVector.angle(walkPathVector.getFrameVector2dCopy()); 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); YoFramePoint yoFramePoint = yoFramePose.getPosition(); transformToParent.setTranslation(yoFramePoint.getX(), yoFramePoint.getY(), yoFramePoint.getZ()); }
public void variableChanged(YoVariable<?> v) { if (legInverseKinematicsCalculators == null) return; reader.read(); sdfRobot.update(); if (sliderSpace.getEnumValue() == SliderSpace.CARTESIAN) { for (RobotSide robotSide : RobotSide.values()) { YoFramePose footIK = feetIKs.get(robotSide); FramePoint position = footIK.getPosition().getFramePointCopy(); FrameOrientation orientation = footIK.getOrientation().getFrameOrientationCopy(); FramePose framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getPelvis().getBodyFixedFrame()); framePose.getPose(desiredTransform); legInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); YoFramePose handIK = handIKs.get(robotSide); position = handIK.getPosition().getFramePointCopy(); orientation = handIK.getOrientation().getFrameOrientationCopy(); framePose = new FramePose(position, orientation); framePose.changeFrame(fullRobotModel.getChest().getBodyFixedFrame()); framePose.getPose(desiredTransform); armInverseKinematicsCalculators.get(robotSide).solve(desiredTransform); } writer.updateRobotConfigurationBasedOnFullRobotModel(); } } }