public void handleArmTrajectoryCommands(List<ArmTrajectoryCommand> commands) { for (int i = 0; i < commands.size(); i++) { ArmTrajectoryCommand command = commands.get(i); RobotSide robotSide = command.getRobotSide(); handControlModules.get(robotSide).handleArmTrajectoryCommand(command); } }
@Override public void set(ArmTrajectoryCommand other) { clear(other.getRobotSide()); jointspaceTrajectory.set(other.getJointspaceTrajectory()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetRobotSide() { ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(); assertNull(armTrajectoryCommand.getRobotSide()); armTrajectoryCommand.setRobotSide(RobotSide.LEFT); assertEquals(RobotSide.LEFT, armTrajectoryCommand.getRobotSide()); }
public void handleArmTrajectoryCommand(ArmTrajectoryCommand command) { boolean success; switch (command.getExecutionMode()) { case OVERRIDE: boolean initializeToCurrent = stateMachine.getCurrentStateEnum() != HandControlMode.JOINTSPACE; success = jointspaceControlState.handleArmTrajectoryCommand(command, initializeToCurrent); if (success) requestedState.set(jointspaceControlState.getStateEnum()); else PrintTools.warn(this, "Can't execute ArmTrajectoryCommand! " + command.getRobotSide()); return; case QUEUE: success = jointspaceControlState.queueArmTrajectoryCommand(command); if (!success) { holdPositionInJointspace(); PrintTools.warn(this, "Can't execute ArmTrajectoryCommand! " + command.getRobotSide()); } return; default: PrintTools.warn(this, "Unknown " + ExecutionMode.class.getSimpleName() + " value: " + command.getExecutionMode() + ". Command ignored."); return; } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testClearRobotSide() { ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(); assertNull(armTrajectoryCommand.getRobotSide()); armTrajectoryCommand.clear(RobotSide.LEFT); assertEquals(RobotSide.LEFT, armTrajectoryCommand.getRobotSide()); assertEquals(Packet.VALID_MESSAGE_DEFAULT_ID, armTrajectoryCommand.getJointspaceTrajectory().getCommandId()); assertEquals(0.0, armTrajectoryCommand.getExecutionDelayTime(), 1e-9); assertEquals(ExecutionMode.OVERRIDE, armTrajectoryCommand.getJointspaceTrajectory().getExecutionMode()); assertEquals(0.0, armTrajectoryCommand.getExecutionTime(), 1e-9); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints()); assertEquals(Packet.INVALID_MESSAGE_ID, armTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId()); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getTrajectoryPointLists().size()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000, expected = IndexOutOfBoundsException.class) public void testClear() { ArmTrajectoryCommand armTrajectoryCommand = new ArmTrajectoryCommand(); armTrajectoryCommand.clear(); assertNull(armTrajectoryCommand.getRobotSide()); assertEquals(Packet.VALID_MESSAGE_DEFAULT_ID, armTrajectoryCommand.getJointspaceTrajectory().getCommandId()); assertEquals(0.0, armTrajectoryCommand.getExecutionDelayTime(), 1e-9); assertEquals(ExecutionMode.OVERRIDE, armTrajectoryCommand.getJointspaceTrajectory().getExecutionMode()); assertEquals(0.0, armTrajectoryCommand.getExecutionTime(), 1e-9); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints()); assertEquals(Packet.INVALID_MESSAGE_ID, armTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId()); assertEquals(0, armTrajectoryCommand.getJointspaceTrajectory().getTrajectoryPointLists().size()); armTrajectoryCommand.getJointspaceTrajectory().getJointTrajectoryPoint(0, 0); }
assertEquals(armTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints(), otherArmTrajectoryCommand.getJointspaceTrajectory().getNumberOfJoints()); assertEquals(armTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId(), otherArmTrajectoryCommand.getJointspaceTrajectory().getPreviousCommandId()); assertEquals(armTrajectoryCommand.getRobotSide(), otherArmTrajectoryCommand.getRobotSide());