GroundContactPointRobotSensor(GroundContactPoint groundContactPoint) { this.groundContactPoint = groundContactPoint; position = groundContactPoint.getYoPosition(); force = groundContactPoint.getYoForce(); }
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); } }
YoGraphicPosition yoGraphicPosition = new YoGraphicPosition(name + "Point" + joint.getName() + j, contactPoint.getYoPosition(), 0.02, YoAppearance.Green()); 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); 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); 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 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 DRCSimulationVisualizer(Robot robot, YoGraphicsListRegistry yoGraphicsListRegistry) { this.robot = robot; YoGraphicsList yoGraphicsList = new YoGraphicsList("Simulation Viz"); ArrayList<GroundContactPoint> groundContactPoints = robot.getAllGroundContactPoints(); AppearanceDefinition appearance = YoAppearance.Red(); // BlackMetalMaterial(); for (GroundContactPoint groundContactPoint : groundContactPoints) { double scaleFactor = 0.0015; YoGraphicVector dynamicGraphicVector = new YoGraphicVector(groundContactPoint.getName(), groundContactPoint.getYoPosition(), groundContactPoint.getYoForce(), scaleFactor, appearance); yoGraphicsList.add(dynamicGraphicVector); } if (yoGraphicsListRegistry != null) yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); robot.setController(this, 10); }
testPoint.set(groundContactPoint.getYoPosition());
YoFramePoint3D yoPosition = groundContactPoint.getYoPosition(); xPosition = dataBuffer.getEntry(yoPosition.getYoX()).getData(); yPosition = dataBuffer.getEntry(yoPosition.getYoY()).getData();
point1.setForce(new Vector3D(0.0, 0.0, 0.0)); point0.getYoPosition().set(new Point3D(1.0, 1.0, 0.0)); point1.getYoPosition().set(new Point3D(-1.0, 0.0, 0.0)); point1.setForce(new Vector3D(-1.0, 1.0, 0.0)); point0.getYoPosition().set(new Point3D(0.0, 0.0, 1.0)); point1.getYoPosition().set(new Point3D(-2.0, -2.0, 1.0));