public void setIncludingFrame(ReferenceFrame referenceFrame, double yaw, double pitch, double roll) { this.referenceFrame = referenceFrame; this.setYawPitchRoll(yaw, pitch, roll); }
public FrameOrientation(ReferenceFrame referenceFrame, double[] yawPitchRoll) { this(referenceFrame); setYawPitchRoll(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2]); }
public void setYawPitchRoll(double[] yawPitchRoll) { setYawPitchRoll(yawPitchRoll[0], yawPitchRoll[1], yawPitchRoll[2]); }
public void set(double[] yawPitchRoll) { frameOrientation.setYawPitchRoll(yawPitchRoll); getYoValuesFromFrameOrientation(); }
public void set(double yaw, double pitch, double roll) { frameOrientation.setYawPitchRoll(yaw, pitch, roll); getYoValuesFromFrameOrientation(); }
public FrameOrientation(ReferenceFrame referenceFrame, double yaw, double pitch, double roll) { this(referenceFrame); setYawPitchRoll(yaw, pitch, roll); normalize(); }
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 cuteWave(RobotSide robotSide) { FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame()); FrameOrientation desiredHandOnHipOrientation = new FrameOrientation(lowerArmsFrames.get(robotSide.getOppositeSide())); FrameOrientation desiredHandWavingOrientation = new FrameOrientation(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 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 sequenceArmShake(RobotSide armSide) { double halfPi = Math.PI / 2.0; FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame()); boolean mirrorOrientationForRightSide = true; for (int i = 0; i < numberOfCyclesToRun.getIntegerValue(); i++) { double yaw = (i % 2) == 0 ? 1.0 : -1.0; yaw += 0.75; yaw *= Math.toRadians(10.0); desiredUpperArmOrientation.setYawPitchRoll(yaw, 0.0, -1.2); if (armSide == null) submitSymmetricHandPose(desiredUpperArmOrientation, -halfPi, null); else submitHandPose(armSide, desiredUpperArmOrientation, -halfPi, null, mirrorOrientationForRightSide); } }
private void putYoValuesIntoFrameOrientation() { tempFrameOrientation.setToZero(getReferenceFrame()); tempFrameOrientation.setYawPitchRoll(yaw.getDoubleValue(), pitch.getDoubleValue(), roll.getDoubleValue()); }
public void getFrameOrientationIncludingFrame(FrameOrientation orientationToPack) { orientationToPack.setToZero(getReferenceFrame()); orientationToPack.setYawPitchRoll(yaw.getDoubleValue(), pitch.getDoubleValue(), roll.getDoubleValue()); }
private void convertOrientationAndSetOnOutputPort() { transformFromIMUToWorld.setRotationAndZeroTranslation(orientationInputPort.getData()); estimationFrame.getTransformToDesiredFrame(transformFromEstimationFrameToIMUFrame, orientationMeasurementFrame); transformFromEstimationToWorld.multiply(transformFromIMUToWorld, transformFromEstimationFrameToIMUFrame); transformFromEstimationToWorld.getRotation(rotationFromEstimationToWorld); tempOrientationEstimationFrame.setIncludingFrame(ReferenceFrame.getWorldFrame(), rotationFromEstimationToWorld); // Introduce simulated IMU drift tempOrientationEstimationFrame.getYawPitchRoll(estimationFrameYawPitchRoll); estimationFrameYawPitchRoll[0] += imuSimulatedDriftYawAngle.getDoubleValue(); tempOrientationEstimationFrame.setYawPitchRoll(estimationFrameYawPitchRoll); orientationOutputPort.setData(tempOrientationEstimationFrame); }
desiredUpperArmOrientation.setYawPitchRoll(-1.2, -Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, -Math.PI / 2.0, null, mirrorOrientationsForRightSide); desiredUpperArmOrientation.setYawPitchRoll(1.2, Math.PI / 2.0, 0.0); // Normal Running man else desiredUpperArmOrientation.setYawPitchRoll(0.7800, 1.4300, -0.0000); // Running man for Atlas //FIXME check values for atlas submitHandPose(robotSide.getOppositeSide(), desiredUpperArmOrientation, -Math.PI / 2.0, null, mirrorOrientationsForRightSide); desiredUpperArmOrientation.setYawPitchRoll(0.0, 0.0, 1.1); submitSymmetricHandPose(desiredUpperArmOrientation, 0.0, null); // Couldn't find Solution for upper arm
desiredOrientation.setYawPitchRoll(desiredOrientation.getYaw(), footOrientation.getPitch(), footOrientation.getRoll()); desiredOrientation.setYawPitchRoll(footOrientation.getYaw(), desiredOrientation.getPitch(), desiredOrientation.getRoll()); desiredOrientation.setYawPitchRoll(desiredOrientation.getYaw(), 0.0, 0.0);
@Override public void doTransitionIntoAction() { super.doTransitionIntoAction(); // Remember the previous contact normal, in case the foot leaves the ground and rotates holdPositionNormalContactVector.setIncludingFrame(fullyConstrainedNormalContactVector); holdPositionNormalContactVector.changeFrame(worldFrame); momentumBasedController.setPlaneContactStateNormalContactVector(contactableFoot, holdPositionNormalContactVector); desiredSolePosition.setToZero(soleFrame); desiredSolePosition.changeFrame(worldFrame); desiredOrientation.setToZero(soleFrame); desiredOrientation.changeFrame(worldFrame); if (doHoldFootFlatOrientation.getBooleanValue()) { desiredOrientation.setYawPitchRoll(desiredOrientation.getYaw(), 0.0, 0.0); } desiredHoldOrientation.set(desiredOrientation); desiredLinearVelocity.setToZero(worldFrame); desiredAngularVelocity.setToZero(worldFrame); desiredLinearAcceleration.setToZero(worldFrame); desiredAngularAcceleration.setToZero(worldFrame); }
tempYawPitchRoll[0] -= firstDriftYaw.getDoubleValue(); tempYawPitchRoll[0] = AngleTools.trimAngleMinusPiToPi(tempYawPitchRoll[0]); firstFrameOrientation.setYawPitchRoll(tempYawPitchRoll); secondFrameOrientation.setYawPitchRoll(tempYawPitchRoll);
sequenceMovingChestAndPelvisOnly(); desiredUpperArmOrientation.setYawPitchRoll(0.0, 0.0, 0.0); submitHandPose(RobotSide.LEFT, desiredUpperArmOrientation, 0.0, null, true); submitHumanoidArmPose(RobotSide.RIGHT, HumanoidArmPose.ARM_STRAIGHT_DOWN); desiredUpperArmOrientation.setYawPitchRoll(0.0, 0.0, 0.0); submitHandPose(RobotSide.RIGHT, desiredUpperArmOrientation, 0.0, null, true); sequenceChestRotations(0.55);
yawPitchRoll[0] = robotSide.negateIfRightSide(yawPitchRoll[0]); yawPitchRoll[2] = robotSide.negateIfRightSide(yawPitchRoll[2]); temporaryDesiredUpperArmOrientation.setYawPitchRoll(yawPitchRoll);
@Override public void doSpecificAction() { feedbackControlCommandList.clear(); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getYawPitchRoll(tempYawPitchRoll); twistCalculator.getRelativeTwist(footTwist, rootBody, contactableFoot.getRigidBody()); footTwist.changeFrame(contactableFoot.getFrameAfterParentJoint()); toeOffCurrentPitchAngle.set(tempYawPitchRoll[1]); toeOffCurrentPitchVelocity.set(footTwist.getAngularPartY()); desiredPosition.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredPosition.changeFrame(worldFrame); computeDesiredsForFreeMotion(); desiredOrientation.setYawPitchRoll(desiredYawToHold, toeOffDesiredPitchAngle.getDoubleValue(), desiredRollToHold); desiredLinearVelocity.setToZero(worldFrame); desiredAngularVelocity.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchVelocity.getDoubleValue(), 0.0); desiredAngularVelocity.changeFrame(worldFrame); desiredLinearAcceleration.setToZero(worldFrame); desiredAngularAcceleration.setIncludingFrame(contactableFoot.getFrameAfterParentJoint(), 0.0, toeOffDesiredPitchAcceleration.getDoubleValue(), 0.0); desiredAngularAcceleration.changeFrame(worldFrame); orientationFeedbackControlCommand.set(desiredOrientation, desiredAngularVelocity, desiredAngularAcceleration); pointFeedbackControlCommand.set(desiredContactPointPosition, desiredLinearVelocity, desiredLinearAcceleration); setupSingleContactPoint(); feedbackControlCommandList.addCommand(orientationFeedbackControlCommand); feedbackControlCommandList.addCommand(pointFeedbackControlCommand); }