private String getLastVariableNameFromRobotRegistry(Robot robotModel) { int lastIndex = robotModel.getRobotsYoVariableRegistry().getAllVariablesArray().length - 1; return robotModel.getRobotsYoVariableRegistry().getAllVariablesArray()[lastIndex].getName(); }
private GroundContactModel createGroundContactModel(Robot robot, GroundProfile3D groundProfile) { double kXY = 5000.0; double bXY = 100.0; double kZ = 1000.0; double bZ = 100.0; double alphaStick = 0.7; double alphaSlip = 0.5; GroundContactModel groundContactModel = new LinearStickSlipGroundContactModel(robot, kXY, bXY, kZ, bZ, alphaSlip, alphaStick, robot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(groundProfile); return groundContactModel; }
private GroundContactModel createGroundContactModel(Robot robot, GroundProfile3D groundProfile) { double kXY = 5000.0; double bXY = 100.0; double kZ = 1000.0; double bZ = 100.0; double alphaStick = 0.7; double alphaSlip = 0.5; GroundContactModel groundContactModel = new LinearStickSlipGroundContactModel(robot, kXY, bXY, kZ, bZ, alphaSlip, alphaStick, robot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(groundProfile); return groundContactModel; }
private String getRegistryNameSpaceFromRobot(Robot robotModel) { return robotModel.getRobotsYoVariableRegistry().getNameSpace().getName(); }
private String getFirstVariableNameFromRobotRegistry(Robot robotModel) { return robotModel.getRobotsYoVariableRegistry().getAllVariablesArray()[0].getName(); }
private GroundContactModel createGroundContactModel(Robot robot, GroundProfile3D groundProfile) { double kXY = 40000.0; double bXY = 10.0; double kZ = 80.0; double bZ = 500.0; double alphaStick = 0.7; double alphaSlip = 0.5; GroundContactModel groundContactModel = new LinearStickSlipGroundContactModel(robot, kXY, bXY, kZ, bZ, alphaSlip, alphaStick, robot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(groundProfile); return groundContactModel; } }
@BeforeEach public void setUp() { offset = new Vector3D(1.0, 2.0, 3.0); robot = new Robot("testRobot"); kinematicPoint = new KinematicPoint("testPoint", offset, robot.getRobotsYoVariableRegistry()); }
public void initializeRobot(Robot robot, DRCRobotModel robotModel, YoGraphicsListRegistry yoGraphicsListRegistry) { robot.setGravity(gravity); LinearGroundContactModel groundContactModel = new LinearGroundContactModel(robot, robot.getRobotsYoVariableRegistry()); robotModel.getContactPointParameters().setupGroundContactModelParameters(groundContactModel); // if ((commonTerrain.getSteppingStones() != null) && (yoGraphicsListRegistry != null)) // commonTerrain.registerSteppingStonesArtifact(yoGraphicsListRegistry); // groundContactModel.setGroundProfile(commonTerrain.getGroundProfile()); if (groundProfile3D != null) groundContactModel.setGroundProfile3D(groundProfile3D); // TODO: change this to scs.setGroundContactModel(groundContactModel); robot.setGroundContactModel(groundContactModel); }
registry = nullRobot.getRobotsYoVariableRegistry(); rawTicks = new IntegerYoVariable("rawTicks", registry); rawPosition = new DoubleYoVariable("rawPosition", registry);
public void initializeRobot(Robot robot, DRCRobotModel robotModel, YoGraphicsListRegistry yoGraphicsListRegistry) { robot.setGravity(gravity); LinearGroundContactModel groundContactModel = new LinearGroundContactModel(robot, robot.getRobotsYoVariableRegistry()); robotModel.getContactPointParameters().setupGroundContactModelParameters(groundContactModel); // if ((commonTerrain.getSteppingStones() != null) && (yoGraphicsListRegistry != null)) // commonTerrain.registerSteppingStonesArtifact(yoGraphicsListRegistry); // groundContactModel.setGroundProfile(commonTerrain.getGroundProfile()); if (groundProfile3D != null) groundContactModel.setGroundProfile3D(groundProfile3D); // TODO: change this to scs.setGroundContactModel(groundContactModel); robot.setGroundContactModel(groundContactModel); }
@Test// timeout=300000 public void testChangeableOffset() { Robot robot = new Robot("testRobot"); KinematicPoint kinematicPoint = new KinematicPoint("kp_test", robot.getRobotsYoVariableRegistry()); Vector3D offset = new Vector3D(0.1, 0.2, 0.3); kinematicPoint.setOffsetJoint(offset); Vector3D offsetCopy = kinematicPoint.getOffsetCopy(); EuclidCoreTestTools.assertTuple3DEquals(offset, offsetCopy, 1e-14); }
public VirtualHoist(Joint jointToAttachHoist, Robot robot, ArrayList<Vector3D> hoistPointPositions, double updateDT) { this.updateDT = updateDT; for (int i = 0; i < hoistPointPositions.size(); i++) { Vector3D hoistPointPosition = hoistPointPositions.get(i); ExternalForcePoint externalForcePoint = new ExternalForcePoint("ef_hoist" + i, hoistPointPosition, robot.getRobotsYoVariableRegistry()); externalForcePoints.add(externalForcePoint); jointToAttachHoist.addExternalForcePoint(externalForcePoint); cableLengths.add(new YoDouble("hoistCableLength" + i, registry)); cableForceMagnitudes.add(new YoDouble("hoistCableForceMagnitude" + i, registry)); } physicalCableLength.set(0.5); // Initial gains and teepee location: hoistStiffness.set(5000.0); hoistDamping.set(1000.0); hoistUpDownSpeed.set(0.08); turnHoistOff(); hoistUp.set(false); hoistDown.set(false); q_x = (YoDouble) robot.getVariable("q_x"); q_y = (YoDouble) robot.getVariable("q_y"); q_z = (YoDouble) robot.getVariable("q_z"); teepeeLocation.set(0.0, 0.0, 1.25); }
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 void initializeRobot(Robot robot, DRCRobotModel robotModel, YoGraphicsListRegistry yoGraphicsListRegistry) { robot.setGravity(gravity); LinearGroundContactModel groundContactModel = new LinearGroundContactModel(robot, robot.getRobotsYoVariableRegistry()); robotModel.getContactPointParameters().setupGroundContactModelParameters(groundContactModel, simulateDT); if (enableGroundSlipping) groundContactModel.enableSlipping(); if (Double.isFinite(groundAlphaStick) && Double.isFinite(groundAlphaSlip)) groundContactModel.setAlphaStickSlip(groundAlphaStick, groundAlphaSlip); if (groundProfile3D != null) groundContactModel.setGroundProfile3D(groundProfile3D); robot.setGroundContactModel(groundContactModel); }
public FootstepVisualizer(GroundProfile3D groundProfile, Graphics3DObject linkGraphics, int maxNumberOfFootstepsPerSide, int maxContactPointsPerFoot, String description) { nullRobot = new Robot("FootstepVisualizerRobot"); if (groundProfile != null) { GroundContactModel gcModel = new LinearGroundContactModel(nullRobot, nullRobot.getRobotsYoVariableRegistry()); gcModel.setGroundProfile3D(groundProfile); nullRobot.setGroundContactModel(gcModel); } scs = new SimulationConstructionSet(nullRobot); if (linkGraphics != null) { scs.setGroundVisible(false); scs.addStaticLinkGraphics(linkGraphics); } printifdebug("Attaching exit listener"); scs.setDT(1, 1); yoGraphicsListRegistry = new YoGraphicsListRegistry(); bagOfSingleFootstepVisualizers = new BagOfSingleFootstepVisualizers(maxNumberOfFootstepsPerSide, maxContactPointsPerFoot, registry, yoGraphicsListRegistry); addText(scs, yoGraphicsListRegistry, description); }
public FootstepVisualizer(GroundProfile3D groundProfile, Graphics3DObject linkGraphics, int maxNumberOfFootstepsPerSide, int maxContactPointsPerFoot, String description) { nullRobot = new Robot("FootstepVisualizerRobot"); if (groundProfile != null) { GroundContactModel gcModel = new LinearGroundContactModel(nullRobot, nullRobot.getRobotsYoVariableRegistry()); gcModel.setGroundProfile3D(groundProfile); nullRobot.setGroundContactModel(gcModel); } scs = new SimulationConstructionSet(nullRobot); if (linkGraphics != null) { scs.setGroundVisible(false); scs.addStaticLinkGraphics(linkGraphics); } printifdebug("Attaching exit listener"); scs.setDT(1, 1); yoGraphicsListRegistry = new YoGraphicsListRegistry(); bagOfSingleFootstepVisualizers = new BagOfSingleFootstepVisualizers(maxNumberOfFootstepsPerSide, maxContactPointsPerFoot, registry, yoGraphicsListRegistry); addText(scs, yoGraphicsListRegistry, description); }
robot.getRobotsYoVariableRegistry().addChild(registry); scs = new SimulationConstructionSet(robot);
robot.getRobotsYoVariableRegistry().addChild(registryOne); YoDouble variableOne = new YoDouble("variableOne", registryOne);
robot.getRobotsYoVariableRegistry().addChild(registry); YoGraphicsListRegistry yoGraphicsListRegistry = footstepVisualizer.getGraphicsListRegistry();
@Test// timeout=300000 public void testSetOffsetJointWithBothVectorAndXYAndZValuesAsParameters() { Robot robot = new Robot("testRobot"); KinematicPoint kinematicPoint = new KinematicPoint("testPoint", robot.getRobotsYoVariableRegistry()); assertTrue(0.0 == kinematicPoint.getOffsetCopy().getX()); assertTrue(0.0 == kinematicPoint.getOffsetCopy().getY()); assertTrue(0.0 == kinematicPoint.getOffsetCopy().getZ()); kinematicPoint.setOffsetJoint(3.0, 4.0, 7.0); assertTrue(3.0 == kinematicPoint.getOffsetCopy().getX()); assertTrue(4.0 == kinematicPoint.getOffsetCopy().getY()); assertTrue(7.0 == kinematicPoint.getOffsetCopy().getZ()); Vector3D vectorTest = new Vector3D(9.0, 1.0, 5.0); kinematicPoint.setOffsetJoint(vectorTest); assertTrue(9.0 == kinematicPoint.getOffsetCopy().getX()); assertTrue(1.0 == kinematicPoint.getOffsetCopy().getY()); assertTrue(5.0 == kinematicPoint.getOffsetCopy().getZ()); }