@Override public void set(ArmDesiredAccelerationsCommand other) { robotSide = other.robotSide; armControlMode = other.armControlMode; armDesiredJointAccelerations.reset(); for (int i = 0; i < other.getNumberOfJoints(); i++) armDesiredJointAccelerations.add(other.getArmDesiredJointAcceleration(i)); }
public static boolean checkArmDesiredAccelerationsCommand(OneDoFJoint[] joints, ArmDesiredAccelerationsCommand command) { return command.getArmControlMode() != ArmControlMode.USER_CONTROL_MODE || command.getNumberOfJoints() == joints.length; }
public void handleArmDesiredAccelerationsCommand(ArmDesiredAccelerationsCommand command) { if (!ControllerCommandValidationTools.checkArmDesiredAccelerationsCommand(controlledJoints, command)) return; switch (command.getArmControlMode()) { case IHMC_CONTROL_MODE: if (stateMachine.getCurrentStateEnum() == HandControlMode.USER_CONTROL_MODE) holdPositionInJointspace(); return; case USER_CONTROL_MODE: boolean success = userControlModeState.handleArmDesiredAccelerationsMessage(command); if (success) requestedState.set(userControlModeState.getStateEnum()); else PrintTools.warn(this, "Can't execute ArmDesiredAccelerationsCommand! " + command.getRobotSide()); return; default: throw new RuntimeException("Unknown ArmControlMode: " + command.getArmControlMode()); } }
public boolean handleArmDesiredAccelerationsMessage(ArmDesiredAccelerationsCommand command) { for (int i = 0; i < userControlledJoints.length; i++) userDesiredJointAccelerations[i].set(command.getArmDesiredJointAcceleration(i)); timeSinceLastUserMesage.set(0.0); timeOfLastUserMesage.set(yoTime.getDoubleValue()); return true; }
public void handleArmDesiredAccelerationsCommands(List<ArmDesiredAccelerationsCommand> commands) { for (int i = 0; i < commands.size(); i++) { ArmDesiredAccelerationsCommand command = commands.get(i); RobotSide robotSide = command.getRobotSide(); handControlModules.get(robotSide).handleArmDesiredAccelerationsCommand(command); } }