FootstepNodeDataListMessage packLowestCostPlanMessage() { if (lowestCostPlan.isEmpty()) return null; FootstepNodeDataListMessage nodeDataListMessage = new FootstepNodeDataListMessage(); Object<FootstepNodeDataMessage> nodeDataList = nodeDataListMessage.getNodeData(); nodeDataList.clear(); for (int i = 0; i < lowestCostPlan.size(); i++) { FootstepNode node = lowestCostPlan.get(i); FootstepNodeDataMessage nodeDataMessage = nodeDataList.add(); setNodeDataMessage(nodeDataMessage, node, -1); } nodeDataListMessage.setIsFootstepGraph(false); lowestCostPlan.clear(); return nodeDataListMessage; }
FootstepPlannerOccupancyMapMessage packOccupancyMapMessage() { if (occupiedCells.isEmpty()) return null; Object<FootstepPlannerCellMessage> occupiedCellsForMessage = occupancyMapMessage.getOccupiedCells(); occupiedCellsForMessage.clear(); List<FootstepPlannerCellMessage> occupiedCells = this.occupiedCells.getCopyForReading(); for (int i = 0; i < occupiedCells.size(); i++) occupiedCellsForMessage.add().set(occupiedCells.get(i)); hasOccupiedCells.set(false); return occupancyMapMessage; }
private void computeBackwardFootstepList() { backwardFootstepList.getFootstepDataList().clear(); ArrayList<FootstepDataMessage> footstepDataList = new ArrayList<>(); List<FootstepDataMessage> dataList = forwardFootstepList.getFootstepDataList(); for (int i = 0; i < dataList.size(); i++) { FootstepDataMessage step = dataList.get(i); footstepDataList.add(step); } footstepDataList.remove(footstepDataList.size() - 1); Collections.reverse(footstepDataList); RobotSide initialStanceSide = initialSwingSide.getEnumValue().getOppositeSide(); FootstepDataMessage initialStanceFoot = constructFootstepDataMessage(soleFrames.get(initialStanceSide), 0.0, 0.0, initialStanceSide); footstepDataList.add(initialStanceFoot); MessageTools.copyData(footstepDataList, backwardFootstepList.getFootstepDataList()); backwardFootstepList.setDefaultSwingDuration(swingTime.getDoubleValue()); backwardFootstepList.setDefaultTransferDuration(transferTime.getDoubleValue()); }
footstep.getPredictedContactPoints2d().clear(); return Footstep.FootstepType.FULL_FOOTSTEP;
logProperties.getCameras().clear();
solution.getRobotConfigurations().clear(); solution.setSolutionQuality(0.0);
footstepData.getPredictedContactPoints2d().clear();
ReferenceFrame midFootZUpGroundFrame = humanoidReferenceFrames.getMidFootZUpGroundFrame(); double time = walkingControllerParameters.getDefaultInitialTransferTime(); messageToPack.getFootstepDataList().clear(); messageToPack.setDefaultSwingDuration(swingDuration); messageToPack.setDefaultTransferDuration(transferDuration);
ReferenceFrame midFootZUpGroundFrame = humanoidReferenceFrames.getMidFootZUpGroundFrame(); double time = walkingControllerParameters.getDefaultInitialTransferTime(); messageToPack.getFootstepDataList().clear(); messageToPack.setDefaultSwingDuration(swingDuration); messageToPack.setDefaultTransferDuration(transferDuration);
public static void packFootSupportPolygon(RobotSide robotSide, ConvexPolygon2DReadOnly footPolygon, CapturabilityBasedStatus capturabilityBasedStatus) { int numberOfVertices = footPolygon.getNumberOfVertices(); if (numberOfVertices > CAPTURABILITY_BASED_STATUS_MAXIMUM_NUMBER_OF_VERTICES) { numberOfVertices = CAPTURABILITY_BASED_STATUS_MAXIMUM_NUMBER_OF_VERTICES; } if (robotSide == RobotSide.LEFT) { capturabilityBasedStatus.getLeftFootSupportPolygon2d().clear(); } else { capturabilityBasedStatus.getRightFootSupportPolygon2d().clear(); } for (int i = 0; i < numberOfVertices; i++) { if (robotSide == RobotSide.LEFT) { capturabilityBasedStatus.getLeftFootSupportPolygon2d().add().set(footPolygon.getVertex(i), 0.0); } else { capturabilityBasedStatus.getRightFootSupportPolygon2d().add().set(footPolygon.getVertex(i), 0.0); } } }
@ContinuousIntegrationTest(estimatedDuration = 45.0) @Test(timeout = 70000) public void testForwardStepWithPause() throws SimulationExceededMaximumTimeException { setupTest(); FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); footstepDataListMessage.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.LEFT, new Point3D(stepLength, stepWidth / 2.0, 0.0), new FrameQuaternion())); drcSimulationTestHelper.publishToController(footstepDataListMessage); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(3.0)); footstepDataListMessage.getFootstepDataList().clear(); footstepDataListMessage.getFootstepDataList().add().set(HumanoidMessageTools.createFootstepDataMessage(RobotSide.RIGHT, new Point3D(stepLength, -stepWidth / 2.0, 0.0), new FrameQuaternion())); drcSimulationTestHelper.publishToController(footstepDataListMessage); assertTrue(drcSimulationTestHelper.simulateAndBlockAndCatchExceptions(3.0)); Point3D center = new Point3D(stepLength, 0.0, 0.9); Vector3D plusMinusVector = new Vector3D(0.1, 0.1, 0.15); BoundingBox3D boundingBox = BoundingBox3D.createUsingCenterAndPlusMinusVector(center, plusMinusVector); drcSimulationTestHelper.assertRobotsRootJointIsInBoundingBox(boundingBox); }
private void computeForwardFootstepList() { forwardFootstepList.getFootstepDataList().clear(); RobotSide swingSide = initialSwingSide.getEnumValue(); for (int i = 0; i < numberOfStepsToTake.getIntegerValue(); i++) { FootstepDataMessage footstepDataMessage = constructFootstepDataMessage(midFootZUpFrame, footstepLength.getDoubleValue() * (i + 1), 0.5 * swingSide.negateIfRightSide(footstepWidth.getDoubleValue()), swingSide); forwardFootstepList.getFootstepDataList().add().set(footstepDataMessage); swingSide = swingSide.getOppositeSide(); } forwardFootstepList.setDefaultSwingDuration(swingTime.getDoubleValue()); forwardFootstepList.setDefaultTransferDuration(transferTime.getDoubleValue()); }
FootstepNodeDataListMessage packLowestCostPlanMessage() { if (nodeData.isEmpty()) return null; Object<FootstepNodeDataMessage> nodeDataListForMessage = nodeDataListMessage.getNodeData(); nodeDataListForMessage.clear(); List<FootstepNodeDataMessage> nodeData = this.nodeData.getCopyForReading(); for (int i = 0; i < nodeData.size(); i++) nodeDataListForMessage.add().set(nodeData.get(i)); nodeDataListMessage.setIsFootstepGraph(false); hasNodeData.set(false); return nodeDataListMessage; }
private void addFootsteps(FootstepDataListMessage footsteps) { footsteps.getFootstepDataList().clear(); int idx = 0; for (ContactType contactType : contactSequence) { footsteps.getFootstepDataList().add().set(createFootstep(contactType, idx++)); } footsteps.getFootstepDataList().add().set(createFootstep(ContactType.FULL, idx++)); footsteps.getFootstepDataList().add().set(createFootstep(ContactType.FULL, idx++)); }
public static void packPredictedContactPoints(List<? extends Point2DReadOnly> contactPoints, FootstepDataMessage message) { if (contactPoints == null) return; message.getPredictedContactPoints2d().clear(); for (int i = 0; i < contactPoints.size(); i++) { message.getPredictedContactPoints2d().add().set(contactPoints.get(i), 0.0); } }