/** * 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); }
/** * Updates the selection state for the linear 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 linear z-axis is an axis of interest. */ public void selectLinearZ(boolean select) { linearPart.selectZAxis(select); }
/** * 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)); } }
@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"); }
@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 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 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.selectZAxis(zSelected); assertEquals(xSelected, selectionMatrix3D.isXSelected()); assertEquals(ySelected, selectionMatrix3D.isYSelected());
ChestTrajectoryMessage chestTrajectoryMessage = HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredOrientation, pelvisZUpFrame, pelvisZUpFrame); SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); selectionMatrix3D.selectZAxis(random.nextBoolean()); selectionMatrix3D.selectYAxis(random.nextBoolean()); selectionMatrix3D.selectXAxis(random.nextBoolean());
selectionMatrix.selectZAxis(false); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix)); message.getWeights().set(MessageTools.createWeightMatrix3DMessage(1.0));
selectionMatrix.selectZAxis(false); comMessage.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix)); comMessage.getWeights().set(MessageTools.createWeightMatrix3DMessage(1.0));
ChestTrajectoryMessage chestTrajectoryMessage = HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredOrientation, pelvisZUpFrame, pelvisZUpFrame); SelectionMatrix3D selectionMatrix3D = new SelectionMatrix3D(); selectionMatrix3D.selectZAxis(false); selectionMatrix3D.selectYAxis(false); selectionMatrix3D.selectXAxis(false);
selectionMatrix.selectZAxis(false); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix)); message.getWeights().set(MessageTools.createWeightMatrix3DMessage(1.0));
@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()); } }
selectionMatrix.selectZAxis(false); message.getSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix)); message.getWeights().set(MessageTools.createWeightMatrix3DMessage(1.0));