/** * Sets the selection frame for the angular and linear parts separately. * * @param angularSelectionFrame the new frame to which the angular axes selection is referring * to. * @param linearSelectionFrame the new frame to which the linear axes selection is referring to. */ public void setSelectionFrames(ReferenceFrame angularSelectionFrame, ReferenceFrame linearSelectionFrame) { angularPart.setSelectionFrame(angularSelectionFrame); linearPart.setSelectionFrame(linearSelectionFrame); }
/** * Sets the selection frame to {@code null}. * <p> * When the selection frame is {@code null}, the conversion into a 3-by-3 selection matrix will * be done regardless of the destination frame. * </p> */ public void clearSelectionFrame() { setSelectionFrame(null); }
/** * Creates and initializes a new selection matrix. * * @param selectionFrame the new frame to which the axes selection is referring to. * @param xSelected whether the x-axis is an axis of interest. * @param ySelected whether the y-axis is an axis of interest. * @param zSelected whether the z-axis is an axis of interest. * @see #setSelectionFrame(ReferenceFrame) * @see #setAxisSelection(boolean, boolean, boolean) */ public SelectionMatrix3D(ReferenceFrame selectionFrame, boolean xSelected, boolean ySelected, boolean zSelected) { setSelectionFrame(selectionFrame); setAxisSelection(xSelected, ySelected, zSelected); }
selectionMatrix3D.setSelectionFrame(selectionFrame);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 300000) public void testSelectionForSimpleObjective() throws Exception { ReferenceFrame taskFrame = ReferenceFrame.getWorldFrame(); SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); ReferenceFrame selectionFrame; Vector3D objective; Vector3D result; for (int i = 0; i < 1000; i++) { RigidBodyTransform selectionFrameTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); selectionFrame = ReferenceFrame.constructFrameWithUnchangingTransformFromParent("SelectionFrame", ReferenceFrame.getWorldFrame(), selectionFrameTransform); objective = EuclidCoreRandomTools.nextVector3D(random); selectionMatrix.setSelectionFrame(selectionFrame); selectionMatrix.selectXAxis(random.nextBoolean()); selectionMatrix.selectYAxis(random.nextBoolean()); selectionMatrix.selectZAxis(random.nextBoolean()); result = runTest(taskFrame, selectionMatrix, objective); } if (visualize) { visualize(taskFrame, selectionFrame, objective, result); } }
selectionMatrix3D.setSelectionFrame(selectionFrame);
selectionMatrix3D.setSelectionFrame(selectionFrame);
angularPart3D.setSelectionFrame(selectionFrame); linearPart3D.setAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); linearPart3D.setSelectionFrame(selectionFrame);
@Override public void set(ReferenceFrameHashCodeResolver resolver, EuclideanTrajectoryMessage message) { FrameInformation frameInformation = message.getFrameInformation(); long trajectoryFrameId = frameInformation.getTrajectoryReferenceFrameId(); long dataFrameId = HumanoidMessageTools.getDataFrameIDConsideringDefault(frameInformation); this.trajectoryFrame = resolver.getReferenceFrameFromHashCode(trajectoryFrameId); ReferenceFrame dataFrame = resolver.getReferenceFrameFromHashCode(dataFrameId); clear(dataFrame); setFromMessage(message); ReferenceFrame linearSelectionFrame = resolver.getReferenceFrameFromHashCode(message.getSelectionMatrix().getSelectionFrameId()); selectionMatrix.setSelectionFrame(linearSelectionFrame); ReferenceFrame linearWeightFrame = resolver.getReferenceFrameFromHashCode(message.getWeightMatrix().getWeightFrameId()); weightMatrix.setWeightFrame(linearWeightFrame); }
@Override public void set(ReferenceFrameHashCodeResolver resolver, SO3TrajectoryMessage message) { FrameInformation frameInformation = message.getFrameInformation(); long trajectoryFrameId = frameInformation.getTrajectoryReferenceFrameId(); long dataFrameId = HumanoidMessageTools.getDataFrameIDConsideringDefault(frameInformation); this.trajectoryFrame = resolver.getReferenceFrameFromHashCode(trajectoryFrameId); ReferenceFrame dataFrame = resolver.getReferenceFrameFromHashCode(dataFrameId); clear(dataFrame); setFromMessage(message); ReferenceFrame selectionFrame = resolver.getReferenceFrameFromHashCode(message.getSelectionMatrix().getSelectionFrameId()); selectionMatrix.setSelectionFrame(selectionFrame); ReferenceFrame weightSelectionFrame = resolver.getReferenceFrameFromHashCode(message.getWeightMatrix().getWeightFrameId()); weightMatrix.setWeightFrame(weightSelectionFrame); }
selectionMatrix3D.setSelectionFrame(randomFrame); assertTrue(randomFrame == selectionMatrix3D.getSelectionFrame()); assertNull(selectionMatrix3D.getSelectionFrame()); selectionMatrix3D.setSelectionFrame(randomFrame); selectionMatrix3D.clearSelection(); assertNull(selectionMatrix3D.getSelectionFrame()); selectionMatrix3D.setSelectionFrame(randomFrame); selectionMatrix3D.resetSelection(); assertNull(selectionMatrix3D.getSelectionFrame());
selectionMatrix3D.selectYAxis(random.nextBoolean()); selectionMatrix3D.selectXAxis(random.nextBoolean()); selectionMatrix3D.setSelectionFrame(pelvisZUpFrame); chestTrajectoryMessage.getSo3Trajectory().getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix3D)); drcSimulationTestHelper.publishToController(chestTrajectoryMessage);
public void set(KinematicsPlanningToolboxCenterOfMassMessage message, Map<Long, RigidBodyBasics> rigidBodyNamedBasedHashMap, ReferenceFrameHashCodeResolver referenceFrameResolver) { for (int i = 0; i < message.getWayPointTimes().size(); i++) { waypointTimes.add(message.getWayPointTimes().get(i)); waypoints.add().set(message.getDesiredWayPointPositionsInWorld().get(i)); } selectionMatrix.clearSelectionFrame(); SelectionMatrix3DMessage linearSelection = message.getSelectionMatrix(); selectionMatrix.setAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); weightMatrix.clear(); WeightMatrix3DMessage linearWeight = message.getWeights(); weightMatrix.setWeights(linearWeight.getXWeight(), linearWeight.getYWeight(), linearWeight.getZWeight()); if (referenceFrameResolver != null) { ReferenceFrame linearSelectionFrame = referenceFrameResolver.getReferenceFrameFromHashCode(linearSelection.getSelectionFrameId()); selectionMatrix.setSelectionFrame(linearSelectionFrame); ReferenceFrame linearWeightFrame = referenceFrameResolver.getReferenceFrameFromHashCode(linearWeight.getWeightFrameId()); weightMatrix.setWeightFrame(linearWeightFrame); } }
selectionMatrix3D.selectYAxis(false); selectionMatrix3D.selectXAxis(false); selectionMatrix3D.setSelectionFrame(pelvisZUpFrame); chestTrajectoryMessage.getSo3Trajectory().getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix3D)); drcSimulationTestHelper.publishToController(chestTrajectoryMessage);