/** * Returns true if any of the linear axis are enabled * * @return */ public boolean isLinearPartActive() { return isLinearXSelected() || isLinearYSelected() || isLinearZSelected(); }
return isLinearXSelected(); case 4: return isLinearYSelected(); case 5: return isLinearZSelected();
selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); if (!selectionMatrix.isLinearXSelected() && !selectionMatrix.isLinearYSelected() && !selectionMatrix.isLinearZSelected()) continue; // The position part is not dictated by trajectory, let's not visualize.
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; }
assertTrue(selectionMatrix6D.isAngularPartActive()); assertTrue(selectionMatrix6D.isLinearXSelected()); assertTrue(selectionMatrix6D.isLinearYSelected()); assertTrue(selectionMatrix6D.isLinearZSelected()); assertTrue(selectionMatrix6D.isLinearPartActive()); assertEquals(xAngularSelected || yAngularSelected || zAngularSelected, selectionMatrix6D.isAngularPartActive()); assertEquals(xLinearSelected, selectionMatrix6D.isLinearXSelected()); assertEquals(yLinearSelected, selectionMatrix6D.isLinearYSelected()); assertEquals(zLinearSelected, selectionMatrix6D.isLinearZSelected()); assertEquals(xLinearSelected || yLinearSelected || zLinearSelected, selectionMatrix6D.isLinearPartActive()); assertEquals(zAngularSelected, selectionMatrix6D.isAngularZSelected()); assertEquals(xLinearSelected, selectionMatrix6D.isLinearXSelected()); assertEquals(yLinearSelected, selectionMatrix6D.isLinearYSelected()); assertEquals(zLinearSelected, selectionMatrix6D.isLinearZSelected()); assertTrue(selectionMatrix6D.isAngularPartActive()); assertTrue(selectionMatrix6D.isLinearXSelected()); assertTrue(selectionMatrix6D.isLinearYSelected()); assertTrue(selectionMatrix6D.isLinearZSelected()); assertTrue(selectionMatrix6D.isLinearPartActive()); assertFalse(selectionMatrix6D.isAngularPartActive()); assertTrue(selectionMatrix6D.isLinearXSelected()); assertTrue(selectionMatrix6D.isLinearYSelected()); assertTrue(selectionMatrix6D.isLinearZSelected()); assertTrue(selectionMatrix6D.isLinearPartActive());
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); }
selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); if (!selectionMatrix.isLinearXSelected() || !selectionMatrix.isLinearYSelected() || !selectionMatrix.isLinearZSelected()) positionErrorQ.zero(); if (!selectionMatrix.isAngularXSelected() || !selectionMatrix.isAngularYSelected() || !selectionMatrix.isAngularZSelected())
selectionMatrix.setAngularAxisSelection(angularSelection.getXSelected(), angularSelection.getYSelected(), angularSelection.getZSelected()); selectionMatrix.setLinearAxisSelection(linearSelection.getXSelected(), linearSelection.getYSelected(), linearSelection.getZSelected()); if (!selectionMatrix.isLinearXSelected() || !selectionMatrix.isLinearYSelected() || !selectionMatrix.isLinearZSelected()) positionErrorQ.zero(); if (!selectionMatrix.isAngularXSelected() || !selectionMatrix.isAngularYSelected() || !selectionMatrix.isAngularZSelected())