/** * Returns true if any of the angular axis are enabled * * @return */ public boolean isAngularPartActive() { return isAngularXSelected() || isAngularYSelected() || isAngularZSelected(); }
return isAngularXSelected(); case 1: return isAngularYSelected();
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.getAngularSelectionFrame()); assertNull(selectionMatrix6D.getLinearSelectionFrame()); assertTrue(selectionMatrix6D.isAngularXSelected()); assertTrue(selectionMatrix6D.isAngularYSelected()); assertTrue(selectionMatrix6D.isAngularZSelected()); selectionMatrix6D.setAngularAxisSelection(xAngularSelected, yAngularSelected, zAngularSelected); selectionMatrix6D.setLinearAxisSelection(xLinearSelected, yLinearSelected, zLinearSelected); assertEquals(xAngularSelected, selectionMatrix6D.isAngularXSelected()); assertEquals(yAngularSelected, selectionMatrix6D.isAngularYSelected()); assertEquals(zAngularSelected, selectionMatrix6D.isAngularZSelected()); selectionMatrix6D.selectLinearY(yLinearSelected); selectionMatrix6D.selectLinearZ(zLinearSelected); assertEquals(xAngularSelected, selectionMatrix6D.isAngularXSelected()); assertEquals(yAngularSelected, selectionMatrix6D.isAngularYSelected()); assertEquals(zAngularSelected, selectionMatrix6D.isAngularZSelected()); assertTrue(selectionMatrix6D.isAngularXSelected()); assertTrue(selectionMatrix6D.isAngularYSelected()); assertTrue(selectionMatrix6D.isAngularZSelected()); assertFalse(selectionMatrix6D.isAngularXSelected()); assertFalse(selectionMatrix6D.isAngularYSelected()); assertFalse(selectionMatrix6D.isAngularZSelected()); assertTrue(selectionMatrix6D.isAngularXSelected()); assertTrue(selectionMatrix6D.isAngularYSelected()); assertTrue(selectionMatrix6D.isAngularZSelected());
public static void compareWrenches(WrenchReadOnly inputWrench, Wrench outputWrench, SelectionMatrix6D selectionMatrix) { inputWrench.getBodyFrame().checkReferenceFrameMatch(outputWrench.getBodyFrame()); outputWrench.changeFrame(inputWrench.getReferenceFrame()); inputWrench.getReferenceFrame().checkReferenceFrameMatch(outputWrench.getReferenceFrame()); DenseMatrix64F inputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); DenseMatrix64F outputWrenchMatrix = new DenseMatrix64F(Wrench.SIZE, 1); inputWrench.get(inputWrenchMatrix); outputWrench.get(outputWrenchMatrix); double epsilon = 1e-4; if (selectionMatrix.isAngularXSelected()) Assert.assertEquals(inputWrenchMatrix.get(0, 0), outputWrenchMatrix.get(0, 0), epsilon); if (selectionMatrix.isAngularYSelected()) Assert.assertEquals(inputWrenchMatrix.get(1, 0), outputWrenchMatrix.get(1, 0), epsilon); if (selectionMatrix.isAngularZSelected()) Assert.assertEquals(inputWrenchMatrix.get(2, 0), outputWrenchMatrix.get(2, 0), epsilon); if (selectionMatrix.isLinearXSelected()) Assert.assertEquals(inputWrenchMatrix.get(3, 0), outputWrenchMatrix.get(3, 0), epsilon); if (selectionMatrix.isLinearYSelected()) Assert.assertEquals(inputWrenchMatrix.get(4, 0), outputWrenchMatrix.get(4, 0), epsilon); if (selectionMatrix.isLinearZSelected()) Assert.assertEquals(inputWrenchMatrix.get(5, 0), outputWrenchMatrix.get(5, 0), epsilon); }
if (!selectionMatrix.isLinearXSelected() || !selectionMatrix.isLinearYSelected() || !selectionMatrix.isLinearZSelected()) positionErrorQ.zero(); if (!selectionMatrix.isAngularXSelected() || !selectionMatrix.isAngularYSelected() || !selectionMatrix.isAngularZSelected()) rotationErrorQ.zero();
if (!selectionMatrix.isLinearXSelected() || !selectionMatrix.isLinearYSelected() || !selectionMatrix.isLinearZSelected()) positionErrorQ.zero(); if (!selectionMatrix.isAngularXSelected() || !selectionMatrix.isAngularYSelected() || !selectionMatrix.isAngularZSelected()) rotationErrorQ.zero();