/** * Same as {@link #set(T)} but does not change the trajectory points. * @param other */ public void setPropertiesOnly(T other) { commandId = other.getCommandId(); executionMode = other.getExecutionMode(); previousCommandId = other.getPreviousCommandId(); selectionMatrix.set(other.getSelectionMatrix()); }
/** * Same as {@link #set(SE3TrajectoryControllerCommand)} but does not change the trajectory * points. * * @param other */ public void setPropertiesOnly(SE3TrajectoryControllerCommand other) { setQueueableCommandVariables(other); selectionMatrix.set(other.getSelectionMatrix()); weightMatrix.set(other.getWeightMatrix()); trajectoryFrame = other.getTrajectoryFrame(); }
public static void convertToSE3(EuclideanTrajectoryControllerCommand command, SE3TrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().clearAngularSelection(); commandToPack.getSelectionMatrix().setLinearPart(command.getSelectionMatrix()); commandToPack.getWeightMatrix().clearAngularWeights(); commandToPack.getWeightMatrix().setLinearPart(command.getWeightMatrix()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setToPositionTrajectoryIncludingFrame(command.getTrajectoryPointList()); }
public static void convertToSE3(SO3TrajectoryControllerCommand command, SE3TrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().clearLinearSelection(); commandToPack.getSelectionMatrix().setAngularPart(command.getSelectionMatrix()); commandToPack.getWeightMatrix().clearLinearWeights(); commandToPack.getWeightMatrix().setAngularPart(command.getWeightMatrix()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setToOrientationTrajectoryIncludingFrame(command.getTrajectoryPointList()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSO3() { Random random = new Random(4219L); for (int i = 0; i < 100; i++) { SO3TrajectoryControllerCommand expected = new SO3TrajectoryControllerCommand(random); SE3TrajectoryControllerCommand intermediate = new SE3TrajectoryControllerCommand(); SO3TrajectoryControllerCommand actual = new SO3TrajectoryControllerCommand(); CommandConversionTools.convertToSE3(expected, intermediate); CommandConversionTools.convertToSO3(intermediate, actual); Assert.assertEquals(0, intermediate.getSelectionMatrix().getLinearPart().getNumberOfSelectedAxes()); Assert.assertTrue(expected.epsilonEquals(actual, Double.MIN_VALUE)); } }
@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)); } } }
public static void convertToEuclidean(SE3TrajectoryControllerCommand command, EuclideanTrajectoryControllerCommand commandToPack) { commandToPack.setQueueableCommandVariables(command); commandToPack.setTrajectoryFrame(command.getTrajectoryFrame()); commandToPack.getSelectionMatrix().set(command.getSelectionMatrix().getLinearPart()); commandToPack.getWeightMatrix().set(command.getWeightMatrix().getLinearPart()); commandToPack.setUseCustomControlFrame(command.useCustomControlFrame()); commandToPack.setControlFramePose(command.getControlFramePose()); commandToPack.getTrajectoryPointList().setIncludingFrame(command.getTrajectoryPointList()); } }
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()); }