private Matrix3d computeDesiredFootRotation(
double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
RigidBodyTransform supportFootToWorldTransform =
supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
Matrix3d supportFootToWorldRotation = new Matrix3d();
supportFootToWorldTransform.get(supportFootToWorldRotation);
double maxTurnInAngle = 0.25;
double maxTurnOutAngle = 0.4;
double amountToYaw;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
break;
}
case LEFT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
break;
}
case RIGHT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
break;
}
case FORWARD:
{
amountToYaw = angleToDestination;
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
if (swingLegSide == RobotSide.LEFT) {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);
} else {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
}
Matrix3d yawRotation = new Matrix3d();
yawRotation.rotZ(amountToYaw);
Matrix3d ret = new Matrix3d();
ret.mul(yawRotation, supportFootToWorldRotation);
return ret;
}
@Override
public void compute(double time) {
double trajectoryTime = stepTime.getDoubleValue();
isDone.set(time >= trajectoryTime);
time = MathTools.clipToMinMax(time, 0.0, trajectoryTime);
timeIntoStep.set(time);
double percent = time / trajectoryTime;
trajectory.compute(percent);
}
public void compute(double time, boolean adjustAngle) {
this.currentTime.set(time);
time = MathTools.clipToMinMax(time, 0.0, desiredTrajectoryTime.getDoubleValue());
anglePolynomial.compute(time);
double angle = anglePolynomial.getPosition();
double angleDot = anglePolynomial.getVelocity();
double angleDDot = anglePolynomial.getAcceleration();
double cos = Math.cos(angle);
double sin = Math.sin(angle);
double r = initialRadius.getDoubleValue();
double x = r * cos;
double y = r * sin;
double z = initialZ.getDoubleValue();
currentPosition.setIncludingFrame(circleFrame, x, y, z);
yoCurrentPositionWorld.setAndMatchFrame(currentPosition);
currentRelativeAngle.set(
computeAngleDifferenceMinusPiToPi(angle, initialAngle.getDoubleValue()));
if (adjustAngle)
currentAdjustedRelativeAngle.set(
adjustCurrentDesiredRelativeAngle(currentRelativeAngle.getDoubleValue()));
else currentAdjustedRelativeAngle.set(currentRelativeAngle.getDoubleValue());
angle =
trimAngleMinusPiToPi(
currentAdjustedRelativeAngle.getDoubleValue() + initialAngle.getDoubleValue());
if (isDone()) {
angle = finalAngle.getDoubleValue();
angleDot = 0.0;
angleDDot = 0.0;
}
cos = Math.cos(angle);
sin = Math.sin(angle);
x = r * cos;
y = r * sin;
double xDot = -r * sin * angleDot;
double yDot = x * angleDot;
double zDot = 0.0;
double xDDot = -r * cos * angleDot * angleDot - y * angleDDot;
double yDDot = xDot * angleDot + x * angleDDot;
double zDDot = 0.0;
currentPosition.setIncludingFrame(circleFrame, x, y, z);
currentVelocity.setIncludingFrame(circleFrame, xDot, yDot, zDot);
currentAcceleration.setIncludingFrame(circleFrame, xDDot, yDDot, zDDot);
if (rotateHandAngleAboutAxis) {
rotateInitialOrientation(currentOrientation, angle - initialAngle.getDoubleValue());
currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, angleDot);
currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, angleDDot);
} else {
currentOrientation.setIncludingFrame(initialOrientation);
currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
}
yoCurrentPosition.setAndMatchFrame(currentPosition);
yoCurrentAdjustedPositionWorld.setAndMatchFrame(currentPosition);
yoCurrentVelocity.setAndMatchFrame(currentVelocity);
yoCurrentAcceleration.setAndMatchFrame(currentAcceleration);
currentOrientation.changeFrame(trajectoryFrame);
yoCurrentOrientation.set(currentOrientation);
yoCurrentAngularVelocity.setAndMatchFrame(currentAngularVelocity);
yoCurrentAngularAcceleration.setAndMatchFrame(currentAngularAcceleration);
updateTangentialCircleFrame();
}
private FrameVector2d computeDesiredOffsetFromSupportAnkle(
ReferenceFrame supportAnkleZUpFrame,
RobotSide swingLegSide,
double angleToDestination,
double distanceToDestination) {
if (distanceToDestination < DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE) {
return new FrameVector2d(
supportAnkleZUpFrame,
0.0,
swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue()));
}
double absoluteAngleToDestination;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
absoluteAngleToDestination =
Math.abs(AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI));
break;
}
case LEFT:
{
absoluteAngleToDestination =
Math.abs(
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0));
break;
}
case RIGHT:
{
absoluteAngleToDestination =
Math.abs(
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0));
break;
}
case FORWARD:
{
absoluteAngleToDestination = Math.abs(angleToDestination);
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
double minAngleBeforeShorterSteps = 0.1;
double maxAngleBeforeTurnInPlace = 0.5;
double percentToStepInXY =
1.0
- (absoluteAngleToDestination - minAngleBeforeShorterSteps)
/ (maxAngleBeforeTurnInPlace - minAngleBeforeShorterSteps);
if (percentToStepInXY > 1.0) percentToStepInXY = 1.0;
if (percentToStepInXY < 0.0) percentToStepInXY = 0.0;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
double backwardsDistanceReduction = 0.75;
desiredOffsetFromSquaredUp.set(
-backwardsDistanceReduction * percentToStepInXY * desiredStepForward.getDoubleValue(),
0.0);
break;
}
case LEFT:
{
desiredOffsetFromSquaredUp.set(
0.0, percentToStepInXY * desiredStepSideward.getDoubleValue());
break;
}
case RIGHT:
{
desiredOffsetFromSquaredUp.set(
0.0, -percentToStepInXY * desiredStepSideward.getDoubleValue());
break;
}
case FORWARD:
{
desiredOffsetFromSquaredUp.set(
percentToStepInXY * desiredStepForward.getDoubleValue(), 0.0);
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
double stepLength = desiredOffsetFromSquaredUp.length();
double maxDistanceToAllow =
distanceToDestination - 0.5 * DISTANCE_TO_DESTINATION_FOR_STEP_IN_PLACE;
if (maxDistanceToAllow < 0.0) maxDistanceToAllow = 0.0;
if (stepLength > maxDistanceToAllow) {
desiredOffsetFromSquaredUp.scale(maxDistanceToAllow / stepLength);
stepLength = desiredOffsetFromSquaredUp.length();
}
if (stepLength > maxStepLength.getDoubleValue()) {
desiredOffsetFromSquaredUp.scale(maxStepLength.getDoubleValue() / stepLength);
}
FrameVector2d desiredOffsetFromAnkle =
new FrameVector2d(
supportAnkleZUpFrame,
desiredOffsetFromSquaredUp.getX(),
desiredOffsetFromSquaredUp.getY()
+ swingLegSide.negateIfRightSide(desiredStepWidth.getDoubleValue()));
if (swingLegSide == RobotSide.LEFT) {
desiredOffsetFromAnkle.setY(
MathTools.clipToMinMax(
desiredOffsetFromAnkle.getY(),
minStepWidth.getDoubleValue(),
maxStepWidth.getDoubleValue()));
} else {
desiredOffsetFromAnkle.setY(
MathTools.clipToMinMax(
desiredOffsetFromAnkle.getY(),
-maxStepWidth.getDoubleValue(),
-minStepWidth.getDoubleValue()));
}
return desiredOffsetFromAnkle;
}
public void setIntegralLeakRatio(double integralLeakRatio) {
this.integralLeakRatio.set(MathTools.clipToMinMax(integralLeakRatio, 0.0, 1.0));
}