assertEquals(linearSelectionFrame, selectionMatrix6D.getLinearSelectionFrame()); assertTrue(selectionMatrix6D.equals(selectionMatrixFromCommand));
assertNull(selectionMatrix6D.getLinearSelectionFrame()); assertTrue(selectionMatrix6D.isAngularXSelected()); assertTrue(selectionMatrix6D.isAngularYSelected()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); ReferenceFrame randomFrame = EuclidFrameRandomTools.nextReferenceFrame("blop" + i, random, ReferenceFrame.getWorldFrame()); selectionMatrix6D.setSelectionFrame(randomFrame); assertTrue(randomFrame == selectionMatrix6D.getAngularSelectionFrame()); assertTrue(randomFrame == selectionMatrix6D.getLinearSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); assertTrue(randomFrame == selectionMatrix6D.getLinearSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame());
public static WaypointBasedTrajectoryMessage createWaypointBasedTrajectoryMessage(RigidBodyBasics endEffector, double[] waypointTimes, Pose3D[] waypoints, SelectionMatrix6D selectionMatrix) { WaypointBasedTrajectoryMessage message = new WaypointBasedTrajectoryMessage(); message.setEndEffectorHashCode(endEffector.hashCode()); if (waypointTimes.length != waypoints.length) throw new RuntimeException("Inconsistent array lengths."); message.getWaypointTimes().reset(); message.getWaypointTimes().add(waypointTimes); MessageTools.copyData(waypoints, message.getWaypoints()); if (selectionMatrix != null) { message.getAngularSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getAngularSelectionFrame())); message.getAngularSelectionMatrix().setXSelected(selectionMatrix.isAngularXSelected()); message.getAngularSelectionMatrix().setYSelected(selectionMatrix.isAngularYSelected()); message.getAngularSelectionMatrix().setZSelected(selectionMatrix.isAngularZSelected()); message.getLinearSelectionMatrix().setSelectionFrameId(MessageTools.toFrameId(selectionMatrix.getLinearSelectionFrame())); message.getLinearSelectionMatrix().setXSelected(selectionMatrix.isLinearXSelected()); message.getLinearSelectionMatrix().setYSelected(selectionMatrix.isLinearYSelected()); message.getLinearSelectionMatrix().setZSelected(selectionMatrix.isLinearZSelected()); } return message; }
assertNull(selectionMatrix6D.getLinearSelectionFrame()); SelectionMatrix3D selectionMatrixLinearPart = selectionMatrix6D.getLinearPart(); assertEquals(linearXSelected, selectionMatrixLinearPart.isXSelected());