public void submitAll(T executorKey, List<Task> tasks) { for (Task task : tasks) { submitTaskForPallelPipesStage(executorKey, task); } }
public void submitAll(T executorKey, List<Task> tasks) { for (Task task : tasks) { submitTaskForPallelPipesStage(executorKey, task); } }
private void submitFootstepPose(boolean parallelize, RobotSide robotSide, FramePose3D desiredFootstepPose) { FramePose3D footPose = new FramePose3D(desiredFootstepPose); FootstepTask footstepTask = new FootstepTask(fullRobotModel, robotSide, footstepListBehavior, footPose); if (parallelize) pipeLine.submitTaskForPallelPipesStage(footstepListBehavior, footstepTask); else pipeLine.submitSingleTaskStage(footstepTask); }
private void submitFootstepPose(boolean parallelize, RobotSide robotSide, FramePose desiredFootstepPose) { FramePose footPose = new FramePose(desiredFootstepPose); FootstepTask footstepTask = new FootstepTask(fullRobotModel, robotSide, footstepListBehavior, footPose); if (parallelize) pipeLine.submitTaskForPallelPipesStage(footstepListBehavior, footstepTask); else pipeLine.submitSingleTaskStage(footstepTask); }
private void submitChestHomeCommand(boolean parallelize) { GoHomeMessage goHomeMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.CHEST, trajectoryTime.getDoubleValue()); GoHomeTask goHomeTask = new GoHomeTask(goHomeMessage, chestGoHomeBehavior); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(chestGoHomeBehavior, goHomeTask); pipeLine.submitTaskForPallelPipesStage(chestGoHomeBehavior, createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(goHomeTask); pipeLine.submitSingleTaskStage(createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitPelvisHomeCommand(boolean parallelize) { GoHomeMessage goHomeMessage = new GoHomeMessage(BodyPart.PELVIS, trajectoryTime.getDoubleValue()); GoHomeTask goHomeTask = new GoHomeTask(goHomeMessage, pelvisGoHomeBehavior); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisGoHomeBehavior, goHomeTask); pipeLine.submitTaskForPallelPipesStage(pelvisGoHomeBehavior,createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(goHomeTask); pipeLine.submitSingleTaskStage(createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitPelvisHomeCommand(boolean parallelize) { GoHomeMessage goHomeMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.PELVIS, trajectoryTime.getDoubleValue()); GoHomeTask goHomeTask = new GoHomeTask(goHomeMessage, pelvisGoHomeBehavior); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisGoHomeBehavior, goHomeTask); pipeLine.submitTaskForPallelPipesStage(pelvisGoHomeBehavior, createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(goHomeTask); pipeLine.submitSingleTaskStage(createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitDesiredChestOrientation(boolean parallelize, double yaw, double pitch, double roll) { FrameQuaternion desiredChestOrientation = new FrameQuaternion(pelvisZUpFrame, yaw, pitch, roll); ChestOrientationTask chestOrientationTask = new ChestOrientationTask(desiredChestOrientation, chestTrajectoryBehavior, trajectoryTime.getDoubleValue(), pelvisZUpFrame); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(chestTrajectoryBehavior, chestOrientationTask); pipeLine.submitTaskForPallelPipesStage(chestTrajectoryBehavior, createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(chestOrientationTask); pipeLine.submitSingleTaskStage(createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitChestHomeCommand(boolean parallelize) { GoHomeMessage goHomeMessage = new GoHomeMessage(BodyPart.CHEST, trajectoryTime.getDoubleValue()); GoHomeTask goHomeTask = new GoHomeTask(goHomeMessage, chestGoHomeBehavior); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(chestGoHomeBehavior, goHomeTask); pipeLine.submitTaskForPallelPipesStage(chestGoHomeBehavior,createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(goHomeTask); pipeLine.submitSingleTaskStage(createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitDesiredChestOrientation(boolean parallelize, double yaw, double pitch, double roll) { FrameOrientation desiredChestOrientation = new FrameOrientation(pelvisZUpFrame, yaw, pitch, roll); desiredChestOrientation.changeFrame(worldFrame); ChestOrientationTask chestOrientationTask = new ChestOrientationTask(desiredChestOrientation, chestTrajectoryBehavior, trajectoryTime.getDoubleValue()); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(chestTrajectoryBehavior, chestOrientationTask); pipeLine.submitTaskForPallelPipesStage(chestTrajectoryBehavior,createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(chestOrientationTask); pipeLine.submitSingleTaskStage(createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitDesiredPelvisOrientation(boolean parallelize, double yaw, double pitch, double roll, double trajectoryTime, double sleepTime) { FrameQuaternion desiredPelvisOrientation = new FrameQuaternion(findFixedFrameForPelvisOrientation(), yaw, pitch, roll); desiredPelvisOrientation.changeFrame(worldFrame); Quaternion desiredQuaternion = new Quaternion(desiredPelvisOrientation); PelvisOrientationTrajectoryMessage message = HumanoidMessageTools.createPelvisOrientationTrajectoryMessage(trajectoryTime, desiredQuaternion); PelvisOrientationTrajectoryTask task = new PelvisOrientationTrajectoryTask(message, pelvisOrientationTrajectoryBehavior); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisOrientationTrajectoryBehavior, task); pipeLine.submitTaskForPallelPipesStage(pelvisOrientationTrajectoryBehavior, createSleepTask(sleepTime)); } else { pipeLine.submitSingleTaskStage(task); pipeLine.submitSingleTaskStage(createSleepTask(sleepTime)); } }
private void submitArmGoHomeCommand(boolean parallelize) { for (RobotSide robotSide : RobotSide.values()) { GoHomeMessage goHomeMessage = new GoHomeMessage(BodyPart.ARM, robotSide, trajectoryTime.getDoubleValue()); GoHomeBehavior armGoHomeBehavior = armGoHomeBehaviors.get(robotSide); if (parallelize) pipeLine.submitTaskForPallelPipesStage(armGoHomeBehavior, new GoHomeTask(goHomeMessage, armGoHomeBehavior)); else pipeLine.submitSingleTaskStage(new GoHomeTask(goHomeMessage, armGoHomeBehavior)); } }
private void submitTurnInPlaceAngle(boolean parallelize, double angleToTurn) { if (parallelize) pipeLine.submitTaskForPallelPipesStage(turnInPlaceBehavior, new TurnInPlaceTask(angleToTurn, turnInPlaceBehavior, transferTime.getDoubleValue(), swingTime.getDoubleValue())); else pipeLine.submitSingleTaskStage(new TurnInPlaceTask(angleToTurn, turnInPlaceBehavior, transferTime.getDoubleValue(), swingTime.getDoubleValue())); }
private void submitDesiredPelvisOrientation(boolean parallelize, double yaw, double pitch, double roll, double trajectoryTime, double sleepTime) { FrameOrientation desiredPelvisOrientation = new FrameOrientation(findFixedFrameForPelvisOrientation(), yaw, pitch, roll); desiredPelvisOrientation.changeFrame(worldFrame); Quat4d desiredQuaternion = new Quat4d(desiredPelvisOrientation.getQuaternion()); PelvisOrientationTrajectoryMessage message = new PelvisOrientationTrajectoryMessage(trajectoryTime, desiredQuaternion); PelvisOrientationTrajectoryTask task = new PelvisOrientationTrajectoryTask(message, pelvisOrientationTrajectoryBehavior); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisOrientationTrajectoryBehavior, task); pipeLine.submitTaskForPallelPipesStage(pelvisOrientationTrajectoryBehavior,createSleepTask(sleepTime)); } else { pipeLine.submitSingleTaskStage(task); pipeLine.submitSingleTaskStage(createSleepTask(sleepTime)); } }
private void submitTurnInPlaceAngle(boolean parallelize, double angleToTurn) { if (parallelize) pipeLine.submitTaskForPallelPipesStage(turnInPlaceBehavior, new TurnInPlaceTask(angleToTurn, turnInPlaceBehavior, transferTime.getDoubleValue(), swingTime.getDoubleValue())); else pipeLine .submitSingleTaskStage(new TurnInPlaceTask(angleToTurn, turnInPlaceBehavior, transferTime.getDoubleValue(), swingTime.getDoubleValue())); }
private void submitArmGoHomeCommand(boolean parallelize) { for (RobotSide robotSide : RobotSide.values()) { GoHomeMessage goHomeMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.ARM, robotSide, trajectoryTime.getDoubleValue()); GoHomeBehavior armGoHomeBehavior = armGoHomeBehaviors.get(robotSide); if (parallelize) pipeLine.submitTaskForPallelPipesStage(armGoHomeBehavior, new GoHomeTask(goHomeMessage, armGoHomeBehavior)); else pipeLine.submitSingleTaskStage(new GoHomeTask(goHomeMessage, armGoHomeBehavior)); } }
private void submitDesiredPelvisHeight(boolean parallelize, double offsetHeight) { FloatingInverseDynamicsJointReferenceFrame frameAfterRootJoint = fullRobotModel.getRootJoint().getFrameAfterJoint(); FramePoint desiredPelvisPosition = new FramePoint(frameAfterRootJoint); desiredPelvisPosition.setZ(offsetHeight); desiredPelvisPosition.changeFrame(worldFrame); PelvisHeightTrajectoryTask comHeightTask = new PelvisHeightTrajectoryTask(desiredPelvisPosition.getZ(), pelvisHeightTrajectoryBehavior, trajectoryTime.getDoubleValue()); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior, comHeightTask); pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior,createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(comHeightTask); pipeLine.submitSingleTaskStage(createSleepTask( sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitWalkToLocation(boolean parallelize, double x, double y, double robotYaw, double angleRelativeToPath, double percentOfMaxFootstepLength) { FramePose2d targetPoseInWorld = new FramePose2d(); targetPoseInWorld.setPoseIncludingFrame(midFeetZUpFrame, x, y, robotYaw); targetPoseInWorld.changeFrame(worldFrame); WalkToLocationTask walkToLocationTask = new WalkToLocationTask(targetPoseInWorld, walkToLocationBehavior, angleRelativeToPath, footstepLength.getDoubleValue() * percentOfMaxFootstepLength, swingTime.getDoubleValue(), transferTime.getDoubleValue()); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(walkToLocationBehavior, walkToLocationTask); } else { pipeLine.submitSingleTaskStage(walkToLocationTask); } }
private void submitDesiredPelvisHeight(boolean parallelize, double offsetHeight) { ReferenceFrame frameAfterRootJoint = fullRobotModel.getRootJoint().getFrameAfterJoint(); FramePoint3D desiredPelvisPosition = new FramePoint3D(frameAfterRootJoint); desiredPelvisPosition.setZ(offsetHeight); desiredPelvisPosition.changeFrame(worldFrame); PelvisHeightTrajectoryTask comHeightTask = new PelvisHeightTrajectoryTask(desiredPelvisPosition.getZ(), pelvisHeightTrajectoryBehavior, trajectoryTime.getDoubleValue()); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior, comHeightTask); pipeLine.submitTaskForPallelPipesStage(pelvisHeightTrajectoryBehavior, createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } else { pipeLine.submitSingleTaskStage(comHeightTask); pipeLine.submitSingleTaskStage(createSleepTask(sleepTimeBetweenPoses.getDoubleValue())); } }
private void submitWalkToLocation(boolean parallelize, double x, double y, double robotYaw, double angleRelativeToPath, double percentOfMaxFootstepLength) { FramePose2D targetPoseInWorld = new FramePose2D(); targetPoseInWorld.setIncludingFrame(midFeetZUpFrame, x, y, robotYaw); targetPoseInWorld.changeFrame(worldFrame); WalkToLocationTask walkToLocationTask = new WalkToLocationTask(targetPoseInWorld, walkToLocationBehavior, angleRelativeToPath, footstepLength.getDoubleValue() * percentOfMaxFootstepLength, swingTime.getDoubleValue(), transferTime.getDoubleValue()); if (parallelize) { pipeLine.submitTaskForPallelPipesStage(walkToLocationBehavior, walkToLocationTask); } else { pipeLine.submitSingleTaskStage(walkToLocationTask); } }