public HeadTrajectoryCommand() { so3Trajectory = new SO3TrajectoryControllerCommand(); }
@Override public void clear() { so3Trajectory.clear(); }
@Override public boolean epsilonEquals(PelvisOrientationTrajectoryCommand other, double epsilon) { return so3Trajectory.epsilonEquals(other.so3Trajectory, epsilon); }
@Override public void set(T other) { setIncludingFrame(other); setPropertiesOnly(other); }
@Override public void set(SO3TrajectoryControllerCommand other) { trajectoryPointList.setIncludingFrame(other.getTrajectoryPointList()); setPropertiesOnly(other); trajectoryFrame = other.getTrajectoryFrame(); useCustomControlFrame = other.useCustomControlFrame(); other.getControlFramePose(controlFramePoseInBodyFrame); }
/** * 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()); }
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()); }
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()); }
/** * Same as {@link #set(SO3TrajectoryControllerCommand)} but does not change the trajectory * points. * * @param other */ public void setPropertiesOnly(SO3TrajectoryControllerCommand other) { setQueueableCommandVariables(other); selectionMatrix.set(other.getSelectionMatrix()); weightMatrix.set(other.getWeightMatrix()); }
@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)); } }
@Override public void set(ReferenceFrameHashCodeResolver resolver, SO3TrajectoryMessage message) { FrameInformation frameInformation = message.getFrameInformation(); long trajectoryFrameId = frameInformation.getTrajectoryReferenceFrameId(); long dataFrameId = HumanoidMessageTools.getDataFrameIDConsideringDefault(frameInformation); this.trajectoryFrame = resolver.getReferenceFrameFromHashCode(trajectoryFrameId); ReferenceFrame dataFrame = resolver.getReferenceFrameFromHashCode(dataFrameId); clear(dataFrame); setFromMessage(message); ReferenceFrame selectionFrame = resolver.getReferenceFrameFromHashCode(message.getSelectionMatrix().getSelectionFrameId()); selectionMatrix.setSelectionFrame(selectionFrame); ReferenceFrame weightSelectionFrame = resolver.getReferenceFrameFromHashCode(message.getWeightMatrix().getWeightFrameId()); weightMatrix.setWeightFrame(weightSelectionFrame); }
chestTrajectoryPointList.addTrajectoryPoint(2.0, new Quaternion(-0.2, 0.0, 0.0, 1.0), new Vector3D()); chestTrajectoryPointList.addTrajectoryPoint(3.0, new Quaternion(0.0, 0.0, 0.0, 1.0), new Vector3D()); chestCommand.getSO3Trajectory().setTrajectoryPointList(chestTrajectoryPointList); chestCommand.getSO3Trajectory().setTrajectoryFrame(ReferenceFrame.getWorldFrame()); queuedControllerCommands.add(chestCommand);
public void clear(ReferenceFrame referenceFrame) { clearQueuableCommandVariables(); trajectoryPointList.clear(referenceFrame); selectionMatrix.resetSelection(); weightMatrix.clear(); }
@Override public boolean isCommandValid() { return executionModeValid() && trajectoryPointList.getNumberOfTrajectoryPoints() > 0; }
@Override public void clear() { clearQueuableCommandVariables(); trajectoryPointList.clear(); selectionMatrix.resetSelection(); weightMatrix.clear(); }
public QuadrupedBodyOrientationCommand() { so3Trajectory = new SO3TrajectoryControllerCommand(); }
@Override public void clear() { isExpressedInAbsoluteTime = true; isAnOffsetOrientation = true; so3Trajectory.clear(); }
@Override public boolean epsilonEquals(HeadTrajectoryCommand other, double epsilon) { return so3Trajectory.epsilonEquals(other.so3Trajectory, epsilon); }
public PelvisOrientationTrajectoryCommand() { so3Trajectory = new SO3TrajectoryControllerCommand(); }
@Override public void clear() { so3Trajectory.clear(); }