public SoleTrajectoryCommand() { positionTrajectory = new EuclideanTrajectoryControllerCommand(); }
@Override public boolean epsilonEquals(PelvisHeightTrajectoryCommand other, double epsilon) { if (enableUserPelvisControl != other.enableUserPelvisControl) return false; if (enableUserPelvisControlDuringWalking != other.enableUserPelvisControlDuringWalking) return false; return euclideanTrajectory.epsilonEquals(other.euclideanTrajectory, epsilon); }
public EuclideanTrajectoryControllerCommand(ReferenceFrame dataFrame, ReferenceFrame trajectoryFrame) { clear(dataFrame); this.trajectoryFrame = trajectoryFrame; }
public void set(ReferenceFrame dataFrame, ReferenceFrame trajectoryFrame, EuclideanTrajectoryMessage message) { this.trajectoryFrame = trajectoryFrame; clear(dataFrame); setFromMessage(message); }
@Override public void set(EuclideanTrajectoryControllerCommand other) { trajectoryPointList.setIncludingFrame(other.getTrajectoryPointList()); setPropertiesOnly(other); trajectoryFrame = other.getTrajectoryFrame(); useCustomControlFrame = other.useCustomControlFrame(); other.getControlFramePose(controlFramePoseInBodyFrame); }
command1.getAngularMomentumTrajectory().addTrajectoryPoint(0.0, new Point3D(0.0, 0.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); command1.getAngularMomentumTrajectory().addTrajectoryPoint(1.0, new Point3D(1.0, 1.0, 0.0), new Vector3D(1.5, 1.5, 0.0)); command1.getAngularMomentumTrajectory().setCommandId(0L); command2.getAngularMomentumTrajectory().setExecutionMode(ExecutionMode.QUEUE); command2.getAngularMomentumTrajectory().addTrajectoryPoint(1.0, new Point3D(2.0, 2.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); command2.getAngularMomentumTrajectory().setPreviousCommandId(0L); handler.handleMomentumTrajectory(command2);
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()); }
@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)); } } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testClearPointsInPast() { YoVariableRegistry registry = new YoVariableRegistry("TestRegistry"); YoDouble time = new YoDouble("time", registry); TestTrajectoryHandler trajectoryHandler = new TestTrajectoryHandler("TestHandler", time, registry); EuclideanTrajectoryControllerCommand trajectory = new EuclideanTrajectoryControllerCommand(); trajectory.addTrajectoryPoint(1.0, new Point3D(), new Vector3D()); trajectory.addTrajectoryPoint(2.0, new Point3D(), new Vector3D()); trajectory.addTrajectoryPoint(3.0, new Point3D(), new Vector3D()); trajectory.addTrajectoryPoint(4.0, new Point3D(), new Vector3D()); trajectory.addTrajectoryPoint(5.0, new Point3D(), new Vector3D()); trajectoryHandler.handleTrajectory(trajectory); double epsilon = 1e-7; Assert.assertTrue(trajectoryHandler.isWithinInterval(1.0 + epsilon)); Assert.assertTrue(trajectoryHandler.isWithinInterval(2.5)); Assert.assertTrue(trajectoryHandler.isWithinInterval(5.0 - epsilon)); time.set(4.5); trajectoryHandler.clearPointsInPast(); Assert.assertTrue(!trajectoryHandler.isEmpty()); Assert.assertTrue(trajectoryHandler.isWithinInterval(4.0 + epsilon)); Assert.assertTrue(trajectoryHandler.isWithinInterval(5.0 - epsilon)); }
/** * add a trajectory point to this message */ public void addTrajectoryPoint(double time, double zHeight, double velocity) { tempPoint.setToZero(); tempPoint.setZ(zHeight); tempVector.setToZero(); tempVector.setZ(velocity); euclideanTrajectory.addTrajectoryPoint(time, tempPoint, tempVector); }
/** * Set this command to the contents of the z height of the pelvis trajectory command Copies the z * points, velocities, and the linear z weight and frame * * @param command the other command */ public void set(PelvisTrajectoryCommand command) { clear(command.getSE3Trajectory().getDataFrame()); euclideanTrajectory.setQueueableCommandVariables(command.getSE3Trajectory()); WeightMatrix3D weightMatrix = command.getSE3Trajectory().getWeightMatrix().getLinearPart(); double zAxisWeight = weightMatrix.getZAxisWeight(); WeightMatrix3D currentWeightMatrix = euclideanTrajectory.getWeightMatrix(); currentWeightMatrix.setZAxisWeight(zAxisWeight); currentWeightMatrix.setWeightFrame(weightMatrix.getWeightFrame()); for (int i = 0; i < command.getSE3Trajectory().getNumberOfTrajectoryPoints(); i++) { FrameSE3TrajectoryPoint trajectoryPoint = command.getSE3Trajectory().getTrajectoryPoint(i); double time = trajectoryPoint.getTime(); double position = trajectoryPoint.getPositionZ(); double velocity = trajectoryPoint.getLinearVelocityZ(); tempPoint.setToZero(); tempPoint.setZ(position); tempVector.setToZero(); tempVector.setZ(velocity); euclideanTrajectory.addTrajectoryPoint(time, tempPoint, tempVector); } enableUserPelvisControl = true; enableUserPelvisControlDuringWalking = command.isEnableUserPelvisControlDuringWalking(); }
@Override public double getExecutionDelayTime() { return angularMomentumTrajectory.getExecutionDelayTime(); }
@Override public double getExecutionTime() { return angularMomentumTrajectory.getExecutionTime(); } }
@Override public void clear() { clearQueuableCommandVariables(); trajectoryPointList.clear(); selectionMatrix.resetSelection(); weightMatrix.clear(); }
@Override public boolean isCommandValid() { return executionModeValid() && trajectoryPointList.getNumberOfTrajectoryPoints() > 0; }
command1.getEuclideanTrajectory().addTrajectoryPoint(0.0, new Point3D(0.0, 0.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); command1.getEuclideanTrajectory().addTrajectoryPoint(1.0, new Point3D(1.0, 1.0, 0.0), new Vector3D(1.5, 1.5, 0.0)); command1.getEuclideanTrajectory().addTrajectoryPoint(2.0, new Point3D(2.0, 2.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); command1.getEuclideanTrajectory().setCommandId(0L); handler.handleComTrajectory(command1); command2.getEuclideanTrajectory().addTrajectoryPoint(1.0, new Point3D(0.0, 0.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); command2.getEuclideanTrajectory().addTrajectoryPoint(2.0, new Point3D(1.0, 1.0, 0.0), new Vector3D(1.5, 1.5, 0.0)); command2.getEuclideanTrajectory().addTrajectoryPoint(3.0, new Point3D(2.0, 2.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); command1.getEuclideanTrajectory().setPreviousCommandId(0L); command2.getEuclideanTrajectory().setExecutionMode(ExecutionMode.QUEUE);
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testOutOfBounds() { YoVariableRegistry registry = new YoVariableRegistry("TestRegistry"); YoDouble yoTime = new YoDouble("time", registry); MomentumTrajectoryCommand command = new MomentumTrajectoryCommand(); // assume method x(t) = -1/2 * t^3 + 3/2 * t^2 for x and y and 0.0 for z command.getAngularMomentumTrajectory().addTrajectoryPoint(0.0, new Point3D(0.0, 0.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); command.getAngularMomentumTrajectory().addTrajectoryPoint(1.0, new Point3D(1.0, 1.0, 0.0), new Vector3D(1.5, 1.5, 0.0)); command.getAngularMomentumTrajectory().addTrajectoryPoint(2.0, new Point3D(2.0, 2.0, 0.0), new Vector3D(0.0, 0.0, 0.0)); // offset time by 5.0s MomentumTrajectoryHandler handler = new MomentumTrajectoryHandler(yoTime, registry); handler.handleMomentumTrajectory(command); int samples = 3; RecyclingArrayList<SimpleEuclideanTrajectoryPoint> momentumTrajectory = new RecyclingArrayList<>(SimpleEuclideanTrajectoryPoint.class); // try some examples that are invalid handler.getAngularMomentumTrajectory(-1.0, 0.0, samples, momentumTrajectory); assertEquals(0, momentumTrajectory.size()); handler.getAngularMomentumTrajectory(2.0, 3.0, samples, momentumTrajectory); assertEquals(0, momentumTrajectory.size()); // try valid example handler.getAngularMomentumTrajectory(1.0e-10, 1.0, samples, momentumTrajectory); assertEquals(3, momentumTrajectory.size()); }
@Override public void set(ReferenceFrameHashCodeResolver resolver, EuclideanTrajectoryMessage 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 linearSelectionFrame = resolver.getReferenceFrameFromHashCode(message.getSelectionMatrix().getSelectionFrameId()); selectionMatrix.setSelectionFrame(linearSelectionFrame); ReferenceFrame linearWeightFrame = resolver.getReferenceFrameFromHashCode(message.getWeightMatrix().getWeightFrameId()); weightMatrix.setWeightFrame(linearWeightFrame); }
@Override public double getExecutionDelayTime() { return euclideanTrajectory.getExecutionDelayTime(); }
@Override public double getExecutionTime() { return positionTrajectory.getExecutionTime(); } }