/** * Selects an axis based on {@code axisIndex} and updates update the selection state to * {@code select}. * <p> * For an {@code axisIndex} of 0, the corresponding component is {@code x}, 1 it is {@code y}, 2 * it is {@code z}. * </p> * * @param axisIndex the index of the axis to update the selection state of. * @param select whether the chosen axis is an axis of interest. * @throws IndexOutOfBoundsException if {@code axisIndex} ∉ [0, 2]. */ public void selectAxis(int axisIndex, boolean select) { switch (axisIndex) { case 0: selectXAxis(select); break; case 1: selectYAxis(select); break; case 2: selectZAxis(select); break; default: throw new IndexOutOfBoundsException(Integer.toString(axisIndex)); } }
/** * 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); }
/** * Computes the number of selections. Will be a number between 0 and 3. * @return the number of selected axes. */ public int getNumberOfSelectedAxes() { return (isXSelected() ? 1 : 0) + (isYSelected() ? 1 : 0) + (isZSelected() ? 1 : 0); }
/** * Sets this selection matrix to only select the linear axes. * <p> * The selection frames are cleared and until the selection is changed, this selection matrix is * independent from its selection frame. * </p> */ public void setToLinearSelectionOnly() { angularPart.clearSelection(); linearPart.resetSelection(); }
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); assertNull(selectionMatrix3D.getSelectionFrame()); assertTrue(selectionMatrix3D.isXSelected()); assertTrue(selectionMatrix3D.isYSelected()); assertTrue(selectionMatrix3D.isZSelected()); boolean ySelected = random.nextBoolean(); boolean zSelected = random.nextBoolean(); selectionMatrix3D.setAxisSelection(xSelected, ySelected, zSelected); assertEquals(xSelected, selectionMatrix3D.isXSelected()); assertEquals(ySelected, selectionMatrix3D.isYSelected()); assertEquals(zSelected, selectionMatrix3D.isZSelected()); selectionMatrix3D.selectXAxis(xSelected); selectionMatrix3D.selectYAxis(ySelected); selectionMatrix3D.selectZAxis(zSelected); assertEquals(xSelected, selectionMatrix3D.isXSelected()); assertEquals(ySelected, selectionMatrix3D.isYSelected()); assertEquals(zSelected, selectionMatrix3D.isZSelected()); selectionMatrix3D.resetSelection(); assertTrue(selectionMatrix3D.isXSelected()); assertTrue(selectionMatrix3D.isYSelected()); assertTrue(selectionMatrix3D.isZSelected()); selectionMatrix3D.clearSelection(); assertFalse(selectionMatrix3D.isXSelected()); assertFalse(selectionMatrix3D.isYSelected()); assertFalse(selectionMatrix3D.isZSelected());
@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); } }
public SelectionMatrix6D getSelectionMatrix() { SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); selectionMatrix.clearSelection(); if (trajectorySelectionMatrix.getLinearPart().isXSelected() || explorationSelectionMatrix.getLinearPart().isXSelected()) selectionMatrix.getLinearPart().selectXAxis(true); if (trajectorySelectionMatrix.getLinearPart().isYSelected() || explorationSelectionMatrix.getLinearPart().isYSelected()) selectionMatrix.getLinearPart().selectYAxis(true); if (trajectorySelectionMatrix.getLinearPart().isZSelected() || explorationSelectionMatrix.getLinearPart().isZSelected()) selectionMatrix.getLinearPart().selectZAxis(true); if (trajectorySelectionMatrix.getAngularPart().isXSelected() || explorationSelectionMatrix.getAngularPart().isXSelected()) selectionMatrix.getAngularPart().selectXAxis(true); if (trajectorySelectionMatrix.getAngularPart().isYSelected() || explorationSelectionMatrix.getAngularPart().isYSelected()) selectionMatrix.getAngularPart().selectYAxis(true); if (trajectorySelectionMatrix.getAngularPart().isZSelected() || explorationSelectionMatrix.getAngularPart().isZSelected()) selectionMatrix.getAngularPart().selectZAxis(true); return selectionMatrix; }
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); boolean zSelected = random.nextBoolean(); selectionMatrix3D.setAxisSelection(xSelected, ySelected, zSelected); selectionMatrix3D.setSelectionFrame(selectionFrame); selectionMatrix3D.getFullSelectionMatrixInFrame(destinationFrame, expectedSelectionMatrix); MatrixTools.removeZeroRows(expectedSelectionMatrix, 1.0e-7); selectionMatrix3D.getEfficientSelectionMatrixInFrame(destinationFrame, actualSelectionMatrix);
@Override public void onBehaviorEntered() { System.out.println("kinematics planning behavior"); if (keyFrameTimes.size() == 0) throw new RuntimeException("key frame times should be set ahead."); if (rigidBodyMessages.size() == 0) throw new RuntimeException("rigid body key frames should be set ahead."); toolboxStatePublisher.publish(MessageTools.createToolboxStateMessage(ToolboxState.WAKE_UP)); for (int i = 0; i < rigidBodyMessages.size(); i++) { KinematicsPlanningToolboxRigidBodyMessage message = rigidBodyMessages.get(i); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(defaultRigidBodyWeight)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(defaultRigidBodyWeight)); rigidBodyMessagePublisher.publish(message); } Vector3DReadOnly translationVector = fullRobotModel.getRootBody().getBodyFixedFrame().getTransformToWorldFrame().getTranslationVector(); List<Point3DReadOnly> desiredCOMPoints = new ArrayList<Point3DReadOnly>(); for (int i = 0; i < keyFrameTimes.size(); i++) desiredCOMPoints.add(new Point3D(translationVector)); KinematicsPlanningToolboxCenterOfMassMessage comMessage = HumanoidMessageTools.createKinematicsPlanningToolboxCenterOfMassMessage(keyFrameTimes, desiredCOMPoints); SelectionMatrix3D selectionMatrix = new SelectionMatrix3D(); selectionMatrix.selectZAxis(false); comMessage.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix)); comMessage.getWeights().set(MessageTools.createWeightMatrix3DMessage(defaultCOMWeight)); comMessagePublisher.publish(comMessage); System.out.println("published"); }
@Override public void setFromMessage(KinematicsToolboxCenterOfMassMessage message) { if (message == null) { clear(); return; } desiredPosition.setIncludingFrame(ReferenceFrame.getWorldFrame(), message.getDesiredPositionInWorld()); selectionMatrix.clearSelection(); selectionMatrix.clearSelection(); selectionMatrix.selectXAxis(message.getSelectionMatrix().getXSelected()); selectionMatrix.selectYAxis(message.getSelectionMatrix().getYSelected()); selectionMatrix.selectZAxis(message.getSelectionMatrix().getZSelected()); weightVector.reshape(3, 1); if (message.getWeights() == null) { weightVector.zero(); } else { weightVector.set(0, 0, message.getWeights().getXWeight()); weightVector.set(1, 0, message.getWeights().getYWeight()); weightVector.set(2, 0, message.getWeights().getZWeight()); } }
SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); FrameMatrix3D frameMatrix3D = new FrameMatrix3D(); boolean zSelected = random.nextBoolean(); selectionMatrix3D.setAxisSelection(xSelected, ySelected, zSelected); selectionMatrix3D.setSelectionFrame(selectionFrame); frameMatrix3D.transform(expectedVector); selectionMatrix3D.applySelection(actualVector);
weightMatrix.setWeightFrame(selectionMatrix.getSelectionFrame()); if (selectionMatrix.isXSelected()) if (selectionMatrix.isYSelected()) if (selectionMatrix.isZSelected()) frameResult.changeFrame(selectionMatrix.getSelectionFrame()); frameObjective.changeFrame(selectionMatrix.getSelectionFrame()); for (int i = 0; i < 3; i++) if (!selectionMatrix.isAxisSelected(i))
public static void main(String[] args) { RigidBodyTransform t1 = new RigidBodyTransform(); Vector3D rotationVector = new Vector3D(); DenseMatrix64F rotationVectorMatrix = new DenseMatrix64F(3, 1); t1.appendYawRotation(Math.PI / 8.0); t1.getRotation(rotationVector); rotationVector.get(rotationVectorMatrix); SelectionMatrix3D selectionMatrix3d = new SelectionMatrix3D(); selectionMatrix3d.selectZAxis(false); DenseMatrix64F selectionMatrix = new DenseMatrix64F(3, 3); selectionMatrix3d.getFullSelectionMatrixInFrame(null, selectionMatrix); DenseMatrix64F result = new DenseMatrix64F(3, 1); CommonOps.mult(selectionMatrix, rotationVectorMatrix, result); System.out.println(result); rotationVector.set(result); RotationMatrix rm = new RotationMatrix(rotationVector); System.out.println(rm); } }
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); } }
/** * 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); }
/** * Updates the selection state for the angular z-axis, does not change the state of the other * axes. * <p> * Note that it is preferable to also set selection frame to which this selection is referring * to. * </p> * * @param select whether the angular z-axis is an axis of interest. */ public void selectAngularZ(boolean select) { angularPart.selectZAxis(select); }
@Override public void setFromMessage(SO3TrajectoryMessage message) { HumanoidMessageTools.checkIfDataFrameIdsMatch(message.getFrameInformation(), trajectoryPointList.getReferenceFrame()); List<SO3TrajectoryPointMessage> trajectoryPointMessages = message.getTaskspaceTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { SO3TrajectoryPointMessage so3TrajectoryPointMessage = trajectoryPointMessages.get(i); trajectoryPointList.addTrajectoryPoint(so3TrajectoryPointMessage.getTime(), so3TrajectoryPointMessage.getOrientation(), so3TrajectoryPointMessage.getAngularVelocity()); } setQueueableCommandVariables(message.getQueueingProperties()); selectionMatrix.resetSelection(); selectionMatrix.setAxisSelection(message.getSelectionMatrix().getXSelected(), message.getSelectionMatrix().getYSelected(), message.getSelectionMatrix().getZSelected()); weightMatrix.clear(); weightMatrix.setWeights(message.getWeightMatrix().getXWeight(), message.getWeightMatrix().getYWeight(), message.getWeightMatrix().getZWeight()); useCustomControlFrame = message.getUseCustomControlFrame(); message.getControlFramePose().get(controlFramePoseInBodyFrame); }
/** * Convenience method to create a {@link KinematicsToolboxCenterOfMassMessage} that can be used * to hold the current center of mass of a robot given its root body. * <p> * By default the weight of the task is set to {@value #DEFAULT_CENTER_OF_MASS_WEIGHT} which it a * rather strong weight for the center of mass. As result, the solution should be very close to * the current center of mass position. * </p> * * @param rootBody the root body of the robot for which the center of mass is to be held in * place. * @param holdX whether the x-coordinate should be maintained. * @param holdY whether the y-coordinate should be maintained. * @param holdZ whether the z-coordinate should be maintained. * @return the message ready to send to the {@code KinematicsToolbosModule}. */ public static KinematicsToolboxCenterOfMassMessage holdCenterOfMassCurrentPosition(RigidBodyBasics rootBody, boolean holdX, boolean holdY, boolean holdZ) { KinematicsToolboxCenterOfMassMessage message = new KinematicsToolboxCenterOfMassMessage(); CenterOfMassCalculator calculator = new CenterOfMassCalculator(rootBody, worldFrame); calculator.reset(); message.getDesiredPositionInWorld().set(calculator.getCenterOfMass()); message.getWeights().set(MessageTools.createWeightMatrix3DMessage(DEFAULT_CENTER_OF_MASS_WEIGHT)); SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); selectionMatrix3D.setAxisSelection(holdX, holdY, holdZ); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix3D)); message.setDestination(PacketDestination.KINEMATICS_TOOLBOX_MODULE.ordinal()); return message; }
/** * Updates the selection state for the angular x-axis, does not change the state of the other * axes. * <p> * Note that it is preferable to also set selection frame to which this selection is referring * to. * </p> * * @param select whether the angular x-axis is an axis of interest. */ public void selectAngularX(boolean select) { angularPart.selectXAxis(select); }
/** * Updates the selection state for the angular y-axis, does not change the state of the other * axes. * <p> * Note that it is preferable to also set selection frame to which this selection is referring * to. * </p> * * @param select whether the angular y-axis is an axis of interest. */ public void selectAngularY(boolean select) { angularPart.selectYAxis(select); }