static YoGraphicCoordinateSystem createAsRemoteYoGraphic(String name, YoVariable<?>[] yoVariables, double[] constants) { return new YoGraphicCoordinateSystem(name, yoVariables, constants); }
hasData = feedbackControllerDataHolder.getPositionData(endEffector, position, Type.DESIRED); if (!hasData) coordinateSystem.hide(); else coordinateSystem.setPosition(position); coordinateSystem.hide(); else coordinateSystem.setOrientation(orientation); hasData = feedbackControllerDataHolder.getPositionData(endEffector, position, Type.CURRENT); if (!hasData) coordinateSystem.hide(); else coordinateSystem.setPosition(position); coordinateSystem.hide(); else coordinateSystem.setOrientation(orientation);
@Override public YoGraphicCoordinateSystem duplicate(YoVariableRegistry newRegistry) { if (isUsingYawPitchRoll()) return new YoGraphicCoordinateSystem(getName(), position.duplicate(newRegistry), yawPitchRoll.duplicate(newRegistry), scale, arrowColor); else return new YoGraphicCoordinateSystem(getName(), position.duplicate(newRegistry), quaternion.duplicate(newRegistry), scale, arrowColor); }
public void setPose(FramePose3DReadOnly pose) { setPosition(pose.getPosition()); setOrientation(pose.getOrientation()); }
endeffectorFrame.put(robotSide, new YoGraphicCoordinateSystem("" + robotSide + "endeffectorPoseFrame", endeffectorPose.get(robotSide), 0.25)); endeffectorFrame.get(robotSide).setVisible(true); yoGraphicsListRegistry.registerYoGraphic("" + robotSide + "endeffectorPoseViz", endeffectorFrame.get(robotSide)); testFrameViz = new YoGraphicCoordinateSystem("testFrameViz", testFramePose, 0.25); yoGraphicsListRegistry.registerYoGraphic("testFrameYoGraphic", testFrameViz);
feetCoordinateSystems.get(robotSide).setToReferenceFrame(footFrame); palmFrame.update(); handCoordinateSystems.get(robotSide).setToReferenceFrame(palmFrame);
public void setTransformToWorld(RigidBodyTransform transformToWorld) { transformToWorld.getTranslation(translationToWorld); x.set(translationToWorld.getX()); y.set(translationToWorld.getY()); z.set(translationToWorld.getZ()); orientation.setIncludingFrame(ReferenceFrame.getWorldFrame(), transformToWorld); setOrientation(orientation); }
feetCoordinateSystems.get(robotSide).setToReferenceFrame(footFrame); palmFrame.update(); handCoordinateSystems.get(robotSide).setToReferenceFrame(palmFrame);
/** * Convenience method that should only be used for setting up the visualization. * <p> * Simply creates a graphic coordinate system visualizable in {@code SCSVisualizer}. * </p> * * @param endEffector used to create a name prefix required for creating a * {@link YoGraphicCoordinateSystem}. * @param type used to create a name prefix required for creating a * {@link YoGraphicCoordinateSystem}. * @param appearanceDefinition the appearance of the coordinate system's arrows. * @return the graphic with a good name for the given end-effector. */ private YoGraphicCoordinateSystem createCoodinateSystem(RigidBodyBasics endEffector, Type type, AppearanceDefinition appearanceDefinition) { String namePrefix = endEffector.getName() + type.getName(); return new YoGraphicCoordinateSystem(namePrefix, "", registry, false, 0.2, appearanceDefinition); }
@Override public void update(double time) spinePitchZUpFrameViz.setToReferenceFrame(spinePitchCenterOfMassCalculator.getDesiredFrame()); leftHipPitchZUpFrameViz.setToReferenceFrame(leftHipPitchCenterOfMassCalculator.getDesiredFrame()); leftHipPitchFrameViz.setToReferenceFrame(fullRobotModel.getLegJoint(RobotSide.LEFT, LegJointName.HIP_PITCH).getFrameAfterJoint());
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()); }
@Override public void update(double time) spinePitchZUpFrameViz.setToReferenceFrame(spinePitchCenterOfMassCalculator.getReferenceFrame()); leftHipPitchZUpFrameViz.setToReferenceFrame(leftHipPitchCenterOfMassCalculator.getReferenceFrame()); leftHipPitchFrameViz.setToReferenceFrame(fullRobotModel.getLegJoint(RobotSide.LEFT, LegJointName.HIP_PITCH).getFrameAfterJoint());
private void setupVisualization(String namePrefix, YoGraphicsListRegistry yoGraphicsListRegistry) { YoGraphicsList list = new YoGraphicsList(name); YoGraphicCoordinateSystem desiredPoseViz = new YoGraphicCoordinateSystem(namePrefix + "DesiredPose", yoDesiredPose, 0.3); list.add(desiredPoseViz); yoGraphicsListRegistry.registerYoGraphicsList(list); list.hideYoGraphics(); }
private YoGraphicsListRegistry createStartAndGoalGraphics(FramePose3D initialStancePose, FramePose3D goalPose) { YoGraphicsListRegistry graphicsListRegistry = new YoGraphicsListRegistry(); YoGraphicsList graphicsList = new YoGraphicsList("testViz"); YoFramePoseUsingYawPitchRoll yoInitialStancePose = new YoFramePoseUsingYawPitchRoll("initialStancePose", initialStancePose.getReferenceFrame(), drcSimulationTestHelper.getYoVariableRegistry()); yoInitialStancePose.set(initialStancePose); YoFramePoseUsingYawPitchRoll yoGoalPose = new YoFramePoseUsingYawPitchRoll("goalStancePose", goalPose.getReferenceFrame(), drcSimulationTestHelper.getYoVariableRegistry()); yoGoalPose.set(goalPose); YoGraphicCoordinateSystem startPoseGraphics = new YoGraphicCoordinateSystem("startPose", yoInitialStancePose, 13.0); YoGraphicCoordinateSystem goalPoseGraphics = new YoGraphicCoordinateSystem("goalPose", yoGoalPose, 13.0); graphicsList.add(startPoseGraphics); graphicsList.add(goalPoseGraphics); return graphicsListRegistry; }
YoGraphicCoordinateSystem desiredHandPoseViz = new YoGraphicCoordinateSystem(sidePrefix + "DesiredHandPose", "", registry, 0.2); desiredHandPosesViz.put(robotSide, desiredHandPoseViz); YoGraphicCoordinateSystem desiredFootPoseViz = new YoGraphicCoordinateSystem(sidePrefix + "DesiredFootPose", "", registry, 0.2); desiredFootPosesViz.put(robotSide, desiredFootPoseViz); yoGraphicsListRegistry.registerYoGraphic("DesiredCoords", desiredHandPoseViz);
rightKneeCoMInZUpFrame = new YoFramePoint3D("rightKneeCoMInZUpFrame", rightKneeCenterOfMassCalculator.getReferenceFrame(), registry); spinePitchZUpFrameViz = new YoGraphicCoordinateSystem("spinePitchZUpFrameViz", "", registry, true, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", spinePitchZUpFrameViz); leftHipPitchZUpFrameViz = new YoGraphicCoordinateSystem("leftHipPitchZUpFrameViz", "", registry, true, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchZUpFrameViz); leftHipPitchFrameViz = new YoGraphicCoordinateSystem("leftHipPitchFrameViz", "", registry, true, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchFrameViz);
rightKneeCoMInZUpFrame = new YoFramePoint("rightKneeCoMInZUpFrame", rightKneeCenterOfMassCalculator.getDesiredFrame(), registry); spinePitchZUpFrameViz = new YoGraphicCoordinateSystem("spinePitchZUpFrameViz", "", registry, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", spinePitchZUpFrameViz); leftHipPitchZUpFrameViz = new YoGraphicCoordinateSystem("leftHipPitchZUpFrameViz", "", registry, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchZUpFrameViz); leftHipPitchFrameViz = new YoGraphicCoordinateSystem("leftHipPitchFrameViz", "", registry, 0.3); yoGraphicsListRegistry.registerYoGraphic("CenterOfMassCalibrationTool", leftHipPitchFrameViz);
return new YoGraphicCoordinateSystem(name, (DoubleYoVariable) vars[0], (DoubleYoVariable) vars[1], (DoubleYoVariable) vars[2], (DoubleYoVariable) vars[3], (DoubleYoVariable) vars[4], (DoubleYoVariable) vars[5], consts[0]);