private void flexUp() { submitSymmetricHumanoidArmPose(HumanoidArmPose.FLYING_PALMS_UP); pipeLine.requestNewStage(); submitSymmetricHumanoidArmPose(HumanoidArmPose.FLEX_UP); pipeLine.requestNewStage(); submitSymmetricHumanoidArmPose(HumanoidArmPose.FLYING_PALMS_UP); pipeLine.requestNewStage(); }
private void flexUp() { submitSymmetricHumanoidArmPose(HumanoidArmPose.FLYING_PALMS_UP); pipeLine.requestNewStage(); submitSymmetricHumanoidArmPose(HumanoidArmPose.FLEX_UP); pipeLine.requestNewStage(); submitSymmetricHumanoidArmPose(HumanoidArmPose.FLYING_PALMS_UP); pipeLine.requestNewStage(); }
private void flexDown() { submitSymmetricHumanoidArmPose(HumanoidArmPose.FLYING_SUPPINATE_IN); pipeLine.requestNewStage(); submitDesiredChestOrientation(true, 0.0, 0.7 * maxPitchForward, 0.0); submitSymmetricHumanoidArmPose(HumanoidArmPose.FLEX_DOWN); pipeLine.requestNewStage(); }
private void flexDown() { submitSymmetricHumanoidArmPose(HumanoidArmPose.FLYING_SUPPINATE_IN); pipeLine.requestNewStage(); submitDesiredChestOrientation(true, 0.0, 0.7 * maxPitchForward, 0.0); submitSymmetricHumanoidArmPose(HumanoidArmPose.FLEX_DOWN); pipeLine.requestNewStage(); }
private void cuteWave(RobotSide robotSide) { FrameQuaternion desiredUpperArmOrientation = new FrameQuaternion(fullRobotModel.getChest().getBodyFixedFrame()); FrameQuaternion desiredHandOnHipOrientation = new FrameQuaternion(lowerArmsFrames.get(robotSide.getOppositeSide())); FrameQuaternion desiredHandWavingOrientation = new FrameQuaternion(lowerArmsFrames.get(robotSide)); boolean mirrorOrientationForRightSide = true; submitDesiredPelvisOrientation(true, 0.0, 0.0, Math.toRadians((robotSide == RobotSide.RIGHT ? 20.0 : -20.0))); desiredUpperArmOrientation.setYawPitchRoll(0.0, Math.toRadians(45.0), Math.toRadians(-75.0)); desiredHandOnHipOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide.getOppositeSide(), desiredUpperArmOrientation, -Math.PI / 2.0, desiredHandOnHipOrientation, mirrorOrientationForRightSide); desiredUpperArmOrientation.setYawPitchRoll(0.0, -Math.PI / 2.0, 0.0); desiredHandWavingOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, -Math.PI / 2.0, desiredHandWavingOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); int numberOfWaves = 3; for (int i = 0; i < numberOfWaves; i++) { desiredHandWavingOrientation.setYawPitchRoll(Math.toRadians(0.0), Math.PI / 2.0, Math.toRadians(-30.0)); submitHandPose(robotSide, desiredUpperArmOrientation, -Math.PI / 2.0, desiredHandWavingOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); desiredHandWavingOrientation.setYawPitchRoll(Math.toRadians(0.0), Math.PI / 2.0, Math.toRadians(25.0)); submitHandPose(robotSide, desiredUpperArmOrientation, -Math.PI / 2.0, desiredHandWavingOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); } sequenceGoHome(); }
private void sequenceSquatathon() { boolean parallelize = true; submitDesiredPelvisOrientation(parallelize, 0.0, Math.toRadians(15), 0.0); submitDesiredChestOrientation(parallelize, 0.0, Math.toRadians(15.0), 0.0); submitSymmetricHumanoidArmPose(HumanoidArmPose.REACH_WAY_FORWARD); submitDesiredPelvisHeight(parallelize, minCoMHeightOffset.getDoubleValue()); pipeLine.requestNewStage(); submitChestHomeCommand(parallelize); submitPelvisHomeCommand(parallelize); submitSymmetricHumanoidArmPose(HumanoidArmPose.STAND_PREP); submitDesiredPelvisHeight(parallelize, maxCoMHeightOffset.getDoubleValue()); }
private void sequenceSquatathon() { boolean parallelize = true; submitDesiredPelvisOrientation(parallelize, 0.0, Math.toRadians(15), 0.0); submitDesiredChestOrientation(parallelize, 0.0, Math.toRadians(15.0), 0.0); submitSymmetricHumanoidArmPose(HumanoidArmPose.REACH_WAY_FORWARD); submitDesiredPelvisHeight(parallelize, minCoMHeightOffset.getDoubleValue()); pipeLine.requestNewStage(); submitChestHomeCommand(parallelize); submitPelvisHomeCommand(parallelize); submitSymmetricHumanoidArmPose(HumanoidArmPose.STAND_PREP); submitDesiredPelvisHeight(parallelize, maxCoMHeightOffset.getDoubleValue()); }
private void sequenceHandShakeShake() { RobotSide robotSide = RobotSide.RIGHT; boolean mirrorOrientationForRightSide = true; FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame()); FrameOrientation desiredHandOrientation = new FrameOrientation(lowerArmsFrames.get(robotSide)); desiredUpperArmOrientation.setYawPitchRoll(0.0, -shoulderExtensionAngle, -1.2708); int numberOfShakes = 3; for (int i = 0; i < numberOfShakes; i++) { desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, shakeHandAngle); submitHandPose(robotSide, desiredUpperArmOrientation, shakeHandAngle + shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); } }
private void sequenceHandShakeShake() { RobotSide robotSide = RobotSide.RIGHT; boolean mirrorOrientationForRightSide = true; FrameQuaternion desiredUpperArmOrientation = new FrameQuaternion(fullRobotModel.getChest().getBodyFixedFrame()); FrameQuaternion desiredHandOrientation = new FrameQuaternion(lowerArmsFrames.get(robotSide)); desiredUpperArmOrientation.setYawPitchRoll(0.0, -shoulderExtensionAngle, -1.2708); int numberOfShakes = 3; for (int i = 0; i < numberOfShakes; i++) { desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, shakeHandAngle); submitHandPose(robotSide, desiredUpperArmOrientation, shakeHandAngle + shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); } }
private void sequenceStepsShort() { pipeLine.requestNewStage(); for (int i = 0; i < numberOfCyclesToRun.getIntegerValue(); i++) { // forward submitWalkToLocation(false, 0.5, 0.0, 0.0, 0.0); //backward submitWalkToLocation(false, 0.0, 0.0, 0.0, Math.PI); // Math.PI //sideWalk submitWalkToLocation(false, 0.5, 0.0, -Math.PI / 2.0, -Math.PI / 2.0); submitWalkToLocation(false, 0.0, 0.0, 0.0, Math.PI / 2.0); //turn in place submitWalkToLocation(false, 0.0, 0.0, 0.0, 0.0); } }
private void sequenceStepsShort() { pipeLine.requestNewStage(); for (int i = 0; i < numberOfCyclesToRun.getIntegerValue(); i++) { // forward submitWalkToLocation(false, 0.5, 0.0, 0.0, 0.0); //backward submitWalkToLocation(false, 0.0, 0.0, 0.0, Math.PI); // Math.PI //sideWalk submitWalkToLocation(false, 0.5, 0.0, -Math.PI / 2.0, -Math.PI / 2.0); submitWalkToLocation(false, 0.0, 0.0, 0.0, Math.PI / 2.0); //turn in place submitWalkToLocation(false, 0.0, 0.0, 0.0, 0.0); } }
private void sequenceHandShakePrep() { RobotSide robotSide = RobotSide.RIGHT; boolean mirrorOrientationForRightSide = true; FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame()); FrameOrientation desiredHandOrientation = new FrameOrientation(lowerArmsFrames.get(robotSide)); desiredUpperArmOrientation.setYawPitchRoll(0.0, -shoulderExtensionAngle, -1.2708); desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); }
private void sequenceHandShakePrep() { RobotSide robotSide = RobotSide.RIGHT; boolean mirrorOrientationForRightSide = true; FrameQuaternion desiredUpperArmOrientation = new FrameQuaternion(fullRobotModel.getChest().getBodyFixedFrame()); FrameQuaternion desiredHandOrientation = new FrameQuaternion(lowerArmsFrames.get(robotSide)); desiredUpperArmOrientation.setYawPitchRoll(0.0, -shoulderExtensionAngle, -1.2708); desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); }
private void sequenceFlexDown() { RobotSide robotSide = RobotSide.LEFT; ReferenceFrame ankleZUpFrame = ankleZUpFrames.get(robotSide); //put the left foot forward FramePose desiredFootstepPosition = new FramePose(ankleZUpFrame); Point3d position = new Point3d(0.2, robotSide.negateIfRightSide(0.12), 0.0); Quat4d orientation = new Quat4d(0.0, 0.0, 0.0, 1.0); desiredFootstepPosition.setPose(position, orientation); desiredFootstepPosition.changeFrame(worldFrame); submitFootstepPose(true, robotSide, desiredFootstepPosition); pipeLine.requestNewStage(); flexDown(); sequenceGoHome(); sequenceSquareUp(); submitChestHomeCommand(true); submitPelvisHomeCommand(true); pipeLine.requestNewStage(); }
private void sequenceFlexDown() { RobotSide robotSide = RobotSide.LEFT; ReferenceFrame soleZUpFrame = soleZUpFrames.get(robotSide); //put the left foot forward FramePose3D desiredFootstepPosition = new FramePose3D(soleZUpFrame); Point3D position = new Point3D(0.2, robotSide.negateIfRightSide(0.12), 0.0); Quaternion orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); desiredFootstepPosition.set(position, orientation); desiredFootstepPosition.changeFrame(worldFrame); submitFootstepPose(true, robotSide, desiredFootstepPosition); pipeLine.requestNewStage(); flexDown(); sequenceGoHome(); sequenceSquareUp(); submitChestHomeCommand(true); submitPelvisHomeCommand(true); pipeLine.requestNewStage(); }
private void sequenceFlexUpFlexDown() { RobotSide robotSide = RobotSide.LEFT; ReferenceFrame soleZUpFrame = soleZUpFrames.get(robotSide); //put the left foot forward FramePose3D desiredFootstepPosition = new FramePose3D(soleZUpFrame); Point3D position = new Point3D(0.2, robotSide.negateIfRightSide(0.12), 0.0); Quaternion orientation = new Quaternion(0.0, 0.0, 0.0, 1.0); desiredFootstepPosition.set(position, orientation); desiredFootstepPosition.changeFrame(worldFrame); submitFootstepPose(true, robotSide, desiredFootstepPosition); pipeLine.requestNewStage(); flexUp(); flexDown(); sequenceGoHome(); sequenceSquareUp(); submitChestHomeCommand(true); submitPelvisHomeCommand(true); pipeLine.requestNewStage(); }
private void sequenceFlexUpFlexDown() { RobotSide robotSide = RobotSide.LEFT; ReferenceFrame ankleZUpFrame = ankleZUpFrames.get(robotSide); //put the left foot forward FramePose desiredFootstepPosition = new FramePose(ankleZUpFrame); Point3d position = new Point3d(0.2, robotSide.negateIfRightSide(0.12), 0.0); Quat4d orientation = new Quat4d(0.0, 0.0, 0.0, 1.0); desiredFootstepPosition.setPose(position, orientation); desiredFootstepPosition.changeFrame(worldFrame); submitFootstepPose(true, robotSide, desiredFootstepPosition); pipeLine.requestNewStage(); flexUp(); flexDown(); sequenceGoHome(); sequenceSquareUp(); submitChestHomeCommand(true); submitPelvisHomeCommand(true); pipeLine.requestNewStage(); }
private void setupPipeline() { publishTextToSpeack("Resetting Robot Pose"); pipeLine.clearAll(); //RESET BODY POSITIONS ******************************************* GoHomeMessage goHomeChestMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.CHEST, 2); GoHomeTask goHomeChestTask = new GoHomeTask(goHomeChestMessage, chestGoHomeBehavior); GoHomeMessage goHomepelvisMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.PELVIS, 2); GoHomeTask goHomePelvisTask = new GoHomeTask(goHomepelvisMessage, pelvisGoHomeBehavior); GoHomeMessage goHomeLeftArmMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.ARM, RobotSide.LEFT, 2); GoHomeTask goHomeLeftArmTask = new GoHomeTask(goHomeLeftArmMessage, armGoHomeLeftBehavior); GoHomeMessage goHomeRightArmMessage = HumanoidMessageTools.createGoHomeMessage(HumanoidBodyPart.ARM, RobotSide.RIGHT, 2); GoHomeTask goHomeRightArmTask = new GoHomeTask(goHomeRightArmMessage, armGoHomeRightBehavior); pipeLine.requestNewStage(); if (rightArm) pipeLine.submitSingleTaskStage(goHomeRightArmTask); if (leftArm) pipeLine.submitSingleTaskStage(goHomeLeftArmTask); if (chest) pipeLine.submitSingleTaskStage(goHomeChestTask); if (pelvis) pipeLine.submitSingleTaskStage(goHomePelvisTask); }
private void kraneKick(RobotSide robotSide) { ReferenceFrame ankleZUpFrame = ankleZUpFrames.get(robotSide.getOppositeSide()); // First Lift up the foot submitFootPosition(false, robotSide, new FramePoint3D(ankleZUpFrame, 0.1, robotSide.negateIfRightSide(0.25), 0.2)); submitSymmetricHumanoidArmPose(HumanoidArmPose.KARATE_KID_KRANE_KICK); pipeLine.requestNewStage(); // Go back to stand prep but don't put the foot on the ground yet submitSymmetricHumanoidArmPose(HumanoidArmPose.STAND_PREP); FramePose3D footPose = new FramePose3D(); footPose.setToZero(ankleZUpFrame); footPose.setPosition(0.0, robotSide.negateIfRightSide(0.25), 0.1); footPose.setOrientationYawPitchRoll(0.0, 0.0, 0.0); submitFootPose(true, robotSide, footPose); submitChestHomeCommand(true); // Put the foot back on the ground submitFootPosition(false, robotSide, new FramePoint3D(ankleZUpFrame, 0.0, robotSide.negateIfRightSide(0.25), -0.3)); }
private void kraneKick(RobotSide robotSide) { ReferenceFrame ankleZUpFrame = ankleZUpFrames.get(robotSide.getOppositeSide()); // First Lift up the foot submitFootPosition(false, robotSide, new FramePoint(ankleZUpFrame, 0.1, robotSide.negateIfRightSide(0.25), 0.2)); submitSymmetricHumanoidArmPose(HumanoidArmPose.KARATE_KID_KRANE_KICK); pipeLine.requestNewStage(); // Go back to stand prep but don't put the foot on the ground yet submitSymmetricHumanoidArmPose(HumanoidArmPose.STAND_PREP); FramePose footPose = new FramePose(); footPose.setToZero(ankleZUpFrame); footPose.setPosition(0.0, robotSide.negateIfRightSide(0.25), 0.1); footPose.setYawPitchRoll(0.0, 0.0, 0.0); submitFootPose(true, robotSide, footPose); submitChestHomeCommand(true); // Put the foot back on the ground submitFootPosition(false, robotSide, new FramePoint(ankleZUpFrame, 0.0, robotSide.negateIfRightSide(0.25), -0.3)); }