@Override public YoGraphic duplicate(YoVariableRegistry newRegistry) { return new YoGraphicShape(getName(), getLinkGraphics(), yoFramePoint.duplicate(newRegistry), yoFrameYawPitchRoll.duplicate(newRegistry), scale); } }
private void setupNextFootstepVisualization() { nextFootstepPoses = new ArrayList<>(); for (int i = 0; i < numberOfFootstepsToConsider; i++) { Graphics3DObject nextFootstepGraphic = new Graphics3DObject(); nextFootstepGraphic.addExtrudedPolygon(contactPointsInFootFrame, footstepHeight, nextFootstepColor); YoFramePoseUsingYawPitchRoll nextFootstepPose = new YoFramePoseUsingYawPitchRoll("NextFootstep" + i + "Pose", worldFrame, registry); nextFootstepPoses.add(nextFootstepPose); graphicsListRegistry .registerYoGraphic("UpcomingFootsteps", new YoGraphicShape("NextFootstep" + i + "Viz", nextFootstepGraphic, nextFootstepPose, 1.0)); } }
private void setupCurrentFootPoseVisualization() { currentFootLocations = new SideDependentList<>(); for (RobotSide side : RobotSide.values()) { Graphics3DObject footstepGraphic = new Graphics3DObject(); footstepGraphic.addExtrudedPolygon(contactPointsInFootFrame, footstepHeight, side == RobotSide.LEFT ? leftFootstepColor : rightFootstepColor); YoFramePoseUsingYawPitchRoll footPose = new YoFramePoseUsingYawPitchRoll(side.getCamelCaseName() + "FootPose", worldFrame, registry); currentFootLocations.put(side, footPose); graphicsListRegistry.registerYoGraphic("currentFootPose", new YoGraphicShape(side.getCamelCaseName() + "FootViz", footstepGraphic, footPose, 1.0)); } }
YoGraphicShape comViz = new YoGraphicShape(rigidBodyName + "CoMEllipsoid", createEllipsoid(inertiaMatrix, mass, appearance), comPosition, comOrientation, 1.0); yoGraphics.add(comViz);
boxPosition.set(boxSize / 2.0, boxSize / 2.0, boxSize / 2.0); YoFrameYawPitchRoll boxOrientation = new YoFrameYawPitchRoll("boxOrientation", worldFrame, registry); YoGraphicShape yoGraphicBoxGhost = new YoGraphicShape("boxGhost", boxGhostGraphics, boxPosition, boxOrientation, boxSize);