public void getFrameOrientation2dIncludingFrame(FrameOrientation2d frameOrientation2dToPack) { frameOrientation2dToPack.setIncludingFrame(referenceFrame, getYaw()); }
public void moveToAverageInSupportFoot(RobotSide supportSide) { desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); ReferenceFrame otherAnkleZUpFrame = ankleZUpFrames.get(supportSide.getOppositeSide()); ReferenceFrame supportAnkleZUpFrame = ankleZUpFrames.get(supportSide); tempOrientation.setToZero(otherAnkleZUpFrame); tempOrientation.changeFrame(worldFrame); double yawOtherFoot = tempOrientation.getYaw(); tempOrientation.setToZero(supportAnkleZUpFrame); tempOrientation.changeFrame(worldFrame); double yawSupportFoot = tempOrientation.getYaw(); double finalDesiredPelvisYawAngle = AngleTools.computeAngleAverage(yawOtherFoot, yawSupportFoot); finalPelvisOrientation.set(finalDesiredPelvisYawAngle, 0.0, 0.0); initialize(supportAnkleZUpFrame); }
public void setWithUpcomingFootstep(Footstep upcomingFootstep) { RobotSide upcomingFootstepSide = upcomingFootstep.getRobotSide(); desiredPelvisOrientation.getFrameOrientationIncludingFrame(tempOrientation); initialPelvisOrientation.set(tempOrientation); upcomingFootstep.getOrientationIncludingFrame(upcomingFootstepOrientation); upcomingFootstepOrientation.changeFrame(worldFrame); tempOrientation.setToZero(ankleZUpFrames.get(upcomingFootstepSide.getOppositeSide())); tempOrientation.changeFrame(worldFrame); double finalDesiredPelvisYawAngle = AngleTools.computeAngleAverage(upcomingFootstepOrientation.getYaw(), tempOrientation.getYaw()); upcomingFootstep.getPositionIncludingFrame(upcomingFootstepLocation); upcomingFootstepLocation.changeFrame(ankleZUpFrames.get(upcomingFootstepSide.getOppositeSide())); double desiredSwingPelvisYawAngle = 0.0; if (Math.abs(upcomingFootstepLocation.getX()) > 0.1) { desiredSwingPelvisYawAngle = Math.atan2(upcomingFootstepLocation.getY(), upcomingFootstepLocation.getX()); desiredSwingPelvisYawAngle -= upcomingFootstepSide.negateIfRightSide(Math.PI / 2.0); } swingPelvisYaw.set(desiredSwingPelvisYawAngle); finalPelvisOrientation.set(finalDesiredPelvisYawAngle + swingPelvisYawScale.getDoubleValue() * desiredSwingPelvisYawAngle, 0.0, 0.0); initialize(worldFrame); }
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(); footPolygon.clear(soleFrame); for (int i = 0; i < contactPoints.size(); i++) { contactPoints.get(i).getPosition2d(toeOffContactPoint2d); footPolygon.addVertex(toeOffContactPoint2d); } footPolygon.update(); FramePoint2d rayOrigin; if (!exitCMP2d.containsNaN() && footPolygon.isPointInside(exitCMP2d)) rayOrigin = exitCMP2d; else rayOrigin = footPolygon.getCentroid(); rayThroughExitCMP.set(rayOrigin, exitCMPRayDirection2d); frameConvexPolygonWithRayIntersector2d.intersectWithRay(footPolygon, rayThroughExitCMP); toeOffContactPoint2d.set(frameConvexPolygonWithRayIntersector2d.getIntersectionPointOne()); contactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); contactPointPosition.changeFrame(contactableFoot.getRigidBody().getBodyFixedFrame()); pointFeedbackControlCommand.setBodyFixedPointToControl(contactPointPosition); desiredContactPointPosition.setXYIncludingFrame(toeOffContactPoint2d); desiredContactPointPosition.changeFrame(worldFrame); desiredOrientation.setToZero(contactableFoot.getFrameAfterParentJoint()); desiredOrientation.changeFrame(worldFrame); desiredYawToHold = desiredOrientation.getYaw(); desiredRollToHold = desiredOrientation.getRoll(); }
@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); }
desiredOrientation.setYawPitchRoll(stepYaw, 0.0, 0.0); desiredOrientation.changeFrame(worldFrame); desiredOrientation.setYawPitchRoll(desiredOrientation.getYaw(), stepPitch, stepRoll);