private async Task ExecuteStep(Vector2 direction, float angle, LegFlags forwardMovingLegs, float distance = 1f, float liftHeight = 2)
{
_rotationalOdometry += angle;
angle = angle / 4;
if (forwardMovingLegs != LegFlags.LfRrCross && forwardMovingLegs != LegFlags.RfLrCross)
{
throw new ArgumentException($"{nameof(forwardMovingLegs)} has to be {nameof(LegFlags.RfLrCross)} or {nameof(LegFlags.LfRrCross)}");
}
LegFlags backwardsMovingLegs = forwardMovingLegs == LegFlags.LfRrCross ? LegFlags.RfLrCross : LegFlags.LfRrCross;
var legShift = distance / 4;
// Lift
var nextStep = RelaxedStance
.Transform(new Vector3(legShift * direction.X, legShift * direction.Y, liftHeight), forwardMovingLegs)
.Rotate(new Angle(-angle), forwardMovingLegs)
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs)
.Rotate(new Angle(angle), backwardsMovingLegs);
await ExecuteMove(nextStep);
// Lower
nextStep = nextStep
.Transform(new Vector3(legShift * direction.X, legShift * direction.Y, -liftHeight), forwardMovingLegs)
.Rotate(new Angle(-angle), forwardMovingLegs)
.Transform(new Vector3(-legShift * direction.X, -legShift * direction.Y, 0), backwardsMovingLegs)
.Rotate(new Angle(angle), backwardsMovingLegs);
await ExecuteMove(nextStep);
// Move all
nextStep = nextStep.Transform(new Vector3(-legShift * direction.X * 2, -legShift * direction.Y * 2, 0));
await ExecuteMove(nextStep);
_odometry += Vector2.Transform(direction * distance, Quaternion.CreateFromAxisAngle(Vector3.UnitZ, _rotationalOdometry.DegreeToRad()));
}
private async Task StepToRelaxed(LegFlags legsToAlign, float liftHeight = 2)
{
if (legsToAlign != LegFlags.LfRrCross && legsToAlign != LegFlags.RfLrCross)
{
throw new ArgumentException($"{nameof(legsToAlign)} has to be {nameof(LegFlags.RfLrCross)} or {nameof(LegFlags.LfRrCross)}");
}
await ExecuteMove(RelaxedStance
.Transform(new Vector3(0, 0, liftHeight), legsToAlign));
await ExecuteMove(RelaxedStance);
}
