public void handleHandTrajectoryCommands(List<HandTrajectoryCommand> commands) { for (int i = 0; i < commands.size(); i++) { HandTrajectoryCommand command = commands.get(i); RobotSide robotSide = command.getRobotSide(); handControlModules.get(robotSide).handleHandTrajectoryCommand(command); } }
public void handleHandTrajectoryCommand(HandTrajectoryCommand command) { boolean success; switch (command.getExecutionMode()) { case OVERRIDE: boolean initializeToCurrent = stateMachine.getCurrentStateEnum() != HandControlMode.TASKSPACE; success = taskspaceControlState.handleHandTrajectoryCommand(command, handControlFrame, initializeToCurrent); if (success) requestedState.set(taskspaceControlState.getStateEnum()); else PrintTools.warn(this, "Can't execute HandTrajectoryCommand! " + command.getRobotSide()); return; case QUEUE: success = taskspaceControlState.queueHandTrajectoryCommand(command); if (!success) { holdPositionInJointspace(); PrintTools.warn(this, "Can't execute HandTrajectoryCommand! " + command.getRobotSide()); } return; default: PrintTools.warn(this, "Unknown " + ExecutionMode.class.getSimpleName() + " value: " + command.getExecutionMode() + ". Command ignored."); return; } }
RobotSide robotSide = command.getRobotSide(); FramePose desiredPose = new FramePose(); command.getLastTrajectoryPoint().getPoseIncludingFrame(desiredPose);
RobotSide robotSide = command.getRobotSide(); FramePose desiredPose = new FramePose(); command.getLastTrajectoryPoint().getPoseIncludingFrame(desiredPose);