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; }
public YoGraphicPolygon(String name, YoFrameConvexPolygon2D yoFrameConvexPolygon2d, YoFramePoseUsingYawPitchRoll framePose, double scale, AppearanceDefinition appearance) { this(name, yoFrameConvexPolygon2d, framePose.getPosition(), framePose.getOrientation(), scale, appearance); }
private void set(Quaternion newRotation) { yoFramePose.setOrientation(newRotation); }
public void setXYZYawPitchRoll(double[] pose) { setXYZ(pose[0], pose[1], pose[2]); setYawPitchRoll(pose[3], pose[4], pose[5]); }
public void set(YoFramePoseUsingYawPitchRoll yoFramePose) { set(yoFramePose.getPosition(), yoFramePose.getOrientation().getFrameOrientation()); }
YoFramePoseUsingYawPitchRoll yoPose = new YoFramePoseUsingYawPitchRoll("footPose" + sideName + i, worldFrame, registry); yoPose.setToNaN(); solePosesForVisualization.get(robotSide).add(yoPose); YoGraphicPolygon footstepViz = new YoGraphicPolygon("footstep" + sideName + i, defaultPolygon, yoPose, 1.0, appearance); startStep = new YoFramePoseUsingYawPitchRoll("startFootPose", worldFrame, registry); startStep.setToNaN(); YoGraphicPolygon stanceViz = new YoGraphicPolygon("startFootPose", defaultPolygon, startStep, 1.0, YoAppearance.Black()); graphicsListRegistry.registerYoGraphic("viz", stanceViz); YoFramePoseUsingYawPitchRoll step = new YoFramePoseUsingYawPitchRoll("step" + i, worldFrame, registry); step.setToNaN(); stepPosesTaken.add(step); YoGraphicPolygon polygon = new YoGraphicPolygon("step" + i, defaultPolygon, step, 1.0, YoAppearance.Gray()); YoFramePoseUsingYawPitchRoll pose = new YoFramePoseUsingYawPitchRoll("PolygonPose" + i, worldFrame, registry); pose.setToNaN(); visiblePolygons.add(polygon); visiblePolygonPoses.add(pose); YoGraphicPolygon visualization = new YoGraphicPolygon("Polygon" + i, polygon, pose.getPosition(), pose.getOrientation(), 1.0, 0.02, new YoAppearanceRGBColor(Color.BLUE, 0.8)); polygonVisualizations.add(visualization); visiblePolygonPoses.get(polygonIdx).setToNaN(); visiblePolygonPoses.get(polygonIdx).set(pose); visiblePolygons.get(polygonIdx).set(planarRegion.getConvexHull()); solePosesForVisualization.get(robotSide).get(hideIdx).setToNaN();
public YoReferencePose(String frameName, ReferenceFrame parentFrame, YoVariableRegistry registry) { super(frameName, parentFrame); yoFramePose = new YoFramePoseUsingYawPitchRoll(frameName + "_", this, registry); }
private boolean testTranslationInterpolationToRandomTargets(final Robot robot, YoVariableRegistry registry, int numTargets) throws SimulationExceededMaximumTimeException YoFramePoseUsingYawPitchRoll target = new YoFramePoseUsingYawPitchRoll("target_", ReferenceFrame.getWorldFrame(), registry); RigidBodyTransform[] targets = createRandomCorrectionTargets(numTargets); boolean success = true; target.setYawPitchRoll(yawPitchRoll); target.setXYZ(targetTranslation.getX(), targetTranslation.getY(), targetTranslation.getZ());
swingOverPlanarRegionsTrajectoryExpander.attachVisualizer(this::update); solePose = new YoFramePoseUsingYawPitchRoll("SolePose", WORLD, registry); AppearanceDefinition bubble = YoAppearance.LightBlue(); bubble.setTransparency(0.5); collisionSphere = new YoGraphicEllipsoid("CollisionSphere", solePose.getPosition(), solePose.getOrientation(), bubble, new Vector3D()); stanceFootGraphic = new YoGraphicPolygon("StanceFootGraphic", footPolygon.getNumberOfVertices(), registry, true, 1.0, YoAppearance.Blue()); swingStartGraphic = new YoGraphicPolygon("SwingStartGraphic", footPolygon.getNumberOfVertices(), registry, true, 1.0, YoAppearance.Green());
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); }
public void get(Vector3D translation) { translation.set(yoFramePose.getPosition()); } }
YoFramePoseUsingYawPitchRoll yoFootstepPose = new YoFramePoseUsingYawPitchRoll("footPose" + i, worldFrame, vizRegistry); yoFootstepPose.set(footstepPose); yoFootstepPose.setZ(yoFootstepPose.getZ() + (footstep.getRobotSide() == RobotSide.RIGHT ? 0.001 : 0.0));
public void get(Quaternion rotation) { yoFramePose.getOrientation().getQuaternion(rotation); }
@Override public void doControl() { wristJoint.getTransformToWorld(transform); wristJointPose.set(transform); yoWristJointPose.set(wristJointPose); wristJoint.getTransformToWorld(transform); transform.transform(wristToHandControlFrame, tangentVector); handControlFramePose.set(transform); handControlFramePose.prependTranslation(tangentVector); yoHandControlFramePose.set(handControlFramePose); handControlFramePositionInWorld.set(handControlFramePose.getPosition()); efpHandControlFrame.setPosition(handControlFramePositionInWorld); // efpGravity.setForce(0.0, 0.0, -gGRAVITY * MASSDRILL); // efpGravity.setForce(0.0, 0.0, 0.0); // efpWrist.setForce(exponentialCutForceModel(efpHandControlFrame)); efpWrist.setForce(quadraticCutForceModel(efpHandControlFrame)); }
public SingleFootstepVisualizer(RobotSide robotSide, int maxContactPoints, YoVariableRegistry registry, YoGraphicsListRegistry yoGraphicsListRegistry) { Integer index = indices.get(robotSide); String namePrefix = robotSide.getLowerCaseName() + "Foot" + index; YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix); this.robotSide = robotSide; ArrayList<Point2D> polyPoints = new ArrayList<Point2D>(); yoContactPoints = new YoFramePoint3D[maxContactPoints]; for (int i = 0; i < maxContactPoints; i++) { yoContactPoints[i] = new YoFramePoint3D(namePrefix + "ContactPoint" + i, ReferenceFrame.getWorldFrame(), registry); yoContactPoints[i].set(0.0, 0.0, -1.0); YoGraphicPosition baseControlPointViz = new YoGraphicPosition(namePrefix + "Point" + i, yoContactPoints[i], 0.01, YoAppearance.Blue()); yoGraphicsList.add(baseControlPointViz); polyPoints.add(new Point2D()); } footPolygon = new YoFrameConvexPolygon2D(namePrefix + "yoPolygon", "", ReferenceFrame.getWorldFrame(), maxContactPoints, registry); footPolygon.set(new ConvexPolygon2D(Vertex2DSupplier.asVertex2DSupplier(polyPoints))); soleFramePose = new YoFramePoseUsingYawPitchRoll(namePrefix + "polygonPose", "", ReferenceFrame.getWorldFrame(), registry); soleFramePose.setXYZ(0.0, 0.0, -1.0); footPolygonViz = new YoGraphicPolygon(namePrefix + "graphicPolygon", footPolygon, soleFramePose, 1.0, footPolygonAppearances.get(robotSide)); yoGraphicsList.add(footPolygonViz); if (yoGraphicsListRegistry != null) { yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); yoGraphicsListRegistry.registerGraphicsUpdatableToUpdateInAPlaybackListener(footPolygonViz); } index++; indices.set(robotSide, index); }
private void updateNextFootsteps(int stepIndex) { int numberOfStepToUpdate = (numberOfFootstepsForTest - stepIndex) < numberOfFootstepsToConsider ? (numberOfFootstepsForTest - stepIndex) : numberOfFootstepsToConsider; int nextStepIndex; for (nextStepIndex = 0; nextStepIndex < numberOfStepToUpdate; nextStepIndex++) { nextFootstepPoses.get(nextStepIndex).set(footstepList.get(stepIndex + nextStepIndex).getFootstepPose()); } for (; nextStepIndex < numberOfFootstepsToConsider; nextStepIndex++) { nextFootstepPoses.get(nextStepIndex).setToNaN(); } }
Quaternion desiredFootOrientationInWorld = status.getDesiredFootOrientationInWorld(); desiredFootStatusPoses.get(side).setPosition(desiredFootPositionInWorld); desiredFootStatusPoses.get(side).setOrientation(desiredFootOrientationInWorld); actualFootStatusPoses.get(side).setPosition(actualFootPositionInWorld); actualFootStatusPoses.get(side).setOrientation(actualFootOrientationInWorld);
polygonToSnapPose.setPosition(nonSnappedPosition); polygonToSnapPose.setOrientation(nonSnappedOrientation); polygonToSnapViz.update(); snappedPolygonPose.setToNaN(); snappedPolygonViz.update(); return; snappedPolygonPose.setPosition(snappedPosition); snappedPolygonPose.setOrientation(snappedOrientation); snappedPolygonViz.update();
this.desiredFootStatusPoses.get(stanceSide).getFramePose(tempStanceFootPose); goalPose.setZ(tempStanceFootPose.getZ()); footstepPlannerGoalPose.set(goalPose); footstepPlannerInitialStepPose.set(tempStanceFootPose);
public void setXYZ(double[] pos) { setXYZ(pos[0], pos[1], pos[2]); }