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 public YoGraphicVector duplicate(YoVariableRegistry newRegistry) { return new YoGraphicVector(getName(), base.duplicate(newRegistry), vector.duplicate(newRegistry), scaleFactor, appearance, drawArrowhead, lineRadiusWhenOneMeterLong); } }
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 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); } }
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 PushRobotController(FloatingRootJointRobot pushableRobot, String jointNameToApplyForce, Vector3d forcePointOffset) { yoTime = pushableRobot.getYoTime(); registry = new YoVariableRegistry(jointNameToApplyForce + "_" + getClass().getSimpleName()); forcePoint = new ExternalForcePoint(jointNameToApplyForce + "_externalForcePoint", forcePointOffset, pushableRobot); pushDuration = new DoubleYoVariable(jointNameToApplyForce + "_pushDuration", registry); pushForceMagnitude = new DoubleYoVariable(jointNameToApplyForce + "_pushMagnitude", registry); pushDirection = new YoFrameVector(jointNameToApplyForce + "_pushDirection", worldFrame, registry); pushForce = new YoFrameVector(jointNameToApplyForce + "_pushForce", worldFrame, registry); pushTimeSwitch = new DoubleYoVariable(jointNameToApplyForce + "_pushTimeSwitch", registry); pushNumber = new IntegerYoVariable(jointNameToApplyForce + "_pushNumber", registry); isBeingPushed = new BooleanYoVariable(jointNameToApplyForce + "_isBeingPushed", registry); pushDelay = new DoubleYoVariable(jointNameToApplyForce + "_pushDelay", registry); pushableRobot.getJoint(jointNameToApplyForce).addExternalForcePoint(forcePoint); pushableRobot.setController(this); pushTimeSwitch.set(Double.NEGATIVE_INFINITY); pushForceMagnitude.set(0.0); forceVisualizer = new YoGraphicVector(jointNameToApplyForce + "_pushForce", forcePoint.getYoPosition(), forcePoint.getYoForce(), 0.005, YoAppearance.DarkBlue()); }
public void createAvailableContactPoints(int groupIdentifier, int totalContactPointsAvailable, double forceVectorScale, boolean addYoGraphicForceVectorsForceVectors) { YoGraphicsListRegistry yoGraphicsListRegistry = null; if (addYoGraphicForceVectorsForceVectors) yoGraphicsListRegistry = new YoGraphicsListRegistry(); for (int i = 0; i < totalContactPointsAvailable; i++) { GroundContactPoint contactPoint = new GroundContactPoint("contact_" + name + "_" + i, robot.getRobotsYoVariableRegistry()); getJoint().addGroundContactPoint(groupIdentifier, contactPoint); allGroundContactPoints.add(contactPoint); YoBoolean contactAvailable = new YoBoolean("contact_" + name + "_" + i + "_avail", robot.getRobotsYoVariableRegistry()); contactAvailable.set(true); contactsAvailable.add(contactAvailable); if (addYoGraphicForceVectorsForceVectors) { YoGraphicPosition yoGraphicPosition = new YoGraphicPosition(name + "Point" + i, contactPoint.getYoPosition(), 0.02, YoAppearance.Green()); YoGraphicVector yoGraphicVector = new YoGraphicVector(name + "Force" + i, contactPoint.getYoPosition(), contactPoint.getYoForce(), forceVectorScale, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicPosition); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicVector); } } if (addYoGraphicForceVectorsForceVectors) { robot.addYoGraphicsListRegistry(yoGraphicsListRegistry); } }
public PushRobotController(FloatingRootJointRobot pushableRobot, String jointNameToApplyForce, Vector3D forcePointOffset, double visualScale) { yoTime = pushableRobot.getYoTime(); registry = new YoVariableRegistry(jointNameToApplyForce + "_" + getClass().getSimpleName()); forcePoint = new ExternalForcePoint(jointNameToApplyForce + "_externalForcePoint", forcePointOffset, pushableRobot); pushDuration = new YoDouble(jointNameToApplyForce + "_pushDuration", registry); pushForceMagnitude = new YoDouble(jointNameToApplyForce + "_pushMagnitude", registry); pushDirection = new YoFrameVector3D(jointNameToApplyForce + "_pushDirection", worldFrame, registry); pushForce = new YoFrameVector3D(jointNameToApplyForce + "_pushForce", worldFrame, registry); pushTimeSwitch = new YoDouble(jointNameToApplyForce + "_pushTimeSwitch", registry); pushNumber = new YoInteger(jointNameToApplyForce + "_pushNumber", registry); isBeingPushed = new YoBoolean(jointNameToApplyForce + "_isBeingPushed", registry); pushDelay = new YoDouble(jointNameToApplyForce + "_pushDelay", registry); pushableRobot.getJoint(jointNameToApplyForce).addExternalForcePoint(forcePoint); pushableRobot.setController(this); pushTimeSwitch.set(Double.NEGATIVE_INFINITY); pushForceMagnitude.set(0.0); forceVisualizer = new YoGraphicVector(jointNameToApplyForce + "_pushForce", forcePoint.getYoPosition(), forcePoint.getYoForce(), visualScale, YoAppearance.DarkBlue()); }
yoPlaneNormal = new YoFrameVector(caption, worldFrame, registry); YoGraphicVector planeNormalViz = new YoGraphicVector(caption, yoPlanePoint, yoPlaneNormal, YoAppearance.Blue()); planeNormalViz.setVisible(VISUALIZE); yoGraphicsListRegistry.registerYoGraphic(listName, planeNormalViz);
YoGraphicVector yoGraphicVector = new YoGraphicVector("Example", zero, zero, zero, testPoint.getYoX(), testPoint.getYoY(), testPoint.getYoZ(), 1.0, YoAppearance.Black());
graphics3DNodeType = Graphics3DNodeType.GROUND; simpleExternalForcePoint = new ExternalForcePoint("simpleExternalForcePoint", dummyRegistry); yoGraphic = new YoGraphicVector("simpleDynamicGraphicObject", simpleExternalForcePoint.getYoPosition(), simpleExternalForcePoint.getYoForce(), 1.0 / 50.0); exitActionListenerHasBeenNotified = new YoBoolean("exitActionListenerHasBeenNotified", dummyRegistry);
public ProvidedMassMatrixToolRigidBody(RobotSide robotSide, final FullHumanoidRobotModel fullRobotModel, double gravity, ArmControllerParameters armControllerParameters, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { String name = robotSide.getCamelCaseNameForStartOfExpression() + "Tool"; this.registry = new YoVariableRegistry(name); this.fullRobotModel = fullRobotModel; this.gravity = gravity; this.handFixedFrame = fullRobotModel.getHand(robotSide).getBodyFixedFrame(); this.handControlFrame = fullRobotModel.getHandControlFrame(robotSide); this.elevatorFrame = fullRobotModel.getElevatorFrame(); toolFrame = new PoseReferenceFrame(name + "Frame", elevatorFrame); RigidBodyInertia inertia = new RigidBodyInertia(toolFrame, new Matrix3d(), 0.0); this.toolJoint = new SixDoFJoint(name + "Joint", fullRobotModel.getElevator(), fullRobotModel.getElevator().getBodyFixedFrame()); this.toolBody = new RigidBody(name + "Body", inertia, toolJoint); objectCenterOfMass = new YoFramePoint(name + "CoMOffset", handControlFrame, registry); objectMass = new DoubleYoVariable(name + "ObjectMass", registry); objectForceInWorld = new YoFrameVector(name + "Force", ReferenceFrame.getWorldFrame(), registry); this.objectCenterOfMassInWorld = new YoFramePoint(name + "CoMInWorld", ReferenceFrame.getWorldFrame(), registry); if (yoGraphicsListRegistry != null) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); YoGraphic comViz = new YoGraphicPosition(name + "CenterOfMassViz", objectCenterOfMassInWorld, 0.05, YoAppearance.Red()); yoGraphicsList.add(comViz); YoGraphic vectorViz = new YoGraphicVector(name + "ForceViz", objectCenterOfMassInWorld, objectForceInWorld, YoAppearance.Yellow()); yoGraphicsList.add(vectorViz); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); } parentRegistry.addChild(registry); }
forceVisualizer = new YoGraphicVector("contactForceVisualizer" + suffix, forcePoint.getYoPosition(), contactForce, 0.05, forceAppearance);