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; }
rigidBodyMessage.getControlFrameOrientationInEndEffector().set(command.getControlFramePose().getOrientation()); rigidBodyMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(command.getSelectionMatrix().getLinearPart())); rigidBodyMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(command.getSelectionMatrix().getAngularPart())); rigidBodyMessage.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(command.getWeightMatrix().getLinearPart())); rigidBodyMessage.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(command.getWeightMatrix().getAngularPart()));
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testEuclidean() { Random random = new Random(5204L); for (int i = 0; i < 100; i++) { EuclideanTrajectoryControllerCommand expected = new EuclideanTrajectoryControllerCommand(random); SE3TrajectoryControllerCommand intermediate = new SE3TrajectoryControllerCommand(); EuclideanTrajectoryControllerCommand actual = new EuclideanTrajectoryControllerCommand(); CommandConversionTools.convertToSE3(expected, intermediate); CommandConversionTools.convertToEuclidean(intermediate, actual); Assert.assertEquals(0, intermediate.getSelectionMatrix().getAngularPart().getNumberOfSelectedAxes()); Assert.assertTrue(expected.epsilonEquals(actual, Double.MIN_VALUE)); } } }
ReferenceFrame linearSelectionFrame = referenceFrames.get(random.nextInt(referenceFrames.size())); selectionMatrix6D.setSelectionFrames(angularSelectionFrame, linearSelectionFrame); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart())); assertEquals(linearZSelected, selectionMatrixLinearPart.isZSelected()); SelectionMatrix3D selectionMatrixAngularPart = selectionMatrix6D.getAngularPart(); assertEquals(angularXSelected, selectionMatrixAngularPart.isXSelected()); assertEquals(angularYSelected, selectionMatrixAngularPart.isYSelected());
public static void convertToSO3(SE3TrajectoryControllerCommand command, SO3TrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().set(command.getSelectionMatrix().getAngularPart()); commandToPack.getWeightMatrix().set(command.getWeightMatrix().getAngularPart()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setIncludingFrame(command.getTrajectoryPointList()); }
boolean linearZSelected = random.nextBoolean(); selectionMatrix6D.setLinearAxisSelection(linearXSelected, linearYSelected, linearZSelected); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart())); assertEquals(linearZSelected, selectionMatrixLinearPart.isZSelected()); SelectionMatrix3D selectionMatrixAngularPart = selectionMatrix6D.getAngularPart(); assertEquals(angularXSelected, selectionMatrixAngularPart.isXSelected()); assertEquals(angularYSelected, selectionMatrixAngularPart.isYSelected());
public KinematicsToolboxRigidBodyMessage createMessage(double timeInTrajectory, Pose3D poseToAppend) { Pose3D desiredEndEffectorPose = appendPoseToTrajectory(timeInTrajectory, poseToAppend); KinematicsToolboxRigidBodyMessage message = MessageTools.createKinematicsToolboxRigidBodyMessage(rigidBody); message.getDesiredPositionInWorld().set(desiredEndEffectorPose.getPosition()); message.getDesiredOrientationInWorld().set(desiredEndEffectorPose.getOrientation()); message.getControlFramePositionInEndEffector().set(controlFramePose.getPosition()); message.getControlFrameOrientationInEndEffector().set(controlFramePose.getOrientation()); message.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(getSelectionMatrix().getAngularPart())); message.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(getSelectionMatrix().getLinearPart())); message.getAngularWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weight)); message.getLinearWeightMatrix().set(MessageTools.createWeightMatrix3DMessage(weight)); // Sylvain's value :: 0.5 return message; }
handMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); handMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart())); kinematicsToolboxRigidBodyPublisher.publish(handMessage); chestMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(chestSelectionMatrix.getAngularPart())); chestMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(chestSelectionMatrix.getLinearPart())); kinematicsToolboxRigidBodyPublisher.publish(chestMessage); pelvisMessage.getAngularSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(pelvisSelectionMatrix.getAngularPart())); pelvisMessage.getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(pelvisSelectionMatrix.getLinearPart())); kinematicsToolboxRigidBodyPublisher.publish(pelvisMessage);
selectionMatrix6D.selectLinearZ(true); pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix() .set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); pelvisTrajectoryMessage.getSe3Trajectory().getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));
selectionMatrix6D.selectLinearZ(true); pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix() .set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); pelvisTrajectoryMessage.getSe3Trajectory().getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));
selectionMatrix6D.selectLinearZ(true); pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix() .set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getAngularPart())); pelvisTrajectoryMessage.getSe3Trajectory().getLinearSelectionMatrix().set(MessageTools.createSelectionMatrix3DMessage(selectionMatrix6D.getLinearPart()));