public YoGraphicCoordinateSystem(String name, YoFramePose yoFramePose, double scale) { this(name, yoFramePose.getPosition(), yoFramePose.getOrientation(), scale); }
public DoubleYoVariable getYoYaw() { return getOrientation().getYaw(); } }
public YoGraphicShape(String name, Graphics3DObject linkGraphics, YoFramePose framePose, double scale) { this(name, linkGraphics, framePose.getPosition(), framePose.getOrientation(), scale); }
public DoubleYoVariable getYoPitch() { return getOrientation().getPitch(); }
public DoubleYoVariable getYoRoll() { return getOrientation().getRoll(); }
public void get(Quat4d rotation) { yoFramePose.getOrientation().getQuaternion(rotation); }
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 getRoll() { return getOrientation().getRoll().getDoubleValue(); }
public void add(YoFramePose yoFramePose) { getPosition().add(yoFramePose.getPosition()); getOrientation().add(yoFramePose.getOrientation()); }
public double getYaw() { return getOrientation().getYaw().getDoubleValue(); }
public double getPitch() { return getOrientation().getPitch().getDoubleValue(); }
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()); }
@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()); }
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(), 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);
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(); } } }
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(); } } }