public void setPelvisAngularControl(boolean roll, boolean pitch, boolean yaw) { pelvisSelectionMatrix.setAngularAxisSelection(roll, pitch, yaw); pelvisSelectionMatrix.clearLinearSelection(); }
public void setChestAngularControl(boolean roll, boolean pitch, boolean yaw) { chestSelectionMatrix.setAngularAxisSelection(roll, pitch, yaw); chestSelectionMatrix.clearLinearSelection(); }
public void setDesiredPelvisHeight(FramePoint3D pointContainingDesiredHeight) { yoDesiredPelvisPosition.setMatchingFrame(pointContainingDesiredHeight); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
public void setDesiredPelvisHeight(double desiredHeightInWorld) { yoDesiredPelvisPosition.setZ(desiredHeightInWorld); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
public void holdCurrentPelvisHeight() { yoDesiredPelvisPosition.setFromReferenceFrame(fullRobotModel.getPelvis().getParentJoint().getFrameAfterJoint()); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
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()); }
SelectionMatrix6D selectionMatrix6D = new SelectionMatrix6D(); selectionMatrix6D.clearAngularSelection(); selectionMatrix6D.clearLinearSelection(); selectionMatrix6D.selectLinearZ(true); pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix()
SelectionMatrix6D selectionMatrix6D = new SelectionMatrix6D(); selectionMatrix6D.clearAngularSelection(); selectionMatrix6D.clearLinearSelection(); selectionMatrix6D.selectLinearZ(true); pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix()
SelectionMatrix6D selectionMatrix6D = new SelectionMatrix6D(); selectionMatrix6D.clearAngularSelection(); selectionMatrix6D.clearLinearSelection(); selectionMatrix6D.selectLinearZ(true); pelvisTrajectoryMessage.getSe3Trajectory().getAngularSelectionMatrix()