private static YoGraphicVector yoGraphicVectorFromMessage(String name, YoVariable<?>[] vars, double[] consts, AppearanceDefinition appearance) { return new YoGraphicVector(name, (YoDouble) vars[0], (YoDouble) vars[1], (YoDouble) vars[2], (YoDouble) vars[3], (YoDouble) vars[4], (YoDouble) vars[5], consts[0], appearance, true); }
@Override protected void computeRotationTranslation(AffineTransform transform3D) { if (vector == null) { return; } vector.sub(end, start); super.computeRotationTranslation(transform3D); }
@Override public YoGraphicVector duplicate(YoVariableRegistry newRegistry) { return new YoGraphicVector(getName(), base.duplicate(newRegistry), vector.duplicate(newRegistry), scaleFactor, appearance, drawArrowhead, lineRadiusWhenOneMeterLong); } }
private void updateContactPointDynamicGraphicObjects(int i, ContactPointInterface contactPoint) { if (contactPoint.isInContact()) { contactPoint.getPosition(tempFramePoint); tempFramePoint.changeFrame(worldFrame); contactPointsWorld.get(i).set(tempFramePoint); normalVectors.get(i).set(tempFrameVector); dynamicGraphicPositions.get(i).showGraphicObject(); dynamicGraphicVectors.get(i).showGraphicObject(); } else { contactPointsWorld.get(i).setToNaN(); normalVectors.get(i).set(Double.NaN, Double.NaN, Double.NaN); dynamicGraphicPositions.get(i).hideGraphicObject(); dynamicGraphicVectors.get(i).hideGraphicObject(); } dynamicGraphicPositions.get(i).update(); dynamicGraphicVectors.get(i).update(); } }
yoPlaneNormal = new YoFrameVector(caption, worldFrame, registry); YoGraphicVector planeNormalViz = new YoGraphicVector(caption, yoPlanePoint, yoPlaneNormal, YoAppearance.Blue()); planeNormalViz.setVisible(VISUALIZE); yoGraphicsListRegistry.registerYoGraphic(listName, planeNormalViz);
public YoGraphicVector(String name, YoFramePoint startPoint, YoFrameVector directionVector, double scale, AppearanceDefinition appearance, boolean drawArrowhead, double lineRadiusWhenOneMeterLong) { this(name, startPoint, directionVector, scale, appearance, drawArrowhead); this.setLineRadiusWhenOneMeterLong(lineRadiusWhenOneMeterLong); }
@Override public Artifact createArtifact() { Color3f color3f = appearance.getColor(); return new YoArtifactLineSegment2d(getName(), new YoFrameLineSegment2d(baseX, baseY, x, y, ReferenceFrame.getWorldFrame()), new Color(color3f.getX(), color3f.getY(), color3f.getZ())); }
forcePoint.setMoment(torqueX, torqueY, torqueZ); forceVisualizer.update();
public YoGraphicVector(String name, YoFramePoint3D startPoint, YoFrameVector3D directionVector, double scale, AppearanceDefinition appearance, boolean drawArrowhead, double lineRadiusWhenOneMeterLong) { this(name, startPoint, directionVector, scale, appearance, drawArrowhead); setLineRadiusWhenOneMeterLong(lineRadiusWhenOneMeterLong); }
@Override public Artifact createArtifact() { MutableColor color3f = appearance.getColor(); YoDouble endPointX = new YoDouble(getName() + "ArtifactEndPointX", base.getYoX().getYoVariableRegistry()); YoDouble endPointY = new YoDouble(getName() + "ArtifactEndPointY", base.getYoY().getYoVariableRegistry()); base.getYoX().addVariableChangedListener(v -> endPointX.set(base.getX() + vector.getX())); base.getYoY().addVariableChangedListener(v -> endPointY.set(base.getY() + vector.getY())); vector.getYoX().addVariableChangedListener(v -> endPointX.set(base.getX() + vector.getX())); vector.getYoY().addVariableChangedListener(v -> endPointY.set(base.getY() + vector.getY())); return new YoArtifactLineSegment2d(getName(), new YoFrameLineSegment2D(base.getYoX(), base.getYoY(), endPointX, endPointY, ReferenceFrame.getWorldFrame()), color3f.get()); }
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); }
protected void computeRotationTranslation(Transform3d transform3D) { if (vectorX == null) { return; } vectorX.set(endX.getDoubleValue() - startX.getDoubleValue()); vectorY.set(endY.getDoubleValue() - startY.getDoubleValue()); vectorZ.set(endZ.getDoubleValue() - startZ.getDoubleValue()); super.computeRotationTranslation(transform3D); }
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); }
public WrenchVisualizer(String name, List<RigidBody> rigidBodies, double vizScaling, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry, AppearanceDefinition forceAppearance, AppearanceDefinition torqueAppearance) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); this.rigidBodies.addAll(rigidBodies); for (RigidBody rigidBody : rigidBodies) { String prefix = name + rigidBody.getName(); YoFrameVector force = new YoFrameVector(prefix + "Force", ReferenceFrame.getWorldFrame(), registry); forces.put(rigidBody, force); YoFrameVector torque = new YoFrameVector(prefix + "Torque", ReferenceFrame.getWorldFrame(), registry); torques.put(rigidBody, torque); YoFramePoint pointOfApplication = new YoFramePoint(prefix + "PointOfApplication", ReferenceFrame.getWorldFrame(), registry); pointsOfApplication.put(rigidBody, pointOfApplication); YoGraphicVector forceVisualizer = new YoGraphicVector(prefix + "ForceViz", pointOfApplication, force, FORCE_VECTOR_SCALE * vizScaling, forceAppearance, true); forceVisualizers.put(rigidBody, forceVisualizer); yoGraphicsList.add(forceVisualizer); YoGraphicVector torqueVisualizer = new YoGraphicVector(prefix + "TorqueViz", pointOfApplication, torque, TORQUE_VECTOR_SCALE * vizScaling, torqueAppearance, true); torqueVisualizers.put(rigidBody, torqueVisualizer); yoGraphicsList.add(torqueVisualizer); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); parentRegistry.addChild(registry); }
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 = 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 BasisVectorVisualizer(String name, int rhoSize, double vizScaling, YoGraphicsListRegistry yoGraphicsListRegistry, YoVariableRegistry parentRegistry) { AppearanceDefinition basisAppearance = YoAppearance.Aqua(); this.rhoSize = rhoSize; YoGraphicsList yoGraphicsList = new YoGraphicsList(name); for (int i = 0; i < rhoSize; i++) { String prefix = name + i; YoFrameVector basisVector = new YoFrameVector(prefix + "BasisVector", ReferenceFrame.getWorldFrame(), registry); yoBasisVectors.put(i, basisVector); YoFramePoint pointOfBasis = new YoFramePoint(prefix + "PointOfBasis", ReferenceFrame.getWorldFrame(), registry); pointOfBases.put(i, pointOfBasis); YoGraphicVector basisVisualizer = new YoGraphicVector(prefix + "BasisViz", pointOfBasis , basisVector, BASIS_VECTOR_SCALE * vizScaling, basisAppearance, true); basisVisualizers.put(i, basisVisualizer); yoGraphicsList.add(basisVisualizer); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); parentRegistry.addChild(registry); }
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); } }
YoGraphicVector yoGraphicVector = new YoGraphicVector(name + "Force" +joint.getName() + j, contactPoint.getYoPosition(), contactPoint.getYoForce(), forceVectorScale, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicPosition); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicVector);
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); } }