private int queueExceedingTrajectoryPointsIfNeeded(PelvisHeightTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = offsetHeightTrajectoryGenerator.getMaximumNumberOfWaypoints() - offsetHeightTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; PelvisHeightTrajectoryCommand commandForExcedent = commandQueue.addFirst(); numberOfQueuedCommands.increment(); commandForExcedent.clear(); commandForExcedent.setPropertiesOnly(command); for (int trajectoryPointIndex = maximumNumberOfWaypoints; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { commandForExcedent.addTrajectoryPoint(command.getTrajectoryPoint(trajectoryPointIndex)); } double timeOffsetToSubtract = command.getTrajectoryPoint(maximumNumberOfWaypoints - 1).getTime(); commandForExcedent.subtractTimeOffset(timeOffsetToSubtract); return maximumNumberOfWaypoints; }
/** * set this command to the contents of the message * * @param dataFrame the frame the data is expressed in * @param trajectoryFrame the frame the trajectory will be executed in * @param message the message that has trajectory data */ public void set(ReferenceFrame dataFrame, ReferenceFrame trajectoryFrame, PelvisHeightTrajectoryMessage message) { clear(dataFrame); setFromMessage(message); enableUserPelvisControl = message.getEnableUserPelvisControl(); enableUserPelvisControlDuringWalking = message.getEnableUserPelvisControlDuringWalking(); }
PelvisHeightTrajectoryCommand command = new PelvisHeightTrajectoryCommand(); command.set(message); controllerCommands.add(command);
public void variableChanged(YoVariable<?> v) { if (userDoPelvisHeight.getBooleanValue()) { PelvisHeightTrajectoryCommand pelvisHeightTrajectoryControllerCommand = new PelvisHeightTrajectoryCommand(); pelvisHeightTrajectoryControllerCommand.addTrajectoryPoint(userDesiredPelvisHeightTrajectoryTime.getDoubleValue(), userDesiredPelvisHeight.getDoubleValue(), 0.0); System.out.println("Submitting " + pelvisHeightTrajectoryControllerCommand); controllerCommandInputManager.submitCommand(pelvisHeightTrajectoryControllerCommand); userDoPelvisHeight.set(false); } } });
PelvisHeightTrajectoryCommand command = new PelvisHeightTrajectoryCommand(); command.setFromMessage(message); controllerCommands.add(command);
public void set(PelvisTrajectoryCommand command) { command.checkReferenceFrameMatch(ReferenceFrame.getWorldFrame()); clear(); for (int i = 0; i < command.getNumberOfTrajectoryPoints(); i++) { FrameSE3TrajectoryPoint trajectoryPoint = command.getTrajectoryPoint(i); double time = trajectoryPoint.getTime(); double position = trajectoryPoint.getPositionZ(); double velocity = trajectoryPoint.getLinearVelocityZ(); addTrajectoryPoint(time, position, velocity); } commandId = command.getCommandId(); executionMode = command.getExecutionMode(); previousCommandId = command.getPreviousCommandId(); }
public void handlePelvisTrajectoryCommand(PelvisTrajectoryCommand command) { command.changeFrame(worldFrame); tempPelvisHeightTrajectoryCommand.set(command); handlePelvisHeightTrajectoryCommand(tempPelvisHeightTrajectoryCommand); }
/** * 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 boolean isCommandValid() { return getNumberOfTrajectoryPoints() > 0; } }
@Override public void set(PelvisHeightTrajectoryCommand other) { super.set(other); setPropertiesOnly(other); }
PelvisHeightTrajectoryCommand command = new PelvisHeightTrajectoryCommand(); command.set(message); controllerCommands.add(command);