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()); }
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()); }
footTrajectoryCommand.getSE3Trajectory().setTrajectoryFrame(ReferenceFrame.getWorldFrame()); queuedControllerCommands.add(footTrajectoryCommand);