public void addGroundContactPoint(Robot robot, String groundContactPointName) { ArrayList<GroundContactPoint> allGroundContactPoints = robot.getAllGroundContactPoints(); for (GroundContactPoint groundContactPoint : allGroundContactPoints) { if (groundContactPoint.getName().equals(groundContactPointName)) { this.addGroundContactPoint(groundContactPoint); return; } } }
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 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 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); }
public SDFQuadrupedPerfectSimulatedSensor(FloatingRootJointRobot sdfRobot, FullQuadrupedRobotModel fullRobotModel, CommonQuadrupedReferenceFrames referenceFrames) { super(sdfRobot, fullRobotModel, referenceFrames); sensorOneDoFJoints = fullRobotModel.getOneDoFJoints(); //FootSwitches ArrayList<GroundContactPoint> groundContactPoints = sdfRobot.getAllGroundContactPoints(); for(RobotQuadrant quadrant : RobotQuadrant.values) { String prefix = quadrant.getCamelCaseNameForStartOfExpression(); OneDoFJoint jointBeforeFoot = fullRobotModel.getOneDoFJointBeforeFoot(quadrant); for(GroundContactPoint groundContactPoint : groundContactPoints) { if(groundContactPoint.getParentJoint().getName().equals(jointBeforeFoot.getName())) { footSwitches.set(quadrant, new SimulatedContactBasedFootSwitch(prefix + groundContactPoint.getName(), groundContactPoint, super.getYoVariableRegistry())); } } } enableDrives = new BooleanYoVariable("enableDrives", getYoVariableRegistry()); enableDrives.set(true); }