/** * Same as {@link #set(EuclideanTrajectoryControllerCommand)} but does not change the trajectory * points. * * @param other */ public void setPropertiesOnly(EuclideanTrajectoryControllerCommand other) { setQueueableCommandVariables(other); selectionMatrix.set(other.getSelectionMatrix()); weightMatrix.set(other.getWeightMatrix()); trajectoryFrame = other.getTrajectoryFrame(); }
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()); } }
@Override public void setFromMessage(EuclideanTrajectoryMessage message) { HumanoidMessageTools.checkIfDataFrameIdsMatch(message.getFrameInformation(), trajectoryPointList.getReferenceFrame()); List<EuclideanTrajectoryPointMessage> trajectoryPointMessages = message.getTaskspaceTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { EuclideanTrajectoryPointMessage euclideanTrajectoryPointMessage = trajectoryPointMessages.get(i); trajectoryPointList.addTrajectoryPoint(euclideanTrajectoryPointMessage.getTime(), euclideanTrajectoryPointMessage.getPosition(), euclideanTrajectoryPointMessage.getLinearVelocity()); } setQueueableCommandVariables(message.getQueueingProperties()); selectionMatrix.resetSelection(); selectionMatrix.setAxisSelection(message.getSelectionMatrix().getXSelected(), message.getSelectionMatrix().getYSelected(), message.getSelectionMatrix().getZSelected()); weightMatrix.clear(); weightMatrix.setWeights(message.getWeightMatrix().getXWeight(), message.getWeightMatrix().getYWeight(), message.getWeightMatrix().getZWeight()); useCustomControlFrame = message.getUseCustomControlFrame(); message.getControlFramePose().get(controlFramePoseInBodyFrame); }
/** * 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(); }