command.set(message); controllerCommands.add(command);
command.set(message); 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); }
@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); }