/** * 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()); }
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()); }
@Override public void setFromMessage(SO3TrajectoryMessage message) { HumanoidMessageTools.checkIfDataFrameIdsMatch(message.getFrameInformation(), trajectoryPointList.getReferenceFrame()); List<SO3TrajectoryPointMessage> trajectoryPointMessages = message.getTaskspaceTrajectoryPoints(); int numberOfPoints = trajectoryPointMessages.size(); for (int i = 0; i < numberOfPoints; i++) { SO3TrajectoryPointMessage so3TrajectoryPointMessage = trajectoryPointMessages.get(i); trajectoryPointList.addTrajectoryPoint(so3TrajectoryPointMessage.getTime(), so3TrajectoryPointMessage.getOrientation(), so3TrajectoryPointMessage.getAngularVelocity()); } 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); }