/** * This is where the end-effectors needing a visualization are registered, if you need more, add * it there. * * @param rigidBodies all the rigid bodies for which the desired and actual pose will be * displayed using graphical coordinate systems. */ public void setupVisualization(RigidBodyBasics... rigidBodies) { AppearanceDefinition desiredAppearance = YoAppearance.Red(); AppearanceDefinition currentAppearance = YoAppearance.Blue(); for (RigidBodyBasics rigidBody : rigidBodies) { YoGraphicCoordinateSystem desiredCoodinateSystem = createCoodinateSystem(rigidBody, Type.DESIRED, desiredAppearance); YoGraphicCoordinateSystem currentCoodinateSystem = createCoodinateSystem(rigidBody, Type.CURRENT, currentAppearance); desiredCoodinateSystems.put(rigidBody, desiredCoodinateSystem); currentCoodinateSystems.put(rigidBody, currentCoodinateSystem); yoGraphicsListRegistry.registerYoGraphic("CoordinateSystems", desiredCoodinateSystem); yoGraphicsListRegistry.registerYoGraphic("CoordinateSystems", currentCoodinateSystem); } }
public PlanarRegionBaseOfCliffAvoider(YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { parentRegistry.addChild(registry); if (yoGraphicsListRegistry == null) { visualize = false; } if (visualize) { beforeAdjustmentPosition = new YoGraphicPosition("beforeAdjustmentPosition", "", registry, 0.02, YoAppearance.Red()); afterAdjustmentPosition = new YoGraphicPosition("afterAdjustmentPosition", "", registry, 0.02, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), beforeAdjustmentPosition); yoGraphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), afterAdjustmentPosition); } else { beforeAdjustmentPosition = null; afterAdjustmentPosition = null; } }
public CommonHumanoidReferenceFramesVisualizer(CommonHumanoidReferenceFrames referenceFrames, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { String vizName = referenceFrames.getClass().getSimpleName(); for (RobotSide robotSide : RobotSide.values) { YoGraphicReferenceFrame yoGraphic = new YoGraphicReferenceFrame(referenceFrames.getAnkleZUpFrame(robotSide), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, yoGraphic); referenceFramesVisualizers.add(yoGraphic); } YoGraphicReferenceFrame midFeetFrame = new YoGraphicReferenceFrame(referenceFrames.getMidFeetZUpFrame(), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, midFeetFrame); referenceFramesVisualizers.add(midFeetFrame); YoGraphicReferenceFrame comFrame = new YoGraphicReferenceFrame(referenceFrames.getCenterOfMassFrame(), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, comFrame); referenceFramesVisualizers.add(comFrame); YoGraphicReferenceFrame pelvisFrame = new YoGraphicReferenceFrame(referenceFrames.getPelvisFrame(), registry, 0.2); yoGraphicsListRegistry.registerYoGraphic(vizName, pelvisFrame); referenceFramesVisualizers.add(pelvisFrame); parentRegistry.addChild(registry); }
public FootstepVisualizer(String name, String graphicListName, RobotSide robotSide, ContactablePlaneBody contactableFoot, AppearanceDefinition footstepColor, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry registry) { this.robotSide = robotSide; yoFootstepPose = new YoFramePose(name + "Pose", worldFrame, registry); yoFoothold = new YoFrameConvexPolygon2d(name + "Foothold", "", worldFrame, maxNumberOfContactPoints, registry); double coordinateSystemSize = 0.2; double footholdScale = 1.0; poseViz = new YoGraphicCoordinateSystem(name + "Pose", yoFootstepPose, coordinateSystemSize, footstepColor); footholdViz = new YoGraphicPolygon(name + "Foothold", yoFoothold, yoFootstepPose, footholdScale, footstepColor); yoGraphicsListRegistry.registerYoGraphic(graphicListName, poseViz); yoGraphicsListRegistry.registerYoGraphic(graphicListName, footholdViz); List<FramePoint2d> contactPoints2d = contactableFoot.getContactPoints2d(); for (int i = 0; i < contactPoints2d.size(); i++) defaultContactPointsInSoleFrame.add(contactPoints2d.get(i).getPointCopy()); }
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 createGraphicsAndArtifacts(YoGraphicsListRegistry yoGraphicsListRegistry) { yoGraphicsListRegistry.registerYoGraphic("Frames", leftMidZUpFrameViz); yoGraphicsListRegistry.registerYoGraphic("Frames", rightMidZUpFrameViz); yoGraphicsListRegistry.registerYoGraphic("target", targetViz); yoGraphicsListRegistry.registerArtifact("target", targetViz.createArtifact()); yoGraphicsListRegistry.registerArtifact("centroidViz", centroidViz.createArtifact()); yoGraphicsListRegistry.registerYoGraphic("nominalYaw", nominalYawGraphic); YoArtifactPolygon supportPolygonArtifact = new YoArtifactPolygon("quadSupportPolygonArtifact", supportPolygon, Color.blue, false); YoArtifactPolygon currentTriplePolygonArtifact = new YoArtifactPolygon("currentTriplePolygonArtifact", currentTriplePolygon, Color.GREEN, false); yoGraphicsListRegistry.registerArtifact("nominalYawArtifact", nominalYawArtifact); yoGraphicsListRegistry.registerArtifact("supportPolygon", supportPolygonArtifact); yoGraphicsListRegistry.registerArtifact("currentTriplePolygon", currentTriplePolygonArtifact); yoGraphicsListRegistry.setYoGraphicsUpdatedRemotely(true); }
public ContactPointVisualizer(ArrayList<? extends PlaneContactState> contactStates, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { this.contactStates = contactStates; int totalNumberOfContactPoints = 0; for (int i = 0; i < contactStates.size(); i++) totalNumberOfContactPoints += contactStates.get(i).getTotalNumberOfContactPoints(); maxNumberOfDynamicGraphicPositions = totalNumberOfContactPoints; for (int i = 0; i < maxNumberOfDynamicGraphicPositions; i++) { YoFramePoint contactPointWorld = new YoFramePoint("contactPoint" + i, worldFrame, this.registry); contactPointsWorld.add(contactPointWorld); YoGraphicPosition dynamicGraphicPosition = new YoGraphicPosition("contactViz" + i, contactPointWorld, 0.01, YoAppearance.Crimson()); dynamicGraphicPositions.add(dynamicGraphicPosition); yoGraphicsListRegistry.registerYoGraphic("contactPoints", dynamicGraphicPosition); YoFrameVector normalVector = new YoFrameVector("contactNormal" + i, worldFrame, registry); normalVectors.add(normalVector); YoGraphicVector dynamicGraphicVector = new YoGraphicVector("contactNormalViz" + i, contactPointWorld, normalVector, YoAppearance.Crimson()); dynamicGraphicVectors.add(dynamicGraphicVector); yoGraphicsListRegistry.registerYoGraphic("contactPoints", dynamicGraphicVector); } parentRegistry.addChild(registry); }
private void createCandidateFootstep(String name, int index, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { YoFramePoseUsingYawPitchRoll footstepYoFramePose = new YoFramePoseUsingYawPitchRoll(name + index + "FramePose", ReferenceFrame.getWorldFrame(), registry); footstepYoFramePose.setToNaN(); YoGraphicPolygon footstepYoGraphicPolygon = new YoGraphicPolygon(name + index + "YoGraphicPolygon", footstepYoFramePose, defaultFootPolygon.getNumberOfVertices(), registry, 1.0, YoAppearance.Red()); footstepYoGraphicPolygon.updateConvexPolygon2d(defaultFootPolygon); candidateFootstepPolygons.put(index, footstepYoGraphicPolygon); yoGraphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), footstepYoGraphicPolygon); }
private YoGraphicPolygon createStaticFootstep(String name, FramePose3D framePose, AppearanceDefinition appearance, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { YoFramePoseUsingYawPitchRoll footstepYoFramePose = new YoFramePoseUsingYawPitchRoll(name + "FramePose", ReferenceFrame.getWorldFrame(), registry); footstepYoFramePose.set(framePose); YoGraphicPolygon footstepYoGraphicPolygon = new YoGraphicPolygon(name + "YoGraphicPolygon", footstepYoFramePose, defaultFootPolygon.getNumberOfVertices(), registry, 1.0, appearance); footstepYoGraphicPolygon.updateConvexPolygon2d(defaultFootPolygon); yoGraphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), footstepYoGraphicPolygon); return footstepYoGraphicPolygon; }
private void populateDynamicsGraphicObjects(YoGraphicsListRegistry yoGraphicsListRegistry) { perturbanceApplicationPoint = new YoFramePoint3D("perturbanceApplicationPoint", "", ReferenceFrame.getWorldFrame(), registry); YoGraphicVector perturbanceVisual = new YoGraphicVector("perturbanceVisual" + name, perturbanceApplicationPoint, perturbanceForce, 0.005, YoAppearance.BlackMetalMaterial()); yoGraphicsListRegistry.registerYoGraphic(name, perturbanceVisual); }
public void addYoGraphicForceVectorsToGroundContactPoints(int groupIdentifier, double forceVectorScale, AppearanceDefinition appearance, YoGraphicsListRegistry yoGraphicsListRegistry) { if (yoGraphicsListRegistry == null) return; GroundContactPointGroup groundContactPointGroup = pinJoint.getGroundContactPointGroup(groupIdentifier); System.out.println("GroundContactPointGroup" + groundContactPointGroup.getGroundContactPoints()); ArrayList<GroundContactPoint> groundContactPoints = groundContactPointGroup.getGroundContactPoints(); for (GroundContactPoint groundContactPoint : groundContactPoints) { YoGraphicVector yoGraphicVector = new YoGraphicVector(groundContactPoint.getName(), groundContactPoint.getYoPosition(), groundContactPoint.getYoForce(), forceVectorScale, appearance); yoGraphicsListRegistry.registerYoGraphic("ContactableToroidRobot", yoGraphicVector); } }
public void addYoGraphicForceVectorsToGroundContactPoints(int groupIdentifier, double forceVectorScale, AppearanceDefinition appearance, YoGraphicsListRegistry yoGraphicsListRegistry) { if (yoGraphicsListRegistry == null) return; GroundContactPointGroup groundContactPointGroup = nullJoint.getGroundContactPointGroup(groupIdentifier); System.out.println("GroundContactPointGroup" + groundContactPointGroup.getGroundContactPoints()); ArrayList<GroundContactPoint> groundContactPoints = groundContactPointGroup.getGroundContactPoints(); for (GroundContactPoint groundContactPoint : groundContactPoints) { YoGraphicVector yoGraphicVector = new YoGraphicVector(groundContactPoint.getName(), groundContactPoint.getYoPosition(), groundContactPoint.getYoForce(), forceVectorScale, appearance); yoGraphicsListRegistry.registerYoGraphic("ContactableToroidRobot", yoGraphicVector); } }
public void addYoGraphicForceVectorsToGroundContactPoints(int groupIdentifier, double forceVectorScale, AppearanceDefinition appearance, YoGraphicsListRegistry yoGraphicsListRegistry) { if (yoGraphicsListRegistry == null) return; GroundContactPointGroup groundContactPointGroup = floatingJoint.getGroundContactPointGroup(groupIdentifier); System.out.println("GroundContactPointGroup" + groundContactPointGroup.getGroundContactPoints()); ArrayList<GroundContactPoint> groundContactPoints = groundContactPointGroup.getGroundContactPoints(); for (GroundContactPoint groundContactPoint : groundContactPoints) { YoGraphicVector yoGraphicVector = new YoGraphicVector(groundContactPoint.getName(), groundContactPoint.getYoPosition(), groundContactPoint.getYoForce(), forceVectorScale, appearance); yoGraphicsListRegistry.registerYoGraphic("ContactableToroidRobot", yoGraphicVector); } }
public MomentumVisualizer(String name, TwistCalculator twistCalculator, YoVariableRegistry registry, YoGraphicsListRegistry graphicsRegistry, RigidBody... rigidBodies) { comCalculator = new CenterOfMassCalculator(rigidBodies, ReferenceFrame.getWorldFrame()); momentumCalculator = new MomentumCalculator(twistCalculator, rigidBodies); centerOfMass = new YoFramePoint(name + "CoM", ReferenceFrame.getWorldFrame(), registry); linearMomentum = new YoFrameVector(name + "Momentum", ReferenceFrame.getWorldFrame(), registry); YoGraphicPosition yoCoMGraphics = new YoGraphicPosition(name + "CoM", centerOfMass, 0.05, YoAppearance.Brown()); YoGraphicVector yoMomentumGraphics = new YoGraphicVector(name + "Momentum", centerOfMass, linearMomentum, 0.05, YoAppearance.Brown()); graphicsRegistry.registerYoGraphic(name, yoCoMGraphics); graphicsRegistry.registerYoGraphic(name, yoMomentumGraphics); }
public void addYoGraphicForceVectorsToGroundContactPoints(int groupIdentifier, double forceVectorScale, AppearanceDefinition appearance, YoGraphicsListRegistry yoGraphicsListRegistry) { if (yoGraphicsListRegistry == null) return; GroundContactPointGroup groundContactPointGroup = floatingJoint.getGroundContactPointGroup(groupIdentifier); ArrayList<GroundContactPoint> groundContactPoints = groundContactPointGroup.getGroundContactPoints(); for (GroundContactPoint groundContactPoint : groundContactPoints) { YoGraphicVector yoGraphicVector = new YoGraphicVector(groundContactPoint.getName(), groundContactPoint.getYoPosition(), groundContactPoint.getYoForce(), forceVectorScale, appearance); yoGraphicsListRegistry.registerYoGraphic("ContactableSelectableBoxRobot", yoGraphicVector); } }
public LineTrajectory(double controlDT, Tuple3DReadOnly initialPosition, YoVariableRegistry parentRegistry, YoGraphicsListRegistry graphicsListRegistry) { this.controlDT = controlDT; Vector3D initialPositionA = new Vector3D(initialPosition); Vector3D initialPositionB = new Vector3D(initialPosition); initialPositionA.addX(0.025); initialPositionB.addX(-0.025); pointA = new ParameterVector3D("PointA", initialPositionA, registry); pointB = new ParameterVector3D("PointB", initialPositionB, registry); frequency = new DoubleParameter("Frequency", registry, 0.25); maxVelocity = new DoubleParameter("MaxVelocity", registry, 0.1); limitedPointA = new RateLimitedYoFramePoint("PointALim", "", registry, maxVelocity, controlDT, createFrameTuple(ReferenceFrame.getWorldFrame(), pointA)); limitedPointB = new RateLimitedYoFramePoint("PointBLim", "", registry, maxVelocity, controlDT, createFrameTuple(ReferenceFrame.getWorldFrame(), pointB)); pointAViz = new YoGraphicPosition("PointAViz", limitedPointA, 0.025, YoAppearance.Blue()); pointBViz = new YoGraphicPosition("PointBViz", limitedPointB, 0.025, YoAppearance.Blue()); graphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), pointAViz); graphicsListRegistry.registerYoGraphic(getClass().getSimpleName(), pointBViz); parentRegistry.addChild(registry); }
public static void addGoalViz(FramePose3D goalPose, YoVariableRegistry registry, YoGraphicsListRegistry graphicsListRegistry) { YoFramePoint3D yoGoal = new YoFramePoint3D("GoalPosition", worldFrame, registry); yoGoal.set(goalPose.getPosition()); graphicsListRegistry.registerYoGraphic("viz", new YoGraphicPosition("GoalViz", yoGoal, 0.05, YoAppearance.Yellow())); YoFramePoint3D yoStart = new YoFramePoint3D("StartPosition", worldFrame, registry); graphicsListRegistry.registerYoGraphic("viz", new YoGraphicPosition("StartViz", yoStart, 0.05, YoAppearance.Blue())); PoseReferenceFrame goalFrame = new PoseReferenceFrame("GoalFrame", goalPose); FrameVector3D goalOrientation = new FrameVector3D(goalFrame, 0.5, 0.0, 0.0); goalOrientation.changeFrame(worldFrame); YoFrameVector3D yoGoalOrientation = new YoFrameVector3D("GoalVector", worldFrame, registry); yoGoalOrientation.set(goalOrientation); // graphicsListRegistry.registerYoGraphic("vizOrientation", new YoGraphicVector("GoalOrientationViz", yoGoal, yoGoalOrientation, 1.0, YoAppearance.White())); }
private void setupPositionGraphics() { YoFramePoint3D yoCoMPosition = new YoFramePoint3D("CoMPositionForViz", worldFrame, registry); comPositionGraphic = new YoGraphicPosition("CoMPositionGraphic", yoCoMPosition, trackBallSize * 2, new YoAppearanceRGBColor(comPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); YoFramePoint3D yoICPPosition = new YoFramePoint3D("ICPPositionForViz", worldFrame, registry); icpPositionGraphic = new YoGraphicPosition("ICPPositionGraphic", yoICPPosition, trackBallSize * 2, new YoAppearanceRGBColor(icpPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); YoFramePoint3D yoCMPPosition = new YoFramePoint3D("CMPPositionForViz", worldFrame, registry); cmpPositionGraphic = new YoGraphicPosition("CMPPositionGraphic", yoCMPPosition, trackBallSize * 2, new YoAppearanceRGBColor(cmpPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); YoFramePoint3D yoCoPPosition = new YoFramePoint3D("CoPPositionForViz", worldFrame, registry); copPositionGraphic = new YoGraphicPosition("CoPPositionGraphic", yoCoPPosition, trackBallSize * 2, new YoAppearanceRGBColor(copPointsColor, 0.0), GraphicType.BALL_WITH_ROTATED_CROSS); graphicsListRegistry.registerYoGraphic("GraphicPositions", comPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", comPositionGraphic.createArtifact()); graphicsListRegistry.registerYoGraphic("GraphicPositions", icpPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", icpPositionGraphic.createArtifact()); graphicsListRegistry.registerYoGraphic("GraphicPositions", cmpPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", cmpPositionGraphic.createArtifact()); graphicsListRegistry.registerYoGraphic("GraphicPositions", copPositionGraphic); graphicsListRegistry.registerArtifact("GraphicsArtifacts", copPositionGraphic.createArtifact()); }
private void createPushRobotController() { if (usePushRobotController.get()) { PushRobotController bodyPushRobotController = new PushRobotController(sdfRobot.get(), "body", new Vector3d(0.0, -0.00057633, 0.0383928)); yoGraphicsListRegistry.registerYoGraphic("PushRobotControllers", bodyPushRobotController.getForceVisualizer()); for (QuadrupedJointName quadrupedJointName : modelFactory.get().getQuadrupedJointNames()) { String jointName = modelFactory.get().getSDFNameForJointName(quadrupedJointName); PushRobotController jointPushRobotController = new PushRobotController(sdfRobot.get(), jointName, new Vector3d(0.0, 0.0, 0.0)); yoGraphicsListRegistry.registerYoGraphic("PushRobotControllers", jointPushRobotController.getForceVisualizer()); } } }
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)); } }