- Add the Codota plugin to your IDE and get smart completions
private void myMethod () {SimpleDateFormat s =
String pattern;new SimpleDateFormat(pattern)
- Smart code suggestions by Tabnine
}
public WholeBodyTrajectoryCommand() { for (RobotSide robotSide : RobotSide.values) { HandTrajectoryCommand handTrajectoryControllerCommand = new HandTrajectoryCommand(); ArmTrajectoryCommand armTrajectoryControllerCommand = new ArmTrajectoryCommand(); FootTrajectoryCommand footTrajectoryControllerCommand = new FootTrajectoryCommand(); handTrajectoryControllerCommands.put(robotSide, handTrajectoryControllerCommand); armTrajectoryControllerCommands.put(robotSide, armTrajectoryControllerCommand); footTrajectoryControllerCommands.put(robotSide, footTrajectoryControllerCommand); allControllerCommands.add(handTrajectoryControllerCommand); allControllerCommands.add(armTrajectoryControllerCommand); allControllerCommands.add(footTrajectoryControllerCommand); } allControllerCommands.add(chestTrajectoryControllerCommand); allControllerCommands.add(pelvisTrajectoryControllerCommand); }
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; } }
private int queueExceedingTrajectoryPointsIfNeeded(HandTrajectoryCommand command) { int numberOfTrajectoryPoints = command.getNumberOfTrajectoryPoints(); int maximumNumberOfWaypoints = positionTrajectoryGenerator.getMaximumNumberOfWaypoints() - positionTrajectoryGenerator.getCurrentNumberOfWaypoints(); if (numberOfTrajectoryPoints <= maximumNumberOfWaypoints) return numberOfTrajectoryPoints; HandTrajectoryCommand commandForExcedent = commandQueue.addFirst(); numberOfQueuedCommands.increment(); commandForExcedent.clear(); commandForExcedent.setPropertiesOnly(command); for (int trajectoryPointIndex = maximumNumberOfWaypoints; trajectoryPointIndex < numberOfTrajectoryPoints; trajectoryPointIndex++) { commandForExcedent.addTrajectoryPoint(command.getTrajectoryPoint(trajectoryPointIndex)); } double timeOffsetToSubtract = command.getTrajectoryPoint(maximumNumberOfWaypoints - 1).getTime(); commandForExcedent.subtractTimeOffset(timeOffsetToSubtract); return maximumNumberOfWaypoints; }
HandTrajectoryCommand command = new HandTrajectoryCommand(); command.set(message); controllerCommands.add(command);
RobotSide robotSide = command.getRobotSide(); FramePose desiredPose = new FramePose(); command.getLastTrajectoryPoint().getPoseIncludingFrame(desiredPose); desiredHandPoses.put(robotSide, desiredPose); DenseMatrix64F selectionMatrix = new DenseMatrix64F(command.getSelectionMatrix()); handSelectionMatrices.put(robotSide, selectionMatrix);
HandTrajectoryCommand handTrajectoryControllerCommand = new HandTrajectoryCommand(referenceFrame, userHandPoseSide.getEnumValue(), userHandPoseBaseForControl.getEnumValue()); trajectoryPoint.setAngularVelocity(new Vector3d()); handTrajectoryControllerCommand.addTrajectoryPoint(trajectoryPoint);
message.getSe3Trajectory().getFrameInformation().setTrajectoryReferenceFrameId(MessageTools.toFrameId(chestFrame)); message.getSe3Trajectory().getFrameInformation().setDataReferenceFrameId(MessageTools.toFrameId(worldFrame)); HandTrajectoryCommand command = new HandTrajectoryCommand(); command.getSE3Trajectory().set(worldFrame, chestFrame, message.getSe3Trajectory()); controllerCommands.add(command);
@Override public void set(WholeBodyTrajectoryCommand other) { for (RobotSide robotSide : RobotSide.values) { handTrajectoryControllerCommands.get(robotSide).set(other.handTrajectoryControllerCommands.get(robotSide)); armTrajectoryControllerCommands.get(robotSide).set(other.armTrajectoryControllerCommands.get(robotSide)); footTrajectoryControllerCommands.get(robotSide).set(other.footTrajectoryControllerCommands.get(robotSide)); } chestTrajectoryControllerCommand.set(other.chestTrajectoryControllerCommand); pelvisTrajectoryControllerCommand.set(other.pelvisTrajectoryControllerCommand); }
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); } }
@Override public void set(HandTrajectoryCommand other) { super.set(other); setPropertiesOnly(other); }
@Override public void clear() { for (RobotSide robotSide : RobotSide.values) { handTrajectoryControllerCommands.get(robotSide).clear(); armTrajectoryControllerCommands.get(robotSide).clear(); footTrajectoryControllerCommands.get(robotSide).clear(); } chestTrajectoryControllerCommand.clear(); pelvisTrajectoryControllerCommand.clear(); }
HandTrajectoryCommand command = new HandTrajectoryCommand(); command.set(message); controllerCommands.add(command);
RobotSide robotSide = command.getRobotSide(); FramePose desiredPose = new FramePose(); command.getLastTrajectoryPoint().getPoseIncludingFrame(desiredPose); desiredHandPoses.put(robotSide, desiredPose); DenseMatrix64F selectionMatrix = new DenseMatrix64F(command.getSelectionMatrix()); handSelectionMatrices.put(robotSide, selectionMatrix);
@Override public void set(WholeBodyTrajectoryMessage message) { clear(); for (RobotSide robotside : RobotSide.values) { HandTrajectoryMessage handTrajectoryMessage = message.getHandTrajectoryMessage(robotside); if (handTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) handTrajectoryControllerCommands.get(robotside).set(handTrajectoryMessage); ArmTrajectoryMessage armTrajectoryMessage = message.getArmTrajectoryMessage(robotside); if (armTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) armTrajectoryControllerCommands.get(robotside).set(armTrajectoryMessage); FootTrajectoryMessage footTrajectoryMessage = message.getFootTrajectoryMessage(robotside); if (footTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) footTrajectoryControllerCommands.get(robotside).set(footTrajectoryMessage); } ChestTrajectoryMessage chestTrajectoryMessage = message.getChestTrajectoryMessage(); if (chestTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) chestTrajectoryControllerCommand.set(chestTrajectoryMessage); PelvisTrajectoryMessage pelvisTrajectoryMessage = message.getPelvisTrajectoryMessage(); if (pelvisTrajectoryMessage.getUniqueId() != Packet.INVALID_MESSAGE_ID) pelvisTrajectoryControllerCommand.set(pelvisTrajectoryMessage); }